From af6cfd3d309ad21ea3702b650a6db482be3232eb Mon Sep 17 00:00:00 2001 From: emilnovak Date: Thu, 9 Oct 2025 16:56:19 +0200 Subject: [PATCH 1/4] Add enable pin state interface with polling --- .../src/odrive_hardware_interface.cpp | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index a0691ef..5c20c9f 100644 --- a/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -7,6 +7,7 @@ #include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" #include "socket_can.hpp" +#include namespace odrive_ros2_control { @@ -89,6 +90,29 @@ struct Axis { bool vel_input_enabled_ = false; bool torque_input_enabled_ = false; + // GPIO enable pin state - represents whether the axis enable pin is active + double enable_pin_state_ = 0.0; // 0.0 = disabled, 1.0 = enabled + + // SDO support for reading enable pin state + static constexpr uint16_t ENABLE_PIN_STATE_ENDPOINT_ID = 417; + std::chrono::steady_clock::time_point last_enable_pin_read_time_; + + void request_enable_pin_state() { + // Send SDO read request for axis0.enable_pin.state (endpoint ID 417) + // See flat_endpoints.json (S1, 0.6.11-1) https://docs.odriverobotics.com/releases/firmware + struct can_frame frame; + frame.can_id = node_id_ << 5 | 0x04; // 0x04: RxSdo + frame.can_dlc = 4; + + // Pack: opcode (1 byte), endpoint_id (2 bytes), reserved (1 byte) + frame.data[0] = 0x00; // OPCODE_READ + frame.data[1] = ENABLE_PIN_STATE_ENDPOINT_ID & 0xFF; // endpoint_id low byte + frame.data[2] = (ENABLE_PIN_STATE_ENDPOINT_ID >> 8) & 0xFF; // endpoint_id high byte + frame.data[3] = 0x00; // reserved + + can_intf_->send_can_frame(frame); + } + template void send(const T& msg) const { struct can_frame frame; @@ -183,6 +207,11 @@ std::vector ODriveHardwareInterface::export_ hardware_interface::HW_IF_POSITION, &axes_[i].pos_estimate_ )); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, + "enable_pin_state", + &axes_[i].enable_pin_state_ + )); } return state_interfaces; @@ -254,6 +283,18 @@ return_type ODriveHardwareInterface::perform_command_mode_switch( return_type ODriveHardwareInterface::read(const rclcpp::Time& timestamp, const rclcpp::Duration&) { timestamp_ = timestamp; + // Request enable pin state periodically (10Hz) + auto now = std::chrono::steady_clock::now(); + for (auto& axis : axes_) { + auto time_since_last_read = std::chrono::duration_cast( + now - axis.last_enable_pin_read_time_).count(); + + if (time_since_last_read > 100 || axis.last_enable_pin_read_time_.time_since_epoch().count() == 0) { + axis.request_enable_pin_state(); + axis.last_enable_pin_read_time_ = now; + } + } + while (can_intf_.read_nonblocking()) { // repeat until CAN interface has no more messages } @@ -357,6 +398,23 @@ void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) { torque_target_ = msg.Torque_Target; torque_estimate_ = msg.Torque_Estimate; } + } break; + case 0x05: { // TxSdo - SDO response + if (frame.can_dlc >= 4) { + // Parse SDO response: opcode (1), endpoint_id (2), reserved (1), data... + uint8_t opcode = frame.data[0]; + uint16_t endpoint_id = frame.data[1] | (frame.data[2] << 8); + + if (opcode == 0x00 && endpoint_id == ENABLE_PIN_STATE_ENDPOINT_ID && frame.can_dlc >= 5) { + // Extract boolean value from response + bool enable_pin_value = frame.data[4] != 0; + enable_pin_state_ = enable_pin_value ? 1.0 : 0.0; + + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + "Enable pin state updated: node_id=%d, value=%s (%.1f)", + node_id_, enable_pin_value ? "true" : "false", enable_pin_state_); + } + } } break; // silently ignore unimplemented command IDs } From d1b0e854f5299f6b2e8ef205aabdd571981fef3a Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 13 Nov 2025 15:51:55 +0100 Subject: [PATCH 2/4] Update enable pin state interface name --- odrive_ros2_control/src/odrive_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index 5c20c9f..d6e5205 100644 --- a/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -209,7 +209,7 @@ std::vector ODriveHardwareInterface::export_ )); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, - "enable_pin_state", + "enable_pin_state/bool", &axes_[i].enable_pin_state_ )); } From eabcca0ca790f2dabb7d93b84c7bbd2ec6957faf Mon Sep 17 00:00:00 2001 From: Your Name Date: Fri, 14 Nov 2025 09:01:36 +0100 Subject: [PATCH 3/4] Add bus voltage state interface and update CAN message handling --- .../src/odrive_hardware_interface.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index d6e5205..f214b28 100644 --- a/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -78,7 +78,7 @@ struct Axis { // uint32_t disarm_reason_ = 0; // double fet_temperature_ = NAN; // double motor_temperature_ = NAN; - // double bus_voltage_ = NAN; + double bus_voltage_ = NAN; // double bus_current_ = NAN; // Indicates which controller inputs are enabled. This is configured by the @@ -212,6 +212,11 @@ std::vector ODriveHardwareInterface::export_ "enable_pin_state/bool", &axes_[i].enable_pin_state_ )); +state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, + "bus_voltage/voltage", + &axes_[i].bus_voltage_ + )); } return state_interfaces; @@ -399,6 +404,11 @@ void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) { torque_estimate_ = msg.Torque_Estimate; } } break; +case Get_Bus_Voltage_Current_msg_t::cmd_id: { + if (Get_Bus_Voltage_Current_msg_t msg; try_decode(msg)) { + bus_voltage_ = msg.Bus_Voltage; + } + } break; case 0x05: { // TxSdo - SDO response if (frame.can_dlc >= 4) { // Parse SDO response: opcode (1), endpoint_id (2), reserved (1), data... From 2449914315e914fe418e05f03106f8efa4762ac3 Mon Sep 17 00:00:00 2001 From: Your Name Date: Fri, 14 Nov 2025 09:02:13 +0100 Subject: [PATCH 4/4] Reduce logging --- odrive_ros2_control/src/odrive_hardware_interface.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/odrive_ros2_control/src/odrive_hardware_interface.cpp b/odrive_ros2_control/src/odrive_hardware_interface.cpp index f214b28..87a00c9 100644 --- a/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -212,7 +212,7 @@ std::vector ODriveHardwareInterface::export_ "enable_pin_state/bool", &axes_[i].enable_pin_state_ )); -state_interfaces.emplace_back(hardware_interface::StateInterface( + state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "bus_voltage/voltage", &axes_[i].bus_voltage_ @@ -404,7 +404,7 @@ void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) { torque_estimate_ = msg.Torque_Estimate; } } break; -case Get_Bus_Voltage_Current_msg_t::cmd_id: { + case Get_Bus_Voltage_Current_msg_t::cmd_id: { if (Get_Bus_Voltage_Current_msg_t msg; try_decode(msg)) { bus_voltage_ = msg.Bus_Voltage; } @@ -420,9 +420,9 @@ case Get_Bus_Voltage_Current_msg_t::cmd_id: { bool enable_pin_value = frame.data[4] != 0; enable_pin_state_ = enable_pin_value ? 1.0 : 0.0; - RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), - "Enable pin state updated: node_id=%d, value=%s (%.1f)", - node_id_, enable_pin_value ? "true" : "false", enable_pin_state_); + // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), + // "Enable pin state updated: node_id=%d, value=%s (%.1f)", + // node_id_, enable_pin_value ? "true" : "false", enable_pin_state_); } } } break;