diff --git a/odrive_node/README.md b/odrive_node/README.md index cad4561..7674b93 100644 --- a/odrive_node/README.md +++ b/odrive_node/README.md @@ -60,6 +60,12 @@ For information about installation, prerequisites and getting started, check out If the axis dropped into IDLE because of an error and the intent is to re-enable it, call `/request_axis_state` instead with CLOSED_LOOP_CONTROL, which clears errors automatically. +* `/estop`: Emergency stop service that immediately disarms the axis. + + This service sends an empty payload CAN frame that causes the ODrive axis to enter IDLE state with "ESTOP_REQUESTED" as the disarm reason. The service call completes immediately without waiting for confirmation from the ODrive. + + This is intended for emergency situations where the motor needs to be stopped as quickly as possible. After an estop, the axis will need to be re-enabled using `/request_axis_state` with the desired state, which will automatically clear the estop error (See `/request_axis_state` above). + ### Data Types All of the Message/Service fields are directly related to their corresponding CAN message. For more detailed information about each type, and how to interpet the data, please refer to the [ODrive CAN protocol documentation](https://docs.odriverobotics.com/v/latest/manual/can-protocol.html#messages). diff --git a/odrive_node/include/odrive_can_node.hpp b/odrive_node/include/odrive_can_node.hpp index 78f7417..3535f63 100644 --- a/odrive_node/include/odrive_can_node.hpp +++ b/odrive_node/include/odrive_can_node.hpp @@ -36,8 +36,10 @@ class ODriveCanNode : public rclcpp::Node { void subscriber_callback(const ControlMessage::SharedPtr msg); void service_callback(const std::shared_ptr request, std::shared_ptr response); void service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr response); + void service_estop_callback(const std::shared_ptr request, std::shared_ptr response); void request_state_callback(); void request_clear_errors_callback(); + void request_estop_callback(); void ctrl_msg_callback(); inline bool verify_length(const std::string&name, uint8_t expected, uint8_t length); @@ -69,6 +71,9 @@ class ODriveCanNode : public rclcpp::Node { EpollEvent srv_clear_errors_evt_; rclcpp::Service::SharedPtr service_clear_errors_; + EpollEvent srv_estop_evt_; + rclcpp::Service::SharedPtr service_estop_; + }; #endif // ODRIVE_CAN_NODE_HPP diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index d53fbec..be02a12 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/odrive_node/src/odrive_can_node.cpp @@ -7,6 +7,7 @@ enum CmdId : uint32_t { kHeartbeat = 0x001, // ControllerStatus - publisher + kEstop = 0x002, // Estop - service kGetError = 0x003, // SystemStatus - publisher kSetAxisState = 0x007, // SetAxisState - service kGetEncoderEstimates = 0x009, // ControllerStatus - publisher @@ -48,6 +49,9 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n rclcpp::QoS srv_clear_errors_qos(rclcpp::KeepAll{}); service_clear_errors_ = rclcpp::Node::create_service("clear_errors", std::bind(&ODriveCanNode::service_clear_errors_callback, this, _1, _2), srv_clear_errors_qos.get_rmw_qos_profile()); + + rclcpp::QoS srv_estop_qos(rclcpp::KeepAll{}); + service_estop_ = rclcpp::Node::create_service("estop", std::bind(&ODriveCanNode::service_estop_callback, this, _1, _2), srv_estop_qos.get_rmw_qos_profile()); } void ODriveCanNode::deinit() { @@ -61,6 +65,8 @@ void ODriveCanNode::deinit() { sub_evt_.deinit(); srv_evt_.deinit(); + srv_clear_errors_evt_.deinit(); + srv_estop_evt_.deinit(); can_intf_.deinit(); } @@ -86,6 +92,10 @@ bool ODriveCanNode::init(EpollEventLoop* event_loop) { RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize clear errors service event"); return false; } + if (!srv_estop_evt_.init(event_loop, std::bind(&ODriveCanNode::request_estop_callback, this))) { + RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize estop service event"); + return false; + } RCLCPP_INFO(rclcpp::Node::get_logger(), "node_id: %d", node_id_); RCLCPP_INFO(rclcpp::Node::get_logger(), "interface: %s", interface.c_str()); return true; @@ -160,7 +170,8 @@ void ODriveCanNode::recv_callback(const can_frame& frame) { case CmdId::kSetInputPos: case CmdId::kSetInputVel: case CmdId::kSetInputTorque: - case CmdId::kClearErrors: { + case CmdId::kClearErrors: + case CmdId::kEstop: { break; // Ignore commands coming from another master/host on the bus } default: { @@ -217,6 +228,11 @@ void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr response) { + RCLCPP_WARN(rclcpp::Node::get_logger(), "emergency stop requested"); + srv_estop_evt_.set(); +} + void ODriveCanNode::request_state_callback() { uint32_t axis_state; { @@ -249,6 +265,13 @@ void ODriveCanNode::request_clear_errors_callback() { can_intf_.send_can_frame(frame); } +void ODriveCanNode::request_estop_callback() { + struct can_frame frame; + frame.can_id = node_id_ << 5 | CmdId::kEstop; + frame.can_dlc = 0; + can_intf_.send_can_frame(frame); +} + void ODriveCanNode::ctrl_msg_callback() { uint32_t control_mode;