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: can reconnect #29

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

feat: can reconnect #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7497938
feat: allow the motor to run even after the can communication connect…
uhobeike Oct 13, 2024
5c75aa6
chore: add param
uhobeike Oct 13, 2024
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
5 changes: 5 additions & 0 deletions odrive_base/include/socket_can.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,13 +16,18 @@ class SocketCanIntf {
bool send_can_frame(const can_frame& frame);

bool read_nonblocking();

void reset_can_connection_check();
bool has_can_reconnected();
private:
std::string interface_;
int socket_id_ = -1;
EpollEventLoop* event_loop_ = nullptr;
EpollEventLoop::EvtId socket_evt_id_;
FrameProcessor frame_processor_;
bool broken_ = false;
bool can_send_failed_ = false;
bool has_been_can_send_failed_ = false;

void on_socket_event(uint32_t mask);
void process_can_frame(const can_frame& frame) {
Expand Down
11 changes: 11 additions & 0 deletions odrive_base/src/socket_can.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,11 @@ bool SocketCanIntf::send_can_frame(const can_frame& frame) {
ssize_t nbytes = write(socket_id_, &frame, sizeof(frame));
if (nbytes == -1) {
std::cerr << "Failed to send CAN frame" << std::endl;
can_send_failed_ = true;
has_been_can_send_failed_ = true;
return false;
}
can_send_failed_ = false;

return true;
}
Expand Down Expand Up @@ -135,3 +138,11 @@ bool SocketCanIntf::read_nonblocking() {
process_can_frame(frame);
return true;
}

void SocketCanIntf::reset_can_connection_check() {
has_been_can_send_failed_ = false;
}

bool SocketCanIntf::has_can_reconnected() {
return can_send_failed_ == false && has_been_can_send_failed_ == true;
}
1 change: 1 addition & 0 deletions odrive_botwheel_explorer/description/urdf/diffbot.ros2_control.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<hardware>
<plugin>odrive_ros2_control_plugin/ODriveHardwareInterface</plugin>
<param name="can">can0</param>
<param name="can_reconnect">false</param>
</hardware>
</xacro:unless>
<xacro:if value="${use_mock_hardware}">
Expand Down
65 changes: 60 additions & 5 deletions odrive_ros2_control/src/odrive_hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@

#include "can_helpers.hpp"
#include "can_simple_messages.hpp"
#include "hardware_interface/lexical_casts.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "odrive_enums.h"
Expand Down Expand Up @@ -37,6 +38,8 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface
private:
void on_can_msg(const can_frame& frame);

void on_can_reconnect();

EpollEventLoop event_loop_;
std::vector<Axis> axes_;
std::string can_intf_name_;
Expand All @@ -61,10 +64,10 @@ struct Axis {

// State (ODrives => ros2_control)
// rclcpp::Time encoder_estimates_timestamp_;
// uint32_t axis_error_ = 0;
// uint8_t axis_state_ = 0;
// uint8_t procedure_result_ = 0;
// uint8_t trajectory_done_flag_ = 0;
uint32_t axis_error_ = 0;
uint8_t axis_state_ = 0;
uint8_t procedure_result_ = 0;
uint8_t trajectory_done_flag_ = 0;
double pos_estimate_ = NAN; // [rad]
double vel_estimate_ = NAN; // [rad/s]
// double iq_setpoint_ = NAN;
Expand Down Expand Up @@ -281,6 +284,10 @@ return_type ODriveHardwareInterface::read(const rclcpp::Time& timestamp, const r
}

return_type ODriveHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
if (hardware_interface::parse_bool(info_.hardware_parameters["can_reconnect"]) && can_intf_.has_can_reconnected()) {
on_can_reconnect();
}

for (auto& axis : axes_) {
// Send the CAN message that fits the set of enabled setpoints
if (axis.pos_input_enabled_) {
Expand Down Expand Up @@ -339,7 +346,55 @@ void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) {
torque_estimate_ = msg.Torque_Estimate;
}
} break;
// silently ignore unimplemented command IDs
case Heartbeat_msg_t::cmd_id: {
if (Heartbeat_msg_t msg; try_decode(msg)) {
axis_error_ = msg.Axis_Error;
axis_state_ = msg.Axis_State;
procedure_result_ = msg.Procedure_Result;
trajectory_done_flag_ = msg.Trajectory_Done_Flag;
}
} break;
// silently ignore unimplemented command IDs
}
}

void ODriveHardwareInterface::on_can_reconnect() {
for (size_t i = 0; i < axes_.size(); ++i) {
Axis& axis = axes_[i];
if (!(axis.axis_error_ == 0 && axis.axis_state_ == 8 && axis.procedure_result_ == 0 && axis.trajectory_done_flag_ == 1)) {
Copy link
Member

Choose a reason for hiding this comment

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

The if-condition needs to be revised:

  • !(axis.axis_state_ == 8) means "re-enable whenever the state is something other than CLOSED_LOOP_CONTROL". I prefer the more conservative check axis.axis_state_ == 0 which means "re-enable only when the state is IDLE". So during other states (calibration), the ROS plugin doesn't interfere.
  • The error check as it is !(axis.axis_error_ == 0) is somewhat redundant with the state check. However I think it would be nice to introduce a configurable filter such that the ROS interface only re-enables if the error is signed off by the developer.
  • axis.procedure_result_ needs to be ignored, it is only reported as ProcedureResult.SUCCESS (0) due to an oversight in the firmware. It will be ProcedureResult.BUSY (1) in future firmware versions (but we can't rely on that here either because it would break backwards compatibility)
  • axis.trajectory_done_flag_ needs to be ignored, it is undefined for input modes other than InputMode.TRAP_TRAJ. (It happens to default to true if TRAP_TRAJ has not been used since last reboot)
  • If we allow auto-enabling like that, we should also allow auto-disabling.

So in the end it becomes something like:

bool should_be_enabled = // same expression as any_enabled
if (should_be_enabled && axis.axis_state_ == 0 && (axis.axis_error_ & user_ignored_errors_) == 0) {
    request_closed_loop_control();
} else if (!should_be_enabled && axis.axis_state_ == 8) {
    request_idle();
}

Set_Controller_Mode_msg_t msg;
Copy link
Member

Choose a reason for hiding this comment

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

The content of this if block is identical to part of ODriveHardwareInterface::perform_command_mode_switch() as far as I can tell. It should therefore be refactored into its own function(s).

All things considered, I think two separate functions make sense:

void ODriveHardwareInterface::request_closed_loop_control() {
    // send Set_Controller_Mode_msg_t, Clear_Errors_msg_t and Set_Axis_State_msg_t
}

void ODriveHardwareInterface::request_idle() {
    // send single Set_Axis_State_msg_t message
}

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);
} else {
can_intf_.reset_can_connection_check();
}
}
}

Expand Down
Loading