From 3fb205f1c7b51b18a3673113fe443e9b89dfeb0c Mon Sep 17 00:00:00 2001 From: "marj3220@usherbrooke.ca" Date: Thu, 19 Dec 2024 16:04:47 -0500 Subject: [PATCH 1/2] Adding control and clear error functionality to the on_activate method for ROS2 Control cycling --- .../src/odrive_hardware_interface.cpp | 91 +++++++++++-------- 1 file changed, 54 insertions(+), 37 deletions(-) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index 1c4133e..8eef5d0 100644 --- 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(Axis axis); + bool active_; EpollEventLoop event_loop_; std::vector axes_; std::string can_intf_name_; @@ -121,7 +123,11 @@ CallbackReturn ODriveHardwareInterface::on_init(const hardware_interface::Hardwa CallbackReturn ODriveHardwareInterface::on_configure(const State&) { if (!can_intf_.init(can_intf_name_, &event_loop_, std::bind(&ODriveHardwareInterface::on_can_msg, this, _1))) { - RCLCPP_ERROR(rclcpp::get_logger("ODriveHardwareInterface"), "Failed to initialize SocketCAN on %s", can_intf_name_.c_str()); + RCLCPP_ERROR( + rclcpp::get_logger("ODriveHardwareInterface"), + "Failed to initialize SocketCAN on %s", + can_intf_name_.c_str() + ); return CallbackReturn::ERROR; } RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Initialized SocketCAN on %s", can_intf_name_.c_str()); @@ -138,16 +144,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; @@ -210,8 +221,7 @@ return_type ODriveHardwareInterface::perform_command_mode_switch( std::array, 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; @@ -234,36 +244,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); } } @@ -286,7 +267,7 @@ return_type ODriveHardwareInterface::write(const rclcpp::Time&, const rclcpp::Du if (axis.pos_input_enabled_) { Set_Input_Pos_msg_t msg; msg.Input_Pos = axis.pos_setpoint_ / (2 * M_PI); - msg.Vel_FF = axis.vel_input_enabled_ ? (axis.vel_setpoint_ / (2 * M_PI)) : 0.0f; + msg.Vel_FF = axis.vel_input_enabled_ ? (axis.vel_setpoint_ / (2 * M_PI)) : 0.0f; msg.Torque_FF = axis.torque_input_enabled_ ? axis.torque_setpoint_ : 0.0f; axis.send(msg); } else if (axis.vel_input_enabled_) { @@ -314,6 +295,42 @@ void ODriveHardwareInterface::on_can_msg(const can_frame& frame) { } } +void ODriveHardwareInterface::set_axis_command_mode(Axis axis) { + if (!active_) { + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting 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; + Set_Axis_State_msg_t state_msg; + Clear_Errors_msg_t clear_error_msg; + + control_msg.Input_Mode = INPUT_MODE_PASSTHROUGH; + state_msg.Axis_Requested_State = AXIS_STATE_CLOSED_LOOP_CONTROL; + clear_error_msg.Identify = 0; + + 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"); + state_msg.Axis_Requested_State = AXIS_STATE_IDLE; + } + + axis.send(clear_error_msg); + axis.send(state_msg); + axis.send(control_msg); +} + void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) { uint8_t cmd = frame.can_id & 0x1f; From a8868518794fea779eb6329e577f9709d1d25c43 Mon Sep 17 00:00:00 2001 From: "marj3220@usherbrooke.ca" Date: Tue, 7 Jan 2025 12:51:17 -0500 Subject: [PATCH 2/2] Handling no control mode case independantly + using const. --- .../src/odrive_hardware_interface.cpp | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index 8eef5d0..a0691ef 100644 --- a/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -36,7 +36,7 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface private: void on_can_msg(const can_frame& frame); - void set_axis_command_mode(Axis axis); + void set_axis_command_mode(const Axis& axis); bool active_; EpollEventLoop event_loop_; @@ -90,7 +90,7 @@ struct Axis { bool torque_input_enabled_ = false; template - 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; @@ -295,9 +295,9 @@ void ODriveHardwareInterface::on_can_msg(const can_frame& frame) { } } -void ODriveHardwareInterface::set_axis_command_mode(Axis axis) { +void ODriveHardwareInterface::set_axis_command_mode(const Axis& axis) { if (!active_) { - RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to idle"); + 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); @@ -305,30 +305,32 @@ void ODriveHardwareInterface::set_axis_command_mode(Axis axis) { } Set_Controller_Mode_msg_t control_msg; - Set_Axis_State_msg_t state_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; - clear_error_msg.Identify = 0; if (axis.pos_input_enabled_) { - RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Setting to position control"); + 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"); + 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"); + 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"); + 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); - axis.send(control_msg); } void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) {