From bf46057542685ed966a27f89337eb2c9e69a0718 Mon Sep 17 00:00:00 2001
From: Timo <timoschneider04@gmail.com>
Date: Wed, 9 Apr 2025 08:03:09 +0200
Subject: [PATCH] wip: updated sim launchfile

---
 src/sherpa/launch/simulator.launch.py | 49 +++++++++++++++++----------
 1 file changed, 32 insertions(+), 17 deletions(-)

diff --git a/src/sherpa/launch/simulator.launch.py b/src/sherpa/launch/simulator.launch.py
index 7ae48f1..ef0239a 100644
--- a/src/sherpa/launch/simulator.launch.py
+++ b/src/sherpa/launch/simulator.launch.py
@@ -46,10 +46,27 @@ def generate_launch_description():
         }]
     )
 
-    joint_pub = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
+    # Load controller manager
+    #control_manager = Node(
+    #    package='controller_manager',
+    #    executable='ros2_control_node',
+    #    parameters=[{'use_sim_time': True}, {'robot_description': '/robot_description'}],
+    #    output='screen'
+    #)
+
+    # Spawner for the mecanum controller
+    controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['mecanum_controller'],
+        output='screen'
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        parameters=[{'use_sim_time': True}],
         output='screen'
     )
 
@@ -60,8 +77,8 @@ def generate_launch_description():
         arguments=[
             '0', '0', '0',
             '0', '0', '0',
-            'lidar_link',
-            'camera_init',
+            'velodyne',
+            'base_link',
         ]          
     )
 
@@ -76,17 +93,13 @@ def generate_launch_description():
     )
 
     gz_bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
         arguments=[
-            '/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
-            '/model/sherpa/joint_states@gz.msgs.Model_JointState@ros2/sensor_msgs/msg/JointState',
-        ],
-        remappings=[
-            ('/velodyne_points/points', '/velodyne_points'),
-            ('/model/sherpa/joint_states', '/joint_states'),
-        ],
-        output='screen'
+            "--ros-args",
+            "-p",
+            f"config_file:={os.path.join(get_package_share_directory('sherpa'), 'config', 'gz_bridge.yaml')}",
+        ]
     )
 
 
@@ -99,10 +112,12 @@ def generate_launch_description():
     ld.add_action(gz_env)
     ld.add_action(gz)
     ld.add_action(state_publisher)
-    ld.add_action(joint_pub)
     ld.add_action(static_tf)
     ld.add_action(robot)
     ld.add_action(gz_bridge)
+    ld.add_action(joint_state_broadcaster)
+    ld.add_action(controller)
+    #ld.add_action(control_manager)
     ld.add_action(rviz2)
 
     return ld