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..6ca062ab5 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()
+                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=[