From e497e18a19b4b1626fdae9b418307b3cd46e3acc Mon Sep 17 00:00:00 2001 From: "Libor Wagner (wagnelib)" Date: Fri, 15 Mar 2024 14:41:46 +0100 Subject: [PATCH] Add service to clear errors Co-authored-by: Libor Wagner (wagnelib) --- odrive_node/CMakeLists.txt | 2 ++ odrive_node/README.md | 4 ++++ odrive_node/include/odrive_can_node.hpp | 7 +++++++ odrive_node/package.xml | 1 + odrive_node/src/odrive_can_node.cpp | 21 +++++++++++++++++++++ 5 files changed, 35 insertions(+) diff --git a/odrive_node/CMakeLists.txt b/odrive_node/CMakeLists.txt index e074883..3bfda36 100644 --- a/odrive_node/CMakeLists.txt +++ b/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) diff --git a/odrive_node/README.md b/odrive_node/README.md index b087a1c..674934c 100644 --- a/odrive_node/README.md +++ b/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). diff --git a/odrive_node/include/odrive_can_node.hpp b/odrive_node/include/odrive_can_node.hpp index b151c8b..0b9afdf 100644 --- a/odrive_node/include/odrive_can_node.hpp +++ b/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 @@ -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 request, std::shared_ptr response); + void service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr 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::SharedPtr service_; + EpollEvent srv_clear_errors_evt_; + rclcpp::Service::SharedPtr service_clear_errors_; + }; #endif // ODRIVE_CAN_NODE_HPP diff --git a/odrive_node/package.xml b/odrive_node/package.xml index c174ae8..0db7e05 100644 --- a/odrive_node/package.xml +++ b/odrive_node/package.xml @@ -13,6 +13,7 @@ rosidl_default_generators rclcpp + std_srvs ament_lint_auto ament_lint_common diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index b67b2bb..3ca3a71 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/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("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("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 r response->procedure_result = ctrl_stat_.procedure_result; } +void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr 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(0, frame.data); + frame.can_dlc = 1; + can_intf_.send_can_frame(frame); +} + void ODriveCanNode::ctrl_msg_callback() { uint32_t control_mode;