Skip to content

Feature/arduino hardware bridge - #18

Open
aj0khi wants to merge 16 commits into
mainfrom
feature/arduino-hardware-bridge
Open

aj0khi wants to merge 16 commits into
mainfrom
feature/arduino-hardware-bridge

Conversation

@aj0khi

@aj0khi aj0khi commented Sep 12, 2026

Copy link
Copy Markdown

Arm Hardware and Motion Architecture

Goal

Use the same four-joint arm model in RViz for both simulation and a connected
Arduino, while allowing the motion test to command the physical arm and report
failures instead of only animating a simulated pose.

The current model is old_arm_prototype.urdf with these joints:

  • base_yaw
  • shoulder
  • elbow
  • wrist_roll

Current State

MotionTestNode -- /joint_states --> robot_state_publisher --> RViz RobotModel
       ^
       |
RViz MotionTestPanel -- String RUN/HOME/STOP

The motion test currently publishes generated positions directly to
/joint_states. That is suitable for visualization, but it is not a hardware
control path: it represents commanded positions as if they were measured
positions.

There is currently no Arduino transport, encoder feedback path,
ros2_control hardware plugin, trajectory controller, or motion action in the
workspace.

The archived handoff provides a usable first transport contract:

  • Arduino Uno R4 WiFi at 115200 baud over USB serial.
  • Servo signals: D3 base_yaw, D4 shoulder, D5 elbow, D6 wrist_roll.
  • Hold switch: D7 to ground using INPUT_PULLUP.
  • Commands: ID, MOVE,s1,s2,s3,s4,durationMs, JOG,servo,delta,durationMs,
    and HOLD.
  • Responses: READY,IK4,1, OK,MOVE, OK,JOG, OK,ARRIVED, OK,HOLD,
    OK,SWITCH HOLD, and ERROR,....

The firmware interpolates commands internally every 20 ms and has software
limits, but it does not stream measured servo angles. Its OK,ARRIVED response
means the command timeline completed, not that the mechanism was measured at
the target.

Target Runtime Architecture

                         command path
RViz MotionTestPanel or motion_test
              |
              | FollowJointTrajectory action
              v
joint_trajectory_controller
              |
              v
waybionic_hardware (ros2_control SystemInterface)
              |
              | serial/USB protocol
              v
Arduino + servo hardware
              |
              | command status, health, errors
              v
waybionic_hardware
              |
              +--> /joint_states --> robot_state_publisher --> RViz
              |
              +--> /diagnostics --> DiagnosticsPanel

The current Arduino firmware has no encoder or potentiometer feedback. It
reports command acceptance and arrival, but not measured servo positions. In
the first physical integration, the host bridge can publish an estimated
/joint_states pose from the accepted command timeline. RViz will then show
the commanded pose, not verified mechanical position. When real feedback is
added, measured state should replace that estimate.

Runtime Modes

Simulation

motion_test simulation publisher --> /joint_states --> robot_state_publisher

This preserves the existing visual motion test. It must not run at the same
time as the physical hardware state publisher.

Physical Hardware

motion_test/action client --> trajectory controller --> Arduino
Arduino command status --> estimated /joint_states --> robot_state_publisher --> RViz

The launch file should select exactly one mode, for example with a
hardware_mode argument whose values are simulation and arduino.

ROS Interfaces

These are the proposed stable interfaces between packages:

Purpose Interface
Joint position estimate or measurement /joint_states, sensor_msgs/msg/JointState
Motion command /joint_trajectory_controller/follow_joint_trajectory, control_msgs/action/FollowJointTrajectory
Optional lower-level command stream /joint_trajectory_controller/joint_trajectory, trajectory_msgs/msg/JointTrajectory
Hardware and safety state /diagnostics, diagnostic_msgs/msg/DiagnosticArray

The motion test should become an action client. Actions provide acceptance,
feedback, completion, cancellation, and failure results, which the current
RUN/HOME/STOP string topic cannot provide.

The existing string topic can remain temporarily as a simulation-only adapter
while the action path is introduced.

The first bridge implementation is available as waybionic_hardware and
accepts the existing /old_arm_motion_test/command topic. It translates
RUN, HOME, and STOP into the firmware protocol, publishes the estimated
pose, and publishes connection/command status on /diagnostics.

Package Responsibilities

waybionic_description

  • Own the production URDF and joint limits.
  • Add a ros2_control block for the four actuated joints.
  • Keep the physical-to-model calibration in one documented place.

New waybionic_hardware

  • Own the Arduino transport and packet protocol.
  • Convert model radians to calibrated servo commands.
  • Convert accepted command state to model radians until real sensors exist.
  • Replace estimated state with measured state when encoders or potentiometers
    are added.
  • Publish hardware diagnostics and connection/watchdog faults.
  • Refuse motion when disconnected, stale, out of range, or stopped.

The first implementation can use a small serial bridge if a full
ros2_control plugin is not yet ready. The public ROS interfaces should still
match the target design so the bridge can later be replaced without changing
RViz or the motion test.

waybionic_motion_test

  • Generate safe, bounded test trajectories.
  • Send trajectories through the controller/action interface.
  • Verify Arduino acknowledgment and arrival reports for each target.
  • Verify measured feedback against each target once sensors are available.
  • Abort on action failure, stale feedback, limit violation, or timeout.
  • Keep a simulation implementation for development without hardware.

It must not publish synthetic /joint_states in physical mode.

waybionic_rviz_plugins

  • Keep the existing RobotModel visualization.
  • Update MotionTestPanel to use the action interface.
  • Show connection, current joint values, target, progress, and failure reason.
  • Keep emergency stop separate from normal test completion.

waybionic_bringup

  • Select simulation or Arduino mode.
  • Start robot_state_publisher in both modes.
  • Start the simulation publisher only in simulation mode.
  • Start hardware and controllers only in Arduino mode.
  • Prevent both state publishers from running together.

Safety Rules

  1. Start in a verified HOME pose before running a sequence.
  2. Enforce URDF position and velocity limits before sending commands.
  3. Require a hardware heartbeat and stop on a stale heartbeat.
  4. Stop on serial disconnect, malformed feedback, over-current, or an
    Arduino-reported fault.
  5. Treat the stop command as cancellation plus a hardware stop request; do not
    merely stop publishing messages.
  6. Require an explicit physical-mode launch argument so a development launch
    cannot move the arm accidentally.
  7. Keep the first physical test at low speed with one joint at a time before
    running synchronized trajectories.

Incremental Implementation Plan

  1. Define the contract: confirm Arduino transport, feedback availability,
    servo calibration, joint limits, and the Arduino packet format.
  2. Separate modes: add launch arguments and prevent the current simulated
    /joint_states publisher from starting in physical mode.
  3. Build the transport: add waybionic_hardware with connection state,
    heartbeat, command conversion, command-state estimation, and diagnostics.
  4. Add standard control: connect the transport to ros2_control and load a
    joint state broadcaster plus trajectory controller.
  5. Convert the test: send bounded trajectories through the action and
    verify Arduino arrival status against each target. Add measured-feedback
    verification when sensors become available.
  6. Upgrade the panel: display action and hardware state, and expose stop
    only when the controller reports the arm is connected.
  7. Validate progressively: test conversion math, then a fake serial device,
    then RViz with recorded feedback, and only then the physical arm.

Current Bringup Commands

Simulation remains the default:

ros2 launch waybionic_bringup ground_station.launch.py

Arduino mode requires an explicit serial port:

ros2 launch waybionic_bringup ground_station.launch.py \
   hardware_mode:=arduino arduino_port:=/dev/ttyACM0

The bridge can be exercised without an Arduino:

ros2 launch waybionic_bringup ground_station.launch.py \
   hardware_mode:=arduino arduino_dry_run:=true launch_rviz:=false

The first physical test should use the external servo-power switch, confirm
the arm is supported and clear, then use HOME before RUN. The current
bridge intentionally does not claim measured position feedback.

Definition of Done

  • RViz follows the Arduino command estimate in the first physical version and
    measured feedback once sensors are added.
  • The motion test moves the physical arm through a bounded sequence.
  • A disconnected or faulted Arduino prevents motion and is visible in the
    panel and /diagnostics.
  • A stop request cancels the active command and leaves the arm in a known
    state.
  • Simulation still works without an Arduino.
  • No node other than the selected state source publishes /joint_states.

Decisions Still Needed

  • Whether future feedback will be servo command echo, potentiometer/encoder
    measurement, or both. The current firmware provides command status only.
  • Whether the external servo-power switch is the formal emergency stop.
  • Physical emergency-stop wiring and how its state reaches ROS.
  • Whether the arm is controlled by hobby servos or motor drivers with a
    lower-level controller.

aj0khi and others added 7 commits August 31, 2026 12:47
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Co-authored-by: aj0khi <81395597+aj0khi@users.noreply.github.com>
This PR adds the first physical hardware integration for the WayBionic arm.

## What's new

- **waybionic_hardware package**: ROS 2 Arduino serial bridge using the confirmed
  Uno R4 protocol. Accepts RUN/HOME/STOP commands, publishes estimated joint
  state for RViz, and reports connection/command status on /diagnostics.

- **Hardware mode support**: Launch parameter hardware_mode switches between
  simulation (default) and arduino mode. Prevents simulated publishers from
  running simultaneously with the physical bridge.

- **Dry-run testing**: Arduino bridge supports --dry-run for development without
  hardware or USB connection.

- **Architecture documentation**: Complete hardware path design, protocol details,
  and phased implementation plan in docs/arm_hardware_motion_architecture.md.

## Commands to test

### Dry-run (no Arduino needed):
```bash
source install/setup.bash
ros2 launch waybionic_bringup ground_station.launch.py \
  hardware_mode:=arduino \
  arduino_dry_run:=true
```

### Physical hardware (Arduino connected):
```bash
source install/setup.bash
ros2 launch waybionic_bringup ground_station.launch.py \
  hardware_mode:=arduino \
  arduino_port:=/dev/ttyACM0
```

### Verify motion and state:
```bash
ros2 topic echo /joint_states
ros2 topic echo /diagnostics
```

## Current limitations

- No physical position feedback; RViz shows estimated commanded pose.
- Joint sensors (encoders/potentiometers) not yet integrated.
- MotionTestPanel remains on string topic; trajectory action interface planned
  for phase 2.

## Build and run

```bash
colcon build --symlink-install
source install/setup.bash
```
Copilot AI lite review requested due to automatic review settings September 12, 2026 16:38
@aj0khi
aj0khi requested a review from yassinsolim as a code owner September 12, 2026 16:38
@coderabbitai

coderabbitai Bot commented Sep 12, 2026

Copy link
Copy Markdown

Important

  • 🔍 Trigger review

This repository does not receive automatic reviews because it has fewer than 10 stars.

⚙️ Run configuration

Configuration used: defaults

Review profile: CHILL

Plan: Advanced

Run ID: d839cee5-7e0b-40f1-bb33-192ee4cd60b0


Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

❤️ Share

Comment @coderabbitai help to get the list of available commands.

Co-authored-by: aj0khi <81395597+aj0khi@users.noreply.github.com>

Copilot AI left a comment

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 Changes recommended

Unresolved hardware safety, fault-handling, motion-limit, and launch issues block approval.

Get a fresh assessment by requesting another Copilot review.

Pull request overview

Adds a four-joint arm simulation, Arduino serial bridge, RViz motion and diagnostics tooling, launch configurations, packaging, and architecture documentation.

Changes:

  • Adds simulation trajectories and estimated Arduino joint-state publishing.
  • Extends RViz panels, diagnostics CLI, tests, and configuration.
  • Adds URDF, bringup modes, package metadata, and hardware architecture docs.
File summaries
File Summary
waybionic_rviz_plugins/test/test_ros_diagnostics_source.cpp Adds diagnostics regression coverage.
waybionic_rviz_plugins/src/ros_diagnostics_source.cpp Handles diagnostic alert messages.
waybionic_rviz_plugins/src/motion_test_panel.cpp Implements motion command controls.
waybionic_rviz_plugins/src/diagnostics_panel.cpp Adds movement-test UI.
waybionic_rviz_plugins/scripts/temporary_diagnostics_publisher.py Publishes diagnostic messages.
waybionic_rviz_plugins/plugin_description.xml Registers the motion panel.
waybionic_rviz_plugins/package.xml Adds package dependencies.
waybionic_rviz_plugins/include/waybionic_rviz_plugins/motion_test_panel.hpp Declares the motion panel API.
waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp Declares diagnostics UI members.
waybionic_rviz_plugins/config/engineer_monitoring_view.rviz Configures the engineer monitoring view.
waybionic_rviz_plugins/CMakeLists.txt Builds and exports panel sources.
waybionic_motion_test/waybionic_motion_test/motion_test.py Implements simulated arm trajectories.
waybionic_motion_test/waybionic_motion_test/__init__.py Defines the Python package.
waybionic_motion_test/setup.py Defines motion-test installation.
waybionic_motion_test/setup.cfg Configures script installation.
waybionic_motion_test/resource/waybionic_motion_test Adds the ament package marker.
waybionic_motion_test/package.xml Defines motion-test metadata and dependencies.
waybionic_hardware/waybionic_hardware/arduino_bridge.py Implements Arduino transport and estimated state.
waybionic_hardware/waybionic_hardware/__init__.py Defines the hardware package.
waybionic_hardware/setup.py Defines bridge installation.
waybionic_hardware/setup.cfg Configures script installation.
waybionic_hardware/resource/waybionic_hardware Adds the ament package marker.
waybionic_hardware/package.xml Defines hardware dependencies.
waybionic_diagnostics/waybionic_diagnostics/cli.py Implements the diagnostics CLI.
waybionic_diagnostics/waybionic_diagnostics/__init__.py Defines the diagnostics package.
waybionic_diagnostics/test/test_cli.py Tests CLI behavior.
waybionic_diagnostics/setup.py Defines CLI installation.
waybionic_diagnostics/setup.cfg Configures script installation.
waybionic_diagnostics/resource/waybionic_diagnostics Adds the ament package marker.
waybionic_diagnostics/package.xml Defines diagnostics metadata and dependencies.
waybionic_description/urdf/old_arm_prototype.urdf Adds the four-joint arm model and limits.
waybionic_bringup/rviz/waybionic.rviz Updates the RViz layout.
waybionic_bringup/rviz/waybionic_unified.rviz Adds the unified motion-panel layout.
waybionic_bringup/package.xml Adds bringup dependencies.
waybionic_bringup/launch/ground_station.launch.py Selects simulation or Arduino mode.
waybionic_bringup/launch/display.launch.py Configures standalone display bringup.
docs/arm_hardware_motion_architecture.md Documents the hardware architecture and safety model.
cpsc. No functional content shown.
Review details

Suppressed comments (6)

waybionic_bringup/launch/ground_station.launch.py:54

  • This argument accepts arbitrary strings, so a typo such as hardware_mode:=arduin makes both the simulation and Arduino conditions false and silently launches without a state source. Restrict it to the two supported values so invalid modes fail at launch.
    hardware_mode_arg = DeclareLaunchArgument(
        'hardware_mode', default_value='simulation',
        description='Arm mode: simulation or arduino')

waybionic_hardware/waybionic_hardware/arduino_bridge.py:20

  • These calibration and trajectory constants are duplicated in waybionic_motion_test/motion_test.py and this bridge. A future change to the home offsets, directions, or sequence can make simulation and physical mode command different poses, contrary to the documented single calibration source. Move the shared calibration/sequence into one package-level configuration consumed by both nodes.
HOME_PHYSICAL_DEGREES = [33.5, 112.5, 151.5, 27.5]
SERVO_DIRECTIONS = [1.0, -1.0, 1.0, 1.0]
PHYSICAL_LIMITS = [(0.0, 270.0), (0.0, 112.5), (90.0, 270.0), (0.0, 270.0)]
SEQUENCE = [
    [60.0, 95.0, 125.0, 60.0],
    [45.0, 75.0, 200.0, 100.0],
    [15.0, 100.0, 130.0, 5.0],
    HOME_PHYSICAL_DEGREES,

waybionic_hardware/waybionic_hardware/arduino_bridge.py:20

  • The first physical RUN sends waypoints for all four servos simultaneously. The architecture's safety rules require a low-speed, one-joint-at-a-time commissioning test before synchronized trajectories, so this default hardware path bypasses that guard. Gate the synchronized sequence behind explicit commissioning or use a one-joint sequence first.
SEQUENCE = [
    [60.0, 95.0, 125.0, 60.0],
    [45.0, 75.0, 200.0, 100.0],
    [15.0, 100.0, 130.0, 5.0],
    HOME_PHYSICAL_DEGREES,

waybionic_hardware/waybionic_hardware/arduino_bridge.py:199

  • The estimator state is committed before send_line() succeeds. If a HOME/RUN command is issued while another move is active and the serial write fails, motion_active can remain true while target has been changed to a command the Arduino never received; sequence_active can likewise remain set after a failed RUN. Commit motion state only after a successful write and clear the sequence on failure.
        self.start = list(self.current)
        self.target = list(target)
        self.motion_started = time.monotonic()
        self.motion_duration = max(0.3, min(15.0, self.segment_duration))
        command = 'MOVE,' + ','.join(f'{value:.2f}' for value in target)

waybionic_motion_test/waybionic_motion_test/motion_test.py:71

  • When HOME interrupts an active sequence, sequence_index still points at the interrupted segment. Once the HOME segment completes, update_trajectory() sees an index below the final element and starts the remaining RUN sequence instead of stopping at HOME. Reset the sequence state when handling HOME.
        elif command == 'HOME':
            self.start_segment(self.home)

waybionic_rviz_plugins/src/motion_test_panel.cpp:120

  • The panel reports STOPPED immediately after publishing HOLD, without checking that a bridge received the command or that the Arduino acknowledged it. If the bridge is absent or the write fails, the physical arm may continue while the UI reports a successful stop; make stop completion/failure depend on hardware feedback.
  • Files reviewed: 35/41 changed files
  • Comments generated: 19
  • Review effort level: Lite

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.

Comment thread waybionic_description/urdf/old_arm_prototype.urdf Outdated
Comment on lines +105 to +110
def read_serial(self):
while not self.serial_stop.is_set() and self.serial_port is not None:
try:
line = self.serial_port.readline().decode('ascii', errors='replace').strip()
if line:
self.process_serial_line(line)
Comment on lines +111 to +115
except Exception as error:
self.connected = False
self.last_error = str(error)
self.last_status = 'serial-read-failed'
return

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Confirmed on d77c284 with a fake serial read failure. connected becomes false, but both motion flags remain true, and the next state timer advances the published pose. Please freeze the estimate and cancel the sequence when the transport fails; the current three hardware tests do not exercise this path.

Comment on lines +117 to +119
def process_serial_line(self, line):
self.get_logger().debug(f'Arduino: {line}')
if line.startswith('READY,IK4,1'):
Comment on lines +122 to +126
if line.startswith('ERROR,'):
self.motion_active = False
self.sequence_active = False
self.last_error = line[6:]
self.last_status = 'arduino-error'
Comment thread waybionic_hardware/waybionic_hardware/arduino_bridge.py Outdated
Comment on lines +229 to +230
status.level = DiagnosticStatus.OK if self.connected else DiagnosticStatus.ERROR
status.message = self.last_status
Comment thread waybionic_motion_test/waybionic_motion_test/motion_test.py
Comment on lines +306 to +308
movement_test_button_ = new QPushButton("▶ Run Movement Test");
movement_test_button_->setToolTip(
"Run the robot through its predefined movement test sequence.");
Comment on lines +88 to +97
test_running_ = true;

result_label_->setText("Running motion sequence...");
status_label_->setText("RUNNING: synchronized 4-DOF sequence");

run_button_->setEnabled(false);
home_button_->setEnabled(false);
stop_button_->setEnabled(true);

publishCommand("RUN");
aj0khi and others added 5 commits September 12, 2026 10:47
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
- Resolves colcon test failures due to missing test files
- Both packages now have minimal unit tests that verify module imports
- CI/CD pipeline will now pass the test phase
@aj0khi

aj0khi commented Sep 12, 2026

Copy link
Copy Markdown
Author

Updated todo list

Connect Arduino to WSL

In Windows PowerShell as Administrator:

usbipd list
usbipd bind --busid <BUSID>
usbipd attach --wsl --busid <BUSID>

Replace <BUSID> with the Arduino’s ID, such as 4-4.

In WSL, verify the port:

ls /dev/ttyACM* /dev/ttyUSB*

Usually it will be /dev/ttyACM0. Check permissions:

sudo usermod -aG dialout "$USER"

Restart WSL afterward if needed.

Run the arm

cd ~/waybionic_ground_station
source /opt/ros/jazzy/setup.bash
source install/setup.bash

ros2 launch waybionic_bringup ground_station.launch.py \
  hardware_mode:=arduino \
  arduino_port:=/dev/ttyACM0 \
  launch_rviz:=true

Verify the Arduino connects:

ros2 topic echo /diagnostics

Before using the real arm, test without sending commands:

ros2 launch waybionic_bringup ground_station.launch.py \
  hardware_mode:=arduino \
  arduino_dry_run:=true \
  launch_rviz:=true

To disconnect the USB device from WSL:

usbipd detach --wsl --busid <BUSID>

@yassinsolim yassinsolim left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Reviewed d77c284. The hardware and simulator packages each pass their three tests, but mocked behavior checks exposed the issues below. I also reproduced the existing disconnect finding. All checks were hardware-free; no serial device or motor was accessed. Please address these and the remaining safety threads before merge.

Comment thread cpsc. Outdated

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[P1] Remove the Windows-invalid filename

This empty file has a trailing dot, which Windows cannot check out normally. On Windows, even exporting the selected packages with git archive fails with error: invalid path 'cpsc.'. Please remove the file or rename it without the trailing dot so contributors can check out this branch without weakening Git's path protections.

self.serial_stop.set()
if self.serial_port is not None:
try:
self.serial_port.close()

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[P1] Request a stop before closing an active bridge

I started a MOVE against a fake serial device and called destroy_node(). The port was closed with only the MOVE in the write log; no HOLD was sent. The firmware runs that trajectory independently, so closing the launch does not request a stop for the active move. Please cancel the sequence and request HOLD while the transport is still usable, serialize shutdown with the reader, and add a shutdown-during-motion regression.


if progress >= 1.0:
self.current = list(self.target)
if self.sequence_index < len(self.sequence) - 1:

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[P2] Keep HOME separate from sequence playback

HOME also reaches this sequence-advance branch. Starting from a fresh node, I sent HOME and advanced 151 timer ticks with the default duration: the node moved on to sequence index 1 with a non-home target, without receiving RUN. Please advance the sequence only while an explicit RUN is active. Cover HOME both before a run and while interrupting a run so it finishes at home and stays there.

Comment on lines +111 to +115
except Exception as error:
self.connected = False
self.last_error = str(error)
self.last_status = 'serial-read-failed'
return

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Confirmed on d77c284 with a fake serial read failure. connected becomes false, but both motion flags remain true, and the next state timer advances the published pose. Please freeze the estimate and cancel the sequence when the transport fails; the current three hardware tests do not exercise this path.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants