Odrive error detection

src/Bringup/launch/control.launch.py

@@ -157,13 +157,15 @@ 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",
         executable="drive",
         name="drive_teleop_node",
         parameters=[joy_parameters_file],
         remappings=[("/joy", "/drive/joy")],
+        condition=IfCondition(use_drive),
     )
 
     ld = LaunchDescription()

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,
+        ]
+    )

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
   }

src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg

@@ -9,4 +9,6 @@ float64 position
 float64 velocity
 
 bool fwd_limit
-bool rev_limit
+bool rev_limit
+
+bool error