Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: Using lifecycle nodes to their full potential #35

Merged
merged 2 commits into from
Jan 10, 2025
Merged

feat: Using lifecycle nodes to their full potential #35

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3fb205f
Adding control and clear error functionality to the on_activate metho…
marj3220 Dec 19, 2024
a886851
Handling no control mode case independantly + using const.
marj3220 Jan 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
95 changes: 57 additions & 38 deletions odrive_ros2_control/src/odrive_hardware_interface.cpp
View file
Open in desktop
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().

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The comment above suggests that the axis should not be put to CLOSED_LOOP_CONTROL in here. But I need to fill in the gap in understanding to know what's the best thing to do.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If it helps, I show when on_activate() and perform_command_mode_switch() is called during the initialization in my screenshots.

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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it's best to treat this case and the !active_ case the same way, by sending a single IDLE request message. So only send the clear_error_msg and control_msg conditionally. The way it's written here, if all interfaces are stopped, it's sending a Set_Controller_Mode_msg_t with an undefined Control_Mode.

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
Loading