Relay stamped cmd vel (#282)

* Remap velocity command topic and add relay node Signed-off-by: Gary Servin <[email protected]> * Use Stamped message for velocity commands Signed-off-by: Gary Servin <[email protected]> --------- Signed-off-by: Gary Servin <[email protected]>
Ekumen-OS · Dec 20, 2024 · 75b681c · 75b681c
1 parent aee0145
commit 75b681c
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 5 deletions.
diff --git a/andino_bringup/config/joystick.yaml b/andino_bringup/config/joystick.yaml
@@ -25,3 +25,4 @@ teleop_node:
     enable_turbo_button: 5
 
     require_enable_button: true
+    publish_stamped_twist: true
diff --git a/andino_bringup/launch/teleop_joystick.launch.py b/andino_bringup/launch/teleop_joystick.launch.py
@@ -42,7 +42,7 @@ def generate_launch_description():
 
     cmd_vel_topic_arg = DeclareLaunchArgument(
             'cmd_vel_topic',
-            default_value='/cmd_vel',
+            default_value='/cmd_vel_stamped',
             description='Indicates the cmd_vel topic.')
     cmd_vel_topic =  LaunchConfiguration('cmd_vel_topic')
 

diff --git a/andino_bringup/launch/teleop_keyboard.launch.py b/andino_bringup/launch/teleop_keyboard.launch.py
@@ -29,15 +29,19 @@
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 from launch import LaunchDescription
+
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     teleop_node = Node(
             package='teleop_twist_keyboard',
             executable='teleop_twist_keyboard',
             name='teleop_twist_keyboard_node',
             output='screen',
             prefix = 'xterm -e',
+            parameters=[{'stamped': True}],
+            remappings=[('/cmd_vel', 'cmd_vel_stamped')],
          )
 
     return LaunchDescription([

diff --git a/andino_control/launch/andino_control.launch.py b/andino_control/launch/andino_control.launch.py
@@ -55,10 +55,9 @@ def generate_launch_description():
         parameters=[{'robot_description': ParameterValue(robot_description, value_type=str)},
                     controller_params_file],
         remappings=[
-            ('/diff_controller/cmd_vel', '/cmd_vel'), # Used if use_stamped_vel param is true
-            ('/diff_controller/cmd_vel_unstamped', '/cmd_vel'), # Used if use_stamped_vel param is false
-            ('/diff_controller/cmd_vel_out', '/cmd_vel_out'), # Used if publish_limited_velocity param is true
-            ('/diff_controller/odom', '/odom'),
+            ('/diff_controller/cmd_vel', 'cmd_vel_stamped'),
+            ('/diff_controller/cmd_vel_out', 'cmd_vel_out'),
+            ('/diff_controller/odom', 'odom'),
         ],
         output="both",
     )
@@ -83,10 +82,18 @@ def generate_launch_description():
         )
     )
 
+    relay_node = Node(
+        package="topic_tools",
+        executable="relay_field",
+        name="cmd_vel_relay",
+        arguments=["cmd_vel", "cmd_vel_stamped", "geometry_msgs/TwistStamped", "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''}, twist: m}", "--wait-for-start"],
+    )
+
     nodes = [
         control_node,
         joint_state_broadcaster_spawner,
         delay_diff_drive_controller_spawner_after_joint_state_broadcaster_spawner,
+        relay_node,
     ]
 
     return LaunchDescription(nodes)
diff --git a/andino_control/package.xml b/andino_control/package.xml
@@ -18,6 +18,7 @@
   <exec_depend>diff_drive_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
Original file line number	Diff line number	Diff line change
Expand Up		@@ -25,3 +25,4 @@ teleop_node:
		enable_turbo_button: 5

		require_enable_button: true
		publish_stamped_twist: true