Activate MotorHardware

include/ubiquity_motor_ros2/motor_hardware.h

@@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MOTORHARDWARE_H
 
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
-#include <hardware_interface/actuator_interface.hpp>
+#include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/loaned_command_interface.hpp>
 #include <hardware_interface/loaned_state_interface.hpp>
@@ -140,7 +140,7 @@ struct MotorDiagnostics {
     void firmware_date_status(diagnostic_updater::DiagnosticStatusWrapper &stat);
 };
 
-class MotorHardware : public hardware_interface::ActuatorInterface {
+class MotorHardware : public hardware_interface::SystemInterface {
 public:
     // MotorHardware(const std::shared_ptr<rclcpp::Node>& n, NodeParams& node_params, CommsParams& serial_params, FirmwareParams& firmware_params);
     MotorHardware();
@@ -159,11 +159,13 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
     hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
 
-    // hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override
-    // {
+    hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override
+    {
 
-    //     return hardware_interface::CallbackReturn::SUCCESS;
-    // }
+        RCLCPP_INFO(logger, "on_configure");
+
+        return hardware_interface::CallbackReturn::SUCCESS;
+    }
 
     // hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
     // {

src/motor_hardware.cc

@@ -219,7 +219,7 @@ MotorHardware::MotorHardware()
     estopReleaseDeadtime = 0.8;
     estopReleaseDelay    = 0.0;
 
-    RCLCPP_INFO(logger, "MotorHardware constructed");
+    // RCLCPP_INFO(logger, "MotorHardware constructed");
 
 }
 
@@ -278,7 +278,7 @@ void MotorHardware::init(const std::shared_ptr<rclcpp::Node>& n, const std::shar
     }
 
 
-    RCLCPP_INFO(logger, "MotorHardware inited.");
+    // RCLCPP_INFO(logger, "MotorHardware inited.");
 
 }
 
@@ -346,6 +346,8 @@ std::vector<hardware_interface::CommandInterface> MotorHardware::export_command_
 
 hardware_interface::CallbackReturn MotorHardware::on_activate(const rclcpp_lifecycle::State& previous_state)
 {
+    RCLCPP_INFO(logger, "MotorHardware activate");
+
     // Activate the hardware, ensure the motors are ready to receive commands
     // setParams(fw_params); // Don't neeed this sice they are now passed as a reference
     requestFirmwareVersion();
@@ -419,6 +421,9 @@ hardware_interface::return_type MotorHardware::read(const rclcpp::Time& current_
     // current_time = rclcpp::Clock().now();
     // elapsed_loop_time = current_time - last_loop_time;
     // last_loop_time = current_time;
+
+    RCLCPP_INFO(logger, "MotorHardware read");
+
 
     if(node == nullptr){
         // Not yet initialized
@@ -442,6 +447,9 @@ hardware_interface::return_type MotorHardware::read(const rclcpp::Time& current_
 
 hardware_interface::return_type MotorHardware::write(const rclcpp::Time& current_time, const rclcpp::Duration& elapsed_loop_time) {
 
+    RCLCPP_INFO(logger, "MotorHardware write");
+
+
     if(node == nullptr){
         // Not yet initialized
         return hardware_interface::return_type::OK;
@@ -1890,4 +1898,4 @@ void MotorDiagnostics::firmware_options_status(DiagnosticStatusWrapper &stat) {
 }
 
 
-PLUGINLIB_EXPORT_CLASS(MotorHardware, hardware_interface::ActuatorInterface)
+PLUGINLIB_EXPORT_CLASS(MotorHardware, hardware_interface::SystemInterface)

src/motor_node.cc

@@ -57,7 +57,7 @@ hardware_interface::HardwareInfo MotorNode::getHwInfo() {
     hardware_interface::HardwareInfo hardware_info;
     hardware_info.name = "MotorHardware";
     hardware_info.hardware_plugin_name = "ubiquity_motor_ros2/MotorHardwarePlugin";
-    hardware_info.type = "actuator";
+    hardware_info.type = "system";
 
     // Populate joint information
     hardware_interface::ComponentInfo joint_info;
@@ -225,6 +225,7 @@ void MotorNode::run() {
 
     controller_manager::ControllerManager cm(std::move(resource_manager), executor, "controller_manager", get_namespace(), options);
 
+    // cm.init_controller_manager();
 
     // Subscribe to the topic with overall system control ability
     auto sub = this->create_subscription<std_msgs::msg::String>(
@@ -241,7 +242,33 @@ void MotorNode::run() {
     // f = boost::bind(&PID_update_callback, _1, _2);
     // server.setCallback(f);
 
+    // rclcpp::sleep_for(rclcpp::Duration::from_seconds(2.0).to_chrono<std::chrono::nanoseconds>());
 
+    RCLCPP_INFO(get_logger(), "Activating MotorHardware");
+
+
+    // Specify the controllers you want to activate and deactivate
+    std::vector<std::string> controllers_to_activate = {"MotorHardware"};
+    std::vector<std::string> controllers_to_deactivate = {};  // None in this example
+
+    // Set the strictness level (STRICT or BEST_EFFORT)
+    int strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+
+    // Perform the controller switch
+    controller_interface::return_type switch_result = cm.switch_controller(
+        controllers_to_activate,
+        controllers_to_deactivate,
+        strictness
+    );
+
+   if (switch_result == controller_interface::return_type::OK)
+    {
+        RCLCPP_INFO(get_logger(), "Controller %s activated successfully.", controllers_to_activate[0].c_str());
+    }
+    else
+    {
+        RCLCPP_ERROR(get_logger(), "Failed to activate controller %s.", controllers_to_activate[0].c_str());
+    }
 
     // Spin to handle callbacks and manage the control loop
     executor->spin();