From e9f69250f6001ecbf8d5620ccd6d1b0c54ecd330 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Wed, 14 Jun 2023 09:45:55 +0200
Subject: [PATCH] update

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

diff --git a/launch/twist_mux.launch.py b/launch/twist_mux.launch.py
new file mode 100644
index 0000000..33c1b01
--- /dev/null
+++ b/launch/twist_mux.launch.py
@@ -0,0 +1,120 @@
+# Copyright 2022 Factor Robotics
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import LifecycleNode
+from launch_ros.descriptions import ParameterValue
+
+
+def generate_launch_description():
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("bot_mini_description"),
+                    "urdf",
+                    "odrive_diffbot.urdf.xacro"
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("bot_mini_bringup"),
+            "config",
+            "diffbot_controllers.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        output="both",
+    )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+
+    robot_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["diffbot_base_controller", "-c", "/controller_manager"],
+    )
+
+    joystick_spawner = Node(
+        package="joy",
+        executable="joy_node"
+    )
+
+    teleop_spawner = Node(
+        package="rmp220_teleop",
+        executable="rmp220_teleop",
+        remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')]
+    )
+
+    cam_node = Node(
+        package="ros2_cam_openCV",
+        executable="cam_node"
+    )
+
+    lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml')         
+    lidar_node = LifecycleNode(
+        package='lslidar_driver',
+        executable='lslidar_driver_node',
+        name='lslidar_driver_node',
+        output='screen',
+        emulate_tty=True,
+        namespace='',
+        parameters=[lidar_dir],
+    )
+
+    twist_mux_params = os.path.join(get_package_share_directory('bot_mini_bringup'),'config','twist_mux.yaml')
+    twist_mux = 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')]
+        )
+
+    return LaunchDescription([
+        #control_node,
+        #robot_state_pub_node,
+        #joint_state_broadcaster_spawner,
+        #robot_controller_spawner,
+        #joystick_spawner,
+        #teleop_spawner,
+        #cam_node,
+        #lidar_node,
+        twist_mux
+    ])
\ No newline at end of file