From 045c92d222988e077dc56fd0ce5e41eb7e39ffbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Wed, 9 Aug 2023 08:20:26 +0200
Subject: [PATCH] update: changing teleop to twist_mux

---
 launch/robot_joystick.launch.py | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/launch/robot_joystick.launch.py b/launch/robot_joystick.launch.py
index 7438bdf..e56cad3 100644
--- a/launch/robot_joystick.launch.py
+++ b/launch/robot_joystick.launch.py
@@ -59,6 +59,21 @@ def generate_launch_description():
             remappings=[('cmd_vel', 'cmd_vel_out')],
             #namespace = namespace
          )
+    
+    twist_mux_params = os.path.join(get_package_share_directory('cps_rmp220_support'),'config','twist_mux.yaml')
+    twist_mux_node = Node(
+            package="twist_mux",
+            executable="twist_mux",
+            parameters=[twist_mux_params, {'use_sim_time': False}],
+            #remappings=[('/cmd_vel_out','/diffbot_base_controller/cmd_vel_unstamped')]
+        )
+    
+    rmp_teleop_node = Node(
+            package='rmp220_teleop',
+            executable='rmp220_teleop',
+            name='rmp220_teleop',
+        )   
+
 
     
     return LaunchDescription([
@@ -67,5 +82,7 @@ def generate_launch_description():
             default_value='false',
             description='Use sim time if true'),
         joy_node,
-        teleop_node,
+        #teleop_node,
+        rmp_teleop_node,
+        twist_mux_node
     ])
\ No newline at end of file