Skip to content

Commit

Permalink
Merge pull request #35 from Oscar-Robotics/main
Browse files Browse the repository at this point in the history 
feat: Using lifecycle nodes to their full potential
  • Loading branch information
@samuelsadok
samuelsadok authored Jan 10, 2025
2 parents 17acfda + a886851 commit 8f61abb
Showing 1 changed file with 57 additions and 38 deletions.
95 changes: 57 additions & 38 deletions odrive_ros2_control/src/odrive_hardware_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -88,7 +90,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;
Expand Down Expand Up @@ -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());
Expand All @@ -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;
Expand Down Expand Up @@ -210,8 +221,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;

Expand All @@ -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);
}
}

Expand All @@ -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_) {
Expand Down Expand Up @@ -314,6 +295,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;

Expand Down

0 comments on commit 8f61abb

Please sign in to comment.