[WIP] Add safety and fault handling for ROS 2 node

[Copilot]

Thanks for asking me to work on this. I will get started on it and keep this PR's description up to date as I form a plan and make progress.

Original prompt

placeholder

The following is the prior conversation context from the user's chat exploration (may be truncated):

User: You are a robotics engineer who has experience developing full software stack of robots. What features should be there in the main ROS 2 node of the software that interacts with motor drivers and other sensor drivers to do basic locomotion

Assistant:

Analysis of the Pilla Hardware Interface Node

Looking at your odrive_interface.py, you already have a solid foundation — publishers/subscribers for ODrive control, joint state feedback, arm/disarm services, diagnostics, and a trajectory forwarding pipeline. Here's what I'd recommend adding for robust basic locomotion, organized by priority:

---

1. Safety & Fault Handling (Critical)

What you have: Basic arm/disarm, diagnostics reporting.

What's missing:

• Watchdog / Command Timeout: If no trajectory command arrives within N ms, the node should automatically send a safe stop or hold-position command. A motor running on a stale command is dangerous.
• Emergency Stop (E-Stop) Service: A dedicated Trigger service that immediately idles all axes, disengages, and latches until explicitly cleared. Distinct from disarming — e-stop should be instantaneous and non-negotiable.
• ODrive Error Monitoring: Your odrive_status_callback only reads pos_estimate. ODrive controllers report active_errors and disarm_reason — you should monitor these on every status tick, automatically disarm a faulting axis, and publish the error in diagnostics.
• Current/Torque Limiting: Software-side torque caps to protect the gearboxes (your 1.27:1 and 2.26:1 ratios suggest geared joints that can be damaged by excessive torque).
• Joint Position Limits: Enforce min/max joint angle limits before publishing commands. A bad trajectory point should be clamped or rejected, not forwarded blindly to the motor.

2. Joint Offset / Calibration System (Critical)

What you have: Gear ratios and directions — good.

What's missing:

• Per-Joint Encoder Offsets: When the robot powers on, encoder zero ≠ mechanical zero. You need a calibration routine (or stored offsets loaded from a YAML/parameter file) that maps encoder position to actual joint angle. Without this, go_to_zero_pos sends the joints to encoder zero, not mechanical zero.
• ROS 2 Parameters for Offsets: Declare these as node parameters so they can be set at launch or adjusted at runtime without recompiling.

3. Smooth Motion Control (High Priority)

What you have: Position commands forwarded directly from trajectory messages.

What's missing:

• Velocity Feedforward: You set input_vel = 0.0 always. The trajectory message (JointTrajectory) provides velocities in each JointTrajectoryPoint — you should forward these as velocity feedforward to ODrive for much smoother tracking.
• Trajectory Interpolation: You only use msg.points[0]. A proper trajectory may have multiple waypoints with time stamps. Implementing a timer-based interpolator that walks through the trajectory points at the correct times produces smooth, predictable motion instead of jerky jumps.
• Acceleration/Deceleration Ramps: When transitioning between states (e.g., from idle to a standing pose), ramp commands over time instead of step inputs.

4. IMU Integration (High Priority)

What you have: A separate arduino_interface.py exists but isn't integrated into the main node.

What's missing:

• IMU Data Subscription: Subscribe to the IMU topic (likely published by arduino_interface or imu_tools), fuse it into your state estimation.
• Orientation Feedback: Publish a robot_state or feed IMU orientation into the CHAMP controller so it can do body attitude correction. Without this, the robot has no idea if it's tilting or falling.
• IMU Health Monitoring: If IMU data stops arriving, flag it in diagnostics. A locomoting robot without orientation feedback should at minimum warn, and possibly halt.

5. Proper State Machine (High Priority)

What you have: A boolean pilla_armed flag and per-joint armed_state.

What's missing:

• Explicit Node States: Define clear states like IDLE → CALIBRATING → ARMED → ENGAGED → E_STOPPED → FAULT. Transitions between states should have preconditions (e.g., you can't engage without arming first, you can't arm during a fault). This prevents entire classes of bugs where services are called in the wrong order.
• State Publishing: Publish the current state on a topic so monitoring tools and other nodes know what the robot is doing.

6. Predefined Pose / Go-To-Position Service (Medium Priority)

What you have: default_positions array with 5 poses, but only go_to_zero_pos service.

What's missing:

• Go-To-Pose Service: A service that takes a pose index (or a named pose) and smoothly moves the robot to that pose. You already have the data (default_positions), just need the service endpoint...

This pull request was created from Copilot chat.

---

🔒 GitHub Advanced Security automatically protects Copilot coding agent pull requests. You can protect all pull requests by enabling Advanced Security for your repositories. Learn more about Advanced Security.