From e9d86f53c7e44e35abeb5d5b219750db1efd378a Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 23 Dec 2022 10:02:31 +0100 Subject: [PATCH] ROS2: add documentation for using iCubGazeboV3 It depends by https://github.com/robotology/icub-models-generator/pull/228 --- CMakeLists.txt | 7 +-- ros2/README.md | 47 +++++++++++++++++++++ ros2/launch/robot_state_publisher.launch.py | 27 ++++++++++++ 3 files changed, 78 insertions(+), 3 deletions(-) create mode 100644 ros2/README.md create mode 100644 ros2/launch/robot_state_publisher.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ac591d..a2f81b2 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/ros2/README.md b/ros2/README.md new file mode 100644 index 0000000..2703bec --- /dev/null +++ b/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 `//joint_states` ros topic by running `ros2 topic list`. + Also, ensure that the `//joint_states` streams all the robot joint angles by running + `ros2 topic echo //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 +``` diff --git a/ros2/launch/robot_state_publisher.launch.py b/ros2/launch/robot_state_publisher.launch.py new file mode 100644 index 0000000..930e714 --- /dev/null +++ b/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]) + ]) \ No newline at end of file