From b6c3e18b0d30bb0ee418994b5dc4d4dbb5875e78 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Tue, 18 Apr 2023 11:04:28 +0200
Subject: [PATCH] update: launch file working

---
 launch/robot.launch.py  | 81 +++++++++++++++++++++++++++++++++++++++++
 launch/teleop.launch.py | 24 ------------
 package.xml             |  2 +
 setup.py                |  4 ++
 4 files changed, 87 insertions(+), 24 deletions(-)
 create mode 100644 launch/robot.launch.py
 delete mode 100644 launch/teleop.launch.py

diff --git a/launch/robot.launch.py b/launch/robot.launch.py
new file mode 100644
index 0000000..e0288cb
--- /dev/null
+++ b/launch/robot.launch.py
@@ -0,0 +1,81 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+
+# def generate_launch_description():
+#     return LaunchDescription([
+#         Node(
+#             package='segwayrmp',
+#             #namespace='turtlesim1',
+#             executable='SmartCar',
+#             name='SmartCar',
+#             output='screen'
+#         ),
+#         Node(
+#             package='joy',
+#             #namespace='turtlesim2',
+#             executable='joy_node',
+#             name='joy_teleop',
+#             output='screen'
+
+#         ),
+#         Node(
+#             package='rmp220_teleop',
+#             executable='rmp220_teleop',
+#             name='rmp220_teleop',
+#             # remappings=[
+#             #     ('/input/pose', '/turtlesim1/turtle1/pose'),
+#             #     ('/output/cmd_vel', '/turtlesim2/turtle1/cmd_vel'),
+#             # ]
+#             output='screen'
+
+#         )
+#     ])
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    #joy_params = os.path.join(get_package_share_directory('rmp220_teleop'))
+
+    joy_node = Node(
+            package='joy',
+            executable='joy_node',
+            #parameters=[joy_params, {'use_sim_time': use_sim_time}],
+         )
+
+    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')]
+         )
+    
+    control_node = Node(
+            package='segwayrmp',
+            executable='SmartCar',
+            name='SmartCar',
+            #parameters=[joy_params, {'use_sim_time': use_sim_time}],
+            #remappings=[('/cmd_vel','/cmd_vel_joy')]
+         )
+
+    # twist_stamper = Node(
+    #         package='twist_stamper',
+    #         executable='twist_stamper',
+    #         parameters=[{'use_sim_time': use_sim_time}],
+    #         remappings=[('/cmd_vel_in','/diff_cont/cmd_vel_unstamped'),
+    #                     ('/cmd_vel_out','/diff_cont/cmd_vel')]
+    #      )
+
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use sim time if true'),
+        joy_node,
+        teleop_node,
+        control_node,
+        # twist_stamper       
+    ])
\ No newline at end of file
diff --git a/launch/teleop.launch.py b/launch/teleop.launch.py
deleted file mode 100644
index 1f9d68f..0000000
--- a/launch/teleop.launch.py
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-
-  <!-- Start the segwayrmp SmartCar node -->
-  <node
-    pkg="segwayrmp"
-    type="SmartCar"
-    name="SmartCar"
-    output="screen"/>
-
-  <!-- Start the joy_teleop node -->
-  <node
-    pkg="joy"
-    type="joy_teleop"
-    name="joy_teleop"
-    output="screen"/>
-
-  <!-- Start the rmp220_teleop.py node -->
-  <node
-    pkg="rmp220_teleop"
-    type="rmp220_teleop.py"
-    name="rmp220_teleop"
-    output="screen"/>
-
-</launch>
diff --git a/package.xml b/package.xml
index 6b8578d..56aec88 100644
--- a/package.xml
+++ b/package.xml
@@ -10,6 +10,8 @@
   <depend>rclpy</depend>
   <depend>segway_msgs</depend>
 
+  <exec_depend>ros2launch</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
diff --git a/setup.py b/setup.py
index c63ecb9..e32820f 100644
--- a/setup.py
+++ b/setup.py
@@ -1,4 +1,6 @@
 from setuptools import setup
+import os
+from glob import glob
 
 package_name = 'rmp220_teleop'
 
@@ -10,6 +12,7 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name), glob('launch/*.launch.py')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -23,4 +26,5 @@ setup(
             'rmp220_teleop = rmp220_teleop.rmp220_teleop:main'
         ],
     },
+    
 )