diff --git a/CMakeLists.txt b/CMakeLists.txt index a7be9bd..9af85f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,12 +15,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ControllerStatus.msg" "msg/ODriveStatus.msg" "srv/AxisState.srv" + "srv/ClearErrors.srv" ) ament_export_dependencies(rosidl_default_runtime) include_directories(include) -add_executable(odrive_can_node +add_executable(odrive_can_node src/epoll_event_loop.cpp src/socket_can.cpp src/odrive_can_node.cpp diff --git a/include/odrive_can_node.hpp b/include/odrive_can_node.hpp index b151c8b..3e0c547 100644 --- a/include/odrive_can_node.hpp +++ b/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 "odrive_can/srv/clear_errors.hpp" #include "socket_can.hpp" #include @@ -23,28 +24,31 @@ using ControllerStatus = odrive_can::msg::ControllerStatus; using ControlMessage = odrive_can::msg::ControlMessage; using AxisState = odrive_can::srv::AxisState; +using ClearErrors = odrive_can::srv::ClearErrors; class ODriveCanNode : public rclcpp::Node { public: ODriveCanNode(const std::string& node_name); - bool init(EpollEventLoop* event_loop); + bool init(EpollEventLoop* event_loop); void deinit(); private: 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); - + uint16_t node_id_; SocketCanIntf can_intf_ = SocketCanIntf(); - + short int ctrl_pub_flag_ = 0; std::mutex ctrl_stat_mutex_; ControllerStatus ctrl_stat_ = ControllerStatus(); rclcpp::Publisher::SharedPtr ctrl_publisher_; - + short int odrv_pub_flag_ = 0; std::mutex odrv_stat_mutex_; ODriveStatus odrv_stat_ = ODriveStatus(); @@ -61,6 +65,11 @@ class ODriveCanNode : public rclcpp::Node { std::condition_variable fresh_heartbeat_; rclcpp::Service::SharedPtr service_; + EpollEvent srv_clear_errors_evt_; + uint8_t identify_; + std::mutex clear_errors_mutex_; + rclcpp::Service::SharedPtr service_clear_errors_; + }; #endif // ODRIVE_CAN_NODE_HPP diff --git a/src/odrive_can_node.cpp b/src/odrive_can_node.cpp index b67b2bb..35b5af0 100644 --- a/src/odrive_can_node.cpp +++ b/src/odrive_can_node.cpp @@ -17,6 +17,7 @@ enum CmdId : uint32_t { kGetTemp, // SystemStatus - publisher kGetBusVoltageCurrent = 0x017, // SystemStatus - publisher kGetTorques = 0x01c, // ControllerStatus - publisher + kClearErrors = 0x018, // ClearErrors - service }; enum ControlMode : uint64_t { @@ -27,13 +28,13 @@ enum ControlMode : uint64_t { }; ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_name) { - + rclcpp::Node::declare_parameter("interface", "can0"); rclcpp::Node::declare_parameter("node_id", 0); rclcpp::QoS ctrl_stat_qos(rclcpp::KeepAll{}); ctrl_publisher_ = rclcpp::Node::create_publisher("controller_status", ctrl_stat_qos); - + rclcpp::QoS odrv_stat_qos(rclcpp::KeepAll{}); odrv_publisher_ = rclcpp::Node::create_publisher("odrive_status", odrv_stat_qos); @@ -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; @@ -133,7 +141,7 @@ void ODriveCanNode::recv_callback(const can_frame& frame) { std::lock_guard guard(ctrl_stat_mutex_); ctrl_stat_.torque_target = read_le(frame.data + 0); ctrl_stat_.torque_estimate = read_le(frame.data + 4); - ctrl_pub_flag_ |= 0b1000; + ctrl_pub_flag_ |= 0b1000; break; } default: { @@ -141,12 +149,12 @@ void ODriveCanNode::recv_callback(const can_frame& frame) { break; } } - + if (ctrl_pub_flag_ == 0b1111) { ctrl_publisher_->publish(ctrl_stat_); ctrl_pub_flag_ = 0; } - + if (odrv_pub_flag_ == 0b111) { odrv_publisher_->publish(odrv_stat_); odrv_pub_flag_ = 0; @@ -171,15 +179,32 @@ void ODriveCanNode::service_callback(const std::shared_ptr r auto call_time = std::chrono::steady_clock::now(); fresh_heartbeat_.wait(guard, [this, &call_time]() { bool complete = (this->ctrl_stat_.procedure_result != 1) && // make sure procedure_result is not busy - (std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second - return complete; + (std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second + return complete; }); // wait for procedure_result - + response->axis_state = ctrl_stat_.axis_state; response->active_errors = ctrl_stat_.active_errors; response->procedure_result = ctrl_stat_.procedure_result; } +void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr request, std::shared_ptr response) { + { + std::unique_lock guard(clear_errors_mutex_); + identify_ = request->identify; + RCLCPP_INFO(rclcpp::Node::get_logger(), "clearing errors identify: %d", identify_); + } + srv_clear_errors_evt_.set(); + + std::unique_lock guard(ctrl_stat_mutex_); // define lock for controller status + auto call_time = std::chrono::steady_clock::now(); + fresh_heartbeat_.wait(guard, [this, &call_time]() { + bool complete = (this->ctrl_stat_.procedure_result != 1) && // make sure procedure_result is not busy + (std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second + return complete; + }); // wait for procedure_result +} + void ODriveCanNode::request_state_callback() { struct can_frame frame; frame.can_id = node_id_ << 5 | CmdId::kSetAxisState; @@ -191,6 +216,17 @@ 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; + { + std::unique_lock guard(axis_state_mutex_); + write_le(identify_, frame.data); + } + frame.can_dlc = 4; + can_intf_.send_can_frame(frame); +} + void ODriveCanNode::ctrl_msg_callback() { uint32_t control_mode; @@ -204,7 +240,7 @@ void ODriveCanNode::ctrl_msg_callback() { } frame.can_dlc = 8; can_intf_.send_can_frame(frame); - + frame = can_frame{}; switch (control_mode) { case ControlMode::kVoltageControl: { @@ -237,8 +273,8 @@ void ODriveCanNode::ctrl_msg_callback() { write_le(((int8_t)((ctrl_msg_.input_torque) * 1000)), frame.data + 6); frame.can_dlc = 8; break; - } - default: + } + default: RCLCPP_ERROR(rclcpp::Node::get_logger(), "unsupported control_mode: %d", control_mode); return; }