From 9550c9b22e8bc33f9a9d91598c9ccfee92248b5d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Fri, 4 Aug 2023 11:23:38 +0200
Subject: [PATCH] update: new teleop layout

---
 config/joystick.yaml            |  3 +-
 launch/robot_joystick.launch.py | 50 +++++++++++++++++++++++----------
 2 files changed, 37 insertions(+), 16 deletions(-)

diff --git a/config/joystick.yaml b/config/joystick.yaml
index 2112f35..f4536a6 100644
--- a/config/joystick.yaml
+++ b/config/joystick.yaml
@@ -2,7 +2,8 @@ joy_node:
   ros__parameters:
     device_id: 0
     deadzone: 0.05
-    autorepeat_rate: 20.0
+    autorepeat_rate: 50.0
+    coalesce_interval: 0.001 # merging messeges in this interval
 
 teleop_node:
   ros__parameters:
diff --git a/launch/robot_joystick.launch.py b/launch/robot_joystick.launch.py
index 42f2f50..a5ec859 100644
--- a/launch/robot_joystick.launch.py
+++ b/launch/robot_joystick.launch.py
@@ -12,6 +12,35 @@ from ament_index_python.packages import get_package_share_directory
 def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time')
 
+    # joy_params = os.path.join(get_package_share_directory('cps_rmp220_support'),'config','joystick.yaml')
+    # namespace = "/rmp"
+    # joy_node = Node(
+    #         package='joy',
+    #         executable='joy_node',
+    #         parameters=[joy_params, {'use_sim_time': use_sim_time}],
+    #         namespace = namespace
+    #      )
+
+    # teleop_node = Node(
+    #         package='rmp220_teleop',
+    #         executable='rmp220_teleop',
+    #         name='rmp220_teleop',
+    #         parameters=[joy_params, {'use_sim_time': use_sim_time}],
+    #         remappings=[('/cmd_vel', 'cmd_vel_joy')],
+    #         namespace = namespace
+    #      )
+    
+    # twist_mux_params = os.path.join(get_package_share_directory('cps_rmp220_support'),'config','twist_mux.yaml')
+    # twist_mux = Node(
+    #         package="twist_mux",
+    #         executable="twist_mux",
+    #         parameters=[twist_mux_params, {'use_sim_time': False}],
+    #         #remappings=[('cmd_vel_joy','/rmp/cmd_vel_joy')],
+    #         namespace = namespace
+    #     )
+
+    # Using the more 'ROS' way:
+
     joy_params = os.path.join(get_package_share_directory('cps_rmp220_support'),'config','joystick.yaml')
     namespace = "/rmp"
     joy_node = Node(
@@ -20,24 +49,16 @@ def generate_launch_description():
             parameters=[joy_params, {'use_sim_time': use_sim_time}],
             namespace = namespace
          )
-
+    
     teleop_node = Node(
-            package='rmp220_teleop',
-            executable='rmp220_teleop',
-            name='rmp220_teleop',
+            package='teleop_twist_joy',
+            executable='teleop_node',
+            name='teleop_twist_joy',
             parameters=[joy_params, {'use_sim_time': use_sim_time}],
-            remappings=[('/cmd_vel', 'cmd_vel_joy')],
+            remappings=[('cmd_vel', 'cmd_vel_joy')],
             namespace = namespace
          )
-    
-    twist_mux_params = os.path.join(get_package_share_directory('cps_rmp220_support'),'config','twist_mux.yaml')
-    twist_mux = Node(
-            package="twist_mux",
-            executable="twist_mux",
-            parameters=[twist_mux_params, {'use_sim_time': False}],
-            #remappings=[('cmd_vel_joy','/rmp/cmd_vel_joy')],
-            namespace = namespace
-        )
+
     
     return LaunchDescription([
         DeclareLaunchArgument(
@@ -46,5 +67,4 @@ def generate_launch_description():
             description='Use sim time if true'),
         joy_node,
         teleop_node,
-        twist_mux
     ])
\ No newline at end of file