ros2 moveit TF 표시

moveit_setup_assistant를 사용하면 대상이 되는 package에서 urdf를 추출한 다음, 필요한 설정 파일을 만든 후 package+moveit_config 디렉토리에 생성한 파일을 저장한다. lanch 디렉토리에 demo.launch.py 파일이 있는데, 코드에는 별 내용은 없고, 같은 디렉토리 launch 파일들을 실행한다. 최소한 어느 부분을 실행해야 rviz에서 로봇이 제대로 보일지 궁금했다

demo.launch.py 파일이 다음을 실행한다.

• static_virtual_joint_tfs
• robot_state_publisher
• move_group
• rviz
• ros2_contol_node + controller spawners

robot_state_publisher만 실행하면 로봇이 정확하게 보일 줄 알았다. robot_state_publisher가 입력으로 robot_description을 받아들이고, TF는 robot_description에 있는 링크, 조인트 관계만 읽어들여 알아낼 줄 알았다. move_group은 path를 찾을 때 사용하므로 당장 필요해 보이지 않았다.

from launch import LaunchDescription
from moveit_configs_utils import MoveItConfigsBuilder
from launch_ros.actions import Node
from moveit_configs_utils.launches import generate_rsp_launch, generate_moveit_rviz_launch
from launch.actions import (
        DeclareLaunchArgument,
        IncludeLaunchDescription,
        )
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("abb_irb2400", package_name="hello_moveit_config").to_moveit_configs()
    #dict2 = moveit_config.to_dict()
    #for key, val in (dict2.items()):
    #    print(key, ":", val) 
    ld = LaunchDescription()
    ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0"))

    rsp_node = Node(
            name = "rsp_node",
            executable = "robot_state_publisher",
            #prefix = "xterm -e gdb run --args",
            package = "robot_state_publisher",
            output = "both",
            respawn = True,
            #namespace = 'my_robot',
            parameters=[
                moveit_config.robot_description,
                {
                    "publish_frequency": LaunchConfiguration("publish_frequency"),
                    },
                #moveit_config.robot_description_semantic,
                #moveit_config.robot_description_kinematics,
                ],
            )
    ld.add_action(rsp_node)
    return ld

launch를 실행하면 tf가 제대로 표시되지 않는다. tf_static은 정확하게 표시된다.

root@rygen3600:/home/ros2_test/ws_root/ws_user# ros2 topic list 
/attached_collision_object
/clicked_point
/display_planned_path
/goal_pose
/initialpose
/joint_states
/parameter_events
/planning_scene
/planning_scene_world
/recognized_object_array
/robot_description
/rosout
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/tf
/tf_static
/trajectory_execution_event
/visualization_marker_array
root@rygen3600:/home/ros2_test/ws_root/ws_user# ros2 topic echo /tf


^Croot@rygen3600:/home/ros2_test/ws_root/ws_user# ros2 topic echo /tf_static 
transforms:
- header:
    stamp:
      sec: 1691830604
      nanosec: 3856748
    frame_id: base_link
  child_frame_id: base
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 1691830604
      nanosec: 3856748
    frame_id: link_6
  child_frame_id: tool0
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
---

왜 이런지 한참 고민하다, 결국 control node와 controller spawner가 실행되어야 TF가 제대로 표시됨을 알았다.

root@rygen3600:/home/ros2_test/ws_root/ws_user# ros2 topic echo /tf
...
- header:
    stamp:
      sec: 1691830991
      nanosec: 709634293
    frame_id: base_link
  child_frame_id: link_1
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 1691830991
      nanosec: 709634293
    frame_id: link_1
  child_frame_id: link_2
  transform:
    translation:
      x: 0.1
      y: 0.0
      z: 0.615
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
- header:
    stamp:
      sec: 1691830991
      nanosec: 709634293
    frame_id: link_2
  child_frame_id: link_3
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 0.705
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

최소 2개 node가 필요하다. robot_state_publisher와 controller node.이거 알아낸다고 별 짓을 다했다. 다수 로봇을 처리하려면 namespace로 할당하여 불러야 할텐데, 잘 안된다.