Add E-stop service to send estop CAN frames

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).

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<AxisState::Request> request, std::shared_ptr<AxisState::Response> response);
     void service_clear_errors_callback(const std::shared_ptr<Empty::Request> request, std::shared_ptr<Empty::Response> response);
+    void service_estop_callback(const std::shared_ptr<Empty::Request> request, std::shared_ptr<Empty::Response> 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<Empty>::SharedPtr service_clear_errors_;
 
+    EpollEvent srv_estop_evt_;
+    rclcpp::Service<Empty>::SharedPtr service_estop_;
+
 };
 
 #endif // ODRIVE_CAN_NODE_HPP

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<Empty>("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<Empty>("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<Empty::R
     srv_clear_errors_evt_.set();
 }
 
+void ODriveCanNode::service_estop_callback(const std::shared_ptr<Empty::Request> request, std::shared_ptr<Empty::Response> 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;