diff --git a/odrive_node/README.md b/odrive_node/README.md index 674934c..29910ef 100644 --- a/odrive_node/README.md +++ b/odrive_node/README.md @@ -10,6 +10,7 @@ For information about installation, prerequisites and getting started, check out * `node_id`: The node_id of the device this node will attach to * `interface`: the network interface name for the can bus +* `axis_idle_on_shutdown`: Whether to set ODrive to IDLE state when the node is terminated ### Subscribes to diff --git a/odrive_node/include/odrive_can_node.hpp b/odrive_node/include/odrive_can_node.hpp index 0b9afdf..78f7417 100644 --- a/odrive_node/include/odrive_can_node.hpp +++ b/odrive_node/include/odrive_can_node.hpp @@ -42,6 +42,7 @@ class ODriveCanNode : public rclcpp::Node { inline bool verify_length(const std::string&name, uint8_t expected, uint8_t length); uint16_t node_id_; + bool axis_idle_on_shutdown_; SocketCanIntf can_intf_ = SocketCanIntf(); short int ctrl_pub_flag_ = 0; diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index 3ca3a71..6af79c5 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/odrive_node/src/odrive_can_node.cpp @@ -1,4 +1,5 @@ #include "odrive_can_node.hpp" +#include "odrive_enums.h" #include "epoll_event_loop.hpp" #include "byte_swap.hpp" #include @@ -31,6 +32,7 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n rclcpp::Node::declare_parameter("interface", "can0"); rclcpp::Node::declare_parameter("node_id", 0); + rclcpp::Node::declare_parameter("axis_idle_on_shutdown", false); rclcpp::QoS ctrl_stat_qos(rclcpp::KeepAll{}); ctrl_publisher_ = rclcpp::Node::create_publisher("controller_status", ctrl_stat_qos); @@ -49,6 +51,14 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n } void ODriveCanNode::deinit() { + if (axis_idle_on_shutdown_) { + struct can_frame frame; + frame.can_id = node_id_ << 5 | CmdId::kSetAxisState; + write_le(ODriveAxisState::AXIS_STATE_IDLE, frame.data); + frame.can_dlc = 4; + can_intf_.send_can_frame(frame); + } + sub_evt_.deinit(); srv_evt_.deinit(); can_intf_.deinit(); @@ -57,6 +67,7 @@ void ODriveCanNode::deinit() { bool ODriveCanNode::init(EpollEventLoop* event_loop) { node_id_ = rclcpp::Node::get_parameter("node_id").as_int(); + axis_idle_on_shutdown_ = rclcpp::Node::get_parameter("axis_idle_on_shutdown").as_bool(); std::string interface = rclcpp::Node::get_parameter("interface").as_string(); if (!can_intf_.init(interface, event_loop, std::bind(&ODriveCanNode::recv_callback, this, _1))) {