From 683883b7ea9ec94b1746e2b377c1617a02ed2e3e Mon Sep 17 00:00:00 2001 From: ConnorN Date: Mon, 11 May 2026 23:20:06 +0000 Subject: [PATCH 1/2] feat: Odrive error detection --- .../src/odrive_hardware_interface.cpp | 11 +++++++++-- src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg | 4 +++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp b/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp index aa97de6b..2fe04eff 100644 --- a/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/src/HW-Devices/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -93,8 +93,8 @@ struct Axis { // double iq_measured_ = NAN; double torque_target_ = NAN; // [Nm] double torque_estimate_ = NAN; // [Nm] - // uint32_t active_errors_ = 0; - // uint32_t disarm_reason_ = 0; + uint32_t active_errors_ = 0; + uint32_t disarm_reason_ = 0; // double fet_temperature_ = NAN; // double motor_temperature_ = NAN; double bus_voltage_ = NAN; @@ -346,6 +346,7 @@ void ODriveHardwareInterface::pub_status() { status_msg.output_current = axis.bus_current_; status_msg.position = axis.pos_estimate_; status_msg.velocity = axis.vel_estimate_; + status_msg.error = axis.active_errors_ || axis.disarm_reason_; axis.debug_pub_->publish(status_msg); } } @@ -577,6 +578,12 @@ void Axis::on_can_msg(const rclcpp::Time &, const can_frame &frame) { bus_voltage_ = msg.Bus_Voltage; bus_current_ = msg.Bus_Current; } + } break; + case Get_Error_msg_t::cmd_id: { + if (Get_Error_msg_t msg; try_decode(msg)) { + active_errors_ = msg.Active_Errors; + disarm_reason_ = msg.Disarm_Reason; + } } break; // silently ignore unimplemented command IDs } diff --git a/src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg b/src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg index 988b6a93..10ab1180 100644 --- a/src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg +++ b/src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg @@ -9,4 +9,6 @@ float64 position float64 velocity bool fwd_limit -bool rev_limit \ No newline at end of file +bool rev_limit + +bool error \ No newline at end of file From 88c31782ee640cfb015e2f2dbac2455ea9bb3546 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Thu, 14 May 2026 03:09:38 +0000 Subject: [PATCH 2/2] chore: only launch control nodes as necessary --- src/Bringup/launch/control.launch.py | 2 ++ src/Bringup/launch/core.launch.py | 17 ++++++++++++++++- 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/Bringup/launch/control.launch.py b/src/Bringup/launch/control.launch.py index 1759823a..f6f01c3a 100644 --- a/src/Bringup/launch/control.launch.py +++ b/src/Bringup/launch/control.launch.py @@ -157,6 +157,7 @@ def generate_launch_description(): name="arm_teleop_node", parameters=[joy_parameters_file], remappings=[("/joy", "/arm/joy")], + condition=IfCondition(use_arm), ) drive_control_node = Node( package="joystick_control", @@ -164,6 +165,7 @@ def generate_launch_description(): name="drive_teleop_node", parameters=[joy_parameters_file], remappings=[("/joy", "/drive/joy")], + condition=IfCondition(use_drive), ) ld = LaunchDescription() diff --git a/src/Bringup/launch/core.launch.py b/src/Bringup/launch/core.launch.py index 5fa7782d..12c64e56 100644 --- a/src/Bringup/launch/core.launch.py +++ b/src/Bringup/launch/core.launch.py @@ -3,6 +3,7 @@ from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node from launch_xml.launch_description_sources import XMLLaunchDescriptionSource @@ -20,6 +21,7 @@ def get_included_launch_descriptions(launch_files): raise ValueError(f"Unsupported launch file format: {file}") included_launches.append(IncludeLaunchDescription(source)) + return included_launches @@ -28,4 +30,17 @@ def generate_launch_description(): ("servo_pkg", "usb_servo_launch.launch.py"), ("gpio_controller", "core_gpio_devices.launch.py"), ] - return LaunchDescription(get_included_launch_descriptions(launch_files)) + + snmp_node = Node( + package="system-telemetry-cpp", + executable="snmp_network_stats", + name="snmp_network_stats", + output="screen", + ) + + return LaunchDescription( + get_included_launch_descriptions(launch_files) + + [ + snmp_node, + ] + )