Update hardware interface and launch configuration for QEV3D controller

QUT-Motorsport · Feb 7, 2025 · f320797 · f320797
1 parent e666e6a
commit f320797
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 49 deletions.
diff --git a/src/control/ros2_control/CMakeLists.txt b/src/control/ros2_control/CMakeLists.txt
@@ -71,6 +71,7 @@ install(
 install(DIRECTORY description/
   DESTINATION share/${PROJECT_NAME}
 )
+
 install(
   DIRECTORY bringup/launch bringup/config
   DESTINATION share/${PROJECT_NAME}

diff --git a/src/control/ros2_control/bringup/config/qev3d_controller.yaml b/src/control/ros2_control/bringup/config/qev3d_controller.yaml
@@ -4,15 +4,25 @@
 controller_manager:
   ros__parameters:
     update_rate: 10  # Hz
-    hardware_interface: "qev3d_hw_interfaces/Qev3dHardwareInterface"
+    hardware_interface: "qev3d_ros2_control/Qev3dHardwareInterface"
+
+
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-  bicycle_steering_controller:
+    bicycle_steering_controller: # The main controller for the robot
+      type: bicycle_steering_controller/BicycleSteeringController
+
+    drive_pid_controller: # The PID Controller for rear-wheel joints
+      type: pid_controller/PidController
+
+    steering_pid_controller: # The PID Controller for front-wheel joints
+      type: pid_controller/PidController
 
-    type: bicycle_steering_controller/BicycleSteeringController
 
+bicycle_steering_controller:
+  ros__parameters:
     # TODO Set up controllers chaining
     in_chaied_mode: false
 
@@ -51,8 +61,8 @@ controller_manager:
     use_stamped_vel: true # Enable to use stamped velocity messages
 
 
-  drive_pid_controller: # The PID Controller for rear-wheel joints
-    type: pid_controller/PidController
+drive_pid_controller: # The PID Controller for rear-wheel joints
+  ros__parameters:
     dof_names:
       - virtual_rear_wheel_joint # The joint that PID controller will control
 
@@ -63,8 +73,8 @@ controller_manager:
       virtual_rear_wheel_joint: {p: 30.0, i: 0.0, d: 0.0, antiwindup: true, i_clamp_max: 5.0, i_clamp_min: -5.0}
 
 
-  steering_pid_controller: # The PID Controller for front-wheel joints
-    type: pid_controller/PidController
+steering_pid_controller: # The PID Controller for front-wheel joints
+  ros__parameters:
     dof_names:
       - virtual_front_wheel_joint # The joint that PID controller will control
 

diff --git a/src/control/ros2_control/bringup/launch/qev3d.launch.py b/src/control/ros2_control/bringup/launch/qev3d.launch.py
@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, RegisterEventHandler
 from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
+from launch.event_handlers import OnProcessStart
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
@@ -112,36 +112,29 @@ def generate_launch_description():
     )
 
     # Event Handlers for delayed execution
-    delay_robot_bicycle_controller_spawner_after_steering_controller_spawner = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=steering_pid_controller,
-            on_exit=[robot_bicycle_controller_spawner],
-        )
-    )
-    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[rviz_node],
-        )
-    )
     delay_controller_spawners = RegisterEventHandler(
-        event_handler=OnProcessExit(
+        event_handler=OnProcessStart(
             target_action=robot_state_pub_bicycle_node,
-            on_exit=[
+            on_start=[
                 joint_state_broadcaster_spawner,
                 drive_pid_controller,
                 steering_pid_controller,
             ],
         )
     )
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=joint_state_broadcaster_spawner,
+            on_start=[rviz_node],
+        )
+    )
 
     # Nodes to launch
     nodes = [
         control_node,
         joint_state_publisher_node,
         robot_state_pub_bicycle_node,
         delay_controller_spawners,
-        rviz_node,
         delay_rviz_after_joint_state_broadcaster_spawner,
     ]
 

diff --git a/src/control/ros2_control/description/qev3d.ros2_control.xacro b/src/control/ros2_control/description/qev3d.ros2_control.xacro
@@ -6,7 +6,7 @@
 
     <ros2_control name="${name}" type="system">
       <hardware>
-        <plugin>ros2_control_demo_example_11/CarlikeBotSystemHardware</plugin>
+        <plugin>qev3d_ros2_control/Qev3dHardwareInterface</plugin>
         <param name="example_param_hw_start_duration_sec">0</param>
         <param name="example_param_hw_stop_duration_sec">3.0</param>
       </hardware>

diff --git a/...l/ros2_control/hardware_interface/include/qev3d_ros2_control/qev3d_hardware_interface.hpp b/...l/ros2_control/hardware_interface/include/qev3d_ros2_control/qev3d_hardware_interface.hpp
@@ -88,11 +88,11 @@ class Qev3dHardwareInterface : public hardware_interface::SystemInterface {
     // Write the command to the hardware
     hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
-    // Export command interfaces
-    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+    // // Export command interfaces
+    // std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
-    // Export state interfaces
-    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+    // // Export state interfaces
+    // std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
 
    private:
     // Parameters for the simulation

diff --git a/src/control/ros2_control/hardware_interface/src/qev3d_hardware_interface.cpp b/src/control/ros2_control/hardware_interface/src/qev3d_hardware_interface.cpp
@@ -177,27 +177,28 @@ hardware_interface::return_type Qev3dHardwareInterface::write(const rclcpp::Time
 }
 
 // Export command interfaces
-std::vector<hardware_interface::CommandInterface> Qev3dHardwareInterface::export_command_interfaces() {
-    // std::vector<hardware_interface::CommandInterface> command_interfaces;
-    // command_interfaces.emplace_back(
-    //     hardware_interface::CommandInterface(steering_joint_, hardware_interface::HW_IF_POSITION,
-    //     &command.position));
-    // command_interfaces.emplace_back(
-    //     hardware_interface::CommandInterface(drive_joint_, hardware_interface::HW_IF_VELOCITY, &command.velocity));
-    // return command_interfaces;
-}
-
-// Export state interfaces
-std::vector<hardware_interface::StateInterface> Qev3dHardwareInterface::export_state_interfaces() {
-    // std::vector<hardware_interface::StateInterface> state_interfaces;
-    // state_interfaces.emplace_back(
-    //     hardware_interface::StateInterface(steering_joint_, hardware_interface::HW_IF_POSITION, &state.position));
-    // state_interfaces.emplace_back(
-    //     hardware_interface::StateInterface(drive_joint_, hardware_interface::HW_IF_VELOCITY, &state.velocity));
-    // state_interfaces.emplace_back(
-    //     hardware_interface::StateInterface(drive_joint_, hardware_interface::HW_IF_POSITION, &state.position));
-    // return state_interfaces;
-}
+// std::vector<hardware_interface::CommandInterface> Qev3dHardwareInterface::export_command_interfaces() {
+//     // std::vector<hardware_interface::CommandInterface> command_interfaces;
+//     // command_interfaces.emplace_back(
+//     //     hardware_interface::CommandInterface(steering_joint_, hardware_interface::HW_IF_POSITION,
+//     //     &command.position));
+//     // command_interfaces.emplace_back(
+//     //     hardware_interface::CommandInterface(drive_joint_, hardware_interface::HW_IF_VELOCITY,
+//     &command.velocity));
+//     // return command_interfaces;
+// }
+
+// // Export state interfaces
+// std::vector<hardware_interface::StateInterface> Qev3dHardwareInterface::export_state_interfaces() {
+//     // std::vector<hardware_interface::StateInterface> state_interfaces;
+//     // state_interfaces.emplace_back(
+//     //     hardware_interface::StateInterface(steering_joint_, hardware_interface::HW_IF_POSITION, &state.position));
+//     // state_interfaces.emplace_back(
+//     //     hardware_interface::StateInterface(drive_joint_, hardware_interface::HW_IF_VELOCITY, &state.velocity));
+//     // state_interfaces.emplace_back(
+//     //     hardware_interface::StateInterface(drive_joint_, hardware_interface::HW_IF_POSITION, &state.position));
+//     // return state_interfaces;
+// }
 
 }  // namespace qev3d_ros2_control