Merge pull request odriverobotics#27 from CMU-cabot/add-clear-errors

Add clear errors

odrive_node/CMakeLists.txt

@@ -9,6 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(std_srvs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ControlMessage.msg"
@@ -30,6 +31,7 @@ add_executable(odrive_can_node
 
 ament_target_dependencies(odrive_can_node
   rclcpp
+  std_srvs
 )
 
 target_compile_features(odrive_can_node PRIVATE cxx_std_20)

odrive_node/README.md

@@ -50,6 +50,10 @@ For information about installation, prerequisites and getting started, check out
 
   This service requires regular heartbeat messages from the ODrive to determine the procedure result and will block until the procedure completes, with a minimum call time of 1 second.
 
+* `/clear_errors`: Clears disarm_reason and procedure_result and re-arms the brake resistor if applicable
+
+  If the axis dropped into IDLE because of an error, clearing the errors does not put the axis back into CLOSED_LOOP_CONTROL. To do so, you must request CLOSED_LOOP_CONTROL again explicitly.
+
 ### 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

@@ -6,6 +6,7 @@
 #include "odrive_can/msg/controller_status.hpp"
 #include "odrive_can/msg/control_message.hpp"
 #include "odrive_can/srv/axis_state.hpp"
+#include "std_srvs/srv/empty.hpp"
 #include "socket_can.hpp"
 
 #include <mutex>
@@ -23,6 +24,7 @@ using ControllerStatus = odrive_can::msg::ControllerStatus;
 using ControlMessage = odrive_can::msg::ControlMessage;
 
 using AxisState = odrive_can::srv::AxisState;
+using Empty = std_srvs::srv::Empty;
 
 class ODriveCanNode : public rclcpp::Node {
 public:
@@ -33,7 +35,9 @@ class ODriveCanNode : public rclcpp::Node {
     void recv_callback(const can_frame& frame);
     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 request_state_callback();
+    void request_clear_errors_callback();
     void ctrl_msg_callback();
     inline bool verify_length(const std::string&name, uint8_t expected, uint8_t length);
 
@@ -61,6 +65,9 @@ class ODriveCanNode : public rclcpp::Node {
     std::condition_variable fresh_heartbeat_;
     rclcpp::Service<AxisState>::SharedPtr service_;
 
+    EpollEvent srv_clear_errors_evt_;
+    rclcpp::Service<Empty>::SharedPtr service_clear_errors_;
+
 };
 
 #endif // ODRIVE_CAN_NODE_HPP

odrive_node/package.xml

@@ -13,6 +13,7 @@
   <build_depend>rosidl_default_generators</build_depend>
 
   <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

odrive_node/src/odrive_can_node.cpp

@@ -16,6 +16,7 @@ enum CmdId : uint32_t {
     kGetIq = 0x014,                // ControllerStatus  - publisher
     kGetTemp,                      // SystemStatus      - publisher
     kGetBusVoltageCurrent = 0x017, // SystemStatus      - publisher
+    kClearErrors = 0x018,          // ClearErrors       - service
     kGetTorques = 0x01c,           // ControllerStatus  - publisher
 };
 
@@ -42,6 +43,9 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n
 
     rclcpp::QoS srv_qos(rclcpp::KeepAll{});
     service_ = rclcpp::Node::create_service<AxisState>("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos.get_rmw_qos_profile());
+
+    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());
 }
 
 void ODriveCanNode::deinit() {
@@ -67,6 +71,10 @@ bool ODriveCanNode::init(EpollEventLoop* event_loop) {
         RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize service event");
         return false;
     }
+    if (!srv_clear_errors_evt_.init(event_loop, std::bind(&ODriveCanNode::request_clear_errors_callback, this))) {
+        RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize clear errors 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;
@@ -180,6 +188,11 @@ void ODriveCanNode::service_callback(const std::shared_ptr<AxisState::Request> r
     response->procedure_result = ctrl_stat_.procedure_result;
 }
 
+void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr<Empty::Request> request, std::shared_ptr<Empty::Response> response) {
+    RCLCPP_INFO(rclcpp::Node::get_logger(), "clearing errors");
+    srv_clear_errors_evt_.set();
+}
+
 void ODriveCanNode::request_state_callback() {
     struct can_frame frame;
     frame.can_id = node_id_ << 5 | CmdId::kSetAxisState;
@@ -191,6 +204,14 @@ void ODriveCanNode::request_state_callback() {
     can_intf_.send_can_frame(frame);
 }
 
+void ODriveCanNode::request_clear_errors_callback() {
+    struct can_frame frame;
+    frame.can_id = node_id_ << 5 | CmdId::kClearErrors;
+    write_le<uint8_t>(0, frame.data);
+    frame.can_dlc = 1;
+    can_intf_.send_can_frame(frame);
+}
+
 void ODriveCanNode::ctrl_msg_callback() {
 
     uint32_t control_mode;