From 30eefdbe2c8679912053c008cf92c80b8ddb5ce7 Mon Sep 17 00:00:00 2001
From: Kelvin <nguyenleminh1002@gmail.com>
Date: Fri, 7 Feb 2025 11:32:28 +1000
Subject: [PATCH] Enhance QEV3D launch configuration: add vehicle_urdf
 dependency, include hardware plugin, and improve node organization

---
 src/control/ros2_control/CMakeLists.txt       |  7 +++++-
 .../bringup/launch/qev3d.launch.py            | 23 ++++++++++++++++---
 src/control/ros2_control/package.xml          |  5 ++++
 3 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/control/ros2_control/CMakeLists.txt b/src/control/ros2_control/CMakeLists.txt
index bcc858991..e13962864 100644
--- a/src/control/ros2_control/CMakeLists.txt
+++ b/src/control/ros2_control/CMakeLists.txt
@@ -1,6 +1,6 @@
 # Minimum CMake version required
 cmake_minimum_required(VERSION 3.16)
-
+find_package(vehicle_urdf REQUIRED)
 # Define the project name macro
 set(PROJECT_NAME qev3d_ros2_control)
 # Project name and language
@@ -79,6 +79,11 @@ install(
   DESTINATION share/${PROJECT_NAME}
 
 )
+
+install(FILES
+qev3d_hardware_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
 # Uncomment and modify the following lines to add tests
 # if(BUILD_TESTING)
 #   find_package(ament_cmake_pytest REQUIRED)
diff --git a/src/control/ros2_control/bringup/launch/qev3d.launch.py b/src/control/ros2_control/bringup/launch/qev3d.launch.py
index 1b16541c6..370cef319 100644
--- a/src/control/ros2_control/bringup/launch/qev3d.launch.py
+++ b/src/control/ros2_control/bringup/launch/qev3d.launch.py
@@ -101,12 +101,16 @@ def generate_launch_description():
             "bicycle_steering_controller",
             "--param-file",
             robot_controllers,
-            "--controller-ros-args",
-            "-r /bicycle_steering_controller/tf_odometry:=/tf",
         ],
         condition=IfCondition(remap_odometry_tf),
     )
 
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher",
+        condition=IfCondition(gui),  # Only launch the node if gui is set to true
+    )
+
     # Event Handlers for delayed execution
     delay_robot_bicycle_controller_spawner_after_steering_controller_spawner = RegisterEventHandler(
         event_handler=OnProcessExit(
@@ -120,16 +124,29 @@ def generate_launch_description():
             on_exit=[rviz_node],
         )
     )
+    delay_controller_spawners = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=robot_state_pub_bicycle_node,
+            on_exit=[
+                joint_state_broadcaster_spawner,
+                drive_pid_controller,
+                steering_pid_controller,
+            ],
+        )
+    )
 
     # Nodes to launch
     nodes = [
         control_node,
-        robot_state_pub_bicycle_node,
         joint_state_broadcaster_spawner,
+        joint_state_publisher_node,
         drive_pid_controller,
         steering_pid_controller,
         delay_robot_bicycle_controller_spawner_after_steering_controller_spawner,
+        robot_state_pub_bicycle_node,
         rviz_node,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_controller_spawners,
     ]
 
     return LaunchDescription(declared_arguments + nodes)
diff --git a/src/control/ros2_control/package.xml b/src/control/ros2_control/package.xml
index f299384f1..1dac5f888 100644
--- a/src/control/ros2_control/package.xml
+++ b/src/control/ros2_control/package.xml
@@ -29,6 +29,7 @@
 
   <!-- Additional dependencies -->
   <exec_depend>bicycle_steering_controller</exec_depend>
+  <exec_depend>pid_controller</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
@@ -40,6 +41,10 @@
   <exec_depend>ros2launch</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
+   <!-- Packages -->
+    <exec_depend>robot_state_publisher</exec_depend>
+    <exec_depend>joint_state_publisher</exec_depend>
+    <depend>xacro</depend>
 
   <!-- Test dependencies -->
   <test_depend>ament_cmake_pytest</test_depend>