From 246226c474744ac8637cbcd4500638ca7e031730 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 6 Nov 2024 17:17:23 +0100 Subject: [PATCH 1/2] Refactor diff_drive demo. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim_demos/CMakeLists.txt | 6 ++ ros_gz_sim_demos/README.md | 14 +--- ros_gz_sim_demos/config/diff_drive.yaml | 36 ++++++++++ ros_gz_sim_demos/launch/diff_drive.launch.py | 69 ------------------- ros_gz_sim_demos/launch/diff_drive.launch.xml | 6 ++ 5 files changed, 50 insertions(+), 81 deletions(-) create mode 100644 ros_gz_sim_demos/config/diff_drive.yaml delete mode 100644 ros_gz_sim_demos/launch/diff_drive.launch.py create mode 100644 ros_gz_sim_demos/launch/diff_drive.launch.xml diff --git a/ros_gz_sim_demos/CMakeLists.txt b/ros_gz_sim_demos/CMakeLists.txt index 8e0f580a..9b69104b 100644 --- a/ros_gz_sim_demos/CMakeLists.txt +++ b/ros_gz_sim_demos/CMakeLists.txt @@ -9,6 +9,12 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install( + DIRECTORY + config/ + DESTINATION share/${PROJECT_NAME}/config +) + install( DIRECTORY launch/ diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md index c404a4dc..55f3ffa3 100644 --- a/ros_gz_sim_demos/README.md +++ b/ros_gz_sim_demos/README.md @@ -56,22 +56,12 @@ Trigger the camera: Send commands to a differential drive vehicle and listen to its odometry. - ros2 launch ros_gz_sim_demos diff_drive.launch.py + ros2 launch ros_gz_sim_demos diff_drive.launch.xml -Then unpause and send a command +Then send a command. ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0}, angular: {z: 0.5}}" -This demo also shows the use of custom QoS parameters. The commands are -subscribed to as "reliable", so trying to publish "best-effort" commands -won't work. See the difference between: - - ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0}, angular: {z: 0.0}}" --qos-reliability reliable - -And - - ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0}, angular: {z: 0.0}}" --qos-reliability best_effort - ![](images/diff_drive_demo.png) ## Depth camera diff --git a/ros_gz_sim_demos/config/diff_drive.yaml b/ros_gz_sim_demos/config/diff_drive.yaml new file mode 100644 index 00000000..37c8d229 --- /dev/null +++ b/ros_gz_sim_demos/config/diff_drive.yaml @@ -0,0 +1,36 @@ +# Diff drive configuration. +- ros_topic_name: "/model/vehicle_blue/cmd_vel" + gz_topic_name: "/model/vehicle_blue/cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + subscriber_queue: 5 + publisher_queue: 6 + lazy: false + direction: ROS_TO_GZ + +- ros_topic_name: "/model/vehicle_blue/odometry" + gz_topic_name: "/model/vehicle_blue/odometry" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + subscriber_queue: 5 + publisher_queue: 6 + lazy: false + direction: GZ_TO_ROS + +- ros_topic_name: "/model/vehicle_green/cmd_vel" + gz_topic_name: "/model/vehicle_green/cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + subscriber_queue: 5 + publisher_queue: 6 + lazy: false + direction: ROS_TO_GZ + +- ros_topic_name: "/model/vehicle_green/odometry" + gz_topic_name: "/model/vehicle_green/odometry" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + subscriber_queue: 5 + publisher_queue: 6 + lazy: false + direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/diff_drive.launch.py b/ros_gz_sim_demos/launch/diff_drive.launch.py deleted file mode 100644 index 2d656d1e..00000000 --- a/ros_gz_sim_demos/launch/diff_drive.launch.py +++ /dev/null @@ -1,69 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration - -from launch_ros.actions import Node - - -def generate_launch_description(): - - pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') - pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={ - 'gz_args': '-r diff_drive.sdf' - }.items(), - ) - - # RViz - rviz = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'diff_drive.rviz')], - condition=IfCondition(LaunchConfiguration('rviz')) - ) - - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist', - '/model/vehicle_blue/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry', - '/model/vehicle_green/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist', - '/model/vehicle_green/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry'], - parameters=[{'qos_overrides./model/vehicle_blue.subscriber.reliability': 'reliable', - 'qos_overrides./model/vehicle_green.subscriber.reliability': 'reliable'}], - output='screen' - ) - - return LaunchDescription([ - gz_sim, - DeclareLaunchArgument('rviz', default_value='true', - description='Open RViz.'), - bridge, - rviz - ]) diff --git a/ros_gz_sim_demos/launch/diff_drive.launch.xml b/ros_gz_sim_demos/launch/diff_drive.launch.xml new file mode 100644 index 00000000..02e6f750 --- /dev/null +++ b/ros_gz_sim_demos/launch/diff_drive.launch.xml @@ -0,0 +1,6 @@ + + + + + + From 79ece9ed0babb7869ede938120f8cd324400e34b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 22 Nov 2024 18:09:11 +0100 Subject: [PATCH 2/2] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim_demos/README.md | 12 +++++++- ros_gz_sim_demos/config/diff_drive.yaml | 28 ++++++------------- ros_gz_sim_demos/launch/diff_drive.launch.xml | 12 ++++++-- 3 files changed, 29 insertions(+), 23 deletions(-) diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md index 55f3ffa3..94392157 100644 --- a/ros_gz_sim_demos/README.md +++ b/ros_gz_sim_demos/README.md @@ -58,10 +58,20 @@ Send commands to a differential drive vehicle and listen to its odometry. ros2 launch ros_gz_sim_demos diff_drive.launch.xml -Then send a command. +Then send a command ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0}, angular: {z: 0.5}}" +This demo also shows the use of custom QoS parameters. The commands are +subscribed to as "reliable", so trying to publish "best-effort" commands +won't work. See the difference between: + + ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0}, angular: {z: 0.0}}" --qos-reliability reliable + +And + + ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0}, angular: {z: 0.0}}" --qos-reliability best_effort + ![](images/diff_drive_demo.png) ## Depth camera diff --git a/ros_gz_sim_demos/config/diff_drive.yaml b/ros_gz_sim_demos/config/diff_drive.yaml index 37c8d229..cc6d36e7 100644 --- a/ros_gz_sim_demos/config/diff_drive.yaml +++ b/ros_gz_sim_demos/config/diff_drive.yaml @@ -1,36 +1,24 @@ # Diff drive configuration. -- ros_topic_name: "/model/vehicle_blue/cmd_vel" - gz_topic_name: "/model/vehicle_blue/cmd_vel" +- topic_name: "/model/vehicle_blue/cmd_vel" ros_type_name: "geometry_msgs/msg/Twist" gz_type_name: "gz.msgs.Twist" - subscriber_queue: 5 - publisher_queue: 6 - lazy: false + lazy: true direction: ROS_TO_GZ -- ros_topic_name: "/model/vehicle_blue/odometry" - gz_topic_name: "/model/vehicle_blue/odometry" +- topic_name: "/model/vehicle_blue/odometry" ros_type_name: "nav_msgs/msg/Odometry" gz_type_name: "gz.msgs.Odometry" - subscriber_queue: 5 - publisher_queue: 6 - lazy: false + lazy: true direction: GZ_TO_ROS -- ros_topic_name: "/model/vehicle_green/cmd_vel" - gz_topic_name: "/model/vehicle_green/cmd_vel" +- topic_name: "/model/vehicle_green/cmd_vel" ros_type_name: "geometry_msgs/msg/Twist" gz_type_name: "gz.msgs.Twist" - subscriber_queue: 5 - publisher_queue: 6 - lazy: false + lazy: true direction: ROS_TO_GZ -- ros_topic_name: "/model/vehicle_green/odometry" - gz_topic_name: "/model/vehicle_green/odometry" +- topic_name: "/model/vehicle_green/odometry" ros_type_name: "nav_msgs/msg/Odometry" gz_type_name: "gz.msgs.Odometry" - subscriber_queue: 5 - publisher_queue: 6 - lazy: false + lazy: true direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/diff_drive.launch.xml b/ros_gz_sim_demos/launch/diff_drive.launch.xml index 02e6f750..61a1159c 100644 --- a/ros_gz_sim_demos/launch/diff_drive.launch.xml +++ b/ros_gz_sim_demos/launch/diff_drive.launch.xml @@ -1,6 +1,14 @@ - - + +