From a796f4480f556ae9fac09b69a7e0988032f4bcf2 Mon Sep 17 00:00:00 2001
From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com>
Date: Thu, 17 Aug 2023 10:40:11 +0200
Subject: [PATCH] Create robot_joystick.launch.py

---
 launch/robot_joystick.launch.py | 65 +++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)
 create mode 100644 launch/robot_joystick.launch.py

diff --git a/launch/robot_joystick.launch.py b/launch/robot_joystick.launch.py
new file mode 100644
index 0000000..900575f
--- /dev/null
+++ b/launch/robot_joystick.launch.py
@@ -0,0 +1,65 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.descriptions import ParameterValue
+from launch_ros.actions import LifecycleNode
+
+import os
+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('rmp220_teleop'),'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='teleop_twist_joy',
+            executable='teleop_node',
+            name='teleop_node',
+            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('rmp220_teleop'),'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','/cmd_vel_mux')]
+        )
+    
+    rmp_teleop_node = Node(
+            package='rmp220_teleop',
+            executable='rmp220_teleop',
+            name='rmp220_teleop',
+            remappings=[('cmd_vel', 'cmd_vel_joy')],
+        )
+    
+    rmp_middleware_node = Node(
+            package='rmp220_middleware',
+            executable='rmp220_middleware',
+            name='rmp220_middleware',
+            #namespace = namespace
+         )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use sim time if true'),
+        joy_node,
+        teleop_node,
+        #rmp_teleop_node,
+        twist_mux_node,
+        rmp_middleware_node
+    ])