Improve ROS2 compatibility with real TIAGo robot

[stefaniapedrazzi]

Currently the Webots ROS2 topics are difficult to use with a real TIAGo robot and it would be interesting to publish additional information.

Roadmap: the plan would be to first start focusing on the camera-related topics and then to joints/navigation ones.

Example here on the implementation that was done for gazebo and all the program that is launched the launch file:
https://github.com/pal-robotics/tiago_simulation/blob/humble-devel/tiago_gazebo/launch/tiago_gazebo.launch.py

The real TIAGo is launching this file at startup:
https://github.com/pal-robotics/tiago_robot/blob/humble-devel/tiago_bringup/launch/tiago_bringup.launch.py

Here is the list of the ROS2 topics used by the TIAGo robot:

• Camera topics:
  • /head_front_camera/depth_registered/camera_info [sensor_msgs/msg/CameraInfo]
  • /head_front_camera/depth_registered/image_raw [sensor_msgs/msg/Image]
  • /head_front_camera/depth_registered/points [sensor_msgs/msg/PointCloud2]
  • /head_front_camera/rgb/camera_info [sensor_msgs/msg/CameraInfo]
  • /head_front_camera/rgb/image_raw [sensor_msgs/msg/Image]

Camera_info:

header:
      stamp:
        sec: 35
        nanosec: 723000000
      frame_id: head_front_camera_rgb_optical_frame
    height: 480
    width: 640
    distortion_model: plumb_bob
    d:
    - 1.0e-08
    - 1.0e-08
    - 1.0e-08
    - 1.0e-08
    - 1.0e-08
    k:
    - 522.1910329546544
    - 0.0
    - 320.5
    - 0.0
    - 522.1910329546544
    - 240.5
    - 0.0
    - 0.0
    - 1.0
    r:
    - 1.0
    - 0.0
    - 0.0
    - 0.0
    - 1.0
    - 0.0
    - 0.0
    - 0.0
    - 1.0
    p:
    - 522.1910329546544
    - 0.0
    - 320.5
    - -0.0
    - 0.0
    - 522.1910329546544
    - 240.5
    - 0.0
    - 0.0
    - 0.0
    - 1.0
    - 0.0
    binning_x: 0
    binning_y: 0
    roi:
      x_offset: 0
      y_offset: 0
      height: 0
      width: 0
      do_rectify: false

• Essential topics
  • /arm_controller/joint_trajectory [trajectory_msgs/msg/JointTrajectory]
  • /arm_controller/state [control_msgs/msg/JointTrajectoryControllerState]
  • /arm_controller/transition_event [lifecycle_msgs/msg/TransitionEvent]
  • /clock [rosgraph_msgs/msg/Clock]
  • /gripper_controller/joint_trajectory [trajectory_msgs/msg/JointTrajectory]
  • /gripper_controller/state [control_msgs/msg/JointTrajectoryControllerState]
  • /gripper_controller/transition_event [lifecycle_msgs/msg/TransitionEvent]
  • /head_controller/joint_trajectory [trajectory_msgs/msg/JointTrajectory]
  • /head_controller/state [control_msgs/msg/JointTrajectoryControllerState]
  • /head_controller/transition_event [lifecycle_msgs/msg/TransitionEvent]
  • /joint_states [sensor_msgs/msg/JointState]
  • /joy [sensor_msgs/msg/Joy]
  • /joy_priority [std_msgs/msg/Bool]
  • /joy_vel [geometry_msgs/msg/Twist]
  • /key_vel [geometry_msgs/msg/Twist]
  • /mobile_base_controller/cmd_vel_unstamped [geometry_msgs/msg/Twist]
  • /mobile_base_controller/odom [nav_msgs/msg/Odometry]
  • /mobile_base_controller/transition_event [lifecycle_msgs/msg/TransitionEvent]
  • /nav_vel [geometry_msgs/msg/Twist]
  • /parameter_events [rcl_interfaces/msg/ParameterEvent]
  • /play_motion2/transition_event [lifecycle_msgs/msg/TransitionEvent]
  • /robot_description [std_msgs/msg/String]
  • /rosout [rcl_interfaces/msg/Log]
  • /rviz_joy_vel [geometry_msgs/msg/Twist]
  • /scan_raw [sensor_msgs/msg/LaserScan]
  • /tf [tf2_msgs/msg/TFMessage]
  • /tf_static [tf2_msgs/msg/TFMessage]
  • /torso_controller/joint_trajectory [trajectory_msgs/msg/JointTrajectory]
  • /torso_controller/state [control_msgs/msg/JointTrajectoryControllerState]
  • /torso_controller/transition_event [lifecycle_msgs/msg/TransitionEvent]

[ygoumaz]

I think gitlab links are wrong.

[ygoumaz]

I will take care of this once @stefaniapedrazzi is done with cyberbotics/webots#6046. Are we already aware of which topics are low-level (and should be published by webots_ros2) and which ones are published by higher level nodes like ros2_control and TIAGo packages?

[stefaniapedrazzi]

Yes, this should be addressed after cyberbotics/webots#6046 so that we have the correct camera to work with.
No, I didn't check in detail the topics yet.

[ygoumaz]

I assume that if these topics are also available when using ROS 2 with the real (hardware) TIAGo robot, we could get the information for each topic from PAL Robotics. They are probably using specific nodes for multiple of these topics. It will save us time if we know the list of topics to add to the webots_ros2 code and which ones are directly accessible by launching other packages. The information is probably also, partially or totally, contained in the linked launch files, but I can't access them.