이제 arm_control_app 노드가 카메라에서 영상을 받아서 집어야 할 물체의 좌표를 확인하고 물체를 집어서 지정된 위치로 이동시킬 수 있는 모든 준비가 되었다. 회전도 적용되어 있어서 보드가 어느 각도로 놓여 있어도 긴 변의 양쪽을 잡아 목표 지점에 놓게 했다.

커밋: 위치 적용, 회전 적용

그리퍼 위치와 각도 적용

「마커를 로봇의 좌표계로 넘기기」에서 비전 노드가 마커를 marker_0 프레임으로 발행하게 했다. arm_control_app은 TF 버퍼와 리스너를 만들고 base_link에서 marker_0으로 가는 변환을 얻는다. (Writing a listener (C++)).

tf_stamped = tf_buffer_->lookupTransform(
    "base_link", "marker_0", tf2::TimePointZero,
    tf2::durationFromSec(1.0));

인자는 순서대로 기준 프레임(base_link), 대상 프레임(marker_0), 조회 시각, 변환이 버퍼에 없을 때 기다릴 시간이다. 비전 노드에서는 영상이 찍힌 순간의 변환이 필요하기 때문에 타임스탬프를 넘겨주었지만, 여기서는 물체의 위치로 이동을 시작하는 때의 위치가 필요하므로 가장 최근 변환을 얻기 위해 조회 시간을 tf2::TimePointZero로 설정하였다.

변환 결과의 x, y 좌표값을 잡을 대상의 평면상 위치로 사용한다. 높이는 「마커를 로봇의 좌표계로 넘기기」에서 z를 계산하였을 때 결과 값이 −0.017로 나와 오차가 너무 커서 사용할 수가 없었다. 그래서 이미 알고 있는 보드 높이값을 상수 설정하여 사용하였다. ExecutePickAndPlaceCycle은 집는 위치와 회전, 놓는 위치와 회전을 인자로 받게 바꿨다.

bool ExecutePickAndPlaceCycle(std::shared_ptr<ArmController>& arm_ctrl,
    geometry_msgs::msg::Vector3 marker_pos, geometry_msgs::msg::Quaternion marker_rot,
    geometry_msgs::msg::Vector3 target_pos, geometry_msgs::msg::Quaternion target_rot)

회전 적용의 필요

지금까지 그리퍼 자세는 상수였고 손가락은 항상 월드 y 방향으로 닫혔다. 보드는 60 × 40mm이고, 그리퍼 개방폭은 「ArUco 마커 인식」에서 넓힌 70mm다. 보드가 z 축으로 θ만큼 돌아가 있으면 손가락이 닫히는 방향으로 잰 보드 폭은 이렇다.

폭(θ) = 0.06·|sin θ| + 0.04·|cos θ|

보드의 대각선 길이가 가장 길고 그 값은 √(0.06² + 0.04²) = 0.0721m다. 그때 각도는 tan θ = 0.06/0.04 = 1.5에서 θ는 약 56.3°다. 이 각도에서는 계산상으로 보드가 그리퍼의 손가락 사이에 들어가지도 않고, 비스듬하게 집으려고 하기 때문에 정상적으로 집어 올릴 수 없을 것이다.

마커 회전과 그리퍼 자세

marker_0의 z 축은 위를 향한다. 그리퍼는 아래를 봐야 하므로 마커 자세를 자기 x 축 기준으로 180도 뒤집어야 한다. 단위 벡터인 u를 기준으로 θ만큼 도는 회전의 쿼터니언은 아래 수식과 같다(Quaternions and spatial rotation).

단위 벡터: $$\mathbf{u} = (u_x, u_y, u_z) = u_x\mathbf{i} + u_y\mathbf{j} + u_z\mathbf{k}$$

쿼터니언: $$\mathbf{q} = \cos\frac{\theta}{2} + (u_x\mathbf{i} + u_y\mathbf{j} + u_z\mathbf{k})\sin\frac{\theta}{2}$$

x 축 (1, 0, 0)을 기준으로 180도 회전시킨다면 쿼터니언은 아래와 같다.

q = cos(180°/2) + (1·i + 0·j + 0·k)·sin(180°/2)
  = cos 90° + i·sin 90°
  = 0 + 1·i + 0·j + 0·k

실수부를 제일 끝에 쓰는 ROS 2의 (x, y, z, w) 표기법으로 쓰면 (1, 0, 0, 0)이다.

tf2::Quaternion marker_q;
tf2::fromMsg(marker_rot, marker_q);

tf2::Quaternion align;
align.setRPY(0.0, 0.0, M_PI_2);

tf2::Quaternion tcp_q = marker_q * align * tf2::Quaternion(1, 0, 0, 0);

쿼터니언은 곱하는 순서에 따라 결과가 달라진다(Quaternion fundamentals). a * b는 a만큼 돌아간 좌표계에서, 그 좌표계의 축을 기준으로 다시 b만큼 더 돈다는 뜻이다. 그래서 tcp_q = marker_q * align * tf2::Quaternion(1, 0, 0, 0)은 왼쪽부터 읽는다. 코드에서는 마커의 회전을 기준으로 보드의 긴 변의 양쪽을 그리퍼가 잡게 하기 위해서 z축을 중심으로 90도를 회전시켰다. 그 결과에 다시 x축을 기준으로 180도 회전하도록 하였다.

「마커를 로봇의 좌표계로 넘기기」에서 본 것처럼 OpenCV 4.6의 마커 좌표계에서 x축은 마커 그림의 오른쪽이다. 카메라 TF에서 영상 오른쪽은 base_link의 −y다. 보드가 0°일 때 마커 x축이 −y를 향해 yaw가 −90°가 된 것이다. align으로 z 축 90도를 더해 그리퍼 yaw를 보드 yaw와 맞추면, 손가락이 보드의 짧은 변 방향으로 닫혀 긴 변 양쪽을 잡는다.

파지한 보드를 내려 놓을 때에는 코드에서 정한 보드의 목표 자세에 x축을 기준으로 180도 뒤집기 연산만 하면 된다.

tf2::Quaternion place_q = target_q * tf2::Quaternion(1, 0, 0, 0);

보드 각도별 테스트

bringup launch에 board_yaw 인자를 추가해, 보드를 돌린 상태로 스폰할 수 있게 했다. ros_gz_sim의 create에 -Y 인자로 넘긴다(ros_gz_sim).

실행 중에는 Gazebo의 set_pose 서비스로 보드를 돌렸다(UserCommands). 회전은 쿼터니언으로 주며, z 축 ψ 회전의 쿼터니언은 z = sin(ψ/2), w = cos(ψ/2)다. 위치도 함께 적었다.

gz service -s /world/default/set_pose \
  --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean --timeout 2000 \
  --req 'name: "board", position: {x: 0.4, y: 0.6, z: 0.0025}, orientation: {x: 0, y: 0, z: 0.4719, w: 0.8817}'

다섯 각도 모두에서 보드를 집어 목표 지점에 놓았다.

시뮬레이션으로 카메라 영상으로 물체 위치를 파악하고 보드를 잡아 옮기는 작업을 반복하였을 때 ArUco 마커가 있는 위치까지의 도달은 문제가 없었다. 하지만 물체를 집어서 옮기고 내려 놓는 작업을 반복할 때 처음에는 그리퍼가 대상물을 잘 잡아서 옮기는 것으로 보였지만 반복해서 실행시키다 보면 그리퍼가 물체를 잡지 못하였는데 완료로 판단하거나 물체를 잡을 때 그리퍼가 매우 느리게 움직이거나 하는 이상한 동작이 발생하였다.

그리퍼 동작 테스트

처음에는 물리 엔진에서 내가 모르는 마찰이나 충돌이 발생하는 것으로 생각했었다. 그래서, 테스트를 위해서 보드를 잡기 위해 하강하기 직전 위치의 허공에서 그리퍼를 열었다 닫는 작업을 수행시켰다. 예상 했던 것과는 달리 공중에서도 그리퍼를 다 열거다 닫지 않았는데 작업이 완료되거나, 지정한 값까지만 그리퍼를 닫아야 하는데 다 닫혀버리거나, 아예 움직이지 않는 상황 등 예기치 못했던 상황들이 발생하였다. 즉, 다른 물체와의 충돌이나 마찰이 없어도 발생한다는 것이었다.

Gripper URDF 인자 변경

arm_gripper.urdf.xacro 파일에서 effort, velocity, lower 값을 바꿔가면서 테스트를 해 보았다.

그리퍼가 움직이지 못하고 멈추는 것을 보고 effort를 높여 보았다. 그랬더니 그리퍼의 손가락이 서로 충돌해서 튕겨 나가고 끝에서 부딛혀 다시 되돌아 오는 증상이 발생하였다.

너무 빠르게 움직여서 문제가 생기는 것으로 예상하고 velocity를 낮춰 보았다. 이제는 손가락이 튕겨서 열렸다 닫히는 현상은 사라졌지만 손가락이 움직이지 못해서 그리퍼가 닫히지 않는 현상이 발생하였다.

단순히 인자를 변경하는 것으로는 해결되지 않을 것으로 판단되었다.

URDF에서 SDF로의 변환 확인

런치 파일 arm_study_bringup.launch.py는 xacro로 만든 URDF를 그대로 Gazebo에 넘기고, Gazebo가 이것을 SDF로 변환해서 읽는다. 동일한 변환을 직접 실행하여 mimic joint 부분을 확인하였다.

xacro src/arm_description/urdf/ur5e.urdf.xacro ur_type:=ur5e \
  controllers_yaml:=install/arm_bringup/share/arm_bringup/config/arm_controllers.yaml \
  > /tmp/arm.urdf
gz sdf -p /tmp/arm.urdf > /tmp/arm.sdf

변환된 SDF의 finger2_joint에는 <axis> 아래에 <mimic>이 들어 있었다.

<axis>
  <xyz>0 1 0</xyz>
  <mimic joint='finger1_joint'>
    <multiplier>1</multiplier>
    <offset>0</offset>
    <reference>0</reference>
  </mimic>

Gazebo 물리 엔진 테스트

원래 bullet-featherstone을 사용한 이유는 Gazebo의 기본 물리 엔진이 mimic joint를 지원하지 않는다고 하여서 방법을 찾던 중에 https://github.com/ros-controls/gz_ros2_control/issues/340 링크의 글을 읽고 적용한 것이다.

이 물리 엔진은 launch 파일에서 직접 인자로 넘겨 주는데 이 엔진을 지우면 어떻게 될까하는 생각에 인자로 넘겨주던 엔진을 삭제하고 시뮬레이션을 실행해 보았다. 그랬더니, 여전히 그리퍼가 비슷하게 동작을 한다!!!? 가제보 로그를 살펴 보니 다음과 같은 로그가 출력되고 있었다.

[gazebo-10] [INFO] [1791162200.186989363] [gz_ros_control]: Joint 'finger2_joint'is mimicking joint 'finger1_joint' with multiplier: 1 and offset: 0
[gazebo-10] [Err] [Physics.cc:1808] Attempting to create a mimic constraint for joint [finger2_joint] but the chosen physics engine does not support mimic constraints, so no constraint will be created.

즉, gz_ros_control이 <mimic> 태그를 읽어서 뭔가를 하고 있다는 말이다. 소스 코드에 해당 부분을 찾아보니 다음과 같은 부분이 있다. gz_system.cpp

double position_error =
  position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;

double velocity_sp = (-1.0) * position_error * this->dataPtr->update_rate;

위치 오류에 비례해서 속도 명령을 계산하고 있다.

결론적으로 mimic joint에 bullet-featherstone의 물리 제약과 gz_ros2_control의 속도 명령이 동시에 실행되고 있었던 것이다.

ros2_control의 개입을 막음

둘 중 하나를 없애야 하는데 Gazebo의 로그에서 보듯이 기본 물리 엔진인 dartsim이 mimic 제약을 지원하지 않는다는 내용이 있다. 실제 시뮬레이션 화면에서도 그리퍼 동작은 어색하다. 앞서 언급했던 github issue 내용이 맞다는 뜻이므로 물리 제약을 남기고 ros2_control 쪽이 개입하지 않도록 해야 한다.

ros2_control 문서 에 방법이 있다.

If someone wants to deactivate the mimic joint behavior for whatever reason without changing the URDF, it can be done by setting the attribute mimic=false of the joint tag in the <ros2_control> section of the XML.

gripper_joint_control.xacro 파일의 해당 항목을 아래와 같이 수정하였다.

<joint name="${tf_prefix}finger2_joint" mimic="false">

수정 사항을 적용하고 앞서 증상을 잡으려고 바꿨던 값들은 모두 원래대로 되돌렸다. 이후 테스트에서는 그리퍼가 보드를 정상적으로 잡아서 이동시켰고 반복 테스트에도 문제가 발생하지 않았다.

코드 정리와 예외 처리는 커밋 c8b8f25, Ctrl+C 처리는 커밋 61a317e에 있다.

코드 정리

팔을 움직이는 기능과 관련된 부분을 ArmController 클래스로 옮겼다. rclcpp::Node를 상속하고 InitMoveIt, MoveToNamedTarget, MoveToPose, OperateGripper, Stop을 가진다. 물체를 집고 옮겨 놓는 과정을 main.cpp의 ExecutePickAndPlaceCycle 함수에 모아서 코드가 간결해 질 수 있도록 하였다. 팔을 어떻게 움직이는지는 클래스가, 어떤 순서로 무엇을 하는지는 main.cpp로 나눠지면서 코드를 유지 보수하기 훨씬 좋아졌다.

예외와 실패 처리

정리 전의 main은 노드를 스핀하는 스레드를 직접 만들고, 오류가 날 때마다 finish()라는 함수를 불러 빠져나갔다.

static inline int finish(int retcode, std::thread & spinner)
{
  rclcpp::shutdown();
  spinner.join();
  return retcode;
}
...
    return finish(-1, spinner);

return 하는 곳마다 finish()를 잊지 않고 불러야 했다. 또 MoveGroupInterface 생성처럼 예외를 던질 수 있는 곳에서 예외가 나면 finish()를 거치지 않고 main을 빠져나간다. 이때 join하지 않은 std::thread가 소멸되는 상황이 발생하는데, C++ 표준상 join할 수 있는 상태의 std::thread가 소멸되면 std::terminate가 불려 프로그램이 비정상 종료된다(cppreference: std::thread::~thread).

그래서 정리 작업을 객체의 소멸자로 옮겼다. 스핀 스레드는 ExecutorThread 클래스로 감싸서, 소멸자가 executor_를 멈추고(cancel) 스레드를 기다리게(join) 하였다. main이 정상으로 끝나든, 중간에 return하든, 예외로 빠져나가든 지역 객체의 소멸자는 항상 호출된다.

~ExecutorThread()
{
    try {
        executor_.cancel();
        if (thread_.joinable()) {
            thread_.join();
        }
    } catch (...) {
        // 소멸자에서 예외가 빠져나가면 std::terminate 가 불린다
    }
}

rclcpp::shutdown()도 같은 방식으로 처리했다. 빈 포인터를 가진 shared_ptr를 만들어서 해당 인스턴스가 소멸될 때 원래 가리키던 메모리를 해제하는 작업 대신 shutdown()을 호출하도록 람다 함수를 삭제자(deleter)로 지정하였다. 이렇게 함으로써 main이 어떤 경로로 끝나든 이 객체가 소멸될 때 shutdown()이 호출되도록 하였다.

std::shared_ptr<void> rclcpp_guard(nullptr, [](void*) { rclcpp::shutdown(); });

main의 나머지는 try-catch로 감싸서 예외를 받아 로그를 남기도록 하였다. InitMoveIt 함수에서 MoveGroupInterface 생성, 엔드 이펙터 링크 설정, 그리퍼 액션 클라이언트 생성, 바닥 충돌 객체 추가를 차례로 하면서, 한 단계라도 실패하면 로그를 남기고 false를 반환한다.

Ctrl+C를 입력하였을 때 프로그램의 멈춤

팔이 움직이는 도중에 Ctrl+C를 누르면 프로그램이 끝나지 않고 멈추는 현상이 반복되었다. 이동 명령인 move()가 실행 중일 때 Ctrl+C를 입력하면, 함수가 결과를 반환하지 않고 계속 실행 상태로 남아 있었다. Ctrl+C는 SIGINT를 발생시키고 rclcpp는 기본적으로 이 signal을 처리하는 함수를 rclcpp::init()에서 등록한다. 시스템에 설치된 헤더 rclcpp/init_options.hpp의 설명이 다음과 같다.

/// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
bool shutdown_on_signal = true;

rclcpp의 기본 signal 핸들러는 SIGINT를 받으면 컨텍스트, 즉 rclcpp::init()이 생성한 ROS 통신 환경을 종료한다. 스핀은 이 환경이 살아 있는 동안(rclcpp::ok()가 참인 동안)만 콜백을 처리하므로 같이 멈춘다. arm_control_app 노드의 스핀 스레드도, MoveGroupInterface가 내부에 따로 두는 스레드도 모두 같이 멈춘다. 그리퍼 액션의 결과는 노드의 스핀 스레드가, move()의 결과는 MoveGroupInterface의 스레드가 처리해야 하는데 둘 다 멈췄다. 특히 move()는 결과가 올 때까지 아래 코드와 같이 기다린다.(MoveIt 2.12.4 move_group_interface.cpp).

// wait until send_goal_opts.result_callback is called
while (!done)
{
  std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

이 루프는 result_callback이 done 변수값을 true로 바꿔 줄 때만 끝난다. 그런데 result_callback을 호출해 줄 스핀이 멈췄으므로 done 변수의 값은 바뀌지 않고, move()는 반환되지 않는다.

MoveIt의 API 문서에서 asyncMove() 함수를 찾았지만, 이 함수는 동작이 끝날 때까지 기다리지 않고 바로 반환할 뿐 동작이 끝났는지 알려 주는 방법이 없어서 실행 결과를 확인하고 순서대로 진행하는 현재 방식에는 쓸 수 없었다.

Signal을 직접 처리

rclcpp가 signal을 받아도 컨텍스트를 중지시키지 않도록 옵션을 변경하고(shutdown_on_signal = false), signal을 내가 만든 코드에서 직접 처리하도록 하였다. 일반적인 signal 처리 규칙대로 signal 처리 함수에서는 플래그만 설정하도록 하였다.

rclcpp::InitOptions init_options;
init_options.shutdown_on_signal = false;
rclcpp::init(argc, argv, init_options);

std::signal(SIGINT, signal_handler);
std::signal(SIGTERM, signal_handler);

std::atomic<bool> g_quit{false};

void signal_handler(int signum) {
    if(SIGINT == signum || SIGTERM == signum)
    {
        g_quit = true;
    }
}

100ms 주기 타이머가 signal handler에서 설정하는 플래그(g_quit)를 검사하고 ArmController::Stop()을 호출한다. Stop()은 클래스 안의 취소 플래그를 설정하고 move_group_->stop()을 호출하여 진행 중인 이동을 멈춘다. 컨텍스트가 살아 있어 스핀이 계속 돌고 있으므로 move()는 중단 결과를 받고 반환된다.

그리퍼는 결과를 get()으로 한 번에 기다리지 않고, wait_for(100ms)를 반복하면서 취소 플래그를 모니터링한다. 플래그가 true로 설정되면 async_cancel_goal로 액션을 취소하고, 서버가 취소를 끝낼 때까지 한 번 더 기다린 뒤 실패를 돌려준다.

while (rclcpp::ok() && !quit_flag_) {
    if (std::future_status::ready ==
            result_future.wait_for(std::chrono::milliseconds(100))) {
        break;
    }
}

ExecutePickAndPlaceCycle은 단계 사이마다 g_quit를 확인해서 signal을 수신한 경우라면 다음 동작으로 진행하지 않고 함수를 빠져나오도록 하였다. 작업이 중단되면 main은 팔을 시작 자세(test_configuration)로 돌려놓고 끝난다. 그런 다음 ExecutorThread와 rclcpp_guard의 소멸자가 스레드와 컨텍스트를 정리한다.

코드가 계속 수정되기 때문에 글 내용과 코드를 맞출 수 있도록 커밋을 정리하였다.

자세 추정 , 좌표 변환, TF 발행 , 회전 반영 ,로그 간격 , 공용 헤더 패키지

자세 추정

마커의 꼭짓점 픽셀 좌표로 카메라 기준 3차원 위치와 회전을 구하는 함수가 cv::aruco::estimatePoseSingleMarkers다.

cv::aruco::estimatePoseSingleMarkers(corners, MARKER_LENGTH, camera_matrix_,
    dist_coeffs_, rvecs, tvecs);

• corners: detectMarkers가 찾은 꼭짓점이다.
• MARKER_LENGTH: 마커 한 변의 실제 길이다.
• camera_matrix_, dist_coeffs_: camera_info의 K와 D다. 영상과 camera_info를 함께 받도록 구독을 image_transport::create_camera_subscription으로 바꾸고, 첫 프레임에서 한 번만 꺼내 둔다.
• tvecs, rvecs: 좌표 변환의 결과이다. tvec는 마커 중심의 위치(X, Y, Z)이고, rvec는 마커의 회전이다. 둘 다 광학 좌표계 기준이다.

좌표계 변환

로봇팔은 base_link 기준 좌표로 움직이므로, 광학 좌표계에서 얻은 위치를 base_link 기준으로 다시 표현해야 한다.

먼저 위치를 PoseStamped 메시지에 넣는다. PoseStamped는 위치와 회전을 함께 포함하고, header.frame_id에 그 값이 어느 프레임 기준인지를 적는다. header에는 영상 메시지의 헤더를 그대로 넣어, 이 값이 overhead_camera_link_optical 기준으로 영상이 찍힌 시각의 값임을 알려준다. pose.position에는 tvec를 넣는다.

일단 회전을 고려하지 않고 위치만 고려하여 진행 한 뒤에 회전 추가하는 것이 쉬울 것 같아서 회전 부분에는 단위 쿼터니언을 넣고 시작하였다.

다음으로 두 프레임 사이의 변환을 TF에서 얻는다.

cam_to_base = tf_buffer_->lookupTransform(
    "base_link", "overhead_camera_link_optical",
    image_msg->header.stamp, rclcpp::Duration::from_seconds(0.1));

lookupTransform은 TransformListener가 /tf, /tf_static을 구독해 버퍼에 쌓아 둔 변환들에서 두 프레임 사이의 변환을 계산해 돌려준다. 두 프레임이 직접 이어져 있지 않으면 TF 트리를 따라 변환들을 이어 붙인다. 지금 진행하는 시뮬레이션에서는 base_link → overhead_camera_link → overhead_camera_link_optical이다. 첫번째 인자는 데이터를 옮겨 갈 프레임(base_link), 두번째 인자는 데이터의 원본 프레임(overhead_camera_link_optical), 세번째는 원하는 시각, 네번째는 변환이 버퍼에 없을 때 기다릴 시간이다. 지정된 시간 내에 프레임을 찾지 못하면 예외를 던진다. 조회 시각은 영상에 포함된 타임 스탬프로 설정하였다.

lookupTransform이 돌려주는 cam_to_base는 TransformStamped 메시지로, 광학 좌표계를 base_link로 옮기는 이동(카메라 위치)과 회전(쿼터니언)을 담고 있다. 이 변환은 카메라와 base_link 사이의 것이라 영상 속 마커가 몇 개든 같다. 그래서 영상 한 장마다 한 번 조회해 두고, 그 영상에서 찾은 모든 마커에 같은 변환을 쓴다.

마지막으로 tf2::doTransform(입력, 출력, 변환)으로 좌표를 변환한다. 입력 자세에 변환을 곱해, 위치는 회전시킨 뒤 이동하고 회전은 변환의 회전과 합친다. 계산 결과의 frame_id는 base_link가 된다. 위치와 회전까지 함께 변환하기 위해 PoseStamped를 사용했다.

TF 프레임으로 발행

base_link 기준으로 옮긴 마커 위치는 arm_control_app이 잡을 대상체의 위치로 사용한다. 이 위치를 marker_<marker id> 형식으로 TF 프레임에 발행하면 arm_control_app은 lookupTransform("base_link", "marker_0", …)으로 base_link 기준 위치으로 마커의 위치를 얻을 수 있어서 목표 지점을 설정할 수 있다.

TF 프레임은 TransformBroadcaster의 sendTransform 함수로 발행하는데, 이 함수의 인자는 TransformStamped 메시지 형식이다. 그런데 doTransform의 결과는 PoseStamped라서 그대로 인자로 넘길 수 없고, TransformStamped 형식에 맞춰서 데이터를 옮겨 주어야 한다.

두 메시지의 구조를 살펴보면 아래와 같다.

PoseStamped
  header.frame_id      
  pose.position
  pose.orientation

TransformStamped
  header.frame_id      부모 프레임, base_link
  child_frame_id       대상 프레임, marker_0
  transform.translation
  transform.rotation

PoseStamped는 base_link 기준으로 위치와 자세만 알려준다. TF는 이름 붙은 프레임의 트리라서 마커 위치를 프레임에 붙이려면 새 프레임의 이름이 필요하다. 그 자리가 child_frame_id이며 마커 id를 붙인 이름인 marker_0으로 설정했다. 검출된 마커마다 프레임을 만들어 position은 translation에, orientation은 rotation에 옮기고 모든 마커에 대한 처리가 끝난 뒤 한 번에 발행하도록 구성하였다.

회전에 대한 처리 추가

카메라 영상으로 마커의 위치를 로봇에 전달하는 것은 확인하였다. 이제 앞서 미뤄두었던 보드의 회전에 대한 처리를 추가한다.

estimatePoseSingleMarkers는 위치(tvec)와 함께 회전(rvec)도 계산해 준다. rvec는 벡터의 방향이 회전축이고 길이가 회전각(라디안)인 3차원 벡터다. 그런데 회전을 담을 PoseStamped의 pose.orientation은 쿼터니언(x, y, z, w)이라 rvec를 그대로 넣을 수 없다. 그래서 OpenCV의 cv::Quatd::createFromRvec(rvec)로 rvec를 쿼터니언으로 변환하였다.

rvec가 나타내는 회전은 마커 좌표계 기준이다. 마커 좌표계의 축 방향은 estimatePoseSingleMarkers가 계산에 쓰는 마커 꼭짓점 좌표로 결정된다. OpenCV 4.6의 기본값(CCW_center)에서 네 꼭짓점의 좌표는 (−L/2, L/2), (L/2, L/2), (L/2, −L/2), (−L/2, −L/2)이다(aruco.cpp). 그래서 마커를 정면에서 바라볼 때 x축은 마커 그림의 오른쪽, y축은 위쪽을 향한다. 오른손 좌표계이므로 z축은 x축에서 y축으로 감아 도는 방향의 오른손 엄지 방향, 즉 마커 면에서 바라보는 사람 쪽으로 나오는 방향이다. 작업대 위에 놓인 마커에서는 위쪽 즉, 카메라를 향하는 방향이다.

createFromRvec가 돌려주는 값은 OpenCV의 쿼터니언 타입인 cv::Quatd다. cv::Quat의 멤버는 w, x, y, z 순이고 geometry_msgs::msg::Quaternion은 x, y, z, w 순이다. 그래서 memcpy 같은 방식으로 복사하면 문제가 발생한다. 필드 이름으로 하나씩 대입하는 것으로 진행하였다.

테스트를 위해서 보드를 z 축 기준으로 회전시켜 보드의 방향이 변화하는지 확인해 볼 수 있다. 아래는 45도 회전시키는 가제보 명령이다.

gz service -s /world/default/set_pose --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean \
  --timeout 300 --req 'name: "board", position: {x: 0.4, y: 0.6, z: 0.0025}, orientation: {x: 0, y: 0, z: 0.3827, w: 0.9239}'

순환문에서의 로그 출력

검출 좌표를 매 프레임마다 출력하면 너무 많은 메시지가 출력되어 로그를 살펴보기 힘들다. RCLCPP_INFO_THROTTLE로 출력을 해보려고 했지만 이것도 마지막 출력 시간을 기준으로 시간 차이를 사용하는 형태라서 반복문에서 동일한 매크로를 연속적으로 사용하면 시간 간격 설정에 따라 첫 출력 이후의 메시지를 출력하지 못할 수 있다. 매크로 내용을 일부 발췌하면 아래와 같다.

static rcutils_time_point_value_t __rcutils_logging_last_logged = 0;
...
if (RCUTILS_LIKELY(__rcutils_logging_condition)) {
  __rcutils_logging_last_logged = __rcutils_logging_now;
  rcutils_log_internal(&__rcutils_logging_location, severity, name, __VA_ARGS__);
}

마지막 로그 시각을 저장하는 변수가 static 이기 때문에 매크로를 연이어 호출하면 처음 호출한 로그 이후에는 한동안 로그가 출력되지 않는다. 로그 목적별로 출력 시간을 개별적으로 관리할 수 있도록 하기 위해서 LogThrottle 클래스를 만들어 인스턴스를 여러 개 생성하도록 하였다.

LogThrottle disp_marker_throttle{LOG_SHOW_INTERVAL};
LogThrottle disp_transform_throttle{LOG_SHOW_INTERVAL};

이 클래스를 다른 패키지에서도 쓰기 위해 헤더 전용 패키지인 arm_common을 생성하여 거기에 코드를 작성하였다.

빌드할 소스 파일 없이 헤더만 있는 패키지라서, arm_common은 CMake의 INTERFACE 라이브러리로 선언해 헤더 경로만 다른 타겟에 알려 주도록 했다. 헤더 경로는 $<BUILD_INTERFACE:…>와 $<INSTALL_INTERFACE:…>로 나눠 적는데 이는 패키지를 빌드할 때에는 헤더가 현재 작업 중인 디렉토리(src/arm_common/include)에 있고, 설치한 뒤에는 install 디렉토리(install/arm_common/include)로 복사되기 때문이다. log_throttle.hpp가 rclcpp를 쓰므로 target_link_libraries로 헤더를 쓸 때 rclcpp가 함께 링크되도록 했다.

add_library(arm_common INTERFACE)
target_include_directories(arm_common INTERFACE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_link_libraries(arm_common INTERFACE rclcpp::rclcpp)

다른 패키지가 이 헤더를 쓰려면 두 가지가 필요하다. 하나는 빌드할 때 헤더 파일을 install 디렉토리에 복사해 두는 것이다.

install(DIRECTORY include/ DESTINATION include)

다른 하나는 다른 패키지가 이 패키지를 find_package(arm_common)으로 찾았을 때 헤더 경로를 알 수 있도록 정보를 내보내는 것이다.

install(TARGETS arm_common EXPORT arm_commonTargets)
ament_export_targets(arm_commonTargets HAS_LIBRARY_TARGET)
ament_export_include_directories(include)
ament_export_dependencies(rclcpp)

그러면 이 헤더 패키지를 사용하려는 패키지는 find_package(arm_common)과 ament_target_dependencies에 arm_common을 넣는 것만으로 헤더를 쓸 수 있다.

위 내용은 ROS 2 jazzy 문서에서 확인할 수 있다. (https://docs.ros.org/en/jazzy/How-To-Guides/Ament-CMake-Documentation.html#installing)

CMake 설정 전체는 커밋 0ba54dc에 있다.

결과

$ ros2 run tf2_ros tf2_echo base_link marker_0
- Translation: [0.398, 0.603, -0.017]

시뮬레이션에서 보드의 위치가 (0.4, 0.6)이니 x와 y가 3mm의 오차 범위 내에 있다. z는 「ArUco 마커 인식」 글에서처럼 오차가 너무 커서 사용하지 않고 파지할 때에는 이미 알고 있는 고정값을 사용한다.

카메라 영상에서 ArUco 마커를 찾아 영상 안의 픽셀 위치를 구하는 노드를 만드는 과정이다.

arm_vision 패키지를 새로 생성하고 영상 구독 작업까지의 소스 내용은 커밋 242fe09, 마커 검출은 999578d 에 있다.

OpenCV 사용

ROS 2에서 OpenCV는 ROS 2 패키지를 통해서 사용하는 것이 아니라 OpenCV 함수를 직접 호출해서 이미지를 처리하는 방식이다. 따라서 OpenCV를 쓰려면 개발용 라이브러리(헤더와 라이브러리 파일)가 시스템에 설치되어 있어야 한다.

그런데 이 시스템 패키지는 OS마다 이름과 설치 방식이 다르다. 이를 도와주는 ROS 도구가 rosdep이다. rosdep은 직접 설치하지 않고, package.xml에 적힌 의존성 항목을 각 OS의 패키지 이름으로 바꿔 apt, dnf 같은 시스템 패키지 관리자로 설치한다. (rosdep ROS 2 documentation). 의존성 패키지 목록은 공식 저장소인 https://github.com/ros/rosdistro 의 rosdep/base.yaml 파일에서 확인할 수 있다.
OpenCV 패키지의 rosdep 의존성 이름은 libopencv-dev이며 Ubuntu와 Debian에서는 libopencv-dev로, Fedora에서는 opencv-devel로, Arch에서는 opencv로 바뀌어 각 OS의 패키지 설치 도구로 설치된다.

CMake에서는 자체적인 의존성 추가 방법이 있는데 이것이 CMakeLists.txt의 find_package이다. find_package는 먼저 Module mode로 Find<이름>.cmake 파일을 찾고(CMAKE_MODULE_PATH, 그다음 CMake 설치본의 Find 모듈 순), 찾지 못하면 Config mode로 <이름>Config.cmake 또는 <소문자 이름>-config.cmake 파일을 찾는다 (CMake 3.28 find_package). 이 프로젝트에는 FindOpenCV.cmake가 없고, OpenCV 패키지가 /usr/lib/x86_64-linux-gnu/cmake/opencv4/OpenCVConfig.cmake를 설치하므로 Config mode에서 OpenCV라는 이름으로 찾는다.

find_package(OpenCV REQUIRED)  

find_package로 OpenCV를 찾으면 헤더 경로와 라이브러리 목록이 OpenCV_INCLUDE_DIRS, OpenCV_LIBRARIES 변수로 설정된다. 이 변수로 실행 파일에 헤더 경로와 라이브러리를 연결한다.

target_include_directories(arm_vision PRIVATE ${OpenCV_INCLUDE_DIRS})
target_link_libraries(arm_vision ${OpenCV_LIBRARIES})

참고로 ROS 패키지처럼 직접적으로 헤더와 라이브러리를 연결하지 않고 ament_target_dependencies에 OpenCV를 추가하는 것만으로도 동일한 결과를 얻는다.

다음으로 고려해야 하는 사항은 OpenCV의 버전이다. 현재 사용중인 Podman 컨테이너의 OpenCV 버전은 4.6이므로 공식 문서나 예제를 참고할 때 주의해야 한다.

인식 결과 표시

ArUco 마커는 흑백 픽셀로만 검출 작업을 하기 때문에 영상을 전달 받을 때 mono8로 받는다. 그런데 그 영상에 검출 결과를 표시하기 위해 cv::aruco::drawDetectedMarkers(image, corners, ids)로 결과를 그리려면 흑백이 아닌 다른 색상이 필요하다.

void drawDetectedMarkers(InputOutputArray image, InputArrayOfArrays corners,
                         InputArray ids = noArray(),
                         Scalar borderColor = Scalar(0, 255, 0));

기본 테두리 색은 BGR 순서의 초록 Scalar(0, 255, 0)이다. 검출은 흑백 그대로 하고, 표시용 복사본만 컬러로 바꿔 그렸다.

cv::Mat display;
cv::cvtColor(cv_const_ptr->image, display, cv::COLOR_GRAY2BGR);
cv::aruco::drawDetectedMarkers(display, corners, ids);

초록 사각형이 마커 테두리, 파란 글자가 마커 번호, 왼쪽 위 빨간 사각형이 첫 번째 꼭짓점이다. 검출기는 마커의 비트를 읽어 마커의 회전을 알아낸 뒤 마커 무늬의 왼쪽 위 모서리를 첫 번째 꼭짓점으로 알려주기 때문에 이를 이용해서 마커의 방향을 알 수 있다.

검출 위치 확인

다수의 마커가 있는 경우 검출됐다는 로그만으로는 그게 우리가 찾으려는 보드의 마커인지 알 수 없다. 이러한 상황을 가정하여 마커 ID와 꼭짓점 픽셀 좌표를 계산값과 대조해 보았다.

camera_info

영상이 투영된 2차원 픽셀 좌표 계산을 위해서는 카메라가 발행하는 camera_info의 K 행렬이 필요하다.

$ ros2 topic echo /overhead_camera/camera_info --once
...
height: 720
width: 1280
distortion_model: plumb_bob
d: [0.0, 0.0, 0.0, 0.0, 0.0]
k: [1758.7457275390625, 0.0, 640.0,
    0.0, 1758.7458229064941, 360.0,
    0.0, 0.0, 1.0]

d는 렌즈 왜곡 계수다. 실제 카메라는 렌즈의 형상 때문에 영상이 휘어지므로 체커보드 같은 보정판으로 캘리브레이션해서 이 계수를 구하고, 왜곡된 카메라 영상을 펴는 데 사용한다. Gazebo 카메라도 SDF의 <distortion> 요소로 왜곡을 흉내 낼 수 있지만, 지금 시뮬레이션 카메라에는 지정하지 않았으므로 계수가 모두 0이다.

카메라 좌표계의 3차원 점을 2차원 이미지 픽셀 좌표로 투영

K는 카메라 내부 행렬(camera intrinsic matrix)이라 부르는데 카메라 좌표계의 3차원 점을 이차원 이미지의 픽셀 좌표로 투영할 때 사용하는 행렬이다. (https://github.com/ros2/common_interfaces/blob/jazzy/sensor_msgs/msg/CameraInfo.msg)

# Intrinsic camera matrix for the raw (distorted) images.
#     [fx  0 cx]
# K = [ 0 fy cy]
#     [ 0  0  1]

OpenCV의 공식 문서에 잘 설명되어 있다(https://docs.opencv.org/4.6.0/d9/d0c/group__calib3d.html#:~:text=Detailed%20Description). 해당 문서에는 camera intrinsic matrix가 A로 표기되어 있다.

왜곡이 없는 핀홀 카메라의 투영 변환은 아래와 같이 표현된다.

$$sp=A[R|t]P_w$$

여기서 $$P_w$$는 월드 좌표계 관점에서 표현된 3차원상의 한 점이다. $$p$$는 이미지 평면상의 2차원 픽셀이며, A는 카메라 내부 파라미터 행렬(Intrinsic Matrix), $$R$$과 $$t$$는 월드 좌표계에서 카메라 좌표계(또는 카메라 프레임)로의 변환을 나타내는 회전(Rotation)과 이동(Translation)이다.

카메라 내부 파라미터 행렬 $A$ (또는 $K$)는 카메라 좌표계의 3차원 점을 2차원 픽셀 좌표로 투영한다. $$Pc$$는 카메라 좌표계로 변환된 3차원 상의 한 점이다($$[R|t]P_w$$ 계산 후의 3차원 좌표).

$$ p = A P_c $$

내부 행렬 $A$는 픽셀 단위의 초점 거리 $f_x, f_y$와 이미지 중심 부근의 주점(主點, Principal Point) $(c_x, c_y)$로 구성된다.

$$ A = \begin{bmatrix} f_x & 0 & c_x \ 0 & f_y & c_y \ 0 & 0 & 1 \end{bmatrix} $$

따라서 3차원 점 $(X_c, Y_c, Z_c)$와 2차원 픽셀 좌표 $(u, v)$ 사이의 투영 관계는 다음과 같은 행렬식으로 표현된다.

$$ s \begin{bmatrix} u \ v \ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \ 0 & f_y & c_y \ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c \ Y_c \ Z_c \end{bmatrix} $$ 여기서 s는 사영 변환의 스케일 팩터(scale factor)이다. 행렬곱을 전개하면 세 번째 행에서 s = Z_c가 되며, 이는 카메라 광학 축(Z축) 방향으로 내려다 봤을 때 대상 점이 있는 평면까지의 거리이다.

3차원 점이 픽셀이 되는 과정은 ROS image_pipeline 문서의 Derivation Diagram에 그림으로 나와 있다. 네모 친 K, D, R, P가 camera_info에 들어 있는 값이다.

윗줄을 오른쪽에서 왼쪽으로 따라가면 원본 영상으로 투영하는 과정이다. (아랫줄은 왜곡을 펴거나 스테레오 영상을 맞춘 보정 영상을 만드는 경로) 광학 좌표계의 점 X'를 깊이 Z로 나눠, 카메라 앞 거리 1인 정규화 평면에 찍힌 위치(X/Z, Y/Z)를 구한다. 그런 다음 왜곡 계수 D로 점을 휘어진 위치로 옮긴다. K로 초점거리를 곱하고 주점을 더해 픽셀 좌표를 얻는다.

OpenCV의 행렬식에 적용해 보면 다음과 같은 계산식이 나온다.

s·u = fx·X + cx·Z
s·v = fy·Y + cy·Z
s   = Z

행렬식의 세 번째 줄에서 s = Z이므로, 위 두 줄을 Z로 나누면 다음과 같이 픽셀 좌표를 얻을 수 있다.

u = cx + fx · X / Z
v = cy + fy · Y / Z

계산과 관측

월드 좌표계에서 카메라는 (0.5, 0.5, 1.0), 보드는 (0.4, 0.6)에 있고 마커 윗면은 카메라에서 0.995m 떨어져 있다. 보드는 카메라 기준으로 base_link의 x 방향으로 −0.1, y 방향으로 +0.1 떨어져 있다. 카메라 TF로 확인한 축 방향은 영상 오른쪽(광학 x)이 base_link의 −y, 아래쪽(광학 y)이 −x다. 그래서 광학 좌표계에서 보드는 X = −0.1, Y = +0.1, Z = 0.995에 있다.

핀홀 카메라에서 광축에 수직인 평면 위의 점들은 모두 같은 Z를 가진다. 이때 Z는 카메라에서 점까지의 직선 거리가 아니라 광학 좌표계의 원점(핀홀 위치)에서 광축 방향으로 영상이 맺히는 평면까지의 거리이다.

현재 시뮬레이션 상황은 카메라가 바로 위에서 내려다보고 있고 보드가 작업대 위에 평평하게 놓여 있어서 마커 면이 광축에 수직이다. 따라서 윗변의 양 끝은 깊이 Z가 같고 X만 마커 한 변 길이인 0.027m만큼 차이가 나므로, 두 점의 u 차이는 fx × 0.027 / Z다.

검출 노드가 출력한 로그는 이렇다. 마커 번호, 첫 번째와 두 번째 꼭짓점이다. 보드를 회전시키지 않았으므로 두 꼭짓점은 마커 윗변의 양 끝이다.

0 [440, 513] [486, 513]

관측 중심 y는 마커가 회전하지 않은 정사각형이라고 보고 윗변에 한 변의 절반을 더한 값이다. 중심이 맞으므로 카메라 위치와 자세, 초점거리, 보드 배치가 일치한다.

하지만, 계산상 한 변은 47.7픽셀이 나와야 하는데 46픽셀이 나왔다. 위치를 구하는 데는 문제가 없지만, 마커가 영상에서 차지하는 크기로 거리를 계산하는 자세 추정에서는 이 차이만큼 거리가 길게 나온다(마커 한 변의 픽셀 수 = fx × 0.027 / Z에서 픽셀 수가 작으면 Z가 커진다).

원인으로 생각할 수 있는 부분은 OpenCV의 검출 파라미터의 꼭짓점 계산 방식 설정이다. 기본값이 CORNER_REFINE_NONE이라 꼭짓점 좌표가 정수값으로 계산되므로 양쪽 끝에서 1픽셀 안팎으로 어긋날 수 있어서 CORNER_REFINE_SUBPIX로 변경하고 다시 측정해 봤다.

0 [439.285, 513.004] [486.003, 512.989]

축 길이를 픽셀로 계산하면 486.003 − 439.285 = 46.718px이고 이를 광축 깊이로 변환하면 약 1.016이 나온다. 노드가 실제 계산한 위치의 Z값은 아래 출력과 같이 1.017이 나오므로 계산 결과와 마커 인식 결과가 거의 일치한다.

Marker ID: 0, Position: [-0.103, 0.102, 1.017]

결과값으로 봤을 때 보드 높이 오차(0.022m)는 여전히 크다.

오차 보정

카메라 좌표 [-0.103, 0.102, 1.017]을 base_link 기준으로 바꾸면 (0.398, 0.603)이다. 광학 좌표계의 x가 base_link의 -y, y가 -x이므로 x = 0.5 − 0.102, y = 0.5 + 0.103이다. 보드가 (0.4, 0.6)에 있으니 옆방향 오차는 x가 2.0mm, y가 2.5mm, 대각선 3.2mm다. 마커 인식 코드를 작성하고 있던 시점에서 시뮬레이션의 그리퍼는 최대 너비가 50mm였다. 보드 짧은 변이 40mm라 중심을 정확히 측정했다 하더라도 좌우로 각각 5mm의 여유밖에 남지 않는다.

지금 당장 오차를 더 줄이는 것은 힘들 것으로 생각되어 개방폭을 70mm로 넓혔다.

<origin xyz="0 0.04 0.05" rpy="0 0 0"/>
<limit lower="0.0" upper="0.035" effort="20" velocity="0.3"/>

두 면이 맞닿는 지점인 upper도 0.025에서 0.035로 맞췄다. 접촉까지 움직이는 거리가 5mm에서 15mm로 늘어서 velocity도 0.1에서 0.3으로 올렸다.(어떤 영향이 있는지는 확인하지 않았음)

높이를 이미 아는 값으로 고정

높이 방향 오차 22mm는 보상할 방법이 마땅치 않아서 고정값을 쓰는 것으로 결정하였다. 시뮬레이션 환경을 고정된 평판 위에서 작업하는 것으로 가정하였기 때문에 높이가 바뀔 이유가 없다. 높이까지 카메라로 얻어 내려면 stereo camera나 depth camera로 바꿔야 한다. 그렇게 되면 센서 설정과 검출 코드를 모두 변경해야 하기 때문에 시간이 더 걸리게 되고 시뮬레이션을 통한 개념 학습 목적에는 부합하지 않는다고 생각해서 진행하지 않았다.

작업은 세 커밋에 나뉘어 있다. 월드 파일은 7ee5986, 카메라 URDF는 89c55d6, 토픽 이름과 브리지는 2e844e9에 있다.

월드 파일 만들기

지금까지는 Gazebo가 제공하는 default.sdf 월드를 그대로 썼다. 이 파일에는 <plugin>이 하나도 적혀 있지 않아서, Gazebo가 시뮬레이션 구동에 필요한 최소한의 기본 플러그인들(Physics, UserCommands, SceneBroadcaster)을 자동으로 불러온다. 그런데 이 기본 플러그인들에는 센서를 처리하는 Sensors가 포함되어 있지 않아서 카메라를 모델에 넣어줘도 영상을 만들어 출력해 줄 시스템이 없어서 아무런 데이터가 나오지 않는다.

그래서 default.sdf를 복사해 src/arm_bringup/worlds/arm_study.sdf를 만들고 Sensors 플러그인을 추가하였다. 이때 주의할 점이 있다. Gazebo 문서에 따르면 월드에 플러그인을 하나라도 적으면 기본 플러그인을 불러오지 않는다.

By default, if there are no world plugins specified, Gazebo adds the Physics, SceneBroadcaster, and UserCommands plugins. However, if we specify any plugins at all, Gazebo will assume we want to override the defaults, so will not add any default plugins.

Sensors만 적으면 물리 계산까지 사라지므로 기본 세트의 세 가지도 함께 적었다.

<plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
<plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
<plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
<plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
  <render_engine>ogre2</render_engine>
</plugin>

런치 파일의 world_file에 arm_study.sdf를 설정하고, CMakeLists.txt의 설치 대상에 worlds 디렉토리를 추가하였다.

URDF에 카메라 넣기

URDF에 카메라 센서와 링크 정의하기

카메라를 정의하기 위해 overhead_camera.xacro를 새로 만들고 ur5e.urdf.xacro에서 이 파일을 포함하도록 하였다.

카메라 영상에서 찾은 마커 좌표를 로봇 팔의 좌표계로 변환하려면 ROS의 TF 트리에 카메라가 연결되어야 한다. ROS의 robot_state_publisher는 URDF에 정의된 링크에 대해서만 TF를 발행하므로, SDF에 카메라를 정의하면 ROS에서는 카메라의 위치를 알 수 없게된다. 그래서 SDF 파일에는 센서 플러그인 로딩만 하고 카메라 센서를 URDF 파일에 정의하여 TF에 연결할 수 있도록 하였다.

URDF 명세에는 센서를 적는 문법이 없다. 대신 <gazebo reference="링크 이름"> 태그로 적으면 태그 안의 내용은 URDF 파서가 해석하지 않고 그대로 SDF로 넘긴다. 태그 안의 내용은 URDF 변환기가 검사하지 않으므로 틀리게 적어도 URDF 단계에서는 오류가 나지 않고, Gazebo가 SDF를 읽을 때에는 조용히 무시되므로 내용을 잘 검증해야 한다.

<gazebo reference="overhead_camera_link">
  <sensor name="overhead_camera" type="camera">
    <always_on>true</always_on>
    <update_rate>30.0</update_rate>
    <camera>
      <horizontal_fov>0.698</horizontal_fov>
      <optical_frame_id>overhead_camera_link_optical</optical_frame_id>
      <image>
        <width>1280</width>
        <height>720</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>100</far>
      </clip>
    </camera>
  </sensor>
</gazebo>

수평 화각은 앞서 ArUco 마커 생성 때 결정한 40°(0.698 라디안)로 맞췄다. optical_frame_id는 camera_info 메시지 헤더의 frame_id가 된다. 수신한 영상을 어느 좌표계 기준으로 해석할지를 알려 주는 값이다.

인터넷 자료 대부분은 Gazebo Classic 기준이라 주의해야 한다. Gazebo Classic은 센서 안에 libgazebo_ros_camera.so 같은 플러그인을 적어 ROS 토픽까지 발행했다. Gazebo Harmonic은 센서가 Gazebo 토픽에만 발행하고, ROS로 넘기는 일은 브리지가 한다.

질량이 없는 링크에 의한 오류

카메라 링크는 base_link 기준 (0.5, 0.5, 1.0)에 고정하였다. 링크의 x축이 렌즈가 보는 방향이므로, y축을 기준으로 90도(rpy="0 1.5708 0") 돌려 렌즈가 아래를 향하게 하였다.

처음에는 카메라 링크를 형상과 질량 없이 이름만 선언하였다.

<link name="overhead_camera_link" />

실행하니 아래와 같은 에러가 발생하였다.

[Err] [UserCommands.cc:928] Error Code 23: Msg: attached_to name[overhead_camera_link]
specified by frame with name[overhead_camera_joint_optical] does not match a nested
model, link, joint, or frame name in model with name[ur5e].

sdformat의 URDF 변환기 parser_urdf.cc에 그 이유가 있었다.

// Links without an <inertial> block will be considered to have zero mass.
const bool linkHasZeroMass = !_link->inertial || _link->inertial->mass <= 0;

Gazebo는 링크에 관성을 정의하지 않으면 질량이 없는 것으로 간주되어 물리 연산을 할 수 없게된다. 결과적으로 링크로 만들어지지 않게되므로 이 링크를 참조해야 할 자식 관절 (overhead_camera_joint_optical)이 붙을 수 없게 된 것이다. 부모 링크인 overhead_camera_link에 <inertial> 태그를 추가하여 문제를 해결하였다.

광학 링크(overhead_camera_link_optical) 역시 질량이 없지만 끝단 링크이면서 고정 관절이어서 논리 좌표계인 <frame>으로 전환되는 것으로 끝난다.

bullet-featherstone은 모델 하나에 트리 하나만 받는다

질량을 주고 다시 실행하니 다른 에러가 나왔다.

[Err] [SDFFeatures.cc:486] Multiple sub-trees / floating links detected in a model.
This is not supported in bullet-featherstone implementation yet.
[Wrn] [SDFFeatures.cc:946] Floating body / sub-tree detected.
Disabling link: 'overhead_camera_link' from model 'my_robot_arm'.

처음에는 카메라 링크의 부모를 world로 두었다. 그러면 모델 안에 팔과 카메라 두 갈래가 생기고, 둘이 만나는 곳은 world이다. world는 SDF에서 링크로 만들어지지 않으므로, 물리 엔진은 이것을 서로 연결되지 않은 덩어리 두 개(multiple floating links)로 본다. 그런데, 그리퍼의 mimic 조인트 때문에 사용하고 있는 bullet-featherstone 물리 엔진은 다중 서브트리를 지원하지 않는다. 그래서 카메라의 부모를 base_link로 바꿔 트리를 하나로 만들어 주었다. 로봇 팔은 world에서 원점에 고정되어 있으므로 좌표값은 바꿀 필요가 없다.

실제 고정식 카메라는 로봇이 아니라 천장이나 기둥에 붙여서 사용하므로 모델링상으로는 어색하지만 시뮬레이션에는 문제가 없다.

브리지로 영상을 ROS 토픽으로 넘기기

토픽 이름 정하기

Gazebo와 ROS 2는 서로 다른 통신 체계를 사용한다. Gazebo는 gz-transport를, ROS 2는 DDS를 사용하며 메시지 형식도 서로 다르다. 이 둘의 상호 소통을 위해 한쪽의 토픽을 구독하여 다른 쪽 형식으로 변환해 발행해 주는 중계 노드인 ros_gz_bridge를 사용한다.

브리지를 붙이기 전에 Gazebo 쪽 토픽 이름을 확인하니 이렇게 나왔다.

/world/default/model/my_robot_arm/link/base_link/sensor/overhead_camera/image

이 이름은 월드 이름, 모델 이름, 링크 이름, 센서 이름을 이어 붙여 Gazebo가 자동으로 만든 것이다. 따라서, 이 중 하나라도 바뀌면 런치의 브리지 인자도 함께 바꿔야 한다. 아니면 잘못된 토픽을 구독하면서 아무 것도 하지 않는 상황이 발생한다. 그래서 센서에 <topic>으로 이름을 직접 지정하였다.

이름을 정할 때 한 가지 고려해야 할 부분이 있다. 카메라 센서는 두 개의 토픽을 발행한다. 하나가 영상, 다른 하나가 렌즈 정보(camera_info)이다. ROS의 이미지 도구들은 영상 토픽 이름 하나만 전달 받으며 그 전달 받은 이름의 최하위 경로를 camera_info로 변경하여 토픽 이름을 만들어서 구독한다.

예를 들면, overhead_camera 라고 영상 토픽 이름을 지정하면 영상 토픽의 경로는 /overhead_camera로 전달되고 ROS의 이미지 도구는 이 경로에서 overhead_camera를 삭제하고 남은 ‘/’에 camera_info를 붙여서 /camera_info에서 렌즈 정보를 구독하려고 한다. Gazebo도 이와 동일한 방식으로 영상 토픽 이름에서 영상과 카메라 정보 토픽 이름을 만들어서 발행한다.

문제는 이렇게 /camera_info라고 토픽이 발행되면 시스템에 카메라를 여러 개 생성하는 경우 동일한 이름의 토픽을 생성하려고 하는 문제가 발생한다. 따라서, 카메라 센서의 토픽 이름을 최소 2단계의 경로 이름으로 지정하여 특정 이름 아래에 영상과 렌즈 정보 토픽이 생성될 수 있도록 한다. 즉, 센서 토픽 이름을 ‘overhead_camera/image_raw’ 라고 정의하면 렌즈 토픽 이름은 자동적으로 ‘overhead_camera/camera_info’로 생성되므로 토픽 이름이 중복되는 문제를 회피할 수 있다. 관례상 후처리 되지 않은 원본 영상임을 나타내기 위해 image_raw라는 이름을 사용하였다.

<topic>overhead_camera/image_raw</topic>

토픽 이름은 아래와 같이 확인할 수 있다.

gz topic -l | grep overhead_camera
/overhead_camera/camera_info
/overhead_camera/image_raw

브리지 노드

ROS 2에는 ros_gz_image 패키지에 포함된 카메라 이미지 처리 전용 브리지인 image_bridge가 있다. 이 브리지는 image_transport를 거쳐서 영상을 발행하는데 설치된 플러그인에 따라 영상을 원본(raw) 외에도 압축(compressed), 동영상 코덱(theora) 등 여러 토픽으로 함께 내보낼 수 있다. 구독자가 없는 토픽에는 발행하지 않으므로 쓰지 않는 압축에는 연산을 하지 않는다. 압축 전송이 필요해지면 받는 쪽에서 압축 토픽을 구독하기만 하면 된다. 하지만 이 브리지는 camera_info를 처리하지 않기 때문에 이 토픽에 대해서는 일반 브리지가 필요하다.

브리지 매개변수 구조

parameter_bridge 인자는 토픽@ROS 타입[Gazebo 타입 형식으로 이루어져 있다. 이전에 arm_study_bringup.launch.py에 작성했던 clock 브리지를 예로 들면 다음과 같다.

    arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"]

이 문자열은 아래와 같이 5개의 영역으로 분해할 수 있다.

1) /clock (토픽 이름): 브리징할 대상 토픽의 이름 2) @ (구분자): 토픽 이름과 타입을 가르는 기호로 항상 @를 사용 3) rosgraph_msgs/msg/Clock: ROS 2용 메시지 타입 4) [: 방향 기호. 데이터가 흐르는 방향 [는 Gazebo에서 ROS 2 방향으로 전달, ]는 ROS 2에서 Gazebo 방향으로 전달, @는 양방향 통신 5) gz.msgs.Clock: Gazebo측 메시지 타입

런치 파일의 노드 생성

앞서 설명한 바와 같이 camera_info를 전달하기 위해 노드를 따로 생성한다.

overhead_camera_image_bridge = Node(
    package="ros_gz_image",
    executable="image_bridge",
    arguments=["/overhead_camera/image_raw"],
    output="screen",
)

overhead_camera_info_bridge = Node(
    name="overhead_camera_info_gz_bridge",
    package="ros_gz_bridge",
    executable="parameter_bridge",
    arguments=["/overhead_camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo"],
    output="screen",
)

영상의 시각과 use_sim_time

브리지로 받은 영상과 camera_info의 헤더에는 Gazebo가 영상을 만든 시각이 들어 있다. camera_info를 출력해 보면 stamp가 sec: 518처럼 시뮬레이션을 시작한 뒤 흐른 시간으로 찍혀 있다. 그래서 영상의 시각과 TF를 함께 다루는 노드는 자신의 시계도 시뮬레이션 시간에 맞춰야 한다. 노드의 use_sim_time 매개변수를 true로 설정하면 노드의 시계가 시스템 시간 대신 /clock 토픽으로 받은 시뮬레이션 시간을 쓰게 된다. 런치 파일에서 설정하는 경우 매개변수로, ros2 run으로 직접 실행하는 경우 아래와 같이 붙여 준다.

ros2 run arm_control_app arm_control_app --ros-args -p use_sim_time:=true

영상 좌표계와 ROS 좌표계

카메라 링크는 base_link 기준 (0.5, 0.5, 1.0)에 고정하였다.

ROS 축방향은 x축을 전방으로, y축을 왼쪽, z 축을 위로 정의하는 것이 표준이다. 그래서 카메라의 경우 x축이 렌즈가 보는 방향이므로, y축을 기준으로 90도(rpy="0 1.5708 0") 돌려 렌즈가 아래를 향하게 하였다.

여기에서 한 가지 문제가 있다. 카메라 좌표계는 렌즈가 보는 방향이 z축이 된다는 것이다. Camera Info — image_pipeline 3.2.1 documentation

그림에서 보듯이 x가 오른쪽, y가 아래, z가 렌즈 방향이다. OpenCV의 자세 추정 결과도 이 규약으로 나온다. REP 103에서는 이 규약을 쓰는 프레임에 _optical 접미사를 붙이라고 설명하고 있다.

In the case of cameras, there is often a second frame defined with a
"_optical" suffix. This uses a slightly different convention:

* z forward
* x right
* y down

그래서, 카메라의 위치를 나타내는 overhead_camera_link 링크 하나와 카메라의 광학 좌표계를 나타내는 overhead_camera_link_optical 링크, 이렇게 두 개의 링크를 만들고 카메라 위치 링크의 parent는 base_link로, 광학 좌표계 링크는 카메라 위치 링크를 parent로 두는 방법을 사용한다. 이 두 링크는 위치는 같고 회전만 다르다.

광학 링크의 회전값 구하기

이 부분을 이해하려면 먼저 고정축 기준 회전(fixed-axis rotation, extrinsic rotation)과 회전체의 축을 기준으로하는 회전(rotating axis rotation, intrinsic rotation)을 알아야 한다. 쉽게 말하자면 물체를 외부에 고정된 좌표계 관점에서 회전시킬 것이냐, 물체의 좌표계 관점에서 회전시킬 것이냐를 말하는 것이다. 이와 관련해서는 다음 링크의 영상이 도움이 될 것이다.
Rotation Matrices Explained

ROS는 기본적으로 X축 방향이 카메라의 렌즈 방향으로 되어 있으므로 이 좌표계를 광학 좌표계와 일치시키려면 Z축을 중심으로 -90도 회전하여 x축이 카메라 뒤에서 렌즈가 향하는 쪽을 바라봤을 때 오른쪽을 향하게 회전시키고, 다시 이 회전된 x축을 중심으로 -90도 회전시켜 y축이 아래 방향으로 향하게 하면 된다.

[1단계] ROS의 기본 좌표계

            (위쪽) 
              Z     X (앞쪽, 렌즈 방향)
              |   /
              | /
 Y(왼쪽) ------+


[2단계] Z축을 중심으로 -90도 회전 (Yaw)

           (위쪽) 
             Z     Y (앞쪽, 렌즈 방향)
             |   /
             | /
             +------+ X (오른쪽)


[3단계] 새로운 X축을 중심으로 -90도 회전 (Roll)

                Z (앞쪽, 렌즈 방향)
               /
              /
             +------ X (우측)
             |
             |
             Y (아래쪽)

URDF의 rpy 속성은 고정축(extrinsic)을 기준으로 Roll(X) → Pitch(Y) → Yaw(Z) 순서로 정의한다. 앞서 ROS의 좌표계를 광학 좌표계와 일치시키기 위한 회전의 순서는 움직이는 축(Intrinsic)을 기준으로 Yaw(Z) → Pitch(Y) → Roll(X)의 순으로 진행하였지만, 수학적으로 두 회전 행렬의 최종 결과는 동일하다. 그래서 앞서 수행했던 X축 회전(Roll) -90도, Z축 회전(Yaw) -90도를 rpy에 그대로 적용하면 된다. ROS의 좌표계와 카메라 좌표계는 둘 다 오른손 좌표계이기 때문에 한 축을 뒤집는 거울상 변환이 필요 없다. 결과적으로 rpy="-1.5708 0 -1.5708"이 된다.

확인

tf2_echo base_link overhead_camera_link_optical의 출력을 살펴보면 행렬의 열 세 개가 각각 광학 좌표계의 x, y, z축을 base_link 기준으로 나타낸 것이다.

- Translation: [0.500, 0.500, 1.000]
- Matrix:
  0.000 -1.000 -0.000  0.500
 -1.000  0.000 -0.000  0.500
  0.000  0.000 -1.000  1.000
  0.000  0.000  0.000  1.000

Translation은 광학 좌표계의 원점이 base_link를 기준으로 얼마나 떨어져 있느냐를 보여주는 값이다. x=0.5, y=0.5, z=1.0 이므로 앞서 언급한 카메라의 고정 위치와 일치한다.

아래의 행렬은 4x4 동차 변환 행렬(Homogeneous Transformation Matrix)이다. 물체의 회전과 이동을 행렬 곱셈 하나로 동시에 표현하기 위해 사용한다.

$$ T = \begin{bmatrix} R & t \ 0^T & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \ r_{21} & r_{22} & r_{23} & t_y \ r_{31} & r_{32} & r_{33} & t_z \ 0 & 0 & 0 & 1 \end{bmatrix} $$

최하단의 행은 항상 [0, 0, 0, 1]로 고정된다.

출력된 행렬을 분석하면 아래와 같다.

z가 아래를 향하므로 카메라가 작업대를 내려다보고 있다. 보드는 (0.4, 0.6), 카메라는 (0.5, 0.5)에 있으므로 보드는 카메라 기준으로 x가 -0.1, y가 +0.1만큼 떨어져 있다. 위 표를 적용하면 영상 오른쪽(-y) 방향으로 -0.1, 즉 왼쪽이고, 아래쪽(-x) 방향으로 +0.1, 즉 아래쪽이다. Gazebo 화면의 우측 하단 image display에서 확인해 보면 보드가 중앙의 왼쪽 아래에 보인다.

마커 생성 스크립트, 모델 파일, 런치 변경은 커밋 3a0b49d에 있다. 여기서는 마커 종류의 결정 과정과 적용 시 발생한 문제에 대해서 쓴다.

1. 마커 종류 선택

후보는 ArUco, 피두셜 마크, 돔보(트림마크), 로고의 네 가지였다. 돔보나 로고는 검출기부터 만들어야 하는데, 그 작업은 지금 확인하려는 것(카메라 좌표를 로봇 좌표로 옮기는 경로)과는 다른 작업이기 때문에 제외하였다. 실제 PCB에 사용하는 피두셜 마크의 경우 보드의 방향을 알려면 최소 두 개의 마크를 찾아야 하고, 마크가 작아서 카메라가 보드에 가까이 다가가야 한다. 그러려면, 카메라를 로봇팔 끝에 붙여야 하는데 지금 당장 하기에는 난이도가 있는 일이어서 제외하였다. 결과적으로 ArUco 마커를 선택하였다. 검출기가 OpenCV에 들어 있어 따로 만들 필요가 없고, 마커 하나에서 위치와 자세가 함께 나와서 방향 판별이 쉽다.

ArUco 마크의 이진 패턴은 사전이라고 부르는데 격자 크기와 마커 개수에 따라 종류가 달라진다. 이 사전들 중 가장 크기가 작은 DICT_4X4_50를 선택하였다. 격자가 작을수록 같은 물리 크기에서 칸 하나가 커져 멀리서도 검출이 잘 된다. 컨테이너의 OpenCV 버전은 4.6이라 4.7부터 바뀐 API(drawMarker → generateImageMarker등)의 예제를 그대로 쓰면 동작하지 않는다.

2. 검출을 위한 흰 테두리 추가

ArUco 검출기는 검은 테두리와 배경의 경계를 찾아 사각형을 인식한다. 마커 주위에 흰색 테두리(quiet zone)가 없으면 경계를 판단할 수 없다.

4×4 데이터 격자에 검은 테두리를 한 칸 두르면 마커 본체는 6×6칸이다. 그 바깥에 흰 여백을 한 칸 둘러 이미지 전체를 8×8칸으로 만들었다. 이미지는 파이썬 스크립트로 생성하였고 한 칸을 60px로 하여 본체 360px, 전체 480×480px이 되었다.

3. 치수 결정

검출이 잘 되려면 마커가 커야하고, 그러려면 판이 커야 하고, 판이 크려면 보드가 넓어져야 하는데, 보드는 그리퍼 개방폭 안에 들어가야 한다. 기존 시뮬레이션에서 사용하던 보드 크기는 40 x 20mm이어서 마커를 넣기에는 너무 작은 크기라 보드를 키우기로 하였고 그리퍼의 최대 개방폭인 50mm 안에 들어가면서 약간 여유가 있을 수 있도록 60 x 40mm로 변경하였다.

3.1 마커 크기 결정

판은 보드 짧은 변 40mm 안에 들어가도록 36mm로 하였고, 8칸 중 6칸이 마커 본체이므로 36 × 6/8 = 27mm가 된다.

OpenCV의 마커 검출기는 마커가 비스듬히 찍힌 경우 마커의 네 군데 모서리를 찾아낸 뒤 이를 평평한 정사각형 이미지로 잘라낸다. 이때 마커의 1칸(cell)을 몇 픽셀로 샘플링해서 비트를 만들 것인지를 지정하는 perspectiveRemovePixelPerCell 변수가 있다. 이 변수의 기본값은 4, 즉 4 픽셀이다. 즉 마커의 한 셀당 최소 4픽셀은 되어야 한다. arucogrid.com의 가이드에서는 이 값을 선명하고 조명이 좋으며 정면에서 본 경우의 최소값으로 보고, 실전에서는 그 3배를 목표로 하라고 권한다. 시뮬레이션 카메라는 흐림이나 흔들림이 없고 마커를 정면에서 내려다보므로, 최소값의 약 2배인 한 칸 7픽셀을 기준으로 잡았다.

처음에는 카메라 높이를 1m, 카메라 화각이 60도, 해상도를 1280x720으로 가정하고 시작하였다. 이 조건에서 화면의 한 픽셀에 대한 바닥면에서의 실제 길이를 계산하면 다음과 같다.

시야 폭   = 2 × 1m × tan(60°/2) = 2 × 0.577 = 1.155m
픽셀 크기 = 1.155m / 1280 = 0.90mm

마커 한 칸을 7픽셀로 잡았으므로 한 칸이 7 × 0.90 = 6.3mm, 마커가 6칸이므로 약 38mm이다. 하지만, 흰색 여백까지 포함하면 8칸이 되면서 50.4mm가 된다. 마커는 보드 위에 올라가는 형태이므로 기존의 보드 크기에는 마커가 다 올라갈 수 없게 된다. 게다가 그리퍼의 최대 간격보다도 마커의 크기가 크다.

이 문제를 해결하기 위해서는 해상도를 1920x1080으로 높여서 마커의 크기를 줄일 수도 있는데 연산 부하가 커지는 단점이 있기 때문에 카메라의 화각을 줄이는 방향으로 진행하였다.

아래는 카메라 해상도와 화각별 한 픽셀의 크기를 계산한 표이다.

화각을 40°로 좁혀 해상도를 높이지 않고 27mm 마커의 한 칸이 7.9픽셀이 되도록 결정하였다.

4. 텍스처를 이름으로 찾게 하기

SDF 파일에서 텍스처 이미지를 지정할 때에는 아래와 같은 방식으로 지정한다.

<albedo_map>model://board/materials/textures/aruco_4x4_id0.png</albedo_map>

여기서 model://board/materials/textures/aruco_4x4_id0.png에 보이는 것처럼 파일이 위치한 경로가 아니라 모델 이름으로 찾는다. Gazebo는 GZ_SIM_RESOURCE_PATH에 등록된 디렉토리를 뒤져서 model.config가 있는 디렉토리를 모델로 보고, 그중 이름이 board인 것을 고른다. 공유 라이브러리를 LD_LIBRARY_PATH 에서 찾는 것과 같은 구조다.

기존의 작업 디렉토리 구조는 models/ 아래에 board.sdf 파일 하나만 있고 model.config 파일이 없어서 가제보에서 모델 파일로 인식되지 않았고, GZ_SIM_RESOURCE_PATH에 아무것도 등록하지 않아 모델 파일을 검색할 디렉토리도 없었다. 이전에는 런치에서 -file로 파일 경로를 직접 넘겨 주었기 때문에 문제가 없었다.

이제 보드 위에 텍스처를 입히기 위해 디렉토리 구조를 변경해서 src/arm_bringup/models/board/ 디렉토리 아래에 model.config, model.sdf, 텍스처를 모두 모으고, 런치 파일에 GZ_SIM_RESOURCE_PATH를 설정했다.

src/arm_bringup/models/
└── board/                      ← 모델 디렉토리
    ├── model.config            ← 신규. 이게 있어야 모델로 인식
    ├── model.sdf               ← 기존의 board.sdf 
    └── materials/textures/
        └── aruco_4x4_id0.png      

5. 마커가 검게 보임

환경 설정을 마무리하고 시뮬레이션을 실행하니 마커가 정상적으로 보이지 않고 검은색으로만 보이는 현상이 발생하였다. 결론을 먼저 말하자면 마커를 붙여둔 판의 재질이 원인이었다.

<pbr>
  <metal>
    <albedo_map>model://board/materials/textures/aruco_4x4_id0.png</albedo_map>
  </metal>
</pbr>

metalness를 지정하지 않은 것이 문제였다. 지정하지 않으면 기본값이 적용되는데, 그 값이 0이 아니다. 시스템에 설치되어 있는 SDF 명세 파일을 찾아보면 아래와 같이 적혀있다.

/opt/ros/jazzy/opt/sdformat_vendor/share/sdformat14/1.9/material.sdf : 83행
<element name="metalness" type="string" default="0.5" required="0">

기본값이 0.5, 즉 절반은 금속인 재질이 된다. 비금속은 빛을 표면에서 사방으로 흩뿌리므로 광원 하나만 있어도 색이 보이지만, 금속은 확산 반사를 거의 하지 않고 주변 환경을 거울처럼 비추는 것으로만 보인다. 지금 월드에는 태양광 하나뿐인 환경이어서 금속 성분만큼 어둡게 나온다.

metalness를 0으로 두면 확산 반사가 살아나 텍스처가 보인다. roughness도 기본값이 0.5인데 표면이 너무 매끈하면 조명 각도에 따라 흰 부분이 번들거려 검출이 불안정해질 수 있어서 값을 높였다.

<metalness>0.0</metalness>
<roughness>0.9</roughness>

실제 작업 중 발생한 문제들에 대해 정리해 보았다.

1. PATH_TOLERANCE_VIOLATED: 시뮬레이션 바닥 충돌

증상 임의의 좌표로 이동 명령 시 다음과 같이 경로 허용 오차를 위반했다는 오류 메시지를 출력하면서 로봇이 멈추었다.

[ERROR] [moveit.ros.move_group_interface]: MoveGroupInterface::move() failed or timeout reached
[ERROR] [arm_control_app]: Move to position failed

동일한 시간에 출력된 MoveIt 컨트롤러의 출력은 다음과 같았다.

[moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[WARN] [follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller'
       failed with error PATH_TOLERANCE_VIOLATED: Aborted due to path tolerance violation
[INFO] [move_group.move_action]: CONTROL_FAILED

원인 "Goal request accepted!"가 출력된 것으로 보아 목표 경로를 못 찾은 것이 아니라 찾아낸 경로를 로봇이 따라가다가 문제가 발생한 것으로 판단했다. 시뮬레이션 화면을 자세히 보니 내가 지정한 목표 지점이 바닥에 바짝 붙어 있는 곳이어서 이동 중에 로봇 팔이 바닥에 부딪히면서 이동을 못하는 것으로 보였다. 좀 더 알아 보니 MoveIt의 Planning Scene은 URDF에 정의된 로봇 본체의 링크만 알 뿐, Gazebo 환경의 '바닥' 존재를 모른다. 따라서 바닥을 파고드는 궤적을 정상으로 판정해 내보냈으나, 실제 시뮬레이터(Gazebo)에서는 팔이 물리적인 바닥에 막혀 더 내려가지 못했다. 결국 궤적과 실제 관절 위치의 오차가 설정된 허용치(0.2 rad)를 초과하여 강제 중단되었다.

해결책: Planning Scene에 바닥 형상 추가 MoveIt이 바닥을 인식할 수 있도록 C++ 코드에서 Planning Scene에 BOX 형상을 직접 추가했다.

moveit::planning_interface::PlanningSceneInterface iface_ps;
moveit_msgs::msg::CollisionObject ground;
ground.id = "ground";
ground.header.frame_id = "world";
ground.primitives.resize(1);
ground.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
ground.primitives[0].dimensions = { 1.0, 1.0, 0.005 };
ground.primitive_poses.resize(1);
ground.primitive_poses[0].position.z = -0.0025;   // 박스 두께의 절반
ground.operation = moveit_msgs::msg::CollisionObject::ADD;

iface_ps.applyCollisionObject(ground);

• Z 좌표 오프셋 주의: 박스 모델은 중심점을 기준으로 배치된다. 두께 0.005m 박스 윗면을 z=0에 맞추려면 중심을 -0.0025로 설정해야 한다. -0.005로 설정하면 바닥에 2.5mm 틈새가 생겨 로봇이 여전히 파고든다.
• 동기식 반영: 비동기 토픽 방식인 addCollisionObjects() 대신 서비스 호출 방식인 applyCollisionObject()를 사용했다. 호출이 반환됨과 동시에 환경 반영이 보장되므로, 바로 다음 줄에서 안전하게 move() 플래닝을 수행할 수 있다.

2. GOAL_STATE_INVALID: 물리적 도달 불가 목표

증상

[ERROR] [RRTConnect.cpp:265]: Unable to sample any valid states for goal tree
[ERROR] [move_group]: Planner 'OMPL' failed with error code GOAL_STATE_INVALID

원인 테스트용으로 입력한 목표 좌표 (0.0, 0.0, 0.1) 자체가 문제였다. 이 좌표는 베이스 관절 축 바로 위의 매우 낮은 높이로, 로봇이 자기 몸체와 부딪히지 않고는 도달할 수 없는 공간이다. 플래너가 역운동학(IK) 해를 구하지 못해 탐색 시작조차 포기했다.

** 확실한 좌표 도출 및 TCP 지정** 머릿속 추측 대신 다음 2가지 실무적 방법으로 좌표를 구해야 한다.

1. RViz의 MotionPlanning 패널("Joints" 탭)에서 관절을 움직여 원하는 자세를 만든 뒤 tcp_link 좌표 읽기.
2. 시뮬레이션에서 로봇을 움직인 뒤 TF로 직접 측정: ros2 run tf2_ros tf2_echo world tcp_link

또한 툴 중심점 지정 누락에 주의해야 한다. 목표 좌표는 '물체의 위치'가 아니라 '로봇의 특정 링크'를 기준으로 정렬된다.

move_group_interface.setEndEffectorLink("tcp_link");

이를 빼먹으면 기본값인 손목 끝단(tool0)이 목표 좌표로 이동하여 충돌이 발생한다.

3. INVALID_MOTION_PLAN: 그리퍼와 팔뚝 자기 충돌

증상

[INFO]  [moveit_collision_detection_fcl]: Found a contact between 'forearm_link' 
        and 'finger2_link', which constitutes a collision.
[ERROR] [move_group]: PlanningResponseAdapter 'ValidateSolution' failed with error code INVALID_MOTION_PLAN

원인 플래닝 최종 검증 단계에서 자기 충돌(Self-collision)이 감지되었다. 손목이 크게 꺾이면서 손가락(finger2_link)이 로봇 팔뚝(forearm_link)에 닿는 궤적이 생성된 것이다. 기존 ur_moveit_config의 SRDF 충돌 예외 목록(disable_collisions)은 그리퍼가 없는 팔 단독 모델을 기준으로 작성되었기 때문에, 새로 추가된 그리퍼 링크에 대한 검사 규칙이 없다.

** 이동 경로 추가 ** 실제 물리적 충돌 가능성이기 때문에 disable_collisions에 두 링크를 수동으로 예외 처리하는 것은 바람직한 해결책이 아니다. 일단 움직이는 위치를 잡을 대상의 z축 바로 위로 먼저 이동하는 방식으로 경로를 추가하여 로봇 팔의 자세가 좀 더 안정적일 수 있도록 하는 것으로 처리하였다.

4. TIMED_OUT: IK(역운동학) 알고리즘의 수렴 실패

증상 동일한 시작/목표 지점임에도 성공과 실패가 무작위로 번갈아 발생한다. 시간(setPlanningTime)과 시도 횟수(setNumPlanningAttempts)를 크게 늘려도 0.0005초 만에 즉시 실패하는 로그가 반복된다.

[WARN] [ParallelPlan.cpp:138]: Unable to find solution by any of the threads in 0.000516 seconds
[ERROR] [move_group]: Planner 'OMPL' failed with error code TIMED_OUT

경로를 찾지 못한다는 점만 가지고 경로 탐색 엔진을 바꾸 보는 방향으로 진행하였다.

TRAC-IK로 경로 탐색 알로리즘 교체 실패 확률을 낮추기 위해 병렬 연산이 적용된 TRAC-IK를 설치(ros-jazzy-trac-ik-kinematics-plugin)했다. 하지만 이 경로 계획 엔진도 실패 확률이 높기는 마찬가지 였다.

작업 시작점의 자세 변경: 이것 저것 실패 원인에 대한 정보를 찾아 보던 중에 로봇의 자세가 역운동학의 해를 찾기 힘든 원인이 될 수 있다는 정보를 얻었다. 살펴보니 지금까지의 작업 시작점이었던 UR 로봇의 home 자세는 역운동학(IK) 해를 구하기 까다로운 특이점(Singularity)에 해당하는 위치였다 (참고 1, 참고 2). 시작 자세를 test_position으로 변경하고 테스트하니 알고리즘 변경 없이도 반복 테스트 하였을때 목표 지점으로의 경로 계획이 잘 이루어 졌다.

1. 그리퍼 전용 컨트롤러 분리

6축 팔과 그리퍼 조인트(finger1_joint)의 제어를 분리한다. 궤적 제어가 필요한 팔과 달리 그리퍼는 개폐 동작만 수행하므로 전용 컨트롤러를 사용하는 것이 구조적으로 명확하다.

최신 평행 그리퍼 플러그인인 parallel_gripper_action_controller/GripperActionController를 사용한다.

먼저 arm_controllers.yaml 파일을 수정한다. 기존 joint_trajectory_controller 목록에서 그리퍼 조인트를 삭제하고, 새 컨트롤러 설정을 추가한다.

controller_manager:
  ros__parameters:
    # 기존 컨트롤러들...
    gripper_controller:
      type: parallel_gripper_action_controller/GripperActionController

gripper_controller:
  ros__parameters:
    joint: finger1_joint
    allow_stalling: true       # 물체에 걸려 목표치에 도달하지 못해도(stall) 성공으로 처리
    stall_velocity_threshold: 0.001
    stall_timeout: 0.5
    goal_tolerance: 0.002      # 기본값 0.01은 전체 개방폭(0.025m) 대비 헐거우므로 축소

arm_study_bringup.launch.py에 그리퍼 컨트롤러 스포너(Spawner) 노드를 추가한다.

    gripper_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["gripper_controller", "--controller-manager", "/controller_manager"],
    )

    return LaunchDescription([
        # ... 기존 항목 ...
        joint_trajectory_controller_spawner,
        gripper_controller_spawner,
        gz_launch_description,
    ])

2. 시뮬레이션용 대상 물체 추가

그리퍼로 잡을 PCB 보드 모양의 가상 물체를 src/arm_bringup/models/board.sdf에 정의한다.

시뮬레이션에서 물체를 쥐려면 표면 마찰력(friction) 값이 필수다. 마찰력이 없으면 그리퍼가 닫혀도 물체가 미끄러져 떨어진다. SDFormat 스펙에 따라 <collision> 내부 <surface>에 마찰 속성을 추가해야 한다.

가제보 스폰(Spawn) 시 위치 설정에 문제가 있었다. ros_gz_sim create 노드는 파일(SDF) 내부에 기재된 <pose> 값을 읽지 않고 기본값인 원점(0, 0, 0)에 오브젝트를 생성한다. 이를 해결하기 위해 launch 파일에서 스폰 인자로 위치를 직접 명시하는 방식으로 변경하였다.

board_spawn_entity = Node(
    package="ros_gz_sim",
    executable="create",
    output="screen",
    arguments=[
        "-file", PathJoinSubstitution([FindPackageShare("arm_bringup"), "models", "board.sdf"]),
        "-name", "board",
        "-x", "0.4",
        "-y", "0.6",
        "-z", "0.0025",
    ],
)

3. TCP (Tool Center Point) 설정

물체를 파지하는 물리적 기준점이 될 가상 링크를 추가한다.

src/arm_description/urdf/arm_gripper.urdf.xacro 파일에 TCP 링크를 고정 조인트로 정의한다. tool0 링크를 기준으로 그리퍼 끝단(z축 방향 0.075m 지점)에 위치시킨다.

<link name="tcp_link"/>

<joint name="tcp_joint" type="fixed">
  <parent link="tool0"/>
  <child link="tcp_link"/>
  <origin xyz="0 0 0.075" rpy="0 0 0"/>
</joint>

4. 그리퍼 액션 클라이언트 연동

그리퍼 제어는 MoveGroupInterface가 아닌 액션 클라이언트를 통해 gripper_controller로 직접 전달된다. 타겟 메시지 타입은 control_msgs/action/ParallelGripperCommand다. 컨트롤러 이름이 GripperActionController라서 GripperCommand를 쓸 것 같지만 그렇지 않다. 컨트롤러 문서 첫 문장에 ParallelGripperCommand를 실행하는 컨트롤러라고 적혀 있고, 실행 중인 시스템에서는 ros2 action info /gripper_controller/gripper_cmd -t로 확인할 수 있다.

<depend>control_msgs</depend>
<depend>rclcpp_action</depend>

find_package(control_msgs REQUIRED)
find_package(rclcpp_action REQUIRED)
ament_target_dependencies(arm_control_app rclcpp moveit_ros_planning_interface control_msgs rclcpp_action)

ParallelGripperCommand의 목표와 결과는 sensor_msgs/JointState로 되어 있어서 관절 이름과 값을 배열로 넣는다. 배열의 몇 번째가 어떤 관절인지는 보장되지 않으므로 결과를 읽을 때는 이름으로 찾는다.

#include <control_msgs/action/parallel_gripper_command.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

using gripper_action_client_t = rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::SharedPtr;
gripper_action_client_t action_client =
  rclcpp_action::create_client<control_msgs::action::ParallelGripperCommand>(
      node, "gripper_controller/gripper_cmd");

control_msgs::action::ParallelGripperCommand::Goal goal;
goal.command.name = {"finger1_joint"};
goal.command.position = {0.0};     // 0.0 열림, 0.025 닫힘 

if (!action_client->wait_for_action_server(std::chrono::seconds(10))) {
  RCLCPP_ERROR(logger, "Cannot find gripper action server.");
  return -1;
}

rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::SendGoalOptions options;
using GoalHandle = rclcpp_action::ClientGoalHandle<control_msgs::action::ParallelGripperCommand>;

options.goal_response_callback = [=](const GoalHandle::SharedPtr & goal_handle) {
  if (!goal_handle) RCLCPP_ERROR(logger, "Goal was rejected by server");
  else RCLCPP_INFO(logger, "Goal accepted by server, waiting for result");
};

options.result_callback = [=](const GoalHandle::WrappedResult & result) {
  if (result.code != rclcpp_action::ResultCode::SUCCEEDED) {
    RCLCPP_ERROR(logger, "Goal failed with code: %d", static_cast<int>(result.code));
    return;
  }
  const auto & st = result.result->state;
  auto it = std::find(st.name.begin(), st.name.end(), "finger1_joint");
  if (it != st.name.end()) {
    size_t idx = std::distance(st.name.begin(), it);
    RCLCPP_INFO(logger, "Final result: position=%.3f", st.position[idx]);
  }
  RCLCPP_INFO(logger, "stalled=%d reached_goal=%d",
    result.result->stalled, result.result->reached_goal);
};

action_client->async_send_goal(goal, options);

이제 물체를 집을 그리퍼가 필요하다. 상용 그리퍼의 URDF를 가져다 적용할 수도 있겠지만 단순히 물체를 잡는 목적이라면 URDF 작성법에 익숙해질 겸해서 직접 작성하여 사용하는 것이 더 빠를 것으로 생각되어 이 방향으로 진행한다.

1. 손가락 형상

tool0을 부모 링크로 하여 상자 모양 손가락 두 개를 prismatic 관절(일직선으로 미끄러지는 선형 관절)로 붙인다.

src/arm_description/urdf/arm_gripper.urdf.xacro

핵심적인 설정 내용은 다음과 같다.

• axis를 Y축을 기준으로 서로 반대로 설정. 같은 방향이면 관절 값을 증가시킬 때 두 손가락이 나란히 한쪽으로 밀린다. 반대로 줘야 중심을 향해 모인다.
• 관절 값 0을 완전히 열린 상태로 정의한다. 손가락 원점을 Y축 상의 ±0.03 m로, 두께를 0.01 m로 설정한다. 원점이 손가락 링크의 중심에 있으므로 두께 0.01 m의 절반인 0.005 m를 제외하면 안쪽 면의 실제 위치는 ±0.025 m가 된다. 따라서 두 손가락이 중심을 향해 0.025 m만큼 움직였을 때 그리퍼가 완전히 맞닿게 된다. URDF에서는 joint의 <limit> 태그 속성을 lower=0.0, upper=0.025로 지정한다.
• finger2_joint에 <mimic> 속성을 부여. 평행 그리퍼는 두 축이 물리적으로 연동되므로 한쪽만 제어하면 된다. 조인트에 mimic 속성을 주어 finger1_joint가 움직인 만큼 axis 속성에 따라 움직이도록 만든다.

물체를 잡는 지점으로 쓸 tool center point인 tcp_link도 고정 관절(fixed joint)로 연결하여 추가한다. tool0은 UR 로봇의 공구 장착면이고, 손가락 관절 원점이 거기서 z=0.05, 손가락 길이가 0.05이므로 그리퍼의 중심 좌표의 z축 위치는 z=0.075 m다. tcp_link를 그 위치에 둔다.

이렇게 설계한 그리퍼가 파지할 테스트 대상물로 두께 5 mm, 가로 20 mm, 세로 40 mm 크기의 직육면체를 가정했다. 손가락이 맞닿는 가로 폭(20 mm)을 그리퍼의 최대 개방 폭(50 mm)보다 충분히 좁게 설정하여 안정적인 파지 여유를 두었다.

2. 그리퍼 제어 인터페이스

UR 로봇의 ROS 2 패키지인 ur_description 내부의 매크로를 참고하여, 그리퍼용 매크로를 같은 형태로 하나 더 만든다.

src/arm_description/urdf/gripper_joint_control.xacro

finger2_joint에는 <command_interface>를 두지 않는다. mimic joint는 명령을 받는 대상이 아니라 상태만 보고하는 대상이다. 여기에 command_interface를 달면 관절을 움직이는 주체가 둘이 되어 동작에 문제가 발생한다. gz_ros2_control의 GazeboSimSystem 플러그인은 URDF 내의 조인트에 <mimic> 태그가 있으면 이를 읽어서 mimic 쌍으로 등록한다.

생성한 gripper_joint_control.xacro 매크로를 arm_study.ros2_control.xacro에서 포함시키고 시뮬레이션용 Gazebo 플러그인을 하드웨어로 설정한다.

<xacro:include filename="$(find arm_description)/urdf/gripper_joint_control.xacro"/>

3. 그리퍼 전용 컨트롤러

처음에는 그리퍼가 목표 위치로 이동해야 하므로 finger1_joint를 팔의 joint_trajectory_controller에 넣어서 MoveIt이 tcp_link의 위치를 계산해야 한다고 생각했다. 하지만, 살펴보니 tcp_link는 tool0에 fixed 조인트로 부착된 링크이며, MoveIt의 계획 그룹인 ur_manipulator에는 UR 로봇의 6축 관절만 포함되어 있어 그리퍼는 제외된다. 또한, 하나의 command interface는 한 컨트롤러만 점유할 수 있다. joint_trajectory_controller가 finger1_joint의 position 명령을 점유하고 있으면 그리퍼를 여닫기 위한 그리퍼용 컨트롤러가 해당 인터페이스를 제어할 수 없다. (tcp_link를 목표 위치로 설정하는 것과 관련된 내용은 다음에 다시 정리하도록 하겠다.)

우선, 그리퍼 전용 컨트롤러를 새로 만들어 준다.

src/arm_bringup/config/arm_controllers.yaml

controller_manager:
  ros__parameters:
    gripper_controller:
      type: parallel_gripper_action_controller/GripperActionController

gripper_controller:
  ros__parameters:
    joint: finger1_joint
    allow_stalling: true       # 물체를 붙잡아 목표까지 못 가도 성공으로 처리
    stall_velocity_threshold: 0.001
    stall_timeout: 0.5
    goal_tolerance: 0.002 

joint에는 finger1_joint만 적는다. finger2_joint는 mimic이라 컨트롤러가 직접 다루지 않는다.

allow_stalling: true가 핵심이다. 물체를 잡으면 손가락이 목표 위치까지 가지 못한 채 멈추는데, 이를 실패가 아닌 성공으로 처리하라는 뜻이다. 물체를 잡았을 때 성공 신호는 stalled=true가 된다.

4. 작업 중 겪은 문제

RViz에 로봇 모델이 표시되지 않음

형상 확인용으로 joint_state_publisher_gui + RViz 런치를 따로 만들었는데 로봇이 제대로 그려지지 않았다.

이미지에서 보이듯이 RobotModel의 상태가 'Error'로 표시되고 "No transform from [//base] to [base_link]"처럼 모든 링크에 대한 TF를 받지 못하고 있는 상태이다. 원인은 URDF가 아니라 RViz 설정 파일 문제였다. RobotModel 디스플레이의 TF Prefix가 /로 남아 있어 모든 링크 이름 앞에 /를 붙여 TF를 찾다가 실패하고 있었다. 좌측 Display에서 TF Prefix 항목의 '/'를 삭제하면 정상적으로 나타난다.

ResourceStorage: already existing key

실행 시 모든 관절에서 중복 등록 에러가 발생했다.

ResourceStorage: ... already existing key

arm_study.ros2_control.xacro를 ur5e.urdf.xacro와 arm_gripper.urdf.xacro 양쪽에서 include 하고 있었다. 이 파일의 <ros2_control> 블록은 매크로로 감싸여 있지 않아 include 횟수만큼 그대로 펼쳐진다. 그리퍼 관절만이 아니라 모든 관절이 두 번씩 등록된 것이다. 매크로가 아닌 xacro 파일은 include 지점을 한 곳으로 유지해야 한다.

mimic을 인식했다는 로그는 찍히는데 follower가 움직이지 않음

<mimic> 태그를 달고 follower의 인터페이스도 정리한 상태에서, gz_ros2_control은 mimic 쌍을 인식했다는 로그를 정확히 출력했다.

Joint 'finger2_joint' is mimicking joint 'finger1_joint' with multiplier: 1 and offset: 0

그런데 Gazebo에서 finger1_joint를 움직여도 finger2_joint는 따라오지 않았다. 인식은 하고 있으니 URDF나 ros2_control 설정의 문제는 아니었다.

원인은 물리 엔진이었다. gz_ros2_control은 mimic 관계를 파악한 뒤, 두 관절을 묶어 달라고 물리 엔진에 제약(constraint) 생성을 요청한다. 관절을 실제로 연동시키는 주체는 ROS 계층이 아니라 물리 엔진인 것이다. 그런데 Gazebo의 기본 엔진인 dartsim은 이 제약을 구현하고 있지 않다.

같은 증상이 업스트림에도 보고되어 있다 — Error mimic_joints [ROS Jazzy + Gazebo Harmonic] #340.

다행히 bullet-featherstone이라는 시뮬레이션 엔진이 이를 구현하고 있고 Gazebo에 이미 포함되어 있어서, 런치의 gz_args에 엔진을 변경하여 지정하면 되었다.

src/arm_bringup/launch/arm_study_bringup.launch.py

"gz_args": IfElseSubstitution(
    gazebo_gui,
    if_value=[" -r -v 4 --physics-engine gz-physics-bullet-featherstone-plugin ", world_file],
    else_value=[" -s -r -v 4 --physics-engine gz-physics-bullet-featherstone-plugin ", world_file],
)

이 변경으로 두 손가락이 대칭으로 움직이기 시작했다.

====== 2026.10 변경 사항 ======= 이 당시에는 물리 엔진 교체로 모든 문제가 해결된 것으로 보았으나, 반복 테스트에서 그리퍼가 오동작하는 경우가 계속 발생하였다. 원인은 제어기 명령과 물리 엔진 제약이 서로 충돌하면서 발생하는 오류였다. 이 내용은 뒤에 나올 「Mimic Joint 동작 문제」 글에서 다룬다.

지금까지는 RViz의 Motion Planning 패널에서 임의의 위치로 목적지를 설정한 후 Plan/Execute를 실행하여 로봇이 움직이는지 확인하였다. 이번엔 RViz 없이, 내가 직접 C++로 코드를 작성하여 MoveIt에게 목표를 주고 로봇이 움직이도록 하는 노드를 만들고자 한다.

1. 패키지 생성

cd ~/robot-arm-study/src
ros2 pkg create arm_control_app --build-type ament_cmake --dependencies rclcpp moveit_ros_planning_interface

arm_description/arm_bringup과 달리 이 패키지는 실행 노드가 목적이므로 --build-type ament_cmake에 add_executable로 실행 파일을 직접 빌드하도록 구성한다.

• rclcpp: ROS 2 C++ 노드 작성을 위한 기본 클라이언트 라이브러리
• moveit_ros_planning_interface: MoveGroupInterface 등 MoveIt을 코드에서 쓰기 위한 ROS2 패키지.

2. MoveGroupInterface로 목표 지정

src/arm_control_app/src/main.cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <memory>
#include <thread>

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  // NodeOptions에 automatically_declare_parameters_from_overrides(true) 설정 안 하면
  // MoveIt 파라미터를 못 읽어서 초기화 실패
  auto const node = std::make_shared<rclcpp::Node>(
    "arm_control_app",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  auto const logger = rclcpp::get_logger("arm_control_app");

  // MoveGroupInterface 생성자가 내부적으로 서비스 응답을 기다리는데,
  // 그 응답을 처리해줄 스핀이 안 돌고 있으면 여기서 영원히 멈춘다.
  // 그래서 spin을 별도 스레드로 먼저 띄워 둔다.
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // 두 번째 인자 "ur_manipulator"는 SRDF에서 정의한 planning group 이름과
  // 정확히 같아야 한다.
  moveit::planning_interface::MoveGroupInterface move_group_interface(node, "ur_manipulator");

  // SRDF의 group_state 중 "home"으로 이동
  move_group_interface.setNamedTarget("home");
  auto ok = static_cast<bool>(move_group_interface.move());
  if (!ok) {
    RCLCPP_ERROR(logger, "Move to 'home' failed");
  }

  if (true == ok) {
    // 이번엔 좌표를 직접 지정
    geometry_msgs::msg::Pose target_pose;
    target_pose.position.x = 0.4;
    target_pose.position.y = 0.1;
    target_pose.position.z = 0.4;
    target_pose.orientation.w = 1.0;
    move_group_interface.setPoseTarget(target_pose);
    ok = static_cast<bool>(move_group_interface.move());
  }

  if (!ok) {
    RCLCPP_ERROR(logger, "Move to position failed");
  }

  rclcpp::shutdown();
  spinner.join();
  return 0;
}

코드 작성 시 주의해야 할 핵심 포인트는 다음과 같다.

• automatically_declare_parameters_from_overrides(true) — 이 부분을 설정하지 않으면 MoveIt이 필요로 하는 파라미터(로봇 모델, 플래닝 파이프라인 등)를 노드가 못 읽어서 MoveGroupInterface 생성이 실패한다.
• spin을 별도 스레드로 먼저 시작 — MoveGroupInterface의 생성 과정에서 발생하는 내부 서비스 요청/응답을 처리하기 위해 비동기 스레드를 만들어서 spin()을 호출하도록 한다. Asio의 run() 또는 libuv의 uv_run()을 생각하면 된다.
• "ur_manipulator" — SRDF에서 정의한 planning group 이름이다. 정확하게 입력하지 않으면 MoveGroupInterface 생성 단계에서 바로 실패한다.
• setNamedTarget("home") vs setPoseTarget(...) — SRDF에 미리 정의해 둔 named target(관절 각도 조합)으로 갈 수도 있고, 작업 공간 좌표(position + orientation)로 직접 갈 수도 있다. move()는 계획과 실행을 한 번에 한다 — 계획만 하고 싶으면 plan()을 쓴다.

3. CMakeLists.txt

find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

add_executable(arm_control_app src/main.cpp)
ament_target_dependencies(arm_control_app
  rclcpp
  moveit_ros_planning_interface
  geometry_msgs
)

install(TARGETS
  arm_control_app
  DESTINATION lib/${PROJECT_NAME}/
)

4. 빌드 및 실행

colcon build --packages-select arm_control_app --symlink-install

source ~/robot-arm-study/install/setup.bash

Gazebo + ros2_control, MoveIt + RViz가 이미 떠 있는 상태에서, 세 번째 터미널로 이 노드를 실행한다.

터미널 1: Gazebo + ros2_control

vglrun ros2 launch arm_bringup arm_study_bringup.launch.py

터미널 2: MoveIt + RViz

vglrun ros2 launch arm_bringup move_group.launch.py

터미널 3: arm_control_app

vglrun ros2 run arm_control_app arm_control_app

RViz를 보지 않아도, Gazebo 속 팔이 코드에서 지정한 대로 home 자세로 갔다가 지정한 좌표로 움직이면 성공이다.

이전과 마찬가지로 전체 코드는 https://github.com/hwjeon0123/robot-arm-study/ 를 참고한다.

1. 새로운 워크스페이스 생성

기존 ur_ws 환경과 새 코드를 분리하기 위해 robot-arm-study 워크스페이스를 생성하고 오버레이 환경을 구성한다.

• 컨테이너 볼륨 마운트 추가 ```bash

• v "${UR_WS}:/home/${CTR_USERNAME}/ur_ws:rw,z"

• v "${ARM_WS}:/home/${CTR_USERNAME}/robot-arm-study:rw,z"

• ** 컨테이너 파일에서 환경 변수 소싱 (.bashrc)**

  RUN echo '[ -f ~/ur_ws/install/setup.bash ] && source ~/ur_ws/install/setup.bash' >> /home/$USERNAME/.bashrc && \
    echo '[ -f ~/robot-arm-study/install/setup.bash ] && source ~/robot-arm-study/install/setup.bash' >> /home/$USERNAME/.bashrc

  jazzy → ur_ws(underlay) → robot-arm-study(overlay) 순서로 덮어씌운다. -f 옵션으로 빌드 결과물이 존재할 때만 소싱되도록 처리했다.

2. 패키지 분리 생성

ROS 2 매니퓰레이터 패키지 관례에 따라 형상(Description)과 실행(Bringup)을 분리하여 생성한다.

• arm_description: 로봇 모델 형상 정의 (xacro/URDF). 시뮬레이션 및 실물 환경에서 재사용.
• arm_bringup: 실행 구성 (launch, config yaml). 시나리오별로 분리.

mkdir -p ~/robot-arm-study/src
cd ~/robot-arm-study/src
ros2 pkg create arm_description --build-type ament_cmake --dependencies xacro ur_description
ros2 pkg create arm_bringup --build-type ament_cmake --dependencies arm_description robot_state_publisher ros_gz_sim xacro

두 패키지 모두 실행 노드가 아닌 리소스 파일 설치가 목적이므로 --build-type ament_cmake를 사용한다.

패키지 생성 때 의존성 패키지를 지정하면 src/package.xml파일에 추가된다. 각 패키지의 패키지 의존성에 대한 내용은 아래와 같다.

• ur_description: ur_ws의 UR 매크로/메시/파라미터를 $(find ur_description)로 참조하기 위해 필요
• arm_description: 내가 만들 형상 패키지
• robot_state_publisher: URDF 파일을 읽어서 TF(Transform, 좌표계 변환)를 계산하고 발행(Publish)해 주는 ROS 2의 공식 패키지
• ros_gz_sim: ROS 2와 Gazebo를 연결해주는 브릿지 패키지
• xacro → launch에서 xacro 실행

3. URDF 및 Xacro 작성 (arm_description)

ur_description의 매크로를 가져와 최상위 ur5e.urdf.xacro를 작성한다.

~/robot-arm-study/src/arm_description/urdf/ur5e.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur5e">
  <xacro:arg name="name"      default="ur5e"/>
  <xacro:arg name="ur_type"   default="ur5e"/>
  <xacro:arg name="tf_prefix" default=""/>

  <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>

  <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
  <xacro:arg name="kinematics_params"  default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
  <xacro:arg name="physical_params"    default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
  <xacro:arg name="visual_params"      default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>

  <!-- Gazebo 고정 기준 링크 -->
  <link name="world"/>

  <xacro:ur_robot
    name="$(arg name)"
    tf_prefix="$(arg tf_prefix)"
    parent="world"
    joint_limits_parameters_file="$(arg joint_limit_params)"
    kinematics_parameters_file="$(arg kinematics_params)"
    physical_parameters_file="$(arg physical_params)"
    visual_parameters_file="$(arg visual_params)"
    force_abs_paths="true">
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:ur_robot>
</robot>

• parent="world": Gazebo 환경에서 로봇 베이스를 바닥에 고정(fixed joint)하기 위해 추가.
• force_abs_paths="true": Gazebo가 메시 파일을 정상적으로 로드하도록 절대 경로(file:///) 출력을 강제함.

작성 후 CMakeLists.txt에 install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) 규칙을 추가하고, check_urdf 명령으로 파싱 결과를 검증한다.

4. Gazebo 실행 및 ros2_control 설정 (arm_bringup)

4.1 제어 인터페이스 Xacro 작성

컨트롤러 태그를 분리하여 arm_study.ros2_control.xacro를 생성하고, 최상위 URDF 파일에서 include 한다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
 <xacro:arg name="controllers_yaml" default=""/>
 <xacro:include filename="$(find ur_description)/urdf/inc/ur_joint_control.xacro"/>

 <ros2_control name="$(arg name)" type="system">
   <hardware>
     <plugin>gz_ros2_control/GazeboSimSystem</plugin>
   </hardware>
   <xacro:ur_joint_control_description tf_prefix="$(arg tf_prefix)"/>
 </ros2_control>

 <gazebo>
   <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
     <parameters>$(arg controllers_yaml)</parameters>
   </plugin>
 </gazebo>
</robot>

4.2 컨트롤러 YAML 설정

arm_bringup/config 디렉토리에 arm_controllers.yaml을 생성하여 joint_state_broadcaster와 joint_trajectory_controller를 정의한다. 궤적 컨트롤러의 joints 목록은 ur_joint_control.xacro에 정의된 관절 이름과 동일해야 한다.

4.3 런치 파일 구성

arm_bringup/launch 디렉토리에 arm_study_bringup.launch.py를 만들고 다음 구성 요소를 포함한다:

1. xacro 실행 결과 파싱: robot_description_content를 xacro를 이용해서 생성하고 파싱할 때 yaml 파일로 가정하지 않고 단순 문자열로 취급하도록(ParameterValue(..., value_type=str))하여 'Unable to parse the value of parameter robot_description as yaml. 문제를 회피
2. robot_state_publisher 노드 실행: use_sim_time 매개 변수를 true로 설정
3. ros_gz_sim 패키지에서 제공하는 기본 런치 파일(gz_sim.launch.py)을 이용하여 Gazebo 실행
4. ros_gz_sim 패키지의 create 실행 파일을 이용하여 앞서 생성한 robot_description_content를 읽어서 로봇팔이 생성되도록 노드를 실행한다.
5. controller_manager 패키지의 spawner를 실행하여 joint_state_broadcaster(로봇 관절 각도를 보고)와 joint_trajectory_controller(궤적 명령을 받아서 관절 제어) 제어기를 로드한다.

런치 실행 후 ros2 control list_controllers를 통해 2개의 컨트롤러가 active 상태인지 확인한다.

5. MoveIt 2 연동

경로를 계획하고 생성하는 MoveIt이 제어기(Gazebo/ros2_control)로 제어 명령을 전달할 수 있도록 Action 통신 채널을 설정하는 과정이다.

참고할 소스: ~/ur_ws/src/ur_driver/ur_moveit_config/config/moveit_controllers.yaml

패키지 의존성 추가 (arm_bringup/package.xml)

MoveIt 연동을 위한 런치 파일 작성 및 노드 실행을 위해, arm_bringup 패키지의 package.xml에 다음 의존성을 추가해야 한다.

<depend>moveit_configs_utils</depend>
<depend>ament_index_python</depend>
<depend>moveit_ros_move_group</depend>

• moveit_configs_utils: 복잡한 MoveIt 파라미터를 조립해 주는 MoveItConfigsBuilder 모듈 사용 • ament_index_python: 런치 파일 내에서 타 패키지의 절대 경로를 찾기 위한 get_package_share_directory 함수 사용 • moveit_ros_move_group: 최종적으로 구동할 move_group 노드 패키지

5.1 SRDF 및 컨트롤러 주소록 작성

• arm_study.srdf.xacro: ur_moveit_config의 기본 ur_macro.srdf.xacro를 가져와서 기존 로봇의 SRDF 설정을 그대로 사용한다.
• moveit_controllers.yaml: MoveIt 내부에서 궤적 데이터를 던질 Action Client의 정보를 명세한다. Gazebo의 joint_trajectory_controller로 정의한다.

5.2 move_group 런치 파일 작성

MoveItConfigsBuilder를 활용하여 move_group.launch.py를 구성한다. 의존성 파일들의 위치가 각기 다르므로 패키지 기준점을 명확히 지정한다.

    arm_description_share = Path(get_package_share_directory("arm_description"))
    ur_moveit_config_share = Path(get_package_share_directory("ur_moveit_config"))

    moveit_config = (
        MoveItConfigsBuilder(robot_name="ur5e", package_name="arm_bringup")
        .robot_description_semantic(
            arm_description_share / "srdf" / "arm_study.srdf.xacro", {"name": "ur5e"},
        )
        .robot_description_kinematics(ur_moveit_config_share / "config" / "kinematics.yaml")
        .joint_limits(ur_moveit_config_share / "config" / "joint_limits.yaml")
        .planning_pipelines(pipelines=["ompl"])
        .trajectory_execution()
        .to_moveit_configs()
    )

• URDF를 포함하지 않는 이유: MoveItConfigsBuilder에 URDF 경로를 지정하지 않으면, move_group 노드는 자동으로 /robot_description 토픽을 구독하여 값을 채운다.
• trajectory_execution(): 파일 경로를 명시하지 않으면 arm_bringup/config 디렉토리 내의 moveit_controllers.yaml을 자동 탐색한다.

이전과 마찬가지로 전체 코드는 https://github.com/hwjeon0123/robot-arm-study/ 를 참고한다.

6. 최종 실행 및 검증

터미널 2개를 열어 구축한 하위 계층(Gazebo)과 상위 계층(MoveIt 2)을 차례로 기동하여 연동을 확인한다. (하위 제어기가 켜지기 전에 MoveIt을 먼저 실행하면 통신 대상을 찾지 못해 에러가 발생한다.)

• 터미널 1

vglrun ros2 launch arm_bringup arm_study_bringup.launch.py

• 터미널 2

  vglrun ros2 launch arm_bringup move_group.launch.py

  (vglrun은 앞의 글에서 언급했듯이 컨테이너에서 3차원 가속을 사용하기 위해 설치해 둔 VirtualGL을 사용하기 위해 추가하였다)

터미널 로그에 [move_group-1] You can start planning now! 메시지가 출력된다.

(이 글에서 다루는 모든 설정 파일과 자동화 스크립트는 GitHub 저장소: sunshine-headless-setup에서 다운로드할 수 있다.)

🖥️ GPU 가속 원격 데스크탑

모니터가 없는 서버에서 3D 렌더링(예: RViz, Gazebo)이 필요한 어플리케이션을 구동하려면, 일반적인 VNC, XRDP로는 불가능하다.그래서 다른 방법을 찾아 보던 중 Sunshine server와 moonlight 클라이언트가 내가 원하는 3D 렌더링 상태에서의 원격 데스크탑이 가능하다는 것을 알게되어 이를 내 리눅스 시스템에 설정하였다.

X11과 MATE desktop

Ubuntu의 기본 데스크탑인 Gnome-Wayland 조합은 너무 무겁고 모니터가 물리적으로 연결되지 않은 상태에서는 원격 데스크탑 설정이 너무 어렵다. 또한, 컨테이너에서 VirtualGL을 사용하려면 X11이 가장 안정적이라는 점도 있다. 그래서 MATE desktop을 기본 데스크탑으로 변경하였다. 최소 패키지만 필요하기 때문에 mate-desktop-environment를 설치하였다.

sudo apt update
sudo apt install mate-desktop-environment

설치 완료 시점에 디스플레이 매니저(Display Manager)를 선택하는 화면이 나타나면 lightdm을 선택한다. (좀 더 가벼움)

Dummy Display 설정

기본적으로 Sunshine은 항상 Display:0를 찾는다. 하지만, 나는 headless 시스템이므로 가상 디스플레이를 만들고 Sunshine에 이 디스플레이를 설정해 주어야 했다.(저장소의 sunshine-dummy.conf 참고)

• EDID 파일 생성 및 배치: 모니터의 해상도 및 주사율 정보가 담긴 EDID 바이너리 파일을 생성(또는 추출)하여 /etc/X11/dummy_edid.bin 경로에 복사.
• Xorg 설정 파일 작성: /etc/X11/sunshine-dummy.conf 파일을 생성하고, NVIDIA 드라이버가 가상 모니터 EDID를 읽어 화면을 강제 출력하도록 Option "CustomEDID"와 "AllowEmptyInitialConfiguration"을 추가.

Sunshine을 통한 Nvidia NVENC 하드웨어 스트리밍

가상 디스플레이에 그려진 화면을 클라이언트로 전송하기 위해 Sunshine server와 Moonlight client를 사용한다. Nvidia 하드웨어 인코더(NVENC)를 활용하여 시스템 부하를 줄이고 부드러운 화면을 볼 수 있다.

https://github.com/LizardByte/Sunshine/releases 일반적인 RTX-2xxx 이상의 그래픽 카드를 사용 중이라면 위의 공식 릴리즈 페이지에서 자신의 OS에 맞는 패키지를 다운로드 받아서 설치하면 된다. 나의 경우에는 구형 GTX-1050을 사용해야 해서 소스를 받아서 패치 후 빌드하여 인스톨하였다. 아래에 이와 관련된 내용이 설명되어 있다.

가상 모니터를 위한 초기화 스크립트

가상 모니터를 통해 Sunshine을 사용하는 경우 리눅스의 Display Manager를 거치지 않고 시스템 서비스를 통해 실행된다. 따라서, 시스템 환경 변수 설정이나 MATE 데스크톱 환경을 시작하고 Sunshine을 실행시키기 위한 스크립트를 만들어서 시스템 서비스 시작 때 이를 호출하도록 하였다. (https://github.com/hwjeon0123/sunshine-headless-setup/blob/main/configs/start-sunshine-x.sh)

해상도 동적 변경 스크립트 (xrandr)

접속하는 기기(태블릿, 노트북 등)의 화면 비율에 맞춰 해상도가 자동으로 변경되도록 하기 위해 Sunshine의 apps.json 설정에서 준비 명령(prep-cmd)으로 커스텀 쉘 스크립트(sunshine-resolution.sh)를 호출하여, 접속 시 xrandr 명령어로 가상 모니터의 해상도를 동적으로 추가하고 변경할 수 있도록 하였다.

systemd 서비스 등록 (sunshine-xorg.service)

시스템 부팅 시 백그라운드에서 가상 디스플레이를 할당하고 MATE 데스크탑 세션(start-sunshine-x.sh)을 실행하도록 시스템 서비스 유닛을 작성해서 시스템에 등록한다. YOUR_USERNAME은 자신의 아이디로 교체한다.

• WorkingDirectory 항목을 설정해야지 원격 접속 때 홈 디렉토리에서 시작한다.

  [Unit]
  Description=Ultimate Sandbox for Headless Sunshine
  After=network.target
  

[Service]

Replace 'YOUR_USERNAME' with your actual linux username

User=YOUR_USERNAME

CRITICAL FIX: Ensure the working directory is set to the user's home.

Without this, systemd defaults to '/' (root), causing terminal/file managers

to open in '/' and breaking screenshot tools due to write permission errors.

WorkingDirectory=/home/YOUR_USERNAME

Start X server with the dummy configuration and execute the MATE session script

ExecStart=/usr/bin/xinit /home/YOUR_USERNAME/.config/sunshine/start-sunshine-x.sh -- :99 -config sunshine-dummy.conf Restart=always RestartSec=3

[Install] WantedBy=graphical.target

### 참고: 크로미움(Brave/Chrome) 브라우저 GPU 가속 에러

NVIDIA 가상 드라이버(Dummy Xorg)를 통해 하드웨어 가속 세션을 구축했음에도 불구하고, 세션 내에서 Brave나 Chrome 같은 최신 크로미움 기반 브라우저를 실행하면 창이 뜨지 않고 터미널에 다음과 같은 에러를 출력하며 멈추는 증상이 발생하였다.

> `MESA-LOADER: failed to open dri: /usr/lib/x86_64-linux-gnu/gbm/dri_gbm.so: 동적 오브젝트 파일을 열 수 없습니다: 허가 거부`

** 원인: DRM Modesetting 누락 **

이 문제는 그래픽카드가 Xorg에서는 정상적으로 작동하고 있으나, **리눅스 커널 단에서 NVIDIA의 화면 렌더링 신호를 사용자 공간(User Space)의 어플리케이션들에게 넘겨주는 기능(`DRM Modesetting`)이 꺼져 있기 때문**이다.

최신 브라우저들은 하드웨어 가속 렌더링을 위해 GBM(Generic Buffer Management) API를 사용하여 `/dev/dri/renderD128` 등의 렌더링 전용 장치 파일에 접근하려고 시도한다. 하지만 NVIDIA 드라이버 설정에서 DRM Modesetting이 비활성화되어 있어서 이 장치 파일(`/dev/dri/*`)이 아예 생성되지 않았다.

** 해결 방법: 커널 부트 파라미터 추가 **

아래 링크의 문서를 참고하여 커널 부트 파라미터를 추가하였다. 
https://download.nvidia.com/XFree86/Linux-x86_64/580.173.02/README/kms.html

`/etc/default/grub` 파일을 관리자 권한(`sudo`)으로 열고 `GRUB_CMDLINE_LINUX_DEFAULT` 항목을 찾아 맨 끝에 `nvidia-drm.modeset=1` 옵션을 추가한다.
ex) `GRUB_CMDLINE_LINUX_DEFAULT="quiet splash nvidia-drm.modeset=1"` 
수정 후 아래 명령어로 GRUB을 업데이트하고 시스템을 재부팅한다.
```bash
sudo update-grub
sudo reboot

GTX 1050 (Pascal architecture) GPU의 NVENC 인코딩 문제와 해결

나의 경우, 그래픽 카드 2개를 가지고 AI 관련 기능은 고성능 주력 GPU에 맡기고 일반적인 GUI는 구형 GTX 1050 GPU에 전담시키고자 하였다. 그러나 이 구형 그래픽카드 환경에서 Moonlight client로 접속을 시도하면 Multiple reference frames are not supported라는 에러 메시지와 함께 스트리밍 화면이 까맣게 나오거나 끊어지는 현상이 발생하였다.

이 문제는 GTX 1050이 사용하는 Pascal 아키텍처 GPU의 하드웨어적 한계 때문에 발생한다. Sunshine 내부에서 영상 인코딩을 담당하는 FFmpeg은 기본적으로 다중 참조 프레임(B-프레임, refs > 0)을 사용하도록 요청하는데, 구형 GPU의 하드웨어 인코더(NVENC)가 이 다중 참조를 지원하지 못해 인코더에서 오류가 발생하게 되는 것이다.

안타깝게도 공식 배포되는 패키지나 설정 파일 옵션만으로는 이 참조 프레임 요청을 완전히 비활성화할 수 없다. 따라서 Sunshine 소스코드를 직접 다운로드하여 NVENC 초기화 로직을 수정한 뒤 소스 빌드를 진행해야 한다.

src/video.cpp 파일 내 NVENC 초기화 구문을 찾아, 아래와 같이 참조 프레임 변수를 강제로 0 (AUTO)으로 고정하도록 코드를 수정하였다.

// src/video.cpp 
// ... 기존 코드 ...
if (config.numRefFrames && video_format[encoder_t::REF_FRAMES_RESTRICT]) {
  ctx->refs = config.numRefFrames;
} else {
  // GTX 1050 (Pascal) 등 구형 GPU 인코더 에러 방지를 위해 
  // refs를 0 (AUTO)으로 강제 고정
  ctx->refs = 0; 
}

또한 h264_nvenc 및 hevc_nvenc 설정 배열에 {"b_ref_mode"s, 0}과 {"bf"s, 0} 옵션을 명시적으로 추가하여 B-프레임 사용을 차단하였다.

소스 빌드 시 화면 캡처 권한 에러 (Cannot create capture session)

앞서 설명한 GTX 1050 NVENC 패치를 위해 Sunshine 소스코드를 직접 빌드(make install)하여 설치한 후 구동해 보면, 스트리밍이 시작되지 않고 터미널에 아래와 같은 에러가 발생하는 것을 확인할 수 있다.

Cannot create capture session: the display server is in modeset

이 문제는 Linux의 setcap 명령어를 사용하여, 빌드된 Sunshine 실행 파일에 시스템 관리 권한(cap_sys_admin)과 프로세스 우선순위 권한(cap_sys_nice)을 명시적으로 부여해주면 해결된다.

sudo setcap cap_sys_admin,cap_sys_nice+p /usr/local/bin/sunshine

이전에 복제해 둔 저장소는 UR 로봇(ur_driver, ur_description, ur_simulation_gz 등), 모션 플래닝(moveit2 등), 제어(ros2_control 등) 세 영역이 있다. 바이너리 대신 소스로 받는 이유는 업스트림 코드를 직접 읽으며 배우는 게 목적이고, 이 저장소에서 ur_description의 형상 매크로 등을 include해서 재사용하기 때문이다.

주의 — 이미지를 빌드할 때 ur-description, ur-moveit-config, ur-msgs, ur-robot-driver를 apt 바이너리로 같이 설치하면 안 된다. 소스와 바이너리가 겹치면 시뮬레이션 실행 시 충돌한다.

컨테이너 안에서 빌드

컨테이너를 띄운 뒤, 언더레이부터 빌드한다. 오버레이가 언더레이를 소싱해서 쓰는 구조라 순서가 중요하다.

먼저 내가 올려둔 개발 환경을 복제한 위치로 이동한 후 컨테이너를 실행한다.

environment/jazzy.sh

컨테이너 쉘이 나타나면 아래와 같이 ur_ws로 이동한 후 빌드한다.

cd ~/ur_ws 
colcon build
source install/setup.bash

최초 빌드 때에는 패키지가 많아서 시간이 꽤 걸린다.

UR 공식 시뮬레이터로 확인

언더레이가 제대로 빌드됐는지부터 UR 공식 예제로 확인했다.

vglrun ros2 launch ur_simulation_gz ur_sim_moveit.launch.py ur_type:=ur5e

이 런치를 실행하면 Gazebo와 Rviz 둘 다 실행된다. ros2 실행 명령 앞에 vglrun을 넣은 이유는 3D 가속을 컨테이너 환경에서 사용하기 위해서이다. vglrun을 넣을 때와 뺄 때에 대해서 CPU 리소스 사용량을 확인해 보면 된다.

RViz와 Gazebo의 연동 문제

앞의 스크린샷과 같이 RViz와 Gazebo 둘 다 정상적으로 실행 되었다. 그런데 RViz에서 Plan → Execute를 하면 RViz 속 팔만 움직이고 Gazebo 속 팔은 그대로였다.

원인은 컨트롤러 이름 불일치였다. ur_moveit_config는 원래 실물 로봇용 패키지라, 실물 드라이버가 쓰는 scaled_joint_trajectory_controller를 기본적으로 사용하도록 설정되어 있다. 반면 ur_simulation_gz에는 그 컨트롤러가 아예 정의돼 있지 않고 joint_trajectory_controller를 사용하도록 되어 있다. 즉, MoveIt이 존재하지 않는 컨트롤러에게 명령을 보내고 있었던 것.

ros2 control list_controllers로 실제 떠 있는 컨트롤러 이름을 확인하고, MoveIt을 멈추게 한뒤 joint controller(JTC)에 직접 궤적을 던져서 그 아래 구간(JTC → gz_ros2_control → Gazebo)은 이상이 없다는 것을 확인. 그러고 나서 ur_ws/src/ur_driver/ur_moveit_config/config/moveit_controllers.yaml 파일에서 'scaled_joint_trajectory_controller:' 항목을 찾아 'default:'를 false로 바꾸고 'joint_trajectory_controller:' 항목의 'default:'를 true로 변경하여 다시 런치를 실행하였다.

(--symlink-install로 빌드해 둔 덕분에 yaml만 고치면 리빌드 없이 런치 재실행만으로 바로 반영됐다.)

진단 과정과 원인 분석은 저장소 docs에 더 자세히 정리해 뒀다.

아래는 RViz와 Gazebo 연동 화면

정리

• MoveIt을 건너뛰고 컨트롤러에 직접 명령을 넣어본 게 유효했다.
• 지금 수정은 업스트림 파일(ur_moveit_config)을 직접 고친 임시 조치라 git pull 하면 사라진다. 다음 단계에서 직접 작성하는 arm_bringup 쪽에서 파라미터를 덮어쓰는 구조로 바꿔야 한다.

1단계: 필수 권한 설정 (호스트 환경)

Rootless(비-루트 권한) 환경에서 Podman이 호스트의 그래픽 장치에 접근하려면 사용자에게 직접 렌더링 권한이 있어야 한다.

1) 사용자 그룹에 렌더링/비디오 권한 추가

 sudo usermod -aG render $USER
 sudo usermod -aG video $USER

2) 권한 적용을 위해 시스템 재부팅

 sudo reboot

재부팅 후 터미널에 groups 명령어를 입력해 render와 video가 출력되는지 확인한다.

──────

2단계: NVIDIA Container Toolkit 설치 및 설정

GPU 드라이버 라이브러리들을 컨테이너 내부로 자동으로 연결해주는 도구(CDI)를 설치해야 한다.

• CDI (Container Device Interface): Podman이나 Docker 등 컨테이너 런타임이 호스트의 하드웨어(GPU)를 컨테이너 내부에 주입(Injection)하기 위해 사용하는 표준 규격

• 설정 파일 위치: /etc/cdi/nvidia.yaml (시스템 공용) 및 ~/.config/cdi/nvidia.yaml (사용자 전용)

1) NVIDIA Container Toolkit 설치 (이미 설치되어 있다면 최신 상태로 업데이트) 저장소 키 및 리스트 추가

    curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg
    curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list

2) 패키지 업데이트 및 설치

sudo apt-get update
sudo apt-get install -y nvidia-container-toolkit

3) 기존 CDI 파일 제거 과거에 잘못 생성된 설정이 하나라도 남아있으면 Podman이 전체 장치 인식을 거부한다.

sudo rm -f /etc/cdi/nvidia.yaml /var/run/cdi/nvidia.yaml  ~/.config/cdi/nvidia.yaml

Podman(Docker) CDI 연동 및 NVIDIA 드라이버 업데이트 트러블슈팅 가이드

호스트 시스템의 NVIDIA 드라이버가 apt update를 통해 업데이트 된 후 갑자기 컨테이너 실행 때 cannot stat ... No such file or directory 에러가 발생.

원인은 호스트 시스템의 NVIDIA 드라이버가 업데이트되면서 기존 설치 경로가 변경됨. 하지만 기존 생성되어 있던 nvidia.yaml에는 이전 드라이버 버전(libEGL_nvidia.so.580.159.03 등)의 경로가 정적으로 하드코딩되어 있어, 컨테이너 실행 시 오류 발생.

해결 방법

드라이버가 업데이트될 때마다 수동으로 CDI 설정 파일을 새로 발급받아야 한다.
NVIDIA Container Toolkit을 사용하여 호스트 시스템의 현재 드라이버 상태에 맞게 설정 파일을 새로 생성한다.

sudo nvidia-ctk cdi generate --output=/etc/cdi/nvidia.yaml

주의사항 (버전 호환성 문제)

최신 버전의 nvidia-ctk 도구로 파일을 재생성하면 CDI 스펙 v0.7.0 규격으로 만들어지며, 내부에 additionalGids라는 최신 필드가 추가된다. 만약 호스트에 설치된 podman(내부 런타임 crun)이 구형 버전이라면 이 필드를 파싱하지 못해 다음과 같은 에러 메시지가 나타난다.

Error: setting up CDI devices: unresolvable CDI devices nvidia.com/gpu=1

이 에러가 발생할 경우, 재생성된 nvidia.yaml 파일을 구형 Podman이 읽을 수 있도록 수정해줘야 한다.

참고 링크 https://github.com/NVIDIA/nvidia-container-toolkit/issues/1860

1) 파일 에디터로 열기 (예: nano, vim)

    sudo vi /etc/cdi/nvidia.yaml

2) 버전 변경 최상단의 cdiVersion: 0.7.0 (또는 이상)을 cdiVersion: "0.5.0"으로 수정한다. (Podman 5.1.0 부터 cdiVersion: 0.7.0을 지원하기 시작)

3) 미지원 필드 삭제 파일 내부에 있는 additionalGids: 블록과 그 하위 항목(예: - 44, - 110 등)들을 모두 찾아 삭제한다. 주의: additionalGids 외의 장치 이름(- name: "1")이나 다른 설정이 함께 지워지지 않도록 주의해야 한다.

4) 저장 후 완료 수정한 파일을 덮어쓴 뒤 Podman 컨테이너를 재시작하면 정상적으로 GPU가 할당된다.

3단계: 컨테이너에서 3D 가속 쓰기 - VirtualGL

여기까지 하면 컨테이너가 GPU 장치와 드라이버 라이브러리에 접근할 수 있다. 하지만 이것만으로 RViz나 Gazebo 같은 OpenGL 애플리케이션이 GPU로 렌더링되지는 않는다. 이 애플리케이션들은 X 서버를 통해 렌더링하려 하는데, 컨테이너 안에는 X 서버가 없기 때문이다. 이때 필요한 것이 VirtualGL이다.

VirtualGL 설치

GitHub 릴리스에서 .deb를 받아 설치하는 방법이 일반적이지만, 이미지 빌드에서는 apt 저장소를 등록하는 쪽이 관리하기 편하다.

RUN wget -qO - https://packagecloud.io/dcommander/virtualgl/gpgkey \
      | gpg --dearmor -o /etc/apt/trusted.gpg.d/VirtualGL.gpg && \
    echo "deb https://packagecloud.io/dcommander/virtualgl/any/ any main" \
      > /etc/apt/sources.list.d/VirtualGL.list && \
    apt-get update && \
    apt-get install -y --no-install-recommends virtualgl && \
    apt-get clean && rm -rf /var/lib/apt/lists/*

VirtualGL은 기본적으로 렌더링을 담당할 X 디스플레이(보통 :0)를 필요로 한다. 하지만 EGL 백엔드를 쓰면 X 서버를 거치지 않고 GPU 장치에 직접 렌더링하기 때문에 헤드리스 컨테이너 환경에서 쓰기 좋다.

ENV VGL_DISPLAY=egl
ENV PATH="/opt/VirtualGL/bin:${PATH}"

이미지에 환경 변수로 넣어 두면 실행할 때마다 -d egl 옵션을 붙이지 않아도 된다.

그리고, podman 실행 때 아래 옵션을 추가한다. --device nvidia.com/gpu=1은 내 환경에서 맞춘 것이고 --device nvidia.com/gpu=all로 설정해도 된다.

--device nvidia.com/gpu=1
-e NVIDIA_DRIVER_CAPABILITIES=graphics,display,utility
-e VGL_DISPLAY=egl

NVIDIA_DRIVER_CAPABILITIES의 기본값은 compute,utility라서 CUDA는 되지만 OpenGL은 되지 않는다. graphics와 display를 명시해야 컨테이너 안으로 OpenGL/EGL 관련 라이브러리가 함께 연결된다. CDI 설정이 다 맞는데도 RViz만 안 뜬다면 이 항목을 먼저 확인하는 것이 좋다.

GPU 인덱스는 nvidia-smi 출력 순서와 같고, /etc/cdi/nvidia.yaml의 - name: "1" 항목에 대응한다. 앞의 트러블슈팅에서 나온 unresolvable CDI devices nvidia.com/gpu=1 에러가 이 지정이 해소되지 않을 때 발생하는 것이다.

옵션이 빠졌거나 Container Device Interface(CDI) 설정이 잘못되어 있으면 EGL 초기화 실패로 나타난다. 아래 명령으로 GPU 이름이 나오는지 확인한다.

vglrun glxinfo | grep "OpenGL renderer" 

이 설정을 실제로 적용한 Containerfile은 아래 링크에 있다. https://github.com/hwjeon0123/robot-arm-study/blob/main/environment/Containerfile

호스트 쪽 원격 접속 환경은 Sunshine을 사용한 headless Linux의 원격 데스크탑 설정에 정리했다.

이 글들은 ROS 2의 기초를 다루지 않는다. 먼저 ROS 2 공식 튜토리얼의 Intermediate 단계 (https://docs.ros.org/en/jazzy/Tutorials/Intermediate.html) 까지는 학습을 마쳐야 한다. 공식 튜토리얼이 아니더라도 기초 개념과 예제는 웹에 충분히 공개되어 있으므로, 공식 문서를 참조하여 해당 과정을 먼저 마치기 바란다.

ROS 2는 Ubuntu 버전에 의존성이 높아서 시스템에서 사용중인 리눅스 배포판에 따라서 실행이 불가능할 수도 있다. 그래서 컨테이너 환경을 적극적으로 고려하였다.

Docker를 몇 번 써보기는 했지만 root 권한이 필요하고 데몬이 항상 실행되어 있어야 한다던지 여러가지 문제점이 있었다. 그러던 중 Podman을 알게되었고 rootless, 데몬 없음, systemd 친화적 등의 장점이 있어 이를 적극적으로 사용하게 되었다.

Podman 설치는 공식 문서를 참고한다. Podman Installation Instructions

컨테이너 파일의 기본 틀은 아래 링크에서 참고하였다. (Docker 기반) https://automaticaddison.com/the-complete-guide-to-docker-for-ros-2-jazzy-projects/

먼저 전체 소스와 환경을 Github에 올려 두었으니 혹 따라해 보고 싶은 사람은 이를 복제해서 사용한다. https://github.com/hwjeon0123/robot-arm-study/

git clone https://github.com/hwjeon0123/robot-arm-study/ 

컨테이너 파일 내용은 아래 Github 링크를 참고하면 된다. https://github.com/hwjeon0123/robot-arm-study/blob/main/environment/Containerfile

컨테이너 파일에 주석을 일부 달아 두었으며 일부 중요한 사항은 다음과 같다.

• 컨테이너에서 ROS2 패키지 설치는 ROS2의 공식 문서를 참고한다. 지금 사용하는 jazzy의 경우 https://docs.ros.org/en/jazzy/Installation.html

• RUN으로 패키지들을 설치할 때 한 번에 가능한 한 많은 패키지를 설치하고 마지막에 rm -rf /var/lib/apt/lists/* 명령을 추가하여 캐시를 삭제하면 생성되는 이미지 크기를 줄일 수 있다.

• 컨테이너 내부에서 root로 작업을 하면 생성된 파일을 호스트에서 접근하거나 반대인 경우에 문제가 많아진다. 호스트에서 사용중인 자신의 id와 group에 맞추어 컨테이너의 사용자 id와 group을 생성하여 이 문제를 미연에 방지할 수 있다.

  RUN usermod -l $USERNAME ubuntu && \
  groupmod -n $USERNAME ubuntu && \
  usermod -d /home/$USERNAME -m $USERNAME

  또한, 컨테이너에서 패키지 설치 때 sudo 실행 시 암호를 물어보는 경우 설치에 문제가 발생할 수 있으므로 sudoer 설정에서 암호를 물어보지 않도록 설정한다.

  RUN echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/$USERNAME && \
  chmod 0440 /etc/sudoers.d/$USERNAME

• Gazebo는 ROS 버전에 따라 호환되는 버전이 달라진다. 아래 Gazebo Github 링크에서 확인할 수 있다. https://github.com/gazebosim/ros_gz

• 컨테이너 이미지를 빌드하기 전에 UR Robot의 ros2 소스는 vcstool로 호스트의 ur_ws에 미리 복제해 두어야 한다. 호스트의 디렉토리 구조는 아래와 같다. <호스트의 작업 디렉토리>/ ├─ robot-arm-study/ ← 내가 작성한 Github 저장소 복제 (오버레이) └─ ur_ws/ ← 여기에 vcs import (언더레이, 저장소 밖)

  • 이미지를 빌드할 때 호스트의 작업 디렉토리가 기준이 되어 그 디렉토리의 ur_ws/src를 컨테이너 내부의 임시 경로로 복사해 rosdep로 의존성만 미리 설치한 뒤 임시로 복사한 소스는 지운다. 컨테이너를 실행할 때 podman 명령줄의 옵션으로 <호스트의 작업 디렉토리>/ur_ws 와 컨테이너의 ~/ur_ws를 매핑하여 사용한다.

위 내용들이 저장소의 README.md와 environment/build.sh, environment/jazzy.sh 에 적용되어 있다.

그리고, 한가지 내 개발 환경이 일반적이지 않은 부분이 있다. environment/jazzy.sh을 보면 NVIDIA 그래픽 카드 번호 1번을 사용하고 있다. 이유는 내가 사용하는 개발 환경에 그래픽 카드가 두 개가 있기 때문이다. 하나는 local LLM을 사용하기 위한 그래픽 카드이고 다른 하나는 일반 용도의 구형 그래픽 카드이다. 이 두 카드 중 구형 그래픽 카드의 번호가 1번이어서 이 그래픽 카드가 Podman 컨테이너에서 사용될 수 있도록 설정되어 있다.

그리고 GPU를 컨테이너 환경에서 사용하는 방법에 대한 설명은 따로 글을 올려 두었으니 참고하기 바란다. Podman 컨테이너 환경에서 NVIDIA 그래픽 라이브러리 사용

이제 컨테이너 파일의 작성이나 수정이 완료되면 호스트에서 Podman 이미지를 빌드 할 수 있다.

<작업 디렉토리>/environment/build.sh 

빌드가 완료되면 컨테이너의 쉘로 진입하여 잘 동작하는지 확인한다.

<작업 디렉토리>/environment/jazzy.sh 

실행중인 컨테이너는 아래와 같이 podman 명령으로 중지할 수 있다.

podman stop robot-arm-study 

Turtlesim을 실행시켜서 잘 동작하는지 확인해 본다.

vglrun ros2 run turtlesim turtlesim_node

참고로 컨테이너 이미지를 실행 할 때 --rm 옵션을 추가하여 컨테이너를 중지하면 컨테이너가 삭제될 수 있도록 하였다. 컨테이너를 지우지 않고 남겨두면 그 안에 임시로 설치한 패키지나 파일이 남게 되는데 이 일이 반복될수록 Containerfile과 컨테이너 내용이 서로 어긋나게 되어 새로 이미지를 빌드할 경우 정상 동작하지 않는 경우가 발생할 수 있다. 필요한 패키지가 늘어나거나 환경을 변경해야 할 상황이 있다면 Containerfile을 수정한 후 이미지를 다시 빌드하는 방향으로 처리하는 것이 좋다.