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

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

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

@@ -1 +1,88 @@
-// Noting yet
+// Copyright 2021 ros2_control Development Team
+//
+// 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.
+
+#ifndef QEV3D_HARDWARE_INTERFACE_HPP_
+#define QEV3D_HARDWARE_INTERFACE_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+
+namespace qev3d_hardware_interface {
+struct JointValue {
+    double position{0.0};
+    double velocity{0.0};
+    double effort{0.0};
+};
+
+struct Joint {
+    explicit Joint(const std::string& name) : joint_name(name) {
+        state = JointValue();
+        command = JointValue();
+    }
+
+    Joint() = default;
+
+    std::string joint_name;
+    JointValue state;
+    JointValue command;
+};
+class Qev3dHardwareInterface : public hardware_interface::SystemInterface {
+   public:
+    RCLCPP_SHARED_PTR_DEFINITIONS(Qev3dHardwareInterface);
+
+    // Initialize the hardware interface with the given hardware information
+    hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
+
+    // Configure the hardware interface with the given lifecycle state
+    hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+    // Activate the hardware interface with the given lifecycle state
+    hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+    // Deactivate the hardware interface with the given lifecycle state
+    hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+    // Read the current state of the hardware
+    hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+    // Write the command to the hardware
+    hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+   private:
+    // Parameters for the simulation
+    double hw_start_sec_;  // Time in seconds to start the hardware
+    double hw_stop_sec_;   // Time in seconds to stop the hardware
+
+    // Joint names
+    std::string steering_joint_;  // Name of the steering joint
+    std::string traction_joint_;  // Name of the traction joint
+};
+
+}  // namespace qev3d_hardware_interface
+
+#endif  // QEV3D_HARDWARE_INTERFACE_HPP_

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

@@ -1 +1,125 @@
-// Nothing yet
+#include "qev3d_hardware_interface.hpp"
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace qev3d_hardware_interface {
+
+hardware_interface::CallbackReturn Qev3dHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) {
+    // Check if the parent class hardware_interface::SystemInterface can be initialized
+    if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
+        return hardware_interface::CallbackReturn::ERROR;
+    }
+
+    // Check if the number of joints is correct based on the mode of operation
+    if (info_.joints.size() != 2) {
+        RCLCPP_ERROR(get_logger(),
+                     "CarlikeBotSystemHardware::on_init() - Failed to initialize, "
+                     "because the number of joints %ld is not 2.",
+                     info_.joints.size());
+        return hardware_interface::CallbackReturn::ERROR;
+    }
+
+    // This code checks if there are exactly 2 joints. It then iterates through the joints to identify and validate them
+    // as either steering or drive joints. Steering joints must have one position command interface and one position
+    // state interface. Drive joints must have one effort command interface and two state interfaces (velocity and
+    // position). If any joint does not meet the expected criteria, it logs an error and returns an error status.
+    for (const hardware_interface::ComponentInfo& joint : info_.joints) {
+        bool joint_is_steering = joint.name.find("front") != std::string::npos;
+
+        // Steering joints have a position command interface and a position state interface
+        if (joint_is_steering) {
+            steering_joint_ = joint.name;
+            RCLCPP_INFO(get_logger(), "Joint '%s' is a steering joint.", joint.name.c_str());
+
+            if (joint.command_interfaces.size() != 1) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.",
+                             joint.name.c_str(), joint.command_interfaces.size());
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(),
+                             joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces.size() != 1) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(),
+                             joint.state_interfaces.size());
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(),
+                             joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+        } else {
+            RCLCPP_INFO(get_logger(), "Joint '%s' is a drive joint.", joint.name.c_str());
+            traction_joint_ = joint.name;
+
+            // Drive joints have an effort command interface and velocity and position state interfaces
+            if (joint.command_interfaces.size() != 1) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.",
+                             joint.name.c_str(), joint.command_interfaces.size());
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(),
+                             joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces.size() != 2) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %zu state interfaces. 2 expected.", joint.name.c_str(),
+                             joint.state_interfaces.size());
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(),
+                             joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+
+            if (joint.state_interfaces[1].name != hardware_interface::HW_IF_POSITION) {
+                RCLCPP_FATAL(get_logger(), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(),
+                             joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_POSITION);
+                return hardware_interface::CallbackReturn::ERROR;
+            }
+        }
+    }
+
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn Qev3dHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) {
+    // ...implementation...
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn Qev3dHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state) {
+    // ...implementation...
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn Qev3dHardwareInterface::on_deactivate(
+    const rclcpp_lifecycle::State& previous_state) {
+    // ...implementation...
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type Qev3dHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& period) {
+    // ...implementation...
+    return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type Qev3dHardwareInterface::write(const rclcpp::Time& time,
+                                                              const rclcpp::Duration& period) {
+    // ...implementation...
+    return hardware_interface::return_type::OK;
+}
+
+}  // namespace qev3d_hardware_interface