From 107cd76404bc98e25dead2f79955a6c1aee302d5 Mon Sep 17 00:00:00 2001 From: yushi-minemura <67675657+yushi-minemura@users.noreply.github.com> Date: Fri, 27 Sep 2024 14:42:14 +0900 Subject: [PATCH 1/4] add axis_idle_on_shutdown parameter --- odrive_node/include/odrive_can_node.hpp | 1 + odrive_node/src/odrive_can_node.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+) 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..de51c7c 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,17 @@ 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; + { + std::unique_lock guard(axis_state_mutex_); + 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 +70,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))) { From 08114507e9a51980ae2bc395398fb4eb25581866 Mon Sep 17 00:00:00 2001 From: "yushi minemura (kufusha)" <67675657+yushi-minemura@users.noreply.github.com> Date: Tue, 22 Oct 2024 17:37:54 +0900 Subject: [PATCH 2/4] update README.md --- odrive_node/README.md | 1 + 1 file changed, 1 insertion(+) 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 From 320e44c757da014a1ab8ea7719a1c2c43a7904a9 Mon Sep 17 00:00:00 2001 From: yushi-minemura <67675657+yushi-minemura@users.noreply.github.com> Date: Thu, 7 Nov 2024 11:37:40 +0900 Subject: [PATCH 3/4] fix if() format --- odrive_node/src/odrive_can_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index de51c7c..0450bde 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/odrive_node/src/odrive_can_node.cpp @@ -51,7 +51,7 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n } void ODriveCanNode::deinit() { - if(axis_idle_on_shutdown_) { + if (axis_idle_on_shutdown_) { struct can_frame frame; frame.can_id = node_id_ << 5 | CmdId::kSetAxisState; { From d70c6564e262f075283b02ee1efe4d77b9fa2b65 Mon Sep 17 00:00:00 2001 From: yushi-minemura <67675657+yushi-minemura@users.noreply.github.com> Date: Thu, 7 Nov 2024 11:38:06 +0900 Subject: [PATCH 4/4] remove unnecessary mutex lock --- odrive_node/src/odrive_can_node.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index 0450bde..6af79c5 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/odrive_node/src/odrive_can_node.cpp @@ -54,10 +54,7 @@ void ODriveCanNode::deinit() { if (axis_idle_on_shutdown_) { struct can_frame frame; frame.can_id = node_id_ << 5 | CmdId::kSetAxisState; - { - std::unique_lock guard(axis_state_mutex_); - write_le(ODriveAxisState::AXIS_STATE_IDLE, frame.data); - } + write_le(ODriveAxisState::AXIS_STATE_IDLE, frame.data); frame.can_dlc = 4; can_intf_.send_can_frame(frame); }