From 99d91172c2e62916eb65003a45b3efe0816bae84 Mon Sep 17 00:00:00 2001 From: Shubham Date: Mon, 26 Feb 2024 13:43:03 +0100 Subject: [PATCH] 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 | 40 +++++++++++++------ .../test/test_system_turtlebot.py | 28 +++++++++---- webots_ros2_tiago/launch/robot_launch.py | 6 ++- 4 files changed, 60 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..6cf53c8a1 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,33 @@ 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() + 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..7264a9b82 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() + 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=[