Merge branch 'odriverobotics-main'

ErikParkerrr · Jan 19, 2025 · 77e5538 · 77e5538
2 parents 8c36d2d + 1172e67
commit 77e5538
Showing 1 changed file with 51 additions and 48 deletions.
diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp
@@ -36,7 +36,9 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface
 
 private:
     void on_can_msg(const can_frame& frame);
+    void set_axis_command_mode(const Axis& axis);
 
+    bool active_;
     EpollEventLoop event_loop_;
     std::vector<Axis> axes_;
     std::string can_intf_name_;
@@ -94,7 +96,7 @@ struct Axis {
     bool torque_input_enabled_ = false;
 
     template <typename T>
-    void send(const T& msg) {
+    void send(const T& msg) const {
         struct can_frame frame;
         frame.can_id = node_id_ << 5 | msg.cmd_id;
         frame.can_dlc = msg.msg_length;
@@ -159,16 +161,21 @@ CallbackReturn ODriveHardwareInterface::on_activate(const State&) {
 
     // This can be called several seconds before the controller finishes starting.
     // Therefore we enable the ODrives only in perform_command_mode_switch().
+
+    active_ = true;
+    for (auto& axis : axes_) {
+        set_axis_command_mode(axis);
+    }
+
     return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn ODriveHardwareInterface::on_deactivate(const State&) {
     RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "deactivating ODrives...");
 
+    active_ = false;
     for (auto& axis : axes_) {
-        Set_Axis_State_msg_t msg;
-        msg.Axis_Requested_State = AXIS_STATE_IDLE;
-        axis.send(msg);
+        set_axis_command_mode(axis);
     }
 
     return CallbackReturn::SUCCESS;
@@ -231,8 +238,7 @@ return_type ODriveHardwareInterface::perform_command_mode_switch(
         std::array<std::pair<std::string, bool*>, 3> interfaces = {
             {{info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, &axis.pos_input_enabled_},
              {info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, &axis.vel_input_enabled_},
-             {info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, &axis.torque_input_enabled_}}
-        };
+             {info_.joints[i].name + "/" + hardware_interface::HW_IF_EFFORT, &axis.torque_input_enabled_}}};
 
         bool mode_switch = false;
 
@@ -255,48 +261,7 @@ return_type ODriveHardwareInterface::perform_command_mode_switch(
         }
 
         if (mode_switch) {
-            Set_Controller_Mode_msg_t msg;
-            if (axis.pos_input_enabled_) {
-                RCLCPP_INFO(
-                    rclcpp::get_logger("ODriveHardwareInterface"),
-                    "Setting %s to position control",
-                    info_.joints[i].name.c_str()
-                );
-                msg.Control_Mode = CONTROL_MODE_POSITION_CONTROL;
-                msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
-            } else if (axis.vel_input_enabled_) {
-                RCLCPP_INFO(
-                    rclcpp::get_logger("ODriveHardwareInterface"),
-                    "Setting %s to velocity control",
-                    info_.joints[i].name.c_str()
-                );
-                msg.Control_Mode = CONTROL_MODE_VELOCITY_CONTROL;
-                msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
-            } else {
-                RCLCPP_INFO(
-                    rclcpp::get_logger("ODriveHardwareInterface"),
-                    "Setting %s to torque control",
-                    info_.joints[i].name.c_str()
-                );
-                msg.Control_Mode = CONTROL_MODE_TORQUE_CONTROL;
-                msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
-            }
-
-            bool any_enabled = axis.pos_input_enabled_ || axis.vel_input_enabled_ || axis.torque_input_enabled_;
-
-            if (any_enabled) {
-                axis.send(msg); // Set control mode
-            }
-
-            // Set axis state
-            Clear_Errors_msg_t msg1;
-            msg1.Identify = 0;
-            axis.send(msg1);
-
-            // Set axis state
-            Set_Axis_State_msg_t msg2;
-            msg2.Axis_Requested_State = any_enabled ? AXIS_STATE_CLOSED_LOOP_CONTROL : AXIS_STATE_IDLE;
-            axis.send(msg2);
+            set_axis_command_mode(axis);
         }
     }
 
@@ -391,6 +356,44 @@ void ODriveHardwareInterface::on_can_msg(const can_frame& frame) {
     }
 }
 
+void ODriveHardwareInterface::set_axis_command_mode(const Axis& axis) {
+    if (!active_) {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Interface inactive. Setting axis to idle.");
+        Set_Axis_State_msg_t idle_msg;
+        idle_msg.Axis_Requested_State = AXIS_STATE_IDLE;
+        axis.send(idle_msg);
+        return;
+    }
+
+    Set_Controller_Mode_msg_t control_msg;
+    Clear_Errors_msg_t clear_error_msg;
+    Set_Axis_State_msg_t state_msg;
+
+    clear_error_msg.Identify = 0;
+    control_msg.Input_Mode = INPUT_MODE_PASSTHROUGH;
+    state_msg.Axis_Requested_State = AXIS_STATE_CLOSED_LOOP_CONTROL;
+
+    if (axis.pos_input_enabled_) {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to position control.");
+        control_msg.Control_Mode = CONTROL_MODE_POSITION_CONTROL;
+    } else if (axis.vel_input_enabled_) {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to velocity control.");
+        control_msg.Control_Mode = CONTROL_MODE_VELOCITY_CONTROL;
+    } else if (axis.torque_input_enabled_) {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to torque control.");
+        control_msg.Control_Mode = CONTROL_MODE_TORQUE_CONTROL;
+    } else {
+        RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "No control mode specified. Setting to idle.");
+        state_msg.Axis_Requested_State = AXIS_STATE_IDLE;
+        axis.send(state_msg);
+        return;
+    }
+
+    axis.send(control_msg);
+    axis.send(clear_error_msg);
+    axis.send(state_msg);
+}
+
 void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) {
     uint8_t cmd = frame.can_id & 0x1f;