Merge branch 'feature/ros2_control_migration_setup' of https://github…

….com/QUT-Motorsport/QUTMS_Driverless into feature/ros2_control_migration_setup

.pre-commit-config.yaml

@@ -8,8 +8,6 @@ repos:
     rev: 'v14.0.6'
     hooks:
       - id: clang-format
-        args: ["-i"]  # Apply changes directly
-        pass_filenames: false  # Apply to all files, avoiding conflicts
   - repo: https://github.com/psf/black
     rev: 22.6.0
     hooks:

src/control/ros2_control/CMakeLists.txt

@@ -21,6 +21,14 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
 )
 
+# Specify the required version of ros2_control
+find_package(controller_manager 4.0.0)
+# Handle the case where the required version is not found
+if(NOT controller_manager_FOUND)
+  message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. "
+  "Are you using the correct branch of the ros2_control_demos repository?")
+endif()
+
 # Find additional dependencies
 find_package(backward_ros REQUIRED)
 find_package(ament_cmake REQUIRED)
@@ -36,6 +44,7 @@ add_library(
 )
 
 # Set include directories for the library
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
 target_include_directories(
   ${PROJECT_NAME}
   PUBLIC
@@ -50,7 +59,7 @@ ament_target_dependencies(
 )
 
 # Export hardware plugins
-pluginlib_export_plugin_description_file(hardware_interface qev3d_hardware_plugin.xml)  # Rename to your own plugin description file
+pluginlib_export_plugin_description_file(hardware_interface qev3d_hardware_plugin.xml)
 
 # Install include directory
 install(
@@ -60,7 +69,11 @@ install(
 
 # Install the description directory
 install(DIRECTORY description/
-  DESTINATION share/${PROJECT_NAME}/description
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY bringup/launch bringup/config
+  DESTINATION share/${PROJECT_NAME}
 )
 
 # Install the library
@@ -84,13 +97,10 @@ 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)
-#   ament_add_pytest_test(example_11_urdf_xacro test/test_urdf_xacro.py)  # Rename to your own test files
-#   ament_add_pytest_test(view_example_11_launch test/test_view_robot_launch.py)  # Rename to your own test files
-#   ament_add_pytest_test(run_example_11_launch test/test_carlikebot_launch.py)  # Rename to your own test files
-#   ament_add_pytest_test(run_example_11_launch_remapped test/test_carlikebot_launch_remapped.py)  # Rename to your own test files
-# endif()
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(example_11_urdf_xacro test/test_qev3d_ros2_control_launch.py)  # Rename to your own test files
+endif()
 
 # Export include directories
 ament_export_include_directories(
@@ -99,7 +109,9 @@ ament_export_include_directories(
 # Export libraries
 ament_export_libraries(${PROJECT_NAME})
 
-# Export targets and dependencies
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+# Export targets (e.g. libraries, executables, etc.), used when you want to export both library and executables
+# ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+# Export dependencies
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

src/control/ros2_control/bringup/config/qev3d_controller.yaml

@@ -4,6 +4,7 @@
 controller_manager:
   ros__parameters:
     update_rate: 10  # Hz
+    hardware_interface: "qev3d_hw_interfaces/Qev3dHardwareInterface"
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster

src/control/ros2_control/hardware_interface/include/qev3d_hardware_interface.hpp → src/control/ros2_control/hardware_interface/include/qev3d_ros2_control/qev3d_hardware_interface.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef QEV3D_HARDWARE_INTERFACE_HPP_
-#define QEV3D_HARDWARE_INTERFACE_HPP_
+#ifndef QEV3D_ROS2_CONTROL_HARDWARE_INTERFACE_HPP_
+#define QEV3D_ROS2_CONTROL_HARDWARE_INTERFACE_HPP_
 
 #include <map>
 #include <memory>
@@ -36,7 +36,8 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
-namespace qev3d_hardware_interface {
+namespace qev3d_ros2_control {
+
 struct JointValue {
     double position{0.0};
     double velocity{0.0};
@@ -54,6 +55,7 @@ struct Joint {
     JointValue state;
     JointValue command;
 };
+
 class Qev3dHardwareInterface : public hardware_interface::SystemInterface {
     struct Config {
         std::string steering_joint = "";
@@ -108,6 +110,6 @@ class Qev3dHardwareInterface : public hardware_interface::SystemInterface {
     Config config_;
 };
 
-}  // namespace qev3d_hardware_interface
+}  // namespace qev3d_ros2_control
 
-#endif  // QEV3D_HARDWARE_INTERFACE_HPP_
+#endif  // QEV3D_ROS2_CONTROL_HARDWARE_INTERFACE_HPP_

src/control/ros2_control/hardware_interface/src/qev3d_hardware_interface.cpp

@@ -1,9 +1,10 @@
-#include "qev3d_hardware_interface.hpp"
+#include "qev3d_ros2_control/qev3d_hardware_interface.hpp"
 
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "pluginlib/class_list_macros.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-namespace qev3d_hardware_interface {
+namespace qev3d_ros2_control {
 
 // pass in the hardware info (command, state interfaces and joints) in the qev3d_ros2_control.xacro
 hardware_interface::CallbackReturn Qev3dHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) {
@@ -119,17 +120,16 @@ hardware_interface::CallbackReturn Qev3dHardwareInterface::on_init(const hardwar
         }
     }
 
-    // TODO: CLARIFICATION NEEDED: invalid parameter names
+    // Initialize the configuration parameters for the hardware interface
     config_.drive_joint = info_.joints[1].name;     // info_.joints[1] is the drive joint (virtual_rear_wheel_joint)
     config_.steering_joint = info_.joints[0].name;  // info_.joints[0] is the steering joint (virtual_front_wheel_joint)
-    // config_.drive_joint = info_.hardware_parameters["virtual_rear_wheel_joint"];
-    // config_.steering_joint = info_.hardware_parameters["virtual_front_wheel_joint"];
     config_.hw_start_sec = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
     config_.hw_stop_sec = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
 
     return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+// Configure the hardware interface with the given lifecycle state
 hardware_interface::CallbackReturn Qev3dHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) {
     // ...implementation...
 
@@ -138,31 +138,36 @@ hardware_interface::CallbackReturn Qev3dHardwareInterface::on_configure(const rc
     return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+// Cleanup the hardware interface with the given lifecycle state
 hardware_interface::CallbackReturn Qev3dHardwareInterface::on_cleanup(const rclcpp_lifecycle::State& previous_state) {
     // Implementation for cleanup state
     RCLCPP_INFO(get_logger(), "Cleaning up hardware interface.");
     // ...additional cleanup code...
     return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+// Activate the hardware interface with the given lifecycle state
 hardware_interface::CallbackReturn Qev3dHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) {
     // ...implementation...
     return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+// Deactivate the hardware interface with the given lifecycle state
 hardware_interface::CallbackReturn Qev3dHardwareInterface::on_deactivate(
     const rclcpp_lifecycle::State& previous_state) {
     // ...implementation...
     return hardware_interface::CallbackReturn::SUCCESS;
 }
 
+// Read the current state of the hardware
 hardware_interface::return_type Qev3dHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& period) {
     // ...implementation...
 
     // can_translator_node_.canmsg_timer();
     return hardware_interface::return_type::OK;
 }
 
+// Write the command to the hardware
 hardware_interface::return_type Qev3dHardwareInterface::write(const rclcpp::Time& time,
                                                               const rclcpp::Duration& period) {
     // ...implementation...
@@ -171,25 +176,29 @@ hardware_interface::return_type Qev3dHardwareInterface::write(const rclcpp::Time
     return hardware_interface::return_type::OK;
 }
 
-// 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;
-// }
-
-// 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_hardware_interface
+// 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;
+}
+
+}  // namespace qev3d_ros2_control
+
+PLUGINLIB_EXPORT_CLASS(qev3d_ros2_control::Qev3dHardwareInterface, hardware_interface::SystemInterface)

src/control/ros2_control/qev3d_hardware_plugin.xml

@@ -1,9 +1,9 @@
 
 <!-- These are currently copied code from Walden's branch. Change later when Hardware interface is made -->
 
-<library path="qev3d_hw_interfaces">
-  <class name="qev_hw_interfaces/Qev3dTopicInterface"
-         type="qutms_hw_interfaces::Qev3dTopicInterface"
+<library path="qev3d_ros2_control">
+  <class name="qev3d_ros2_control/Qev3dHardwareInterface"
+         type="qev3d_ros2_control::Qev3dHardwareInterface"
          base_class_type="hardware_interface::SystemInterface">
     <description>
       ros2_control hardware interface.