From b1db54458c99d9255c0f0442b6420d2a39f1f5df Mon Sep 17 00:00:00 2001 From: Shubham Date: Mon, 26 Feb 2024 13:43:03 +0100 Subject: [PATCH 1/3] unstamped cmd_vel option is removed https://github.com/ros-controls/ros2_controllers/pull/812 --- webots_ros2_epuck/launch/robot_launch.py | 7 +++- webots_ros2_tests/test/test_system_epuck.py | 41 +++++++++++++------ .../test/test_system_turtlebot.py | 28 +++++++++---- webots_ros2_tiago/launch/robot_launch.py | 6 ++- 4 files changed, 61 insertions(+), 21 deletions(-) diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index c3bca5f36..225f425a7 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -88,7 +88,12 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'epuck_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') - mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] + + use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + if use_twist_stamped: + mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] + else: + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] epuck_driver = WebotsController( robot_name='e-puck', parameters=[ diff --git a/webots_ros2_tests/test/test_system_epuck.py b/webots_ros2_tests/test/test_system_epuck.py index 8b626d933..b5ab6ae24 100644 --- a/webots_ros2_tests/test/test_system_epuck.py +++ b/webots_ros2_tests/test/test_system_epuck.py @@ -22,7 +22,7 @@ import pytest import rclpy from sensor_msgs.msg import Range, Image, LaserScan -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, TwistStamped from nav_msgs.msg import Odometry from launch import LaunchDescription import launch_testing.actions @@ -89,17 +89,34 @@ def on_range_message_received(message): self.wait_for_messages(self.__node, Range, '/tof', condition=on_range_message_received) def testMovement(self): - publisher = self.__node.create_publisher(Twist, '/cmd_vel', 1) - - def on_position_message_received(message): - twist_message = Twist() - twist_message.linear.x = 0.1 - publisher.publish(twist_message) - - # E_puck should move forward - if message.pose.pose.position.x > 0.5: - return True - return False + use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + + if use_twist_stamped: + publisher = self.__node.create_publisher(TwistStamped, '/cmd_vel', 1) + + def on_position_message_received(message): + twist_message = TwistStamped() + twist_message.header.stamp = self.__node.get_clock().now().to_msg() + twist_message.twist.linear.x = 0.1 + publisher.publish(twist_message) + + # E_puck should move forward + if message.pose.pose.position.x > 0.5: + return True + return False + + else: + publisher = self.__node.create_publisher(Twist, '/cmd_vel', 1) + + def on_position_message_received(message): + twist_message = Twist() + twist_message.linear.x = 0.1 + publisher.publish(twist_message) + + # E_puck should move forward + if message.pose.pose.position.x > 0.5: + return True + return False self.wait_for_messages(self.__node, Odometry, '/odom', condition=on_position_message_received) diff --git a/webots_ros2_tests/test/test_system_turtlebot.py b/webots_ros2_tests/test/test_system_turtlebot.py index d49d011d8..549b07c91 100644 --- a/webots_ros2_tests/test/test_system_turtlebot.py +++ b/webots_ros2_tests/test/test_system_turtlebot.py @@ -21,7 +21,7 @@ import os import pytest import rclpy -from geometry_msgs.msg import PointStamped, Twist +from geometry_msgs.msg import PointStamped, Twist, TwistStamped from sensor_msgs.msg import LaserScan from launch import LaunchDescription import launch_testing.actions @@ -61,14 +61,28 @@ def setUp(self): self.wait_for_clock(self.__node, messages_to_receive=20) def testMovement(self): - publisher = self.__node.create_publisher(Twist, '/cmd_vel', 1) + use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' - def on_position_message_received(message): - twist_message = Twist() - twist_message.linear.x = 0.1 - publisher.publish(twist_message) + if use_twist_stamped: + publisher = self.__node.create_publisher(TwistStamped, '/cmd_vel', 1) - return message.point.x > 6.7 + def on_position_message_received(message): + twist_message = TwistStamped() + twist_message.header.stamp = self.__node.get_clock().now().to_msg() + twist_message.twist.linear.x = 0.1 + publisher.publish(twist_message) + + return message.point.x > 6.7 + + else: + publisher = self.__node.create_publisher(Twist, '/cmd_vel', 1) + + def on_position_message_received(message): + twist_message = Twist() + twist_message.linear.x = 0.1 + publisher.publish(twist_message) + + return message.point.x > 6.7 self.wait_for_messages(self.__node, PointStamped, '/TurtleBot3Burger/gps', condition=on_position_message_received) diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index aba74ad4e..20f84b8b5 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -84,7 +84,11 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'tiago_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') - mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] + use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + if use_twist_stamped: + mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] + else: + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] tiago_driver = WebotsController( robot_name='Tiago_Lite', parameters=[ From b7b6d7df67d021d00d2ff2cc23ead6c8ac985cd8 Mon Sep 17 00:00:00 2001 From: Shubham Date: Mon, 26 Feb 2024 15:35:56 +0100 Subject: [PATCH 2/3] enable stamped cmd_vel for nav2 https://github.com/ros-planning/navigation2/pull/4077 --- webots_ros2_tiago/launch/robot_launch.py | 1 + webots_ros2_turtlebot/launch/robot_launch.py | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index 20f84b8b5..857526a54 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -126,6 +126,7 @@ def generate_launch_description(): ('map', nav2_map), ('params_file', nav2_params), ('use_sim_time', use_sim_time), + ('enable_stamped_cmd_vel', use_twist_stamped), ], condition=launch.conditions.IfCondition(use_nav))) diff --git a/webots_ros2_turtlebot/launch/robot_launch.py b/webots_ros2_turtlebot/launch/robot_launch.py index 6ad06aa92..5221ab6c5 100644 --- a/webots_ros2_turtlebot/launch/robot_launch.py +++ b/webots_ros2_turtlebot/launch/robot_launch.py @@ -82,7 +82,11 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'turtlebot_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2control.yml') - mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] + use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + if use_twist_stamped: + mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] + else: + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] turtlebot_driver = WebotsController( robot_name='TurtleBot3Burger', parameters=[ @@ -108,6 +112,7 @@ def generate_launch_description(): ('map', nav2_map), ('params_file', nav2_params), ('use_sim_time', use_sim_time), + ('enable_stamped_cmd_vel', use_twist_stamped), ], condition=launch.conditions.IfCondition(use_nav)) navigation_nodes.append(turtlebot_navigation) From f2bef4e8ffee29b37db51dcb6337fae4d16b1a19 Mon Sep 17 00:00:00 2001 From: Shubham Date: Mon, 26 Feb 2024 15:50:26 +0100 Subject: [PATCH 3/3] don't enable twist stamped cmd_vel yet as nav2 packages are not available for rolling --- webots_ros2_epuck/launch/robot_launch.py | 1 - webots_ros2_tiago/launch/robot_launch.py | 1 - webots_ros2_turtlebot/launch/robot_launch.py | 1 - 3 files changed, 3 deletions(-) diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index 225f425a7..2eff20ecd 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -88,7 +88,6 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'epuck_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') - use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' if use_twist_stamped: mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index 857526a54..20f84b8b5 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -126,7 +126,6 @@ def generate_launch_description(): ('map', nav2_map), ('params_file', nav2_params), ('use_sim_time', use_sim_time), - ('enable_stamped_cmd_vel', use_twist_stamped), ], condition=launch.conditions.IfCondition(use_nav))) diff --git a/webots_ros2_turtlebot/launch/robot_launch.py b/webots_ros2_turtlebot/launch/robot_launch.py index 5221ab6c5..8b75e44ae 100644 --- a/webots_ros2_turtlebot/launch/robot_launch.py +++ b/webots_ros2_turtlebot/launch/robot_launch.py @@ -112,7 +112,6 @@ def generate_launch_description(): ('map', nav2_map), ('params_file', nav2_params), ('use_sim_time', use_sim_time), - ('enable_stamped_cmd_vel', use_twist_stamped), ], condition=launch.conditions.IfCondition(use_nav)) navigation_nodes.append(turtlebot_navigation)