Skip to content

Add vision docking, docking controller, and mock LiDAR#2

Open
bong7233 wants to merge 21 commits into
mainfrom
claude/amazing-albattani-majtxb
Open

Add vision docking, docking controller, and mock LiDAR#2
bong7233 wants to merge 21 commits into
mainfrom
claude/amazing-albattani-majtxb

Conversation

@bong7233

@bong7233 bong7233 commented Jun 23, 2026

Copy link
Copy Markdown
Owner

Summary

Builds a complete vision-based docking feature on top of the baseline mock AMR stack: perception → behaviour → sensor. The core math is split into ROS-free Python modules so it is unit-tested without a running graph; the ROS nodes are thin wrappers.

New packages

  • amr_vision — OpenCV ArUco docking-marker detection, a mock camera (no hardware needed), and /docking_state. aruco_compat hides the OpenCV 4.6 (Jazzy) vs 4.7+ ArUco API differences; pose uses SOLVEPNP_ITERATIVE with NaN filtering. Reports translation signals (range/lateral/bearing); single-marker yaw is omitted due to planar pose ambiguity.
  • amr_docking — alignment/approach controller (ALIGN/APPROACH/DOCKED), /scan obstacle stop (BLOCKED, ignoring the dock itself via a margin), and an odom-driven fully closed-loop demo. Output goes through the existing /cmd_vel → safety-monitor path.
  • amr_lidar_driver — mock 2D LiDAR ray-casting simulator publishing /scan (Nav2 input prep, no Gazebo).

Other changes

  • amr_interfaces: add DockingState.msg.
  • amr_description: add camera_link / camera_optical_frame to the xacro.
  • amr_tools: health_report now covers /docking_state and /scan; summarization logic pulled into a tested report_model.
  • CI: run the ROS-free unit tests in the static-checks job (venv, no ROS) for fast validation.
  • Docs: docs/10 (vision docking), docs/11 (dev environment setup), English doc sync, roadmap update.

Testing

  • 50 ROS-free unit tests pass locally (perception geometry, control law, obstacle gate, scan model, report summarization), including an end-to-end closed-loop docking simulation that drives the robot to DOCKED.
  • py_compile over all listed Python and XML/SDF/xacro well-formedness checks pass locally.
  • Full colcon build/colcon test (message generation, rclpy/cv_bridge runtime) was not runnable locally without ROS — this PR is what exercises it in CI.

🤖 Generated with Claude Code

https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA

Generated by Claude Code

claude added 21 commits June 22, 2026 20:07
@claude
Close the mock docking loop via odom
f241bb2 
Make the mock camera optionally render a marker fixed in the world (odom)
frame and project it from the live robot pose, so the docking loop closes
end to end without Gazebo:
controller -> /cmd_vel -> safety -> base -> /odom -> camera -> detector ->
/docking_state -> controller.

- amr_vision: add world_marker_to_camera_center geometry (ROS-free, 6 unit
  tests) and a use_odom mode in mock_dock_camera_node that subscribes to
  /odom; add nav_msgs dependency.
- amr_docking: dock_closed_loop.launch.py brings up the mock robot, mock
  camera (use_odom), detector, and controller for an ALIGN -> APPROACH ->
  DOCKED demo.
- Update CI py_compile list, README, and the vision docking guide.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add amr_lidar_driver mock LiDAR (/scan)
99a21b4 
Fill the /scan gap in the pure mock stack (no Gazebo) toward Nav2 readiness,
following the project's "Python for device simulators" split.

- amr_lidar_driver: ROS-free ray-casting scan model (scan_model.py, 7 unit
  tests) that casts beams in a rectangular room with circular obstacles, plus
  a thin mock_lidar_driver_node publishing sensor_msgs/LaserScan with optional
  Gaussian noise and an odom-driven sensor pose.
- Standalone launch (mock_lidar.launch.py) and diagnostics consistent with the
  rest of the stack.
- Update CI py_compile list and README rows.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add obstacle-aware stop to docking controller
746a4de 
Wire the mock LiDAR /scan into the docking controller so it stops for
obstacles in the approach corridor, while ignoring the dock structure itself.

- amr_docking: add forward_corridor_clearance and is_path_blocked (ROS-free,
  5 new unit tests). The dock at the marker range is excluded via dock_margin
  so the controller is not fooled into stopping by the dock it approaches.
- compute_docking_command gains a path_blocked flag -> BLOCKED phase (stop).
- docking_controller_node subscribes /scan and applies the gate; new params
  enable_obstacle_stop / robot_half_width_m / obstacle_stop_distance_m /
  dock_margin_m.
- dock_closed_loop.launch.py now also runs the mock LiDAR (obstacle off the
  corridor by default). Update docs.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Extend health_report with docking and scan; test summarization
c47e5b3 
Make the FAE health report cover the new perception subsystems and pull its
summarization logic into a unit-tested, ROS-free module.

- amr_tools: new report_model.py (worst_diagnostic, topic_liveness,
  scan_summary, level_name) with 8 unit tests.
- health_report now reports /docking_state and /scan liveness and details, and
  uses report_model for the diagnostics roll-up (STALE now ranks above ERROR,
  matching the aggregator convention).
- Update CI py_compile list.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Sync English docs with vision/docking/lidar additions
60d0f87 
Keep the bilingual docs in parity with the new packages.

- README.en.md: add amr_vision / amr_docking / amr_lidar_driver rows, vision and
  closed-loop docking and mock-LiDAR quick-start sections, and doc links.
- Add docs/en/10_vision_docking_guide.en.md and cross-link it from docs/10.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add docking camera frames to robot description
f500422 
Give the docking camera a real place in the TF tree so RViz/Nav2 can locate
detections and a Gazebo camera can be mounted later.

- amr_description: add camera_link (front mount) and camera_optical_frame
  (REP-103 optical convention) to the xacro, matching the frame_id used by
  amr_vision and the controller's camera_forward_offset.
- Note the Gazebo camera sensor as the follow-up in the docking guide.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add dev environment setup guide
8b63c06 
Capture the Windows 11 -> Ubuntu/ROS 2 environment options (WSL2 / external
SSD / dual-boot / laptop) with a recommendation, plus concrete steps to install
ROS 2 Jazzy and build this workspace on WSL2, and how to run the ROS-free unit
tests anywhere. Link it from both READMEs.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add ROS-free closed-loop docking simulation test
8706224 
Wire the real perception (amr_vision) and control law together and integrate
robot motion over time, asserting the robot converges and the controller reports
DOCKED for both a dead-ahead and an offset marker. This exercises the frame
conventions and the perception/control handoff end to end without ROS.

- amr_docking: test_closed_loop_sim.py (uses importorskip so it skips cleanly if
  amr_vision/OpenCV are unavailable); declare amr_vision / python3-opencv /
  python3-numpy as test deps so CI runs it.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Run ROS-free unit tests in CI static checks
0bdae48 
Validate the perception, docking, scan, and report cores on every push/PR
without waiting on the full ROS build, using a venv with pip OpenCV/NumPy.
This gives fast feedback and proves those modules are genuinely ROS-free.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Document implemented perception/docking work in roadmap
20efb70 
Add a "Beyond the Baseline (Implemented)" section to the roadmap summarizing
amr_vision / amr_docking / amr_lidar_driver and the health_report extension, so
the roadmap reflects what is actually built and what comes next.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add Nav2-ready headless bringup (display.launch.py)
3b9f04c 
Provide the core Nav2 inputs (TF, odometry, scan) without Gazebo, matching the
documented odom -> base_link -> laser design.

- amr_bringup: new display.launch.py extends mock_robot.launch.py with
  base_link -> lidar_link / base_link -> camera_link static transforms (offsets
  from the URDF) and the mock LiDAR (/scan, odom-driven). Optional RViz via
  rviz:=true. The base controller already publishes odom -> base_link.
- Add a mock_lidar_driver block to mock_robot.yaml; declare amr_lidar_driver and
  tf2_ros deps; add display.launch.py to the CI py_compile list.
- Document the headless Nav2-ready bringup in both READMEs.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Report docking state in system_manager
2ceb029 
Integrate docking into the central robot state: system_manager subscribes to
/docking_state and reports whether the robot is physically docked.

- amr_interfaces: add bool docked to RobotState.msg.
- amr_system_manager: subscribe /docking_state, track docked = detected &&
  aligned with a staleness timeout (docking_state_timeout_ms), publish it in
  /robot_state, append "(docked)" to the state message, and add it to
  diagnostics. Mode stays operator-controlled; auto CHARGING transition is left
  as a follow-up.
- amr_tools: health_report shows the docked flag.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add amr_navigation waypoint follower (pre-Nav2 autonomy)
104a037 
Exercise the AUTO_RUNNING style of operation without a full Nav2 stack: follow a
list of odom-frame waypoints with a pure-pursuit controller.

- amr_navigation: ROS-free control law (waypoint_follower.py, 9 unit tests
  including a closed-loop square-route simulation) plus a thin node
  (waypoint_follower_node.py) that subscribes /odom, publishes /cmd_vel through
  the safety path, and toggles via /enable_navigation (std_srvs/SetBool).
- Waypoints, gains, tolerance, and looping are parameters; waypoint_demo.launch.py
  drives the mock robot around a square with no Gazebo.
- Run the ROS-free tests in CI, add py_compile entries, README rows, and quick
  start in both languages.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add amr_twist_mux command arbitration
582b095 
With manual jog, docking, and the waypoint follower all wanting /cmd_vel, add a
priority multiplexer so they do not fight over the command.

- amr_twist_mux: ROS-free select_command (7 unit tests) picks the highest-priority
  recently-active source; a thin node subscribes the configured input topics and
  republishes the winner to /cmd_vel with diagnostics for the active source.
- Inputs (name/topic/priority/timeout) are parameters; default order is
  teleop > dock > nav. Run the ROS-free tests in CI; add py_compile entries,
  README rows, and wiring notes.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Auto-enter CHARGING when docked
e7216e7 
Complete the docking <-> mode integration: system_manager switches to CHARGING
on the docked edge and back to MANUAL on undock, edge-triggered so it yields to
an operator who changes mode in between. Configurable via auto_charge_when_docked
(default true); skipped during estop/fault.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Update architecture diagram for autonomy and arbitration
4f5357d 
Reflect the expanded system in the README diagram: add the amr_vision /
amr_docking / amr_navigation / amr_twist_mux layer and show the real command
pipeline (sources -> twist_mux -> safety -> base) plus the docking_state, scan,
and odom flows. Note that all runtime nodes publish /diagnostics.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add full_system.launch.py integrated demo
00bdd91 
Bring up the whole stack with no Gazebo and wire the command pipeline through
the twist mux:
nav/dock/teleop -> twist_mux -> /cmd_vel -> safety -> base -> /odom.

- amr_bringup: full_system.launch.py includes the mock robot and adds the mux,
  mock LiDAR, vision (mock camera + detector), the docking controller (remapped
  to cmd_vel_dock, idle until enabled), and the waypoint follower (remapped to
  cmd_vel_nav, auto-started looping). Priority is teleop > dock > nav.
- Declare the new runtime deps (amr_vision/docking/navigation/twist_mux); add
  the launch to CI py_compile; document the demo in the README.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add amr_mission battery-aware mission coordinator
f46edf4 
Tie battery, navigation, and docking into an autonomous mission: patrol until
the battery is low, return to the dock and charge, then resume.

- amr_mission: ROS-free state machine (mission_coordinator.py, 8 unit tests)
  over PATROL / RETURN_TO_DOCK / CHARGING, plus a node that watches
  /battery_state and /robot_state.docked and drives the controllers' enable
  services (/enable_navigation, /enable_docking), only calling on change.
- amr_bringup: mission_demo.launch.py brings up the full stack with the
  follower/docking idle and the coordinator driving them; depend on amr_mission.
- Run the ROS-free tests in CI; add py_compile entries, README rows, and a
  quick start.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Recharge mock battery while docked/charging
129cf40 
Close the mission loop: the mock battery now recharges while the system is in
CHARGING mode, so PATROL -> dock -> CHARGING -> resume can run without manually
forcing the battery back up.

- amr_battery_driver: subscribe /robot_state and, when mode is CHARGING and
  recharge_rate_vps > 0, charge the pack toward nominal and report
  POWER_SUPPLY_STATUS_CHARGING. Default rate 0 keeps existing behavior.
- amr_bringup: set recharge_rate_vps in mock_robot.yaml; document that raising
  discharge_rate_vps gives a faster hands-free cycle.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add C++ unit tests for diff-drive kinematics
f2dc28d 
Give the C++ control logic its first unit tests. Extract the differential-drive
kinematics and odometry integration from the base controller into a pure,
ROS-free header (diff_drive_kinematics.hpp) so it has a single source of truth
and is gtest-testable, and refactor the node to use it (behaviour preserved).

- amr_base_controller: new header + include/ on the target; gtest
  (test_diff_drive_kinematics.cpp) covering inverse/forward kinematics
  round-trip, straight and rotational odometry integration, and angle wrap;
  wire ament_add_gtest and declare ament_cmake_gtest.
- The header math was verified locally with a standalone build; the gtest/CMake
  wiring is validated in CI.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
@claude
Add C++ unit tests for the safety gate decision
b0ece81 
Extract the safety monitor's "allow command and why" logic into a pure,
ROS-free header (safety_decision.hpp) and refactor the node to use it, so the
safety-critical decision has a single source of truth and gtest coverage.

- amr_safety_monitor: new header + include/ on the target; gtest
  (test_safety_decision.cpp) covering the clear case, each gating condition,
  the motor-fault code string, the require_motor_enabled gate, and multi-reason
  ordering; wire ament_add_gtest and declare ament_cmake_gtest.
- Logic verified locally with a standalone build; gtest/CMake wiring validated
  in CI.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01MEj8QSrWSni8uVEuK3MqyA
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

No reviews

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

2 participants

@bong7233 @claude