Merge pull request #31 from CMU-cabot/axis_idle_on_shutdown

Axis idle on shutdown
odriverobotics · Nov 7, 2024 · d986c24 · d986c24
2 parents d2f0f54 + d70c656
commit d986c24
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 0 deletions.
diff --git 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
@@ -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
@@ -1,4 +1,5 @@
 #include "odrive_can_node.hpp"
+#include "odrive_enums.h"
 #include "epoll_event_loop.hpp"
 #include "byte_swap.hpp"
 #include <sys/eventfd.h>
@@ -31,6 +32,7 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n
 
     rclcpp::Node::declare_parameter<std::string>("interface", "can0");
     rclcpp::Node::declare_parameter<uint16_t>("node_id", 0);
+    rclcpp::Node::declare_parameter<bool>("axis_idle_on_shutdown", false);
 
     rclcpp::QoS ctrl_stat_qos(rclcpp::KeepAll{});
     ctrl_publisher_ = rclcpp::Node::create_publisher<ControllerStatus>("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<uint32_t>(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))) {