ROS2: add documentation for using iCubGazeboV3

It depends by robotology/icub-models-generator#228

CMakeLists.txt

@@ -62,8 +62,9 @@ foreach(subdir ${subdirs})
   file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/iCub_manual/${subdir}/ DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/iCub/${subdir})
 endforeach()
 
-# Copy the ros folder
+# Copy the ros and ros2 folder
 file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/ros DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/iCub)
+file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/ros2 DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/iCub)
 
 # Add the model.config just for a limited number of models that are known
 # to work in Gazebo, unless the ICUB_MODELS_INSTALL_ALL_GAZEBO_MODELS option
@@ -80,7 +81,7 @@ list(APPEND GAZEBO_SUPPORTED_MODELS "iCubGazeboV2_5")
 list(APPEND GAZEBO_SUPPORTED_MODELS "iCubGazeboV2_5_plus")
 list(APPEND GAZEBO_SUPPORTED_MODELS "iCubGazeboV3")
 
-# Note: these models don't need further configuration apart from 
+# Note: these models don't need further configuration apart from
 # model version in model.config. Only one configuration is generated for
 # these models, and no helper models (fixed, no_hans, and so on) are
 # generated.
@@ -182,7 +183,7 @@ install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/iCub DESTINATION share)
 # so that iCub packages if found by ROS2
 # See https://github.com/robotology/icub-models/issues/177
 file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/iCub_empty_file "")
-install(FILES ${CMAKE_CURRENT_BINARY_DIR}/iCub_empty_file 
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/iCub_empty_file
         DESTINATION share/ament_index/resource_index/packages
         RENAME iCub)
 

ros2/README.md

@@ -0,0 +1,47 @@
+:warning: The ROS2 support is in beta :warning
+
+This directory contains the files related for using `iCubGazeboV3` with ROS2. The contents of this directory are installed inside `iCub`.
+ros2 package. The location of `iCub` ros pacakge is discovered by running `roscd iCub`.
+The `launch` directory contains `launch.py` file `robot_state_publisher.launch.py`. The robot model
+that will be used depends `YARP_ROBOT_NAME` set in `.bashrc`.
+
+Right now are published on ROS2 topics:
+
+- FT sensors measurements
+- IMU sensors measurements
+- Joint states
+
+The steps to run rviz visualization correctly are:
+
+- Edit `iCubGazeboV3/model.urdf` putting  [`icub_ROS2.xml`](https://github.com/robotology/robots-configuration/blob/devel/iCub/conf_icub3/wrappers/icub_ROS2.xml) `robotinterface` xml file used at startup.
+- Ensure that the robot is started correctly with ros configuration files for motor control boards.
+- On starting the robot, one should see `/<prefix>/joint_states` ros topic by running `ros2 topic list`.
+  Also, ensure that the `/<prefix>/joint_states` streams all the robot joint angles by running
+  `ros2 topic echo /<prefix>/joint_states`.
+- Start `transform server` by running `yarprobotinterface --config transform-server.xml`.
+  The transform server is a central location for transforms `tfs` and it streams the transforms
+  to `/tf` ros topic.
+- Launch [robot_state_publisher](http://wiki.ros.org/robot_state_publisher)
+
+```
+ros2 launch robot_state_publisher.launch.py
+```
+ - Ensure that the transforms are available correctly by running `ros2 topic echo /tf`.
+
+- At this point you can launch `rviz2` and visualize iCub. There is an issue for visualizing the meshes(https://github.com/robotology/icub-models-generator/issues/229) but the tfs or IMU/FT sensor measurements can be visualized without problems.
+
+The following two parameters are important to ensure correct visualization:
+
+- Under `Global Options`, `Fixed Frame` field is the frame name with respect to
+  which all the other frames transforms are give. The default value set inside the `map`, you have to set it to `root_link`.
+
+![immagine](https://user-images.githubusercontent.com/19152494/206218846-faf4375f-f1d2-4e24-a05d-234ca2e848a5.png)
+
+The contents of this directory are tested on:
+
+##### Linux
+
+```
+Ubuntu 22.04 LTS
+Rosdistro: Humble
+```

ros2/launch/robot_state_publisher.launch.py

@@ -0,0 +1,27 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    urdf = os.path.abspath("$(find iCub)/robots/$(arg YARP_ROBOT_NAME)/model.urdf")
+    with open(urdf, 'r') as infp:
+        robot_desc = infp.read()
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
+            arguments=[urdf])
+    ])