From 83c0d2c517c1b68a5bb1e59c43a96c8f168e9c07 Mon Sep 17 00:00:00 2001
From: Chris Iverach-Brereton <civerachb@clearpathrobotics.com>
Date: Wed, 11 Sep 2024 15:58:49 -0400
Subject: [PATCH] Add the unstamper node, remap the republished cmd_vel topic

---
 turtlebot4_bringup/launch/robot.launch.py | 16 +++++++++++++++-
 turtlebot4_bringup/package.xml            |  1 +
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/turtlebot4_bringup/launch/robot.launch.py b/turtlebot4_bringup/launch/robot.launch.py
index 8c8bd09..e48dc20 100644
--- a/turtlebot4_bringup/launch/robot.launch.py
+++ b/turtlebot4_bringup/launch/robot.launch.py
@@ -38,6 +38,7 @@
 
 
 def generate_launch_description():
+    republisher_ns = '_do_not_use'
 
     pkg_turtlebot4_bringup = get_package_share_directory('turtlebot4_bringup')
     create3_republisher = get_package_share_directory('create3_republisher')
@@ -79,8 +80,20 @@ def generate_launch_description():
         package='create3_republisher',
         executable='create3_republisher',
         parameters=[create3_repub_param_yaml_file,
-                    {'robot_namespace': '_do_not_use'}],
+                    {'robot_namespace': republisher_ns}],
         output='screen',
+        remappings=[
+            ([republisher_ns, 'cmd_vel'], [republisher_ns, 'cmd_vel_unstamped'])
+        ]
+    )
+    unstamper_node = Node(
+        package='twist_stamper',
+        executable='twist_unstamper',
+        name='twist_unstamper',
+        remappings=[
+            ('cmd_vel_in', [republisher_ns, 'cmd_vel']),
+            ('cmd_vel_out', [republisher_ns, 'cmd_vel_unstamped'])
+        ]
     )
 
     ld = LaunchDescription(ARGUMENTS)
@@ -89,4 +102,5 @@ def generate_launch_description():
     ld.add_action(turtlebot4_node)
     ld.add_action(turtlebot4_base_node)
     ld.add_action(create3_republisher_node)
+    ld.add_action(unstamper_node)
     return ld
diff --git a/turtlebot4_bringup/package.xml b/turtlebot4_bringup/package.xml
index 5435032..5833d87 100644
--- a/turtlebot4_bringup/package.xml
+++ b/turtlebot4_bringup/package.xml
@@ -22,6 +22,7 @@
   <depend version_gte="2.6.1">teleop_twist_joy</depend> <!-- TwistStamped support needed -->
   <depend>tf2_ros</depend>
   <depend>create3_republisher</depend>
+  <depend>twist_stamper</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>