마커를 붙였으니 이제 바라볼 카메라가 필요하다. 작업대 전체를 위에서 내려다보는 고정 카메라를 하나 붙이고, Gazebo가 만든 카메라 영상을 ROS 2 토픽으로 받는 것까지 진행하였다.

작업은 세 커밋에 나뉘어 있다. 월드 파일은 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)이다. 물체의 회전과 이동을 행렬 곱셈 하나로 동시에 표현하기 위해 사용한다.

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

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

광학 축	행렬의 열	base_link 기준 방향
x (영상 오른쪽)	(0, -1, 0)	-y
y (영상 아래쪽)	(-1, 0, 0)	-x
z (렌즈가 보는 쪽)	(0, 0, -1)	-z

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