feat: add visual line-following task

line_follower/.gitignore

@@ -0,0 +1,6 @@
+__pycache__/
+*.py[cod]
+
+# Runtime screenshots; the data directory itself is retained with .gitkeep.
+data/raw/
+data/mask/

line_follower/CMakeLists.txt

@@ -0,0 +1,215 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(line_follower)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  geometry_msgs
+  rospy
+  sensor_msgs
+  std_msgs
+  std_srvs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs#   sensor_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES line_follower
+  CATKIN_DEPENDS cv_bridge geometry_msgs rospy sensor_msgs std_msgs std_srvs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/line_follower.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/line_follower_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+  scripts/line_follow.py
+  scripts/camera_offset_calibrator.py
+  scripts/line_follow_control.py
+  scripts/line_follow_debug.py
+  scripts/snapshot_capture.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY launch config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_line_follower.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test/test_single_boundary_fallback.py)
+  catkin_add_nosetests(test/test_state_lookahead.py)
+  catkin_add_nosetests(test/test_state_speed.py)
+endif()

line_follower/README.md

@@ -0,0 +1,100 @@
+# line_follower
+
+ROS Noetic visual line following for the U-CAR.  The active implementation is
+split into perception and control so that the camera can be inspected without
+moving the vehicle.
+
+## Active nodes
+
+- `line_follow_debug.py`: subscribes to `/usb_cam/image_raw`, applies the
+  calibrated inverse-perspective transform and HSV white-line extraction,
+  fits the lane centreline, and publishes a metric lookahead target.
+- `line_follow_control.py`: subscribes to the target and uses pure pursuit to
+  publish `/cmd_vel` only when `~enabled` is explicitly set to `true`.
+
+The metric target topic is:
+
+```text
+/line_follow_debug/lookahead_target  geometry_msgs/PointStamped
+```
+
+It follows `base_link` conventions: `point.x` is metres forward from the
+vehicle control centre and `point.y` is metres to the left.  The controller
+uses:
+
+```text
+curvature = 2 * point.y / (point.x^2 + point.y^2)
+angular.z = linear_speed * curvature_gain * curvature
+```
+
+This is geometric lookahead control.  `ipm_origin_ahead_of_control_m` is an
+extrinsic offset used while converting the IPM curve into the control-centre
+frame; it is not a time or odometry delay.
+
+## Start safely
+
+Start the base driver and the single shared camera first.  Then run:
+
+```bash
+source ~/ucar_ws/devel/setup.bash
+roslaunch line_follower line_follow_debug.launch
+```
+
+Confirm that the green centre curve and purple lookahead target are stable.
+In another terminal run:
+
+```bash
+source ~/ucar_ws/devel/setup.bash
+roslaunch line_follower line_follow_control.launch
+```
+
+The controller starts disabled.  On a clear test route:
+
+```bash
+rosparam set /line_follow_control/enabled true
+```
+
+The integrated task state machine uses the equivalent service interface:
+
+```bash
+rosservice call /line_follow_control/set_enabled "data: true"
+```
+
+Stop immediately with:
+
+```bash
+rosparam set /line_follow_control/enabled false
+```
+
+## Main tuning parameters
+
+- `config/line_follow_debug.yaml`
+  - `lookahead_distance_m`: requested target distance from the control centre.
+  - `ipm_origin_ahead_of_control_m`: forward offset from the control centre to
+    the calibrated IPM origin.
+  - `max_near_target_extrapolation_m`: maximum tangent-only extension when the
+    observed centreline ends farther away than the requested target.
+  - `metric_scale_px_per_m` and `metric_origin_px`: metric IPM calibration.
+- `config/line_follow_control.yaml`
+  - `linear_speed`: commanded forward speed.
+  - `curvature_gain`: pure-pursuit steering gain.
+  - `max_angular_speed`: steering safety limit.
+  - `target_alpha`: frame-to-frame target smoothing.
+
+The current tuning is `lookahead_distance_m=0.30 m`,
+`linear_speed=0.15 m/s`, `curvature_gain=1.8`, and `target_alpha=0.65`.
+The angular velocity remains limited to `0.20 rad/s`.
+
+During the integrated task, `normal_lookahead_distance_m=0.30 m` is used for
+LEFT/RIGHT routes.  The latched task state automatically selects
+`after_second_lookahead_distance_m=0.35 m` in `FOLLOW_AFTER_SECOND`.
+In that state only, curved targets are shifted `0.025 m` toward the detected
+curve's inside once lateral target displacement exceeds `0.015 m`; straight
+targets and LEFT/RIGHT routes remain centred.
+
+The controller also switches from `normal_linear_speed=0.15 m/s` to
+`after_second_linear_speed=0.10 m/s` only in `FOLLOW_AFTER_SECOND`. Other task
+states automatically restore 0.15 m/s.
+
+If no fresh valid target is available, the controller publishes a zero
+velocity command instead of reusing old geometry.

line_follower/config/line_follow_control.yaml

@@ -0,0 +1,31 @@
+# Safety gate: the controller starts in observation mode and never moves the
+# vehicle until this parameter is explicitly changed to true.
+enabled: false
+
+# These are published by line_follow_debug.py after its metric IPM processing.
+lane_valid_topic: /line_follow_debug/lane_valid
+lookahead_target_topic: /line_follow_debug/lookahead_target
+cmd_vel_topic: /cmd_vel
+
+# Real-route controller settings.
+control_rate: 20.0
+linear_speed: 0.15
+# LEFT/RIGHT routes and the STRAIGHT route before turn two use 0.15 m/s.
+# FOLLOW_AFTER_SECOND automatically slows to 0.10 m/s.
+task_state_topic: /traffic_line_task/state
+normal_linear_speed: 0.15
+after_second_linear_speed: 0.10
+# Pure-pursuit angular.z = linear_speed * curvature_gain * 2*y/(x^2+y^2).
+curvature_gain: 1.8
+# Retain the verified steering safety limit after increasing linear speed.
+max_angular_speed: 0.20
+
+# The published metric target follows base_link: +x forward and +y left.
+steering_sign: 1.0
+
+# Smooth target motion slightly without introducing a distance/time delay.
+target_alpha: 0.65
+lateral_deadband_m: 0.01
+min_target_forward_m: 0.10
+max_target_distance_m: 1.50
+target_timeout: 0.50

line_follower/config/line_follow_debug.yaml

@@ -0,0 +1,103 @@
+# This node is visualisation-only: it never publishes /cmd_vel.
+image_topic: /usb_cam/image_raw
+
+# Metric inverse-perspective mapping (IPM), calibrated on 2026-08-10 with
+# four ground marks measured from the vehicle-front centre.  The raw USB
+# camera is horizontally mirrored, so its physical left marks appear at the
+# right side of the source image.  The matching destinations undo that mirror.
+# Point order: near-left, near-right, far-right, far-left.
+use_perspective_transform: true
+perspective_reference_size: [640, 480]
+src_pts: [[562, 304], [75, 307], [193, 240], [466, 237]]
+dst_pts: [[240, 360], [400, 360], [400, 120], [240, 120]]
+
+# Process at half resolution for real-time control, then scale debug views
+# back to the display size.  Published error is converted back to 640-pixel
+# coordinates, so the controller parameters do not need changing.
+processing_scale: 0.5
+# GUI is intentionally slower than perception.  It must never throttle the
+# messages consumed by the controller.
+display_rate: 10.0
+
+# Legacy scanline-offset compensation remains disabled.  The controller now
+# uses a metric target point on the fitted IPM centreline.
+use_camera_front_offset: false
+camera_to_front_offset_m: 0.125
+# Zero means calculate automatically from camera_to_front_offset_m and the
+# metric IPM scale.  A positive value overrides it in 640x480 reference pixels.
+camera_front_offset_px: 0
+metric_scale_px_per_m: 400.0
+metric_origin_px: [320, 480]
+# Pure-pursuit target geometry.  The calibrated IPM origin is treated as
+# 125 mm ahead of the robot control centre; the desired target lies 300 mm
+# ahead of that control centre.  When the nearest reliable curve sample is
+# farther away, continue only its tangent toward the vehicle by at most 250 mm.
+lookahead_distance_m: 0.30
+# Keep 0.30 m for LEFT/RIGHT routes and before the second maneuver. Once the
+# STRAIGHT route completes its fixed second right turn and enters
+# FOLLOW_AFTER_SECOND, automatically use 0.35 m without restarting this node.
+task_state_topic: /traffic_line_task/state
+normal_lookahead_distance_m: 0.30
+after_second_lookahead_distance_m: 0.35
+# Only after the second fixed right turn, move a curved lookahead target 25 mm
+# farther toward that curve's inside. Near-straight targets remain centred.
+after_second_inner_offset_m: 0.025
+inner_offset_activation_m: 0.015
+ipm_origin_ahead_of_control_m: 0.125
+max_near_target_extrapolation_m: 0.25
+lookahead_frame_id: base_link
+# Ordered click targets: near-left, near-right, far-right, far-left.
+# Each is [forward metres from vehicle-front centre, left metres].  These
+# values are the four marks used for the current src_pts/dst_pts calibration.
+calibration_markers_m: [[0.30, 0.20], [0.30, -0.20], [0.90, -0.20], [0.90, 0.20]]
+
+# OpenCV HSV white-line threshold.  The upper scene (LED board, wall and desk)
+# is excluded by roi_top_ratio before any line candidates are selected.
+hsv_lower: [0, 0, 180]
+hsv_upper: [180, 60, 255]
+# In the real first/second junction images, the white route boundaries occupy
+# approximately y=200..330 of a 480-pixel image.  Do not start at y=249: that
+# misses the branches entirely.
+roi_top_ratio: 0.42
+
+# Several horizontal bands are checked from near to far.  At the first and
+# second forks the visible white lines move high in the image, so one fixed
+# scanline is not reliable.
+scanline_ratios: [0.82, 0.79, 0.76, 0.73, 0.70, 0.66, 0.62, 0.58, 0.54, 0.50, 0.46]
+scanline_half_height: 5
+min_run_width: 3
+
+# Discard isolated white connected components smaller than this area (pixels).
+# Route boundaries in the collected images are long components, while floor
+# speckles and LED artifacts are much smaller.
+min_component_area: 30
+# connectedComponentsWithStats is too slow on the vehicle at 640x480.  The
+# 3x3 morphology above remains enabled; turn this on only for offline tuning.
+use_component_filter: false
+
+# Do not mask the image centre.  Set this above zero only if a later test
+# confirms that LED reflection must be excluded again.
+center_reflection_half_width_ratio: 0.0
+# The metric calibration maps a 410 mm lane to about 25% of the processing
+# image width.  Run centres and tape thickness can make the measured value a
+# few pixels smaller, and some route sections are narrower, so keep margin.
+min_lane_width_ratio: 0.18
+max_lane_width_ratio: 0.98
+morphology_kernel: 3
+
+# A bend can temporarily hide its inner boundary.  Remember the most recent
+# two-boundary width and infer the midpoint from the remaining outer boundary.
+# The real finish bend hides its inner boundary for several seconds. Continue
+# only while the same outer boundary remains visible, with an eight-second hard
+# limit; complete/ambiguous line loss still publishes lane_valid=false.
+use_single_boundary_fallback: true
+single_boundary_timeout: 8.0
+# Preserve the physical side identity briefly when the curved outer line
+# crosses the image centre or one camera frame is missed.
+single_boundary_side_memory_timeout: 0.75
+lane_width_alpha: 0.2
+
+# Fit a curved centreline through all usable near/far scan bands.  The fitted
+# tangent remains available for diagnostics, while the metric point on this
+# curve is published as /line_follow_debug/lookahead_target.
+max_centerline_heading_rad: 0.70

line_follower/config/snapshot_capture.yaml

@@ -0,0 +1,6 @@
+# The capture node only subscribes to this topic.  Start the camera separately.
+image_topic: /usb_cam/image_raw
+
+# White line segmentation range in OpenCV HSV order: H, S, V.
+hsv_lower: [0, 0, 180]
+hsv_upper: [180, 60, 255]

line_follower/launch/camera_offset_calibration.launch

@@ -0,0 +1,10 @@
+<launch>
+  <!-- Start usb_cam separately.  This launch file is subscriber-only. -->
+  <arg name="config" default="$(find line_follower)/config/line_follow_debug.yaml"/>
+  <arg name="image_topic" default="/usb_cam/image_raw"/>
+
+  <node pkg="line_follower" type="camera_offset_calibrator.py" name="camera_offset_calibrator" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+    <param name="image_topic" value="$(arg image_topic)"/>
+  </node>
+</launch>

line_follower/launch/line_follow.launch

@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find usb_cam)/launch/usb_cam-test.launch"/>
+  <include file="$(find ucar_controller)/launch/base_driver.launch"/>
+  <node pkg="line_follower" type="line_follow.py" name="line_follower" output="screen"/>
+</launch>

line_follower/launch/line_follow_control.launch

@@ -0,0 +1,10 @@
+<launch>
+  <!-- Start line_follow_debug.launch separately.  This file starts no camera. -->
+  <arg name="config" default="$(find line_follower)/config/line_follow_control.yaml"/>
+  <arg name="enabled" default="false"/>
+
+  <node pkg="line_follower" type="line_follow_control.py" name="line_follow_control" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+    <param name="enabled" value="$(arg enabled)"/>
+  </node>
+</launch>

line_follower/launch/line_follow_debug.launch

@@ -0,0 +1,10 @@
+<launch>
+  <!-- Camera and base driver must already be running elsewhere. -->
+  <arg name="config" default="$(find line_follower)/config/line_follow_debug.yaml"/>
+  <arg name="image_topic" default="/usb_cam/image_raw"/>
+
+  <node pkg="line_follower" type="line_follow_debug.py" name="line_follow_debug" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+    <param name="image_topic" value="$(arg image_topic)"/>
+  </node>
+</launch>

line_follower/launch/snapshot_capture.launch

@@ -0,0 +1,12 @@
+<launch>
+  <!-- This launch file deliberately does not start usb_cam or open a camera. -->
+  <arg name="config" default="$(find line_follower)/config/snapshot_capture.yaml"/>
+  <arg name="image_topic" default="/usb_cam/image_raw"/>
+  <arg name="save_root" default="$(find line_follower)/data"/>
+
+  <node pkg="line_follower" type="snapshot_capture.py" name="line_snapshot_capture" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+    <param name="image_topic" value="$(arg image_topic)"/>
+    <param name="save_root" value="$(arg save_root)"/>
+  </node>
+</launch>

line_follower/package.xml

@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>line_follower</name>
+  <version>0.0.0</version>
+  <description>The line_follower package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ucar@todo.todo">ucar</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/line_follower</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>std_srvs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <test_depend>python3-nose</test_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

line_follower/scripts/camera_offset_calibrator.py

@@ -0,0 +1,255 @@
+#!/usr/bin/env python3
+"""Interactive metric ground-plane calibration for the line follower.
+
+Four tape crosses with known positions relative to the vehicle front centre are
+clicked in the live *raw* camera image.  Their matching virtual ground-plane
+positions are used to calculate the homography consumed by line_follow_debug.
+The camera remains forward-facing; the bird's-eye view is the result of the
+calibration, not an assumption about where the camera is mounted.
+"""
+
+import threading
+
+import cv2
+import numpy as np
+import rospy
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+
+
+class CameraOffsetCalibrator:
+    """Subscriber-only four-point perspective calibration tool."""
+
+    DISPLAY_SCALE = 2
+    MARKER_NAMES = ("near-left", "near-right", "far-right", "far-left")
+
+    def __init__(self):
+        rospy.init_node("camera_offset_calibrator")
+        self.image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
+        self.reference_size = self._read_size("~perspective_reference_size", [640, 480])
+        self.marker_positions_m = self._read_world_points(
+            "~calibration_markers_m",
+            [[0.30, 0.20], [0.30, -0.20], [0.90, -0.20], [0.90, 0.20]],
+        )
+        self.metric_scale = max(1.0, float(rospy.get_param("~metric_scale_px_per_m", 400.0)))
+        self.metric_origin = self._read_point("~metric_origin_px", [320, 480])
+
+        self.bridge = CvBridge()
+        self.lock = threading.Lock()
+        self.source_image = None
+        self.clicked_points = []
+        self.last_result = None
+
+        self.subscriber = rospy.Subscriber(
+            self.image_topic, Image, self.image_callback, queue_size=1
+        )
+        rospy.loginfo(
+            "Metric perspective calibration subscribes to %s only; it never publishes /cmd_vel.",
+            self.image_topic,
+        )
+        rospy.loginfo(
+            "Click four marker centres in this order: near-left, near-right, far-right, far-left."
+        )
+
+    @staticmethod
+    def _read_size(name, default):
+        values = rospy.get_param(name, default)
+        if not isinstance(values, (list, tuple)) or len(values) != 2:
+            rospy.logwarn("%s must contain [width, height]; using %s", name, default)
+            values = default
+        return max(1, int(values[0])), max(1, int(values[1]))
+
+    @staticmethod
+    def _read_point(name, default):
+        values = rospy.get_param(name, default)
+        if not isinstance(values, (list, tuple)) or len(values) != 2:
+            rospy.logwarn("%s must contain [x, y]; using %s", name, default)
+            values = default
+        return np.array(values, dtype=np.float32)
+
+    @staticmethod
+    def _read_world_points(name, default):
+        values = rospy.get_param(name, default)
+        valid = isinstance(values, (list, tuple)) and len(values) == 4
+        if valid:
+            valid = all(isinstance(point, (list, tuple)) and len(point) == 2 for point in values)
+        if not valid:
+            rospy.logwarn("%s must contain four [forward_m, left_m] points; using defaults", name)
+            values = default
+        return np.array(values, dtype=np.float32)
+
+    def image_callback(self, message):
+        try:
+            raw = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except CvBridgeError as error:
+            rospy.logwarn_throttle(2.0, "Cannot convert camera image: %s", error)
+            return
+
+        # Calibration values are deliberately recorded in the same 640x480
+        # reference coordinate system as line_follow_debug.yaml.
+        source = cv2.resize(raw, self.reference_size, interpolation=cv2.INTER_AREA)
+        with self.lock:
+            self.source_image = source
+
+    def destination_points(self):
+        """Map vehicle-ground coordinates (forward, left) to a virtual image."""
+        forward = self.marker_positions_m[:, 0]
+        left = self.marker_positions_m[:, 1]
+        return np.column_stack(
+            (
+                self.metric_origin[0] - left * self.metric_scale,
+                self.metric_origin[1] - forward * self.metric_scale,
+            )
+        ).astype(np.float32)
+
+    def result_matrix(self):
+        if len(self.clicked_points) != 4:
+            return None
+        return cv2.getPerspectiveTransform(
+            np.array(self.clicked_points, dtype=np.float32), self.destination_points()
+        )
+
+    def log_result(self):
+        matrix = self.result_matrix()
+        if matrix is None:
+            return
+        source = [[int(x), int(y)] for x, y in self.clicked_points]
+        destination = [[round(float(x), 1), round(float(y), 1)] for x, y in self.destination_points()]
+        self.last_result = (source, destination)
+        rospy.loginfo("=" * 64)
+        rospy.loginfo("METRIC PERSPECTIVE CALIBRATION RESULT")
+        rospy.loginfo("src_pts: %s", source)
+        rospy.loginfo("dst_pts: %s", destination)
+        rospy.loginfo("metric_scale_px_per_m: %.1f", self.metric_scale)
+        rospy.loginfo("metric_origin_px: [%d, %d]", int(self.metric_origin[0]), int(self.metric_origin[1]))
+        rospy.loginfo("Copy only src_pts and dst_pts into line_follow_debug.yaml.")
+        rospy.loginfo("=" * 64)
+
+    def mouse_callback(self, event, x, y, _flags, _userdata):
+        if event == cv2.EVENT_RBUTTONDOWN:
+            self.clicked_points = []
+            self.last_result = None
+            rospy.loginfo("Calibration points cleared.")
+            return
+        if event != cv2.EVENT_LBUTTONDOWN:
+            return
+
+        point = (int(x / self.DISPLAY_SCALE), int(y / self.DISPLAY_SCALE))
+        with self.lock:
+            source = self.source_image
+        if source is None or not (0 <= point[0] < source.shape[1] and 0 <= point[1] < source.shape[0]):
+            return
+        if len(self.clicked_points) >= 4:
+            rospy.logwarn("Four points are already selected. Press R or right-click to start again.")
+            return
+
+        self.clicked_points.append(point)
+        index = len(self.clicked_points) - 1
+        marker = self.marker_positions_m[index]
+        rospy.loginfo(
+            "Point %d/4 (%s): image=[%d, %d], ground=[forward %.3f m, left %.3f m]",
+            index + 1,
+            self.MARKER_NAMES[index],
+            point[0], point[1], marker[0], marker[1],
+        )
+        if len(self.clicked_points) == 4:
+            self.log_result()
+
+    def source_display(self, source):
+        view = source.copy()
+        for index, point in enumerate(self.clicked_points):
+            color = (0, 0, 255) if index < 2 else (0, 255, 255)
+            cv2.circle(view, point, 5, color, -1)
+            cv2.putText(
+                view,
+                "%d %s" % (index + 1, self.MARKER_NAMES[index]),
+                (point[0] + 8, point[1] - 8),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.42,
+                color,
+                1,
+            )
+        if len(self.clicked_points) == 4:
+            cv2.polylines(view, [np.array(self.clicked_points, dtype=np.int32)], True, (0, 255, 255), 1)
+        cv2.putText(view, "RAW CAMERA: CLICK 4 GROUND MARKERS", (10, 28),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+        cv2.putText(view, "1 near-L  2 near-R  3 far-R  4 far-L", (10, 53),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.52, (0, 255, 255), 1)
+        return cv2.resize(
+            view, (view.shape[1] * self.DISPLAY_SCALE, view.shape[0] * self.DISPLAY_SCALE),
+            interpolation=cv2.INTER_LINEAR,
+        )
+
+    def draw_ground_grid(self, image):
+        """Draw a 10 cm vehicle-coordinate grid in the virtual view."""
+        view = image.copy()
+        height, width = view.shape[:2]
+        max_forward = int((self.metric_origin[1] + 1) / self.metric_scale) + 1
+        half_width = int(max(self.metric_origin[0], width - self.metric_origin[0]) / self.metric_scale) + 1
+        for forward_cm in range(0, max_forward * 10 + 1, 10):
+            forward = forward_cm / 10.0
+            y = int(round(self.metric_origin[1] - forward * self.metric_scale))
+            if 0 <= y < height:
+                color = (55, 55, 55) if forward_cm % 50 else (100, 100, 100)
+                cv2.line(view, (0, y), (width - 1, y), color, 1)
+        for left_cm in range(-half_width * 10, half_width * 10 + 1, 10):
+            left = left_cm / 10.0
+            x = int(round(self.metric_origin[0] - left * self.metric_scale))
+            if 0 <= x < width:
+                color = (55, 55, 55) if left_cm % 50 else (100, 100, 100)
+                cv2.line(view, (x, 0), (x, height - 1), color, 1)
+        if 0 <= int(self.metric_origin[0]) < width and 0 <= int(self.metric_origin[1]) < height:
+            cv2.drawMarker(
+                view, tuple(self.metric_origin.astype(int)), (0, 255, 0), cv2.MARKER_CROSS, 14, 2
+            )
+        return view
+
+    def bird_display(self, source):
+        matrix = self.result_matrix()
+        if matrix is None:
+            bird = np.zeros_like(source)
+            text = "Select %d more point(s)" % (4 - len(self.clicked_points))
+            cv2.putText(bird, text, (20, 42), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
+        else:
+            bird = cv2.warpPerspective(source, matrix, self.reference_size, flags=cv2.INTER_LINEAR)
+            bird = self.draw_ground_grid(bird)
+            cv2.putText(bird, "METRIC BIRD VIEW (origin = vehicle front centre)", (10, 28),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.48, (0, 255, 255), 1)
+            cv2.putText(bird, "S: print result  R/right-click: reset  Q/Esc: quit", (10, 50),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.43, (0, 255, 255), 1)
+        return cv2.resize(
+            bird, (bird.shape[1] * self.DISPLAY_SCALE, bird.shape[0] * self.DISPLAY_SCALE),
+            interpolation=cv2.INTER_LINEAR,
+        )
+
+    def run(self):
+        source_window = "Metric perspective calibration: raw camera"
+        bird_window = "Metric perspective calibration: calibrated ground view"
+        cv2.namedWindow(source_window, cv2.WINDOW_AUTOSIZE)
+        cv2.namedWindow(bird_window, cv2.WINDOW_AUTOSIZE)
+        cv2.setMouseCallback(source_window, self.mouse_callback)
+        rate = rospy.Rate(15)
+        while not rospy.is_shutdown():
+            with self.lock:
+                source = None if self.source_image is None else self.source_image.copy()
+            if source is not None:
+                cv2.imshow(source_window, self.source_display(source))
+                cv2.imshow(bird_window, self.bird_display(source))
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("r"), ord("R")):
+                self.clicked_points = []
+                self.last_result = None
+                rospy.loginfo("Calibration points cleared.")
+            elif key in (ord("s"), ord("S")):
+                self.log_result()
+            elif key in (ord("q"), ord("Q"), 27):
+                break
+            rate.sleep()
+        cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    try:
+        CameraOffsetCalibrator().run()
+    except rospy.ROSInterruptException:
+        pass

line_follower/scripts/line_follow.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from cv_bridge import CvBridge
+
+class LineFollower:
+    def __init__(self):
+        rospy.init_node('line_follower')
+        self.bridge = CvBridge()
+        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        self.image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.image_cb)
+        # IPM 变换矩阵（需根据标定与安装角度实测填写）
+        self.M = cv2.getPerspectiveTransform(self.src_pts, self.dst_pts)
+        # PID 参数
+        self.kp = 0.005; self.ki = 0.0; self.kd = 0.002
+        self.last_err = 0.0
+        rospy.spin()
+
+    def image_cb(self, msg):
+        frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+        # 1. 逆透视变换
+        bird = cv2.warpPerspective(frame, self.M, (frame.shape[1], frame.shape[0]))
+        # 2. HSV 白线提取
+        hsv = cv2.cvtColor(bird, cv2.COLOR_BGR2HSV)
+        mask = cv2.inRange(hsv, (0,0,180), (180,60,255))
+        # 3. 形态学去噪
+        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((5,5),np.uint8))
+        # 4. 列投影直方图找右路中线
+        col_sum = np.sum(mask[mask.shape[0]//2:,:], axis=0)
+        # 取最右侧峰作为跟踪目标
+        right_peak = self.find_rightmost_peak(col_sum)
+        # 5. 计算偏移量并 PID
+        err = right_peak - bird.shape[1]/2
+        angular_z = -(self.kp*err + self.kd*(err-self.last_err))
+        self.last_err = err
+        # 6. 发布速度
+        twist = Twist()
+        twist.linear.x = 0.15
+        twist.angular.z = np.clip(angular_z, -0.6, 0.6)
+        self.cmd_pub.publish(twist)
+
+if __name__ == '__main__':
+    LineFollower()

line_follower/scripts/line_follow_control.py

@@ -0,0 +1,284 @@
+#!/usr/bin/env python3
+"""Pure-pursuit controller for the metric IPM lane centreline."""
+
+import math
+import threading
+
+import rospy
+from geometry_msgs.msg import PointStamped, Twist
+from std_msgs.msg import Bool, String
+from std_srvs.srv import SetBool, SetBoolResponse
+
+
+class LineFollowControl:
+    """Publish /cmd_vel only while the explicit enabled safety gate is true."""
+
+    def __init__(self):
+        rospy.init_node("line_follow_control")
+        self.lock = threading.RLock()
+
+        self.lane_valid_topic = rospy.get_param(
+            "~lane_valid_topic", "/line_follow_debug/lane_valid"
+        )
+        self.lookahead_target_topic = rospy.get_param(
+            "~lookahead_target_topic", "/line_follow_debug/lookahead_target"
+        )
+        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/cmd_vel")
+        self.control_rate = max(1.0, float(rospy.get_param("~control_rate", 20.0)))
+        self.linear_speed = max(0.0, float(rospy.get_param("~linear_speed", 0.05)))
+        self.normal_linear_speed = max(
+            0.0,
+            float(rospy.get_param("~normal_linear_speed", self.linear_speed)),
+        )
+        self.after_second_linear_speed = max(
+            0.0,
+            float(
+                rospy.get_param(
+                    "~after_second_linear_speed", self.normal_linear_speed
+                )
+            ),
+        )
+        self.task_state_topic = str(
+            rospy.get_param("~task_state_topic", "/traffic_line_task/state")
+        )
+        self.linear_speed = self.normal_linear_speed
+        self.curvature_gain = max(
+            0.0, float(rospy.get_param("~curvature_gain", 1.0))
+        )
+        self.steering_sign = float(rospy.get_param("~steering_sign", 1.0))
+        self.max_angular_speed = max(
+            0.0, float(rospy.get_param("~max_angular_speed", 0.35))
+        )
+        self.target_alpha = min(
+            1.0, max(0.01, float(rospy.get_param("~target_alpha", 0.45)))
+        )
+        self.lateral_deadband_m = max(
+            0.0, float(rospy.get_param("~lateral_deadband_m", 0.01))
+        )
+        self.min_target_forward_m = max(
+            0.01, float(rospy.get_param("~min_target_forward_m", 0.10))
+        )
+        self.max_target_distance_m = max(
+            self.min_target_forward_m,
+            float(rospy.get_param("~max_target_distance_m", 1.50)),
+        )
+        self.target_timeout = max(
+            0.05, float(rospy.get_param("~target_timeout", 0.50))
+        )
+        self.enabled = self.parameter_is_true(rospy.get_param("~enabled", False))
+
+        self.lane_valid = False
+        self.target_forward = None
+        self.target_left = None
+        self.last_target_time = None
+        self.was_active = False
+
+        self.cmd_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1)
+        self.valid_sub = rospy.Subscriber(
+            self.lane_valid_topic, Bool, self.valid_callback, queue_size=1
+        )
+        self.target_sub = rospy.Subscriber(
+            self.lookahead_target_topic,
+            PointStamped,
+            self.target_callback,
+            queue_size=1,
+        )
+        self.task_state_sub = rospy.Subscriber(
+            self.task_state_topic, String, self.task_state_callback, queue_size=1
+        )
+        self.enable_service = rospy.Service(
+            "~set_enabled", SetBool, self.set_enabled_callback
+        )
+        self.timer = rospy.Timer(
+            rospy.Duration(1.0 / self.control_rate), self.control_callback
+        )
+        rospy.on_shutdown(self.shutdown)
+
+        rospy.loginfo(
+            "Pure-pursuit line controller ready; enabled=%s target=%s.",
+            self.enabled,
+            self.lookahead_target_topic,
+        )
+
+    @staticmethod
+    def parameter_is_true(value):
+        """Avoid treating the string 'false' as truthy."""
+        if isinstance(value, str):
+            return value.strip().lower() in ("1", "true", "yes", "on")
+        return bool(value)
+
+    def valid_callback(self, message):
+        with self.lock:
+            self.lane_valid = bool(message.data)
+
+    def task_state_callback(self, message):
+        requested = (
+            self.after_second_linear_speed
+            if message.data == "FOLLOW_AFTER_SECOND"
+            else self.normal_linear_speed
+        )
+        with self.lock:
+            previous = self.linear_speed
+            self.linear_speed = requested
+        if abs(previous - requested) > 1e-6:
+            rospy.loginfo(
+                "Line-follow speed changed to %.2f m/s for task state %s.",
+                requested,
+                message.data,
+            )
+
+    def set_enabled_callback(self, request):
+        with self.lock:
+            self.enabled = bool(request.data)
+            rospy.set_param("~enabled", self.enabled)
+            if not self.enabled:
+                self.publish_stop()
+                self.was_active = False
+                self.reset_target()
+                rospy.loginfo("Line controller disabled through service.")
+            else:
+                rospy.loginfo("Line controller enabled through service.")
+            return SetBoolResponse(
+                success=True,
+                message="line controller %s"
+                % ("enabled" if self.enabled else "disabled"),
+            )
+
+    def target_callback(self, message):
+        with self.lock:
+            forward = float(message.point.x)
+            left = float(message.point.y)
+            distance = math.hypot(forward, left)
+            if (
+                not math.isfinite(forward)
+                or not math.isfinite(left)
+                or forward < self.min_target_forward_m
+                or distance > self.max_target_distance_m
+            ):
+                rospy.logwarn_throttle(
+                    1.0,
+                    "Rejected lookahead target: forward=%.3f m left=%+.3f m.",
+                    forward,
+                    left,
+                )
+                return
+
+            if self.target_forward is None:
+                self.target_forward = forward
+                self.target_left = left
+            else:
+                alpha = self.target_alpha
+                self.target_forward = (
+                    alpha * forward + (1.0 - alpha) * self.target_forward
+                )
+                self.target_left = alpha * left + (1.0 - alpha) * self.target_left
+            self.last_target_time = rospy.get_time()
+
+    def reset_target(self):
+        self.target_forward = None
+        self.target_left = None
+        self.last_target_time = None
+
+    def publish_stop(self):
+        self.cmd_pub.publish(Twist())
+
+    @staticmethod
+    def pure_pursuit_curvature(forward, left):
+        """Return signed path curvature for a target in base_link."""
+        distance_squared = forward * forward + left * left
+        if distance_squared <= 1e-6:
+            return None
+        return 2.0 * left / distance_squared
+
+    def shutdown(self):
+        with self.lock:
+            self.publish_stop()
+
+    def control_callback(self, _event):
+        with self.lock:
+            self._control_locked()
+
+    def _control_locked(self):
+        # Keep the old rosparam workflow working for manual tests while the
+        # task state machine uses the SetBool service.
+        parameter_enabled = self.parameter_is_true(
+            rospy.get_param("~enabled", self.enabled)
+        )
+        if parameter_enabled != self.enabled:
+            self.enabled = parameter_enabled
+        if not self.enabled:
+            if self.was_active:
+                self.publish_stop()
+                rospy.loginfo(
+                    "Line controller disabled; published a zero-velocity command."
+                )
+            self.was_active = False
+            self.reset_target()
+            return
+
+        now = rospy.get_time()
+        target_fresh = (
+            self.last_target_time is not None
+            and (now - self.last_target_time) <= self.target_timeout
+        )
+        if not self.lane_valid or not target_fresh:
+            self.publish_stop()
+            rospy.logwarn_throttle(
+                1.0,
+                "Pure-pursuit safety stop: lane_valid=%s target_fresh=%s.",
+                self.lane_valid,
+                target_fresh,
+            )
+            self.was_active = False
+            return
+
+        forward = self.target_forward
+        left = self.target_left
+        if abs(left) <= self.lateral_deadband_m:
+            left = 0.0
+        curvature = self.pure_pursuit_curvature(forward, left)
+        if curvature is None:
+            self.publish_stop()
+            rospy.logerr("Pure-pursuit safety stop: target distance is zero.")
+            self.was_active = False
+            return
+
+        # In base_link, +x is forward and +y is left. Pure pursuit for a
+        # unicycle gives curvature=2*y/L^2 and angular velocity=v*curvature.
+        angular = (
+            self.steering_sign
+            * self.curvature_gain
+            * self.linear_speed
+            * curvature
+        )
+        angular = max(
+            -self.max_angular_speed, min(self.max_angular_speed, angular)
+        )
+        if not math.isfinite(angular):
+            self.publish_stop()
+            rospy.logerr("Pure-pursuit safety stop: non-finite angular velocity.")
+            self.was_active = False
+            return
+
+        command = Twist()
+        command.linear.x = self.linear_speed
+        command.angular.z = angular
+        self.cmd_pub.publish(command)
+        self.was_active = True
+        rospy.loginfo_throttle(
+            1.0,
+            "Pure pursuit: target=(%.3f m,%+.3f m left) curvature=%+.3f 1/m angular=%+.3f rad/s linear=%.2f m/s.",
+            forward,
+            left,
+            curvature,
+            angular,
+            self.linear_speed,
+        )
+
+
+if __name__ == "__main__":
+    try:
+        LineFollowControl()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

line_follower/scripts/line_follow_debug.py

@@ -0,0 +1,775 @@
+#!/usr/bin/env python3
+"""Visualise white-line candidates without ever commanding the vehicle."""
+
+import math
+import threading
+
+import cv2
+import numpy as np
+import rospy
+from cv_bridge import CvBridge, CvBridgeError
+from geometry_msgs.msg import PointStamped
+from sensor_msgs.msg import Image
+from std_msgs.msg import Bool, Float32, String
+
+
+class LineFollowDebug:
+    """Subscriber-only line visualiser used to tune the real route safely."""
+
+    def __init__(self):
+        rospy.init_node("line_follow_debug")
+        self.image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
+        self.lower = self._read_hsv("~hsv_lower", [0, 0, 180])
+        self.upper = self._read_hsv("~hsv_upper", [180, 60, 255])
+        self.use_perspective_transform = bool(
+            rospy.get_param("~use_perspective_transform", True)
+        )
+        self.perspective_reference_size = self._read_size(
+            "~perspective_reference_size", [640, 480]
+        )
+        self.src_points = self._read_points(
+            "~src_pts", [[120, 205], [520, 205], [639, 479], [0, 479]]
+        )
+        self.dst_points = self._read_points(
+            "~dst_pts", [[100, 200], [540, 200], [540, 479], [100, 479]]
+        )
+        self.processing_scale = self._read_ratio("~processing_scale", 0.5)
+        self.processing_scale = max(0.25, self.processing_scale)
+        self.display_rate = max(1.0, float(rospy.get_param("~display_rate", 10.0)))
+        self.use_camera_front_offset = self._read_bool("~use_camera_front_offset", True)
+        self.camera_to_front_offset_m = max(
+            0.0, float(rospy.get_param("~camera_to_front_offset_m", 0.125))
+        )
+        self.metric_scale_px_per_m = max(
+            1.0, float(rospy.get_param("~metric_scale_px_per_m", 400.0))
+        )
+        self.metric_origin_px = self._read_metric_origin(
+            "~metric_origin_px", [320.0, 480.0]
+        )
+        self.lookahead_distance_m = max(
+            0.05, float(rospy.get_param("~lookahead_distance_m", 0.50))
+        )
+        self.normal_lookahead_distance_m = max(
+            0.05,
+            float(
+                rospy.get_param(
+                    "~normal_lookahead_distance_m", self.lookahead_distance_m
+                )
+            ),
+        )
+        self.after_second_lookahead_distance_m = max(
+            0.05,
+            float(
+                rospy.get_param(
+                    "~after_second_lookahead_distance_m",
+                    self.normal_lookahead_distance_m,
+                )
+            ),
+        )
+        self.after_second_inner_offset_m = max(
+            0.0, float(rospy.get_param("~after_second_inner_offset_m", 0.025))
+        )
+        self.inner_offset_activation_m = max(
+            0.0, float(rospy.get_param("~inner_offset_activation_m", 0.015))
+        )
+        self.task_state_topic = str(
+            rospy.get_param("~task_state_topic", "/traffic_line_task/state")
+        )
+        self.ipm_origin_ahead_of_control_m = max(
+            0.0, float(rospy.get_param("~ipm_origin_ahead_of_control_m", 0.125))
+        )
+        self.max_near_target_extrapolation_m = max(
+            0.0,
+            float(rospy.get_param("~max_near_target_extrapolation_m", 0.25)),
+        )
+        self.lookahead_frame_id = str(
+            rospy.get_param("~lookahead_frame_id", "base_link")
+        )
+        self.camera_front_offset_px = max(
+            0, int(rospy.get_param("~camera_front_offset_px", 0))
+        )
+        self.roi_top_ratio = self._read_ratio("~roi_top_ratio", 0.52)
+        self.scanline_ratios = self._read_ratios(
+            "~scanline_ratios", [0.70, 0.66, 0.62, 0.58, 0.54, 0.50, 0.46]
+        )
+        self.scanline_half_height = max(1, int(rospy.get_param("~scanline_half_height", 5)))
+        self.min_run_width = max(1, int(rospy.get_param("~min_run_width", 3)))
+        self.min_component_area = max(1, int(rospy.get_param("~min_component_area", 30)))
+        self.use_component_filter = self._read_bool("~use_component_filter", False)
+        self.center_reflection_half_width_ratio = self._read_ratio(
+            "~center_reflection_half_width_ratio", 0.12
+        )
+        self.min_lane_width_ratio = self._read_ratio("~min_lane_width_ratio", 0.25)
+        self.max_lane_width_ratio = self._read_ratio("~max_lane_width_ratio", 0.98)
+        self.use_single_boundary_fallback = self._read_bool(
+            "~use_single_boundary_fallback", True
+        )
+        self.single_boundary_timeout = max(
+            0.1, float(rospy.get_param("~single_boundary_timeout", 1.5))
+        )
+        self.single_boundary_side_memory_timeout = max(
+            0.1,
+            float(rospy.get_param("~single_boundary_side_memory_timeout", 0.75)),
+        )
+        self.lane_width_alpha = min(
+            1.0, max(0.01, float(rospy.get_param("~lane_width_alpha", 0.2)))
+        )
+        self.max_centerline_heading_rad = max(
+            0.05, float(rospy.get_param("~max_centerline_heading_rad", 0.70))
+        )
+        kernel_size = max(1, int(rospy.get_param("~morphology_kernel", 3)))
+        if kernel_size % 2 == 0:
+            kernel_size += 1
+        self.kernel = np.ones((kernel_size, kernel_size), dtype=np.uint8)
+
+        self.bridge = CvBridge()
+        self.lock = threading.Lock()
+        self.display_image = None
+        self.filtered_lane_width_px = None
+        self.last_two_boundary_time = None
+        self.single_boundary_side = None
+        self.last_single_boundary_time = None
+        self.after_second_active = False
+        self.lane_error_pub = rospy.Publisher("~lane_error", Float32, queue_size=1)
+        self.lane_heading_pub = rospy.Publisher("~lane_heading_error", Float32, queue_size=1)
+        self.lane_valid_pub = rospy.Publisher("~lane_valid", Bool, queue_size=1)
+        self.lookahead_target_pub = rospy.Publisher(
+            "~lookahead_target", PointStamped, queue_size=1
+        )
+        self.subscriber = rospy.Subscriber(
+            self.image_topic, Image, self.image_callback, queue_size=1
+        )
+        self.task_state_subscriber = rospy.Subscriber(
+            self.task_state_topic, String, self.task_state_callback, queue_size=1
+        )
+        rospy.loginfo(
+            "Line-follow debug is visualisation only; IPM=%s, subscribed to %s and will not publish /cmd_vel.",
+            self.use_perspective_transform,
+            self.image_topic,
+        )
+
+    def task_state_callback(self, message):
+        after_second = message.data == "FOLLOW_AFTER_SECOND"
+        requested = (
+            self.after_second_lookahead_distance_m
+            if after_second
+            else self.normal_lookahead_distance_m
+        )
+        with self.lock:
+            previous = self.lookahead_distance_m
+            self.lookahead_distance_m = requested
+            self.after_second_active = after_second
+        if abs(previous - requested) > 1e-6:
+            rospy.loginfo(
+                "Line-follow lookahead changed to %.2f m for task state %s.",
+                requested,
+                message.data,
+            )
+
+    def apply_after_second_inner_offset(
+        self, target_left, target_x, metric_scale
+    ):
+        """Move a curved-route target toward its inside only after turn two."""
+        if (
+            not self.after_second_active
+            or abs(target_left) < self.inner_offset_activation_m
+            or self.after_second_inner_offset_m <= 0.0
+        ):
+            return target_left, target_x
+        direction = 1.0 if target_left > 0.0 else -1.0
+        offset = direction * self.after_second_inner_offset_m
+        # base_link +left maps to decreasing IPM image x.
+        return target_left + offset, target_x - offset * metric_scale
+
+    @staticmethod
+    def _read_ratio(name, default):
+        return float(max(0.0, min(1.0, rospy.get_param(name, default))))
+
+    @staticmethod
+    def _read_ratios(name, default):
+        values = rospy.get_param(name, default)
+        if not isinstance(values, (list, tuple)) or not values:
+            rospy.logwarn("%s must be a non-empty list; using %s", name, default)
+            values = default
+        return sorted(
+            [float(max(0.0, min(1.0, value))) for value in values], reverse=True
+        )
+
+    @staticmethod
+    def _read_hsv(name, default):
+        values = rospy.get_param(name, default)
+        if not isinstance(values, (list, tuple)) or len(values) != 3:
+            rospy.logwarn("%s must have three values; using %s", name, default)
+            values = default
+        values = [int(max(0, min(255, value))) for value in values]
+        values[0] = min(180, values[0])
+        return np.array(values, dtype=np.uint8)
+
+    @staticmethod
+    def _read_size(name, default):
+        values = rospy.get_param(name, default)
+        if not isinstance(values, (list, tuple)) or len(values) != 2:
+            rospy.logwarn("%s must contain [width, height]; using %s", name, default)
+            values = default
+        return max(1, int(values[0])), max(1, int(values[1]))
+
+    @staticmethod
+    def _read_metric_origin(name, default):
+        values = rospy.get_param(name, default)
+        if not isinstance(values, (list, tuple)) or len(values) != 2:
+            rospy.logwarn("%s must contain [x, y]; using %s", name, default)
+            values = default
+        return float(values[0]), float(values[1])
+
+    @staticmethod
+    def _read_bool(name, default):
+        value = rospy.get_param(name, default)
+        if isinstance(value, str):
+            return value.strip().lower() in ("1", "true", "yes", "on")
+        return bool(value)
+
+    @staticmethod
+    def _read_points(name, default):
+        values = rospy.get_param(name, default)
+        valid = isinstance(values, (list, tuple)) and len(values) == 4
+        if valid:
+            valid = all(isinstance(point, (list, tuple)) and len(point) == 2 for point in values)
+        if not valid:
+            rospy.logwarn("%s must contain four [x, y] points; using defaults", name)
+            values = default
+        return np.array(values, dtype=np.float32)
+
+    def image_callback(self, message):
+        try:
+            image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except CvBridgeError as error:
+            rospy.logwarn_throttle(2.0, "Cannot convert camera image: %s", error)
+            return
+        # Perception and lane publication run here at camera speed.  OpenCV GUI
+        # display is deliberately kept out of this callback.
+        display_image = self.make_display(image)
+        with self.lock:
+            self.display_image = display_image
+
+    @staticmethod
+    def white_runs(scanline, min_width):
+        """Return contiguous white x-ranges from a horizontal mask scanline."""
+        active = scanline > 0
+        padded = np.pad(active.astype(np.int8), (1, 1), mode="constant")
+        changes = np.flatnonzero(np.diff(padded))
+        runs = []
+        for start, end in zip(changes[0::2], changes[1::2]):
+            if end - start >= min_width:
+                runs.append((int(start), int(end - 1)))
+        return runs
+
+    def scan_data_at(self, mask, roi_top, scan_y):
+        """Return white runs split around the vehicle centre for one band."""
+        height, width = mask.shape
+        centre_x = width // 2
+        exclusion_half_width = int(width * self.center_reflection_half_width_ratio)
+        exclusion_left = centre_x - exclusion_half_width
+        exclusion_right = centre_x + exclusion_half_width
+
+        scan_y = min(height - 1, max(roi_top, int(scan_y)))
+        band_top = max(roi_top, scan_y - self.scanline_half_height)
+        band_bottom = min(height, scan_y + self.scanline_half_height + 1)
+        scanline = np.max(mask[band_top:band_bottom, :], axis=0)
+        runs = self.white_runs(scanline, self.min_run_width)
+
+        left_runs = [run for run in runs if run[1] < exclusion_left]
+        right_runs = [run for run in runs if run[0] > exclusion_right]
+        return (
+            scan_y, band_top, band_bottom, runs, left_runs, right_runs,
+            exclusion_left, exclusion_right,
+        )
+
+    def lane_pair_at(self, mask, roi_top, scan_y):
+        """Find one valid left/right boundary pair in a given horizontal band."""
+        width = mask.shape[1]
+        min_lane_width = int(width * self.min_lane_width_ratio)
+        max_lane_width = int(width * self.max_lane_width_ratio)
+        data = self.scan_data_at(mask, roi_top, scan_y)
+        (
+            scan_y, band_top, band_bottom, runs, left_runs, right_runs,
+            exclusion_left, exclusion_right,
+        ) = data
+
+        # A central highlight may be white in HSV, but it cannot become a
+        # lane boundary.  Keep only runs completely outside its band.
+        if not left_runs or not right_runs:
+            return None
+        left = max(left_runs, key=lambda run: run[1])
+        right = min(right_runs, key=lambda run: run[0])
+        left_x = (left[0] + left[1]) // 2
+        right_x = (right[0] + right[1]) // 2
+        lane_width = right_x - left_x
+        if min_lane_width <= lane_width <= max_lane_width:
+            return scan_y, band_top, band_bottom, runs, left, right, exclusion_left, exclusion_right
+
+        return None
+
+    def single_boundary_at(self, mask, roi_top, scan_y):
+        """Infer the lane midpoint from one boundary and recent measured width."""
+        if self.filtered_lane_width_px is None or self.last_two_boundary_time is None:
+            return None
+        if rospy.get_time() - self.last_two_boundary_time > self.single_boundary_timeout:
+            return None
+
+        width = mask.shape[1]
+        data = self.scan_data_at(mask, roi_top, scan_y)
+        (
+            scan_y, band_top, band_bottom, runs, left_runs, right_runs,
+            _exclusion_left, _exclusion_right,
+        ) = data
+
+        lane_width = float(self.filtered_lane_width_px)
+        now = rospy.get_time()
+        remembered_side = None
+        if (
+            self.single_boundary_side in ("LEFT", "RIGHT")
+            and self.last_single_boundary_time is not None
+            and now - self.last_single_boundary_time
+            <= self.single_boundary_side_memory_timeout
+        ):
+            remembered_side = self.single_boundary_side
+
+        if remembered_side is not None:
+            # During a bend the same physical outer boundary can cross the
+            # image centre. Do not relabel it merely because x changed sides.
+            if not runs:
+                return None
+            if left_runs and right_runs:
+                # Two distinct sides that fail the lane-width check are still
+                # ambiguous; side memory must not turn them into one boundary.
+                return None
+            if remembered_side == "LEFT":
+                boundary = min(runs, key=lambda run: run[0] + run[1])
+                visible_side = "LEFT"
+            else:
+                boundary = max(runs, key=lambda run: run[0] + run[1])
+                visible_side = "RIGHT"
+        else:
+            # At fallback entry exactly one image side must be visible. This
+            # establishes the physical side identity used by later frames.
+            if bool(left_runs) == bool(right_runs):
+                return None
+            if left_runs:
+                boundary = max(left_runs, key=lambda run: run[1])
+                visible_side = "LEFT"
+            else:
+                boundary = min(right_runs, key=lambda run: run[0])
+                visible_side = "RIGHT"
+
+        boundary_x = (boundary[0] + boundary[1]) // 2
+        if visible_side == "LEFT":
+            target_x = int(round(boundary_x + 0.5 * lane_width))
+        else:
+            target_x = int(round(boundary_x - 0.5 * lane_width))
+
+        if not (0 <= target_x < width):
+            return None
+        return scan_y, band_top, band_bottom, runs, boundary, boundary_x, target_x, visible_side
+
+    def select_lane_pair(self, mask, roi_top):
+        """Find the nearest valid left/right pair, rejecting LED reflections."""
+        height, _ = mask.shape
+        for ratio in self.scanline_ratios:
+            selection = self.lane_pair_at(mask, roi_top, int(height * ratio))
+            if selection is not None:
+                return selection
+        return None
+
+    def select_single_boundary(self, mask, roi_top, front_offset_px):
+        """Prefer the compensated near band, then retry the ordinary bands."""
+        height, _ = mask.shape
+        offsets = [front_offset_px]
+        if front_offset_px != 0:
+            offsets.append(0)
+        for offset in offsets:
+            for ratio in self.scanline_ratios:
+                selection = self.single_boundary_at(
+                    mask, roi_top, int(height * ratio) + offset
+                )
+                if selection is not None:
+                    return selection
+        return None
+
+    def front_offset_for_width(self, width):
+        """Convert the configured physical nose margin to processing pixels."""
+        if not self.use_camera_front_offset:
+            return 0
+        reference_width = float(self.perspective_reference_size[0])
+        if self.camera_front_offset_px > 0:
+            reference_offset_px = self.camera_front_offset_px
+        else:
+            reference_offset_px = self.camera_to_front_offset_m * self.metric_scale_px_per_m
+        return int(round(reference_offset_px * width / reference_width))
+
+    def centerline_points(self, mask, roi_top, front_offset_px):
+        """Collect lane-centre samples from several near/far ground bands."""
+        height, _ = mask.shape
+        points_by_y = {}
+        offsets = [front_offset_px]
+        if front_offset_px != 0:
+            offsets.append(0)
+
+        for offset in offsets:
+            for ratio in self.scanline_ratios:
+                requested_y = int(height * ratio) + offset
+                pair = self.lane_pair_at(mask, roi_top, requested_y)
+                if pair is not None:
+                    scan_y, _top, _bottom, _runs, left, right, _el, _er = pair
+                    left_x = (left[0] + left[1]) // 2
+                    right_x = (right[0] + right[1]) // 2
+                    points_by_y[scan_y] = (left_x + right_x) // 2
+                    continue
+
+                if self.use_single_boundary_fallback:
+                    single = self.single_boundary_at(mask, roi_top, requested_y)
+                    if single is not None:
+                        scan_y = single[0]
+                        points_by_y[scan_y] = single[6]
+
+            # Compensated samples are preferred.  Use ordinary bands only if
+            # the near set alone cannot describe a curve.
+            if len(points_by_y) >= 3:
+                break
+
+        return sorted(
+            [(int(x), int(y)) for y, x in points_by_y.items()],
+            key=lambda point: point[1],
+        )
+
+    def fit_centerline(self, points):
+        """Fit x(y), returning drawable curve, heading, and polynomial."""
+        if len(points) < 2:
+            return points, 0.0, None
+
+        xs = np.array([point[0] for point in points], dtype=np.float64)
+        ys = np.array([point[1] for point in points], dtype=np.float64)
+        degree = 2 if len(points) >= 3 else 1
+        coefficients = np.polyfit(ys, xs, degree)
+        sample_ys = np.linspace(float(np.min(ys)), float(np.max(ys)), 30)
+        sample_xs = np.polyval(coefficients, sample_ys)
+        curve = [
+            (int(round(x)), int(round(y)))
+            for x, y in zip(sample_xs, sample_ys)
+            if np.isfinite(x) and np.isfinite(y)
+        ]
+
+        heading_y = 0.5 * (float(np.min(ys)) + float(np.max(ys)))
+        derivative = np.polyval(np.polyder(coefficients), heading_y)
+        # Image x grows to physical right, while forward grows toward smaller
+        # image y.  Positive heading therefore means a right-hand curve.
+        heading = float(np.arctan(-derivative))
+        heading = max(
+            -self.max_centerline_heading_rad,
+            min(self.max_centerline_heading_rad, heading),
+        )
+        return curve, heading, coefficients
+
+    def lookahead_target_from_fit(self, coefficients, center_samples, frame_shape):
+        """Convert a fitted IPM centreline into a metric base-frame target."""
+        if coefficients is None or len(center_samples) < 2:
+            return None
+
+        height, width = frame_shape[:2]
+        reference_width, reference_height = self.perspective_reference_size
+        scale_x = width / float(reference_width)
+        scale_y = height / float(reference_height)
+        # The metric IPM calibration uses the same scale in both directions.
+        metric_scale = self.metric_scale_px_per_m * scale_x
+        if metric_scale <= 0.0:
+            return None
+
+        origin_x = self.metric_origin_px[0] * scale_x
+        origin_y = self.metric_origin_px[1] * scale_y
+        forward_from_ipm_origin = max(
+            0.0, self.lookahead_distance_m - self.ipm_origin_ahead_of_control_m
+        )
+        requested_y = origin_y - forward_from_ipm_origin * metric_scale
+
+        sample_ys = [float(point[1]) for point in center_samples]
+        far_y = min(sample_ys)
+        near_y = max(sample_ys)
+        max_near_y = min(
+            float(height - 1),
+            near_y + self.max_near_target_extrapolation_m * metric_scale,
+        )
+        target_y = min(max(requested_y, far_y), max_near_y)
+        if target_y > near_y:
+            # Continue only the nearest fitted tangent toward the vehicle.
+            # Direct quadratic extrapolation can grow rapidly and select a
+            # false branch at a junction.
+            near_x = float(np.polyval(coefficients, near_y))
+            near_slope = float(np.polyval(np.polyder(coefficients), near_y))
+            max_slope = math.tan(self.max_centerline_heading_rad)
+            near_slope = max(-max_slope, min(max_slope, near_slope))
+            target_x = near_x + near_slope * (target_y - near_y)
+        else:
+            target_x = float(np.polyval(coefficients, target_y))
+        target_forward = (
+            self.ipm_origin_ahead_of_control_m
+            + (origin_y - target_y) / metric_scale
+        )
+        target_left = -(target_x - origin_x) / metric_scale
+        target_left, target_x = self.apply_after_second_inner_offset(
+            target_left, target_x, metric_scale
+        )
+
+        values = (target_forward, target_left, target_x, target_y)
+        if not all(np.isfinite(value) for value in values) or target_forward <= 0.0:
+            return None
+        return values
+
+    def remove_small_components(self, mask):
+        """Keep only connected white regions that can plausibly be route lines."""
+        count, labels, stats, _ = cv2.connectedComponentsWithStats(mask, connectivity=8)
+        # Vectorised lookup: the old per-component loop compared every label
+        # against the full image and reduced the real-time rate to ~2 Hz.
+        keep = stats[:, cv2.CC_STAT_AREA] >= self.min_component_area
+        keep[0] = False  # Label 0 is the black background.
+        return (keep[labels].astype(np.uint8) * 255)
+
+    def scaled_perspective_points(self, frame_shape):
+        """Return source and destination IPM points for the current resolution."""
+        height, width = frame_shape[:2]
+        reference_width, reference_height = self.perspective_reference_size
+        scale = np.array([width / float(reference_width), height / float(reference_height)], dtype=np.float32)
+        src = self.src_points * scale
+        dst = self.dst_points * scale
+        return src, dst
+
+    def perspective_warp(self, frame):
+        """Warp the configured ground trapezoid to a bird's-eye rectangle."""
+        height, width = frame.shape[:2]
+        src, dst = self.scaled_perspective_points(frame.shape)
+        matrix = cv2.getPerspectiveTransform(src, dst)
+        bird = cv2.warpPerspective(frame, matrix, (width, height), flags=cv2.INTER_LINEAR)
+        return bird, src.astype(np.int32)
+
+    def make_line_views(self, frame):
+        height, width = frame.shape[:2]
+        roi_top = int(height * self.roi_top_ratio)
+        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+        mask = cv2.inRange(hsv, self.lower, self.upper)
+        mask[:roi_top, :] = 0
+        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, self.kernel)
+        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel)
+        if self.use_component_filter:
+            mask = self.remove_small_components(mask)
+
+        centre_x = width // 2
+        exclusion_half_width = int(width * self.center_reflection_half_width_ratio)
+        exclusion_left = centre_x - exclusion_half_width
+        exclusion_right = centre_x + exclusion_half_width
+        # LED glare is not a route marking.  It is removed from the binary
+        # image, rather than merely being rejected after candidate selection.
+        mask[roi_top:, exclusion_left:exclusion_right + 1] = 0
+        selection = self.select_lane_pair(mask, roi_top)
+        front_offset_px = self.front_offset_for_width(width)
+        if selection is not None and self.use_camera_front_offset:
+            initial_scan_y = selection[0]
+            # Larger bird-view y is closer to the vehicle.  Move the control
+            # band toward the nose by the requested physical distance.
+            compensated = self.lane_pair_at(mask, roi_top, initial_scan_y + front_offset_px)
+            if compensated is not None:
+                selection = compensated
+
+        single_selection = None
+        if selection is None and self.use_single_boundary_fallback:
+            single_selection = self.select_single_boundary(mask, roi_top, front_offset_px)
+
+        annotated = frame.copy()
+        cv2.rectangle(annotated, (0, roi_top), (width - 1, height - 1), (0, 255, 255), 2)
+        cv2.line(annotated, (centre_x, roi_top), (centre_x, height - 1), (120, 120, 120), 1)
+        overlay = annotated.copy()
+        cv2.rectangle(overlay, (exclusion_left, roi_top), (exclusion_right, height - 1), (0, 0, 0), -1)
+        annotated = cv2.addWeighted(overlay, 0.25, annotated, 0.75, 0)
+        cv2.rectangle(annotated, (exclusion_left, roi_top), (exclusion_right, height - 1), (90, 90, 90), 1)
+        state = "NO LANE BOUNDARIES"
+        scan_y = None
+        lane_result_valid = False
+        if selection is not None:
+            scan_y, band_top, band_bottom, runs, left, right, exclusion_left, exclusion_right = selection
+            cv2.rectangle(annotated, (0, band_top), (width - 1, band_bottom - 1), (255, 255, 0), 1)
+            for start, end in runs:
+                cv2.line(annotated, (start, scan_y), (end, scan_y), (0, 165, 255), 4)
+            cv2.circle(annotated, ((left[0] + left[1]) // 2, scan_y), 7, (255, 0, 0), -1)
+            cv2.circle(annotated, ((right[0] + right[1]) // 2, scan_y), 7, (0, 0, 255), -1)
+            left_x = (left[0] + left[1]) // 2
+            right_x = (right[0] + right[1]) // 2
+            measured_lane_width = float(right_x - left_x)
+            if self.filtered_lane_width_px is None:
+                self.filtered_lane_width_px = measured_lane_width
+            else:
+                self.filtered_lane_width_px = (
+                    self.lane_width_alpha * measured_lane_width
+                    + (1.0 - self.lane_width_alpha) * self.filtered_lane_width_px
+                )
+            self.last_two_boundary_time = rospy.get_time()
+            self.single_boundary_side = None
+            self.last_single_boundary_time = None
+            target_x = (left_x + right_x) // 2
+            # The controller keeps the 640-pixel error convention even while
+            # this node processes a smaller real-time image.
+            error = (target_x - centre_x) * (self.perspective_reference_size[0] / float(width))
+            state = "LANE MIDPOINT x=%d error=%+.0f px scan=%d offset=%d" % (
+                target_x, error, scan_y, front_offset_px
+            )
+            lane_result_valid = True
+            self.lane_valid_pub.publish(Bool(data=True))
+            self.lane_error_pub.publish(Float32(data=float(error)))
+        elif single_selection is not None:
+            (
+                scan_y, band_top, band_bottom, runs, boundary, boundary_x,
+                target_x, visible_side,
+            ) = single_selection
+            cv2.rectangle(annotated, (0, band_top), (width - 1, band_bottom - 1), (255, 0, 255), 1)
+            cv2.line(annotated, (boundary[0], scan_y), (boundary[1], scan_y), (0, 165, 255), 4)
+            cv2.circle(annotated, (boundary_x, scan_y), 7, (0, 0, 255), -1)
+            cv2.circle(annotated, (target_x, scan_y), 7, (255, 0, 255), -1)
+            error = (target_x - centre_x) * (
+                self.perspective_reference_size[0] / float(width)
+            )
+            age = rospy.get_time() - self.last_two_boundary_time
+            self.single_boundary_side = visible_side
+            self.last_single_boundary_time = rospy.get_time()
+            state = "ONE %s BOUNDARY midpoint x=%d error=%+.0f px age=%.1fs" % (
+                visible_side, target_x, error, age
+            )
+            lane_result_valid = True
+            self.lane_valid_pub.publish(Bool(data=True))
+            self.lane_error_pub.publish(Float32(data=float(error)))
+        else:
+            if (
+                self.last_single_boundary_time is not None
+                and rospy.get_time() - self.last_single_boundary_time
+                > self.single_boundary_side_memory_timeout
+            ):
+                self.single_boundary_side = None
+                self.last_single_boundary_time = None
+            self.lane_valid_pub.publish(Bool(data=False))
+
+        if lane_result_valid:
+            center_samples = self.centerline_points(mask, roi_top, front_offset_px)
+            center_curve, heading_error, coefficients = self.fit_centerline(center_samples)
+            if len(center_curve) >= 2:
+                cv2.polylines(
+                    annotated,
+                    [np.array(center_curve, dtype=np.int32)],
+                    False,
+                    (0, 255, 0),
+                    2,
+                )
+            for sample_x, sample_y in center_samples:
+                cv2.circle(annotated, (sample_x, sample_y), 3, (0, 255, 0), -1)
+            self.lane_heading_pub.publish(Float32(data=heading_error))
+            state += " heading=%+.1fdeg" % np.degrees(heading_error)
+            target = self.lookahead_target_from_fit(
+                coefficients, center_samples, frame.shape
+            )
+            if target is not None:
+                target_forward, target_left, target_x, target_y = target
+                message = PointStamped()
+                message.header.stamp = rospy.Time.now()
+                message.header.frame_id = self.lookahead_frame_id
+                message.point.x = target_forward
+                message.point.y = target_left
+                message.point.z = 0.0
+                self.lookahead_target_pub.publish(message)
+                cv2.circle(
+                    annotated,
+                    (int(round(target_x)), int(round(target_y))),
+                    8,
+                    (255, 0, 255),
+                    -1,
+                )
+                cv2.line(
+                    annotated,
+                    (centre_x, height - 1),
+                    (int(round(target_x)), int(round(target_y))),
+                    (255, 0, 255),
+                    2,
+                )
+                state += " target=(%.2fm,%+.2fm left)" % (
+                    target_forward,
+                    target_left,
+                )
+
+        cv2.putText(annotated, "DEBUG ONLY - NO /cmd_vel", (12, 28),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
+        cv2.putText(annotated, state, (12, 56),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2)
+        scan_label = "none" if scan_y is None else str(scan_y)
+        cv2.putText(annotated, "ROI y=%d..%d  selected scan y=%s" % (roi_top, height - 1, scan_label),
+                    (12, height - 14), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 255), 2)
+
+        mask_view = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
+        cv2.putText(mask_view, "WHITE-LINE MASK (ground ROI only)", (12, 28),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2)
+        if scan_y is not None:
+            cv2.line(mask_view, (0, scan_y), (width - 1, scan_y), (255, 255, 0), 1)
+        cv2.rectangle(mask_view, (exclusion_left, roi_top), (exclusion_right, height - 1), (90, 90, 90), 1)
+        return annotated, mask_view
+
+    def make_display(self, raw_frame):
+        display_height, display_width = raw_frame.shape[:2]
+        processing_frame = raw_frame
+        if self.processing_scale < 1.0:
+            processing_size = (
+                max(1, int(display_width * self.processing_scale)),
+                max(1, int(display_height * self.processing_scale)),
+            )
+            processing_frame = cv2.resize(raw_frame, processing_size, interpolation=cv2.INTER_AREA)
+
+        if not self.use_perspective_transform:
+            annotated, mask_view = self.make_line_views(processing_frame)
+            if processing_frame.shape != raw_frame.shape:
+                annotated = cv2.resize(annotated, (display_width, display_height), interpolation=cv2.INTER_LINEAR)
+                mask_view = cv2.resize(mask_view, (display_width, display_height), interpolation=cv2.INTER_NEAREST)
+            return np.hstack((annotated, mask_view))
+
+        bird_frame, _ = self.perspective_warp(processing_frame)
+        source_points, _ = self.scaled_perspective_points(raw_frame.shape)
+        source_points = source_points.astype(np.int32)
+        source_view = raw_frame.copy()
+        cv2.polylines(source_view, [source_points], True, (0, 255, 255), 2)
+        for index, point in enumerate(source_points):
+            point_xy = tuple(int(value) for value in point)
+            cv2.circle(source_view, point_xy, 5, (0, 0, 255), -1)
+            cv2.putText(source_view, str(index + 1), (point_xy[0] + 6, point_xy[1] - 6),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 255), 2)
+        cv2.putText(source_view, "SOURCE: IPM GROUND TRAPEZOID", (12, 28),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
+        annotated, mask_view = self.make_line_views(bird_frame)
+        if processing_frame.shape != raw_frame.shape:
+            annotated = cv2.resize(annotated, (display_width, display_height), interpolation=cv2.INTER_LINEAR)
+            mask_view = cv2.resize(mask_view, (display_width, display_height), interpolation=cv2.INTER_NEAREST)
+        cv2.putText(annotated, "BIRD'S-EYE VIEW", (12, 82),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
+        return np.hstack((source_view, annotated, mask_view))
+
+    def run(self):
+        window = "Line following debug (Q/Esc: quit)"
+        cv2.namedWindow(window, cv2.WINDOW_NORMAL)
+        cv2.resizeWindow(window, 1920, 480)
+        rate = rospy.Rate(self.display_rate)
+        while not rospy.is_shutdown():
+            with self.lock:
+                display_image = self.display_image
+            if display_image is not None:
+                cv2.imshow(window, display_image)
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("q"), ord("Q"), 27):
+                break
+            rate.sleep()
+        cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    try:
+        LineFollowDebug().run()
+    except rospy.ROSInterruptException:
+        pass

line_follower/scripts/snapshot_capture.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python3
+"""Capture paired camera and white-line-mask screenshots from an existing ROS topic."""
+
+from pathlib import Path
+import threading
+import time
+
+import cv2
+import numpy as np
+import rospy
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+
+
+class SnapshotCapture:
+    """A subscriber-only capture tool: it never opens /dev/video* itself."""
+
+    def __init__(self):
+        rospy.init_node("line_snapshot_capture")
+
+        self.image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
+        default_root = Path(__file__).resolve().parent.parent / "data"
+        self.save_root = Path(rospy.get_param("~save_root", str(default_root))).expanduser()
+        self.lower = self._read_hsv("~hsv_lower", [0, 0, 180])
+        self.upper = self._read_hsv("~hsv_upper", [180, 60, 255])
+
+        self.raw_dir = self.save_root / "raw"
+        self.mask_dir = self.save_root / "mask"
+        self.raw_dir.mkdir(parents=True, exist_ok=True)
+        self.mask_dir.mkdir(parents=True, exist_ok=True)
+
+        self.bridge = CvBridge()
+        self.lock = threading.Lock()
+        self.raw_image = None
+        self.mask_image = None
+
+        self.subscriber = rospy.Subscriber(
+            self.image_topic, Image, self.image_callback, queue_size=1
+        )
+        rospy.loginfo(
+            "Line snapshot capture subscribes to %s; saving pairs under %s",
+            self.image_topic,
+            self.save_root,
+        )
+
+    @staticmethod
+    def _read_hsv(param_name, default):
+        values = rospy.get_param(param_name, default)
+        if not isinstance(values, (list, tuple)) or len(values) != 3:
+            rospy.logwarn("%s must contain three numbers; using %s", param_name, default)
+            values = default
+        values = [int(max(0, min(255, value))) for value in values]
+        values[0] = min(180, values[0])
+        return np.array(values, dtype=np.uint8)
+
+    def image_callback(self, message):
+        try:
+            raw = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except CvBridgeError as error:
+            rospy.logwarn_throttle(2.0, "Unable to convert camera image: %s", error)
+            return
+
+        hsv = cv2.cvtColor(raw, cv2.COLOR_BGR2HSV)
+        mask = cv2.inRange(hsv, self.lower, self.upper)
+        with self.lock:
+            self.raw_image = raw.copy()
+            self.mask_image = mask.copy()
+
+    def save_pair(self):
+        with self.lock:
+            if self.raw_image is None or self.mask_image is None:
+                rospy.logwarn("No camera frame received yet; nothing was saved.")
+                return
+            raw = self.raw_image.copy()
+            mask = self.mask_image.copy()
+
+        timestamp = time.time_ns()
+        raw_path = self.raw_dir / ("line_%d_raw.png" % timestamp)
+        mask_path = self.mask_dir / ("line_%d_mask.png" % timestamp)
+        raw_ok = cv2.imwrite(str(raw_path), raw)
+        mask_ok = cv2.imwrite(str(mask_path), mask)
+        if raw_ok and mask_ok:
+            rospy.loginfo("Saved snapshot pair: raw=%s mask=%s", raw_path, mask_path)
+        else:
+            rospy.logerr("Failed to save snapshot pair: raw=%s mask=%s", raw_path, mask_path)
+
+    def run(self):
+        window_name = "Line follower snapshot capture (S: save, Q/Esc: quit)"
+        cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
+        rate = rospy.Rate(30)
+
+        while not rospy.is_shutdown():
+            with self.lock:
+                raw = None if self.raw_image is None else self.raw_image.copy()
+                mask = None if self.mask_image is None else self.mask_image.copy()
+
+            if raw is not None and mask is not None:
+                mask_bgr = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
+                display = np.hstack((raw, mask_bgr))
+                width = raw.shape[1]
+                cv2.putText(display, "RAW: %s" % self.image_topic, (12, 28),
+                            cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+                cv2.putText(display, "WHITE MASK HSV %s-%s" % (self.lower.tolist(), self.upper.tolist()),
+                            (width + 12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
+                cv2.putText(display, "S: save pair    Q/Esc: quit", (12, display.shape[0] - 16),
+                            cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
+                cv2.imshow(window_name, display)
+
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("s"), ord("S")):
+                self.save_pair()
+            elif key in (ord("q"), ord("Q"), 27):
+                rospy.loginfo("Line snapshot capture stopped by keyboard.")
+                break
+            rate.sleep()
+
+        cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    try:
+        SnapshotCapture().run()
+    except rospy.ROSInterruptException:
+        pass

traffic_line_task/.gitignore

@@ -0,0 +1,2 @@
+__pycache__/
+*.py[cod]

traffic_line_task/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(traffic_line_task)
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  message_generation
+  nav_msgs
+  rospy
+  std_msgs
+  std_srvs
+)
+
+add_service_files(
+  FILES
+  MarkDistance.srv
+)
+
+generate_messages()
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs message_runtime nav_msgs rospy std_msgs std_srvs
+)
+
+catkin_install_python(PROGRAMS
+  scripts/route_distance_calibrator.py
+  scripts/traffic_line_state_machine.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY config launch srv
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_nosetests(test/test_calibrator_logic.py)
+  catkin_add_nosetests(test/test_task_logic.py)
+  find_package(rostest REQUIRED)
+  add_rostest(test/traffic_line_task_routes.test)
+endif()

traffic_line_task/README.md

@@ -0,0 +1,85 @@
+# traffic_line_task
+
+This package owns the competition phase that starts after navigation has
+stopped at the LED sign point. It does not start a camera or base driver.
+
+Route policy:
+
+- `STOP` and `NONE`: remain stationary and keep recognising.
+- `LEFT` or `RIGHT`: execute the calibrated first junction maneuver, reacquire
+  the lane, then finish after the final lane has continuously disappeared.
+- `STRAIGHT`: enter the centre route, use odometry only to arm the expected
+  second-junction region, drive straight for a configured duration after
+  confirmed lane loss, trigger its fixed right maneuver, then continue
+  following until the final lane has continuously disappeared.
+
+With `use_finish_distance_limits: false`, odometry does not terminate either
+finish segment. Final lane loss stops the vehicle immediately and is confirmed
+for `finish_lane_loss_confirm_time` before entering `FINISHED`. A stable lane
+that returns during confirmation resumes the same segment. Junction maneuvers
+still retain their calibrated odometry limits and safety checks.
+
+On `FINISHED`, the state machine publishes `任务完成` exactly once on
+`/speech_command/announce`. The separately started `speech_command` node owns
+the actual TTS playback; this package never starts a second speech node.
+
+For endpoint inspection, `stop_after_second_maneuver: true` makes the state
+machine enter `FINISHED` immediately after that fixed right maneuver and keeps
+line following disabled. Set it back to `false` after the turn endpoint has
+been verified.
+
+Route distances live in `config/traffic_line_task.yaml`. The state machine
+reports `CONFIG_INVALID` and cannot move if any required value is zero or
+negative.
+
+## Route-distance calibration
+
+The calibration node only reads `/odom`; it never opens the camera, starts the
+base driver, or publishes `/cmd_vel`. Start it beside the already-running base:
+
+```bash
+source ~/ucar_ws/devel/setup.bash
+roslaunch traffic_line_task route_distance_calibration.launch
+```
+
+At the beginning of one segment, reset the measurement, move the vehicle with
+the same low-speed maneuver or line-follow controller that will be used in the
+task, and then mark the endpoint. For example:
+
+```bash
+rosservice call /route_distance_calibrator/reset "{}"
+rosservice call /route_distance_calibrator/mark \
+  "label: 'first_left_turn_distance_m'"
+```
+
+The response contains path distance and accumulated yaw. Repeat the segment at
+least three times and put a conservative repeatable value in
+`config/traffic_line_task.yaml`. Valid labels are exactly the seven distance
+keys in that file.
+
+The single-boundary fallback remains implemented by `line_follow_debug.py`.
+If no valid lane target remains, this state machine disables line control,
+stops in place, and waits up to two seconds for a stable target before entering
+`FAULT`.
+
+After calibration, start the already-shared camera and base driver, stop every
+other `/cmd_vel` source, and run:
+
+```bash
+source ~/ucar_ws/devel/setup.bash
+roslaunch traffic_line_task traffic_line_task.launch
+```
+
+Monitor:
+
+```bash
+rostopic echo /traffic_line_task/state
+rostopic echo /traffic_line_task/segment_distance
+```
+
+Abort or reset:
+
+```bash
+rosservice call /traffic_line_task/abort "{}"
+rosservice call /traffic_line_task/reset "{}"
+```

traffic_line_task/config/route_distance_calibration.yaml

@@ -0,0 +1,4 @@
+odom_topic: /odom
+odom_timeout: 0.5
+odom_jump_limit_m: 0.25
+publish_rate: 10.0

traffic_line_task/config/traffic_line_task.yaml

@@ -0,0 +1,86 @@
+# The task launch is started only after navigation has stopped at the sign
+# recognition point and every other /cmd_vel source has released control.
+autostart: true
+control_rate: 20.0
+
+cmd_vel_topic: /cmd_vel
+odom_topic: /odom
+lane_valid_topic: /line_follow_debug/lane_valid
+lookahead_target_topic: /line_follow_debug/lookahead_target
+sign_direction_topic: /traffic_sign/direction
+sign_confidence_topic: /traffic_sign/confidence
+sign_enable_service: /traffic_sign_recognition/set_enabled
+line_enable_service: /line_follow_control/set_enabled
+
+# A stable result from the recognition node is confirmed twice here before it
+# is locked for the rest of the task. STOP/NONE keep the vehicle stationary.
+sign_confirmations: 2
+sign_topic_timeout: 3.0
+minimum_sign_confidence: 0.25
+camera_settle_time: 2.0
+
+# Odometry and action safety.
+odom_timeout: 0.5
+odom_jump_limit_m: 0.25
+# Deliberately above a nominal 90-degree turn: yaw is only an over-turn guard;
+# calibrated odometry path length remains the maneuver completion condition.
+maximum_turn_yaw_rad: 2.20
+wrong_way_yaw_tolerance_rad: 0.10
+straight_yaw_limit_rad: 0.35
+maneuver_timeout_scale: 2.0
+maneuver_timeout_min: 3.0
+transition_stop_time: 0.30
+
+# Real-vehicle junction maneuver values. 0.10 m/s forward with 0.20 rad/s
+# turning was verified to keep both sides of the drivetrain moving.
+maneuver_linear_speed: 0.10
+first_turn_angular_speed: 0.20
+second_turn_angular_speed: 0.20
+
+# REQUIRED CALIBRATION VALUES. The state machine refuses to move while any
+# value is zero or negative. Fill these using route_distance_calibrator.py.
+first_left_turn_distance_m: 0.5149
+first_right_turn_distance_m: 0.5274
+first_straight_entry_distance_m: 0.3584
+direct_finish_distance_m: 2.5997
+straight_to_second_distance_m: 0.6053
+second_right_turn_distance_m: 0.35
+after_second_finish_distance_m: 2.1104
+
+# The finish segments do not use their old odometry distances. Once LEFT/RIGHT
+# has entered FOLLOW_DIRECT_FINISH, or STRAIGHT has completed the second fixed
+# right turn and entered FOLLOW_AFTER_SECOND, a continuous total loss of lane
+# boundaries is treated as the endpoint. The controller stops immediately and
+# confirms the loss before publishing FINISHED.
+use_finish_distance_limits: false
+finish_lane_loss_confirm_time: 1.0
+
+# Reuse the speech_command node that is already running for task 1. This task
+# never launches a second speech node and sends this phrase exactly once.
+completion_announcement_topic: /speech_command/announce
+completion_announcement_text: "任务完成"
+
+# On the straight route, odometry no longer directly starts the second turn.
+# It only arms a lane-loss trigger near the measured 0.6053 m junction. Three
+# consecutive invalid-lane control cycles then start the fixed right maneuver.
+# Early lane loss uses ordinary recovery; passing the maximum without lane
+# loss stops in FAULT instead of driving indefinitely.
+second_junction_arm_distance_m: 0.45
+second_junction_max_distance_m: 0.85
+second_junction_lost_frames: 3
+
+# After confirmed lane loss at the second junction, move farther into the
+# junction before starting the fixed right turn.
+second_junction_forward_speed: 0.10
+second_junction_forward_duration: 3.0
+
+# Keep false for the complete route: reacquire the lane after the calibrated
+# second right maneuver and continue with FOLLOW_AFTER_SECOND.
+stop_after_second_maneuver: false
+
+# line_follow_debug.py keeps lane_valid=true for up to 8.0 s while continuously
+# inferring the centre from one remaining boundary. If it finally publishes
+# false, stop and wait in place for a fresh target; never reverse or scan.
+lane_reacquire_timeout: 2.0
+lane_stable_frames: 5
+target_fresh_timeout: 0.5

traffic_line_task/config/traffic_line_task_sim.yaml

@@ -0,0 +1,46 @@
+# Isolated, no-hardware configuration used only by the rostest suite.
+autostart: true
+control_rate: 30.0
+cmd_vel_topic: cmd_vel
+odom_topic: odom
+lane_valid_topic: line/lane_valid
+lookahead_target_topic: line/lookahead_target
+sign_direction_topic: sign/direction
+sign_confidence_topic: sign/confidence
+sign_enable_service: sign/set_enabled
+line_enable_service: line/set_enabled
+sign_confirmations: 2
+sign_topic_timeout: 1.0
+minimum_sign_confidence: 0.25
+camera_settle_time: 0.05
+odom_timeout: 0.5
+odom_jump_limit_m: 0.25
+maximum_turn_yaw_rad: 1.40
+wrong_way_yaw_tolerance_rad: 0.10
+straight_yaw_limit_rad: 0.35
+maneuver_timeout_scale: 3.0
+maneuver_timeout_min: 1.0
+transition_stop_time: 0.05
+maneuver_linear_speed: 0.05
+first_turn_angular_speed: 0.20
+second_turn_angular_speed: 0.20
+first_left_turn_distance_m: 0.02
+first_right_turn_distance_m: 0.02
+first_straight_entry_distance_m: 0.02
+direct_finish_distance_m: 0.03
+straight_to_second_distance_m: 0.03
+second_right_turn_distance_m: 0.02
+after_second_finish_distance_m: 0.03
+use_finish_distance_limits: true
+finish_lane_loss_confirm_time: 0.10
+completion_announcement_topic: speech/announce
+completion_announcement_text: "任务完成"
+second_junction_arm_distance_m: 0.02
+second_junction_max_distance_m: 0.08
+second_junction_lost_frames: 2
+second_junction_forward_speed: 0.10
+second_junction_forward_duration: 0.05
+stop_after_second_maneuver: false
+lane_reacquire_timeout: 0.5
+lane_stable_frames: 2
+target_fresh_timeout: 0.3

traffic_line_task/launch/route_distance_calibration.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="config" default="$(find traffic_line_task)/config/route_distance_calibration.yaml"/>
+
+  <!-- Measurement only: this node never advertises or publishes /cmd_vel. -->
+  <node pkg="traffic_line_task" type="route_distance_calibrator.py"
+        name="route_distance_calibrator" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+  </node>
+</launch>

traffic_line_task/launch/traffic_line_task.launch

@@ -0,0 +1,26 @@
+<launch>
+  <!-- The shared camera and base driver must already be running. -->
+  <arg name="task_config" default="$(find traffic_line_task)/config/traffic_line_task.yaml"/>
+  <arg name="sign_config" default="$(find traffic_sign_recognition)/config/traffic_sign.yaml"/>
+  <arg name="line_debug_config" default="$(find line_follower)/config/line_follow_debug.yaml"/>
+  <arg name="line_control_config" default="$(find line_follower)/config/line_follow_control.yaml"/>
+
+  <include file="$(find traffic_sign_recognition)/launch/traffic_sign_recognition.launch">
+    <arg name="config" value="$(arg sign_config)"/>
+    <arg name="enabled" value="false"/>
+  </include>
+
+  <include file="$(find line_follower)/launch/line_follow_debug.launch">
+    <arg name="config" value="$(arg line_debug_config)"/>
+  </include>
+
+  <include file="$(find line_follower)/launch/line_follow_control.launch">
+    <arg name="config" value="$(arg line_control_config)"/>
+    <arg name="enabled" value="false"/>
+  </include>
+
+  <node pkg="traffic_line_task" type="traffic_line_state_machine.py"
+        name="traffic_line_task" output="screen">
+    <rosparam command="load" file="$(arg task_config)"/>
+  </node>
+</launch>

traffic_line_task/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>traffic_line_task</name>
+  <version>0.1.0</version>
+  <description>Traffic-sign to line-following competition task state machine.</description>
+
+  <maintainer email="ucar@todo.todo">ucar</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rospy</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <exec_depend>line_follower</exec_depend>
+  <exec_depend>traffic_sign_recognition</exec_depend>
+  <test_depend>python3-nose</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <export/>
+</package>

traffic_line_task/scripts/route_distance_calibrator.py

@@ -0,0 +1,164 @@
+#!/usr/bin/env python3
+"""Measure odometry path length and yaw without commanding the robot."""
+
+import math
+import threading
+
+import rospy
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Float32, String
+from std_srvs.srv import Trigger, TriggerResponse
+
+from traffic_line_task.srv import MarkDistance, MarkDistanceResponse
+
+
+class RouteDistanceCalibrator:
+    LABELS = (
+        "first_left_turn_distance_m",
+        "first_right_turn_distance_m",
+        "first_straight_entry_distance_m",
+        "direct_finish_distance_m",
+        "straight_to_second_distance_m",
+        "second_right_turn_distance_m",
+        "after_second_finish_distance_m",
+    )
+
+    def __init__(self):
+        rospy.init_node("route_distance_calibrator")
+        self.lock = threading.RLock()
+        self.odom_topic = rospy.get_param("~odom_topic", "/odom")
+        self.odom_timeout = max(0.1, float(rospy.get_param("~odom_timeout", 0.5)))
+        self.odom_jump_limit_m = max(
+            0.01, float(rospy.get_param("~odom_jump_limit_m", 0.25))
+        )
+        self.publish_rate = max(1.0, float(rospy.get_param("~publish_rate", 10.0)))
+
+        self.position = None
+        self.yaw = None
+        self.last_odom_time = None
+        self.total_distance = 0.0
+        self.total_yaw = 0.0
+        self.zero_distance = 0.0
+        self.zero_yaw = 0.0
+        self.odom_fault = ""
+
+        self.distance_pub = rospy.Publisher("~distance", Float32, queue_size=1)
+        self.yaw_pub = rospy.Publisher("~yaw_change", Float32, queue_size=1)
+        self.last_mark_pub = rospy.Publisher(
+            "~last_mark", String, queue_size=1, latch=True
+        )
+        self.odom_sub = rospy.Subscriber(
+            self.odom_topic, Odometry, self.odom_callback, queue_size=20
+        )
+        self.reset_service = rospy.Service("~reset", Trigger, self.reset_callback)
+        self.mark_service = rospy.Service("~mark", MarkDistance, self.mark_callback)
+        self.timer = rospy.Timer(
+            rospy.Duration(1.0 / self.publish_rate), self.publish_callback
+        )
+        rospy.loginfo(
+            "Route-distance calibrator is measurement only; odom=%s. "
+            "It never publishes /cmd_vel.",
+            self.odom_topic,
+        )
+
+    @staticmethod
+    def wrap_angle(angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    @staticmethod
+    def yaw_from_odometry(message):
+        q = message.pose.pose.orientation
+        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+    def odom_callback(self, message):
+        with self.lock:
+            point = message.pose.pose.position
+            current_position = (float(point.x), float(point.y))
+            current_yaw = self.yaw_from_odometry(message)
+            if self.position is not None:
+                step = math.hypot(
+                    current_position[0] - self.position[0],
+                    current_position[1] - self.position[1],
+                )
+                if step > self.odom_jump_limit_m:
+                    self.odom_fault = "ODOM_JUMP: %.3f m" % step
+                else:
+                    self.total_distance += step
+            if self.yaw is not None:
+                self.total_yaw += self.wrap_angle(current_yaw - self.yaw)
+            self.position = current_position
+            self.yaw = current_yaw
+            self.last_odom_time = rospy.get_time()
+
+    def measurement(self):
+        return (
+            max(0.0, self.total_distance - self.zero_distance),
+            self.total_yaw - self.zero_yaw,
+        )
+
+    def odom_error(self):
+        if self.odom_fault:
+            return self.odom_fault
+        if self.last_odom_time is None:
+            return "ODOM_UNAVAILABLE"
+        age = rospy.get_time() - self.last_odom_time
+        if age > self.odom_timeout:
+            return "ODOM_TIMEOUT: %.3f s" % age
+        return ""
+
+    def reset_callback(self, _request):
+        with self.lock:
+            error = self.odom_error()
+            if error and error != self.odom_fault:
+                return TriggerResponse(success=False, message=error)
+            self.odom_fault = ""
+            self.zero_distance = self.total_distance
+            self.zero_yaw = self.total_yaw
+            self.last_mark_pub.publish(String(data=""))
+            rospy.loginfo("Route-distance calibration measurement reset to zero.")
+            return TriggerResponse(success=True, message="measurement reset")
+
+    def mark_callback(self, request):
+        with self.lock:
+            label = str(request.label).strip()
+            if label not in self.LABELS:
+                return MarkDistanceResponse(
+                    success=False,
+                    distance_m=0.0,
+                    yaw_rad=0.0,
+                    message="unknown label; choose one of: %s" % ", ".join(self.LABELS),
+                )
+            error = self.odom_error()
+            if error:
+                return MarkDistanceResponse(
+                    success=False,
+                    distance_m=0.0,
+                    yaw_rad=0.0,
+                    message=error,
+                )
+            distance, yaw = self.measurement()
+            result = "%s: %.4f  # yaw_change_rad=%.4f" % (label, distance, yaw)
+            self.last_mark_pub.publish(String(data=result))
+            rospy.loginfo("CALIBRATION MARK: %s", result)
+            return MarkDistanceResponse(
+                success=True,
+                distance_m=distance,
+                yaw_rad=yaw,
+                message=result,
+            )
+
+    def publish_callback(self, _event):
+        with self.lock:
+            distance, yaw = self.measurement()
+            self.distance_pub.publish(Float32(data=distance))
+            self.yaw_pub.publish(Float32(data=yaw))
+
+
+if __name__ == "__main__":
+    try:
+        RouteDistanceCalibrator()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

traffic_line_task/scripts/traffic_line_state_machine.py

@@ -0,0 +1,803 @@
+#!/usr/bin/env python3
+"""Coordinate LED-sign recognition, open-loop junctions, and line following."""
+
+import math
+import threading
+
+import rospy
+from geometry_msgs.msg import PointStamped, Twist
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Bool, Float32, String
+from std_srvs.srv import SetBool, Trigger, TriggerResponse
+
+
+class TrafficLineStateMachine:
+    STATES = (
+        "INIT",
+        "WAIT_SIGN",
+        "RESTORE_CAMERA",
+        "FIRST_MANEUVER",
+        "REACQUIRE_LINE",
+        "FOLLOW_DIRECT_FINISH",
+        "FOLLOW_TO_SECOND",
+        "SECOND_JUNCTION_FORWARD",
+        "SECOND_RIGHT_MANEUVER",
+        "FOLLOW_AFTER_SECOND",
+        "WAIT_LANE_RECOVERY",
+        "CONFIRM_FINISH",
+        "FINISHED",
+        "FAULT",
+        "ABORTED",
+    )
+    DIRECTIONS = ("LEFT", "RIGHT", "STRAIGHT")
+    TERMINAL_STATES = ("FINISHED", "FAULT", "ABORTED")
+
+    def __init__(self):
+        rospy.init_node("traffic_line_task")
+        self.lock = threading.RLock()
+
+        self.autostart = self._as_bool(rospy.get_param("~autostart", True))
+        self.control_rate = max(1.0, float(rospy.get_param("~control_rate", 20.0)))
+        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/cmd_vel")
+        self.odom_topic = rospy.get_param("~odom_topic", "/odom")
+        self.lane_valid_topic = rospy.get_param(
+            "~lane_valid_topic", "/line_follow_debug/lane_valid"
+        )
+        self.lookahead_target_topic = rospy.get_param(
+            "~lookahead_target_topic", "/line_follow_debug/lookahead_target"
+        )
+        self.sign_direction_topic = rospy.get_param(
+            "~sign_direction_topic", "/traffic_sign/direction"
+        )
+        self.sign_confidence_topic = rospy.get_param(
+            "~sign_confidence_topic", "/traffic_sign/confidence"
+        )
+        self.sign_enable_service_name = rospy.get_param(
+            "~sign_enable_service", "/traffic_sign_recognition/set_enabled"
+        )
+        self.line_enable_service_name = rospy.get_param(
+            "~line_enable_service", "/line_follow_control/set_enabled"
+        )
+
+        self.sign_confirmations = max(1, int(rospy.get_param("~sign_confirmations", 2)))
+        self.sign_topic_timeout = max(
+            0.5, float(rospy.get_param("~sign_topic_timeout", 3.0))
+        )
+        self.minimum_sign_confidence = max(
+            0.0, float(rospy.get_param("~minimum_sign_confidence", 0.25))
+        )
+        self.camera_settle_time = max(
+            0.0, float(rospy.get_param("~camera_settle_time", 2.0))
+        )
+        self.odom_timeout = max(0.1, float(rospy.get_param("~odom_timeout", 0.5)))
+        self.odom_jump_limit_m = max(
+            0.01, float(rospy.get_param("~odom_jump_limit_m", 0.25))
+        )
+        self.maximum_turn_yaw_rad = max(
+            0.1, float(rospy.get_param("~maximum_turn_yaw_rad", 2.20))
+        )
+        self.wrong_way_yaw_tolerance_rad = max(
+            0.01, float(rospy.get_param("~wrong_way_yaw_tolerance_rad", 0.10))
+        )
+        self.straight_yaw_limit_rad = max(
+            0.05, float(rospy.get_param("~straight_yaw_limit_rad", 0.35))
+        )
+        self.maneuver_timeout_scale = max(
+            1.0, float(rospy.get_param("~maneuver_timeout_scale", 2.0))
+        )
+        self.maneuver_timeout_min = max(
+            1.0, float(rospy.get_param("~maneuver_timeout_min", 3.0))
+        )
+        self.transition_stop_time = max(
+            0.0, float(rospy.get_param("~transition_stop_time", 0.30))
+        )
+        self.maneuver_linear_speed = min(
+            0.10,
+            max(0.01, float(rospy.get_param("~maneuver_linear_speed", 0.10))),
+        )
+        self.first_turn_angular_speed = min(
+            0.20, max(0.01, float(rospy.get_param("~first_turn_angular_speed", 0.20)))
+        )
+        self.second_turn_angular_speed = min(
+            0.20, max(0.01, float(rospy.get_param("~second_turn_angular_speed", 0.20)))
+        )
+        self.second_junction_forward_speed = min(
+            0.10,
+            max(
+                0.01,
+                float(rospy.get_param("~second_junction_forward_speed", 0.10)),
+            ),
+        )
+        self.second_junction_forward_duration = max(
+            0.0,
+            float(rospy.get_param("~second_junction_forward_duration", 3.0)),
+        )
+        self.stop_after_second_maneuver = bool(
+            rospy.get_param("~stop_after_second_maneuver", False)
+        )
+        self.use_finish_distance_limits = self._as_bool(
+            rospy.get_param("~use_finish_distance_limits", True)
+        )
+
+        self.distances = {
+            "first_left_turn": float(rospy.get_param("~first_left_turn_distance_m", 0.0)),
+            "first_right_turn": float(rospy.get_param("~first_right_turn_distance_m", 0.0)),
+            "first_straight_entry": float(
+                rospy.get_param("~first_straight_entry_distance_m", 0.0)
+            ),
+            "direct_finish": float(rospy.get_param("~direct_finish_distance_m", 0.0)),
+            "straight_to_second": float(
+                rospy.get_param("~straight_to_second_distance_m", 0.0)
+            ),
+            "second_right_turn": float(
+                rospy.get_param("~second_right_turn_distance_m", 0.0)
+            ),
+            "after_second_finish": float(
+                rospy.get_param("~after_second_finish_distance_m", 0.0)
+            ),
+        }
+        self.lane_reacquire_timeout = max(
+            0.1, float(rospy.get_param("~lane_reacquire_timeout", 2.0))
+        )
+        self.second_junction_arm_distance = max(
+            0.0,
+            float(
+                rospy.get_param(
+                    "~second_junction_arm_distance_m",
+                    0.75 * self.distances["straight_to_second"],
+                )
+            ),
+        )
+        self.second_junction_max_distance = max(
+            0.0,
+            float(
+                rospy.get_param(
+                    "~second_junction_max_distance_m",
+                    1.40 * self.distances["straight_to_second"],
+                )
+            ),
+        )
+        self.second_junction_lost_frames = max(
+            1, int(rospy.get_param("~second_junction_lost_frames", 3))
+        )
+        self.lane_stable_frames = max(
+            1, int(rospy.get_param("~lane_stable_frames", 5))
+        )
+        self.target_fresh_timeout = max(
+            0.05, float(rospy.get_param("~target_fresh_timeout", 0.5))
+        )
+        self.finish_lane_loss_confirm_time = max(
+            0.1, float(rospy.get_param("~finish_lane_loss_confirm_time", 1.0))
+        )
+        self.completion_announcement_topic = rospy.get_param(
+            "~completion_announcement_topic", "/speech_command/announce"
+        )
+        self.completion_announcement_text = str(
+            rospy.get_param("~completion_announcement_text", "任务完成")
+        ).strip()
+
+        self.state = "INIT" if self.autostart else "ABORTED"
+        self.state_enter_time = rospy.get_time()
+        self.first_direction = "NONE"
+        self.fault_reason = ""
+        self.sign_candidate = "NONE"
+        self.sign_candidate_count = 0
+        self.last_sign_time = None
+        self.sign_confidence = 0.0
+        self.lane_valid = False
+        self.last_target_time = None
+        self.lane_stable_count = 0
+        self.resume_state = None
+        self.resume_segment_start_distance = None
+        self.next_follow_state = None
+        self.second_junction_loss_count = 0
+        self.completion_announcement_sent = False
+        self.odom_position = None
+        self.odom_yaw = None
+        self.last_odom_time = None
+        self.total_odom_distance = 0.0
+        self.odom_fault_reason = ""
+        self.segment_start_distance = 0.0
+        self.segment_start_yaw = None
+
+        self.cmd_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1)
+        self.state_pub = rospy.Publisher("~state", String, queue_size=1, latch=True)
+        self.direction_pub = rospy.Publisher(
+            "~first_direction", String, queue_size=1, latch=True
+        )
+        self.distance_pub = rospy.Publisher("~segment_distance", Float32, queue_size=1)
+        self.finished_pub = rospy.Publisher("~finished", Bool, queue_size=1, latch=True)
+        self.fault_pub = rospy.Publisher("~fault", String, queue_size=1, latch=True)
+        self.completion_announcement_pub = rospy.Publisher(
+            self.completion_announcement_topic, String, queue_size=1
+        )
+
+        self.sign_sub = rospy.Subscriber(
+            self.sign_direction_topic, String, self.sign_callback, queue_size=1
+        )
+        self.confidence_sub = rospy.Subscriber(
+            self.sign_confidence_topic, Float32, self.confidence_callback, queue_size=1
+        )
+        self.lane_sub = rospy.Subscriber(
+            self.lane_valid_topic, Bool, self.lane_callback, queue_size=1
+        )
+        self.target_sub = rospy.Subscriber(
+            self.lookahead_target_topic,
+            PointStamped,
+            self.target_callback,
+            queue_size=1,
+        )
+        self.odom_sub = rospy.Subscriber(
+            self.odom_topic, Odometry, self.odom_callback, queue_size=10
+        )
+
+        self.reset_service = rospy.Service("~reset", Trigger, self.reset_callback)
+        self.abort_service = rospy.Service("~abort", Trigger, self.abort_callback)
+        self.sign_enable = rospy.ServiceProxy(self.sign_enable_service_name, SetBool)
+        self.line_enable = rospy.ServiceProxy(self.line_enable_service_name, SetBool)
+        self.timer = rospy.Timer(
+            rospy.Duration(1.0 / self.control_rate), self.control_callback
+        )
+        rospy.on_shutdown(self.shutdown)
+
+        self.state_pub.publish(String(data=self.state))
+        self.direction_pub.publish(String(data="NONE"))
+        self.finished_pub.publish(Bool(data=False))
+        self.fault_pub.publish(String(data=""))
+        rospy.loginfo("Traffic-line task ready: state=%s autostart=%s", self.state, self.autostart)
+
+    @staticmethod
+    def _as_bool(value):
+        if isinstance(value, str):
+            return value.strip().lower() in ("1", "true", "yes", "on")
+        return bool(value)
+
+    @staticmethod
+    def wrap_angle(angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    @staticmethod
+    def follow_state_after_first(direction):
+        if direction == "STRAIGHT":
+            return "FOLLOW_TO_SECOND"
+        if direction in ("LEFT", "RIGHT"):
+            return "FOLLOW_DIRECT_FINISH"
+        raise ValueError("unsupported first direction: %s" % direction)
+
+    @staticmethod
+    def yaw_from_odometry(message):
+        q = message.pose.pose.orientation
+        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+    def sign_callback(self, message):
+        with self.lock:
+            direction = str(message.data).strip().upper()
+            self.last_sign_time = rospy.get_time()
+            if self.state != "WAIT_SIGN":
+                return
+            if direction not in self.DIRECTIONS:
+                self.sign_candidate = "NONE"
+                self.sign_candidate_count = 0
+                return
+            if self.sign_confidence < self.minimum_sign_confidence:
+                return
+            if direction == self.sign_candidate:
+                self.sign_candidate_count += 1
+            else:
+                self.sign_candidate = direction
+                self.sign_candidate_count = 1
+
+    def confidence_callback(self, message):
+        with self.lock:
+            self.sign_confidence = max(0.0, float(message.data))
+
+    def lane_callback(self, message):
+        with self.lock:
+            self.lane_valid = bool(message.data)
+
+    def target_callback(self, _message):
+        with self.lock:
+            self.last_target_time = rospy.get_time()
+
+    def odom_callback(self, message):
+        with self.lock:
+            position = message.pose.pose.position
+            current = (float(position.x), float(position.y))
+            if self.odom_position is not None:
+                step = math.hypot(
+                    current[0] - self.odom_position[0],
+                    current[1] - self.odom_position[1],
+                )
+                if step > self.odom_jump_limit_m:
+                    self.odom_fault_reason = "ODOM_JUMP: %.3f m" % step
+                else:
+                    self.total_odom_distance += step
+            self.odom_position = current
+            self.odom_yaw = self.yaw_from_odometry(message)
+            self.last_odom_time = rospy.get_time()
+
+    def reset_callback(self, _request):
+        with self.lock:
+            self._disable_line()
+            self._disable_sign()
+            self._publish_stop()
+            self._clear_task()
+            self._transition("INIT" if self.autostart else "ABORTED")
+        return TriggerResponse(success=True, message="traffic-line task reset")
+
+    def abort_callback(self, _request):
+        with self.lock:
+            self._enter_terminal("ABORTED", "operator abort")
+        return TriggerResponse(success=True, message="traffic-line task aborted")
+
+    def _clear_task(self):
+        self.first_direction = "NONE"
+        self.fault_reason = ""
+        self.sign_candidate = "NONE"
+        self.sign_candidate_count = 0
+        self.last_sign_time = None
+        self.sign_confidence = 0.0
+        self.lane_stable_count = 0
+        self.resume_state = None
+        self.next_follow_state = None
+        self.second_junction_loss_count = 0
+        self.completion_announcement_sent = False
+        self.odom_fault_reason = ""
+        self.direction_pub.publish(String(data="NONE"))
+        self.finished_pub.publish(Bool(data=False))
+        self.fault_pub.publish(String(data=""))
+
+    def _transition(self, state, reset_segment=False):
+        if state not in self.STATES:
+            raise ValueError("unknown traffic-line state: %s" % state)
+        previous = self.state
+        self.state = state
+        self.state_enter_time = rospy.get_time()
+        self.lane_stable_count = 0
+        if state == "FOLLOW_TO_SECOND":
+            self.second_junction_loss_count = 0
+        if reset_segment:
+            self.segment_start_distance = self.total_odom_distance
+            self.segment_start_yaw = self.odom_yaw
+        self.state_pub.publish(String(data=state))
+        rospy.loginfo("Traffic-line state: %s -> %s", previous, state)
+
+    def _publish_stop(self):
+        try:
+            self.cmd_pub.publish(Twist())
+        except rospy.ROSException:
+            # A timer callback can overlap ROS shutdown after the publisher has
+            # already been unregistered. The base timeout still provides the
+            # final stop in that teardown-only condition.
+            if not rospy.is_shutdown():
+                raise
+
+    def _publish_maneuver(self, angular):
+        command = Twist()
+        command.linear.x = self.maneuver_linear_speed
+        command.angular.z = angular
+        self.cmd_pub.publish(command)
+
+    def _publish_forward(self, speed):
+        command = Twist()
+        command.linear.x = speed
+        self.cmd_pub.publish(command)
+
+    def _service_ready(self, name):
+        try:
+            rospy.wait_for_service(name, timeout=0.01)
+            return True
+        except rospy.ROSException:
+            return False
+
+    def _set_enabled(self, proxy, name, enabled):
+        try:
+            response = proxy(enabled)
+        except (rospy.ServiceException, rospy.ROSException) as error:
+            rospy.logerr("Service %s failed: %s", name, error)
+            return False
+        if not response.success:
+            rospy.logerr("Service %s rejected request: %s", name, response.message)
+            return False
+        return True
+
+    def _disable_line(self):
+        if self._service_ready(self.line_enable_service_name):
+            return self._set_enabled(
+                self.line_enable, self.line_enable_service_name, False
+            )
+        return False
+
+    def _enable_line(self):
+        return self._set_enabled(self.line_enable, self.line_enable_service_name, True)
+
+    def _disable_sign(self):
+        if self._service_ready(self.sign_enable_service_name):
+            return self._set_enabled(
+                self.sign_enable, self.sign_enable_service_name, False
+            )
+        return False
+
+    def _enable_sign(self):
+        return self._set_enabled(self.sign_enable, self.sign_enable_service_name, True)
+
+    def _configuration_error(self):
+        invalid = [name for name, value in self.distances.items() if value <= 0.0]
+        if invalid:
+            return "CONFIG_INVALID: set positive calibration values for %s" % ", ".join(invalid)
+        if self.second_junction_arm_distance <= 0.0:
+            return "CONFIG_INVALID: second_junction_arm_distance_m must be positive"
+        if self.second_junction_max_distance <= self.second_junction_arm_distance:
+            return (
+                "CONFIG_INVALID: second_junction_max_distance_m must be greater "
+                "than second_junction_arm_distance_m"
+            )
+        return ""
+
+    def _odom_fresh(self, now):
+        return self.last_odom_time is not None and now - self.last_odom_time <= self.odom_timeout
+
+    def _target_fresh(self, now):
+        return self.last_target_time is not None and now - self.last_target_time <= self.target_fresh_timeout
+
+    def _lane_ready(self, now):
+        return self.lane_valid and self._target_fresh(now)
+
+    def _segment_distance(self):
+        return max(0.0, self.total_odom_distance - self.segment_start_distance)
+
+    def _segment_yaw(self):
+        if self.segment_start_yaw is None or self.odom_yaw is None:
+            return 0.0
+        return self.wrap_angle(self.odom_yaw - self.segment_start_yaw)
+
+    def _maneuver_timeout(self, target_distance):
+        nominal = target_distance / self.maneuver_linear_speed
+        return self.transition_stop_time + max(
+            self.maneuver_timeout_min, nominal * self.maneuver_timeout_scale
+        )
+
+    def _enter_fault(self, reason):
+        self._enter_terminal("FAULT", reason)
+
+    def _enter_terminal(self, state, reason=""):
+        self._disable_line()
+        self._disable_sign()
+        self._publish_stop()
+        self.fault_reason = reason if state == "FAULT" else ""
+        self.finished_pub.publish(Bool(data=(state == "FINISHED")))
+        self.fault_pub.publish(String(data=self.fault_reason))
+        self._transition(state)
+        if state == "FINISHED" and not self.completion_announcement_sent:
+            self.completion_announcement_pub.publish(
+                String(data=self.completion_announcement_text)
+            )
+            self.completion_announcement_sent = True
+            rospy.loginfo(
+                "Task completion announcement requested: %s",
+                self.completion_announcement_text,
+            )
+        if reason:
+            rospy.logerr("Traffic-line %s: %s", state, reason)
+
+    def _begin_first_maneuver(self):
+        if not self._disable_line():
+            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+            return
+        self._publish_stop()
+        self._transition("FIRST_MANEUVER", reset_segment=True)
+
+    def _begin_reacquire(self, next_state):
+        if not self._disable_line():
+            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+            return
+        self._publish_stop()
+        self.next_follow_state = next_state
+        self._transition("REACQUIRE_LINE")
+
+    def _enable_follow_state(self, state, reset_segment):
+        self._publish_stop()
+        if not self._enable_line():
+            self._enter_fault("LINE_ENABLE_SERVICE_FAILED")
+            return
+        self._transition(state, reset_segment=reset_segment)
+
+    def _begin_lane_recovery(self):
+        self.resume_state = self.state
+        if not self._disable_line():
+            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+            return
+        self._publish_stop()
+        self._transition("WAIT_LANE_RECOVERY")
+
+    def _run_reacquire(self, now, recovery):
+        self._publish_stop()
+        if self._lane_ready(now):
+            self.lane_stable_count += 1
+        else:
+            self.lane_stable_count = 0
+        if self.lane_stable_count >= self.lane_stable_frames:
+            state = self.resume_state if recovery else self.next_follow_state
+            self._enable_follow_state(state, reset_segment=not recovery)
+            return
+        if now - self.state_enter_time > self.lane_reacquire_timeout:
+            self._enter_fault("LANE_REACQUIRE_TIMEOUT")
+
+    def _first_maneuver_parameters(self):
+        if self.first_direction == "LEFT":
+            return self.distances["first_left_turn"], self.first_turn_angular_speed
+        if self.first_direction == "RIGHT":
+            return self.distances["first_right_turn"], -self.first_turn_angular_speed
+        if self.first_direction == "STRAIGHT":
+            return self.distances["first_straight_entry"], 0.0
+        return 0.0, 0.0
+
+    def _run_maneuver(self, now, target_distance, angular, completed_callback):
+        distance = self._segment_distance()
+        yaw = self._segment_yaw()
+        elapsed = now - self.state_enter_time
+        if elapsed < self.transition_stop_time:
+            self._publish_stop()
+            return
+        if angular != 0.0:
+            expected_sign = 1.0 if angular > 0.0 else -1.0
+            if yaw * expected_sign < -self.wrong_way_yaw_tolerance_rad:
+                self._enter_fault("MANEUVER_WRONG_YAW_DIRECTION: %.3f rad" % yaw)
+                return
+            if abs(yaw) > self.maximum_turn_yaw_rad:
+                self._enter_fault("MANEUVER_YAW_LIMIT: %.3f rad" % yaw)
+                return
+        elif abs(yaw) > self.straight_yaw_limit_rad:
+            self._enter_fault("STRAIGHT_YAW_LIMIT: %.3f rad" % yaw)
+            return
+        if distance >= target_distance:
+            self._publish_stop()
+            completed_callback()
+            return
+        if elapsed > self._maneuver_timeout(target_distance):
+            self._enter_fault("MANEUVER_TIMEOUT: %.3f/%.3f m" % (distance, target_distance))
+            return
+        self._publish_maneuver(angular)
+
+    def _run_follow(self, target_distance, completion):
+        now = rospy.get_time()
+        if not self._lane_ready(now):
+            self._begin_lane_recovery()
+            return
+        if self._segment_distance() >= target_distance:
+            if not self._disable_line():
+                self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+                return
+            self._publish_stop()
+            completion()
+
+    def _run_finish_follow(self, now, target_distance):
+        if not self.use_finish_distance_limits:
+            # Endpoint detection must come from the perception result itself.
+            # A stale target alone may mean CPU/image delay and must not be
+            # mistaken for the physical end of the painted lane.
+            if not self.lane_valid:
+                self.resume_state = self.state
+                if not self._disable_line():
+                    self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+                    return
+                self._publish_stop()
+                self._transition("CONFIRM_FINISH")
+            return
+        self._run_follow(
+            target_distance,
+            lambda: self._enter_terminal("FINISHED"),
+        )
+
+    def _run_finish_confirmation(self, now):
+        """Stop immediately, then distinguish endpoint loss from a short dropout."""
+        self._publish_stop()
+        if self.lane_valid:
+            if self._lane_ready(now):
+                self.lane_stable_count += 1
+                if self.lane_stable_count >= self.lane_stable_frames:
+                    resume_state = self.resume_state
+                    self._enable_follow_state(resume_state, reset_segment=False)
+            else:
+                self.lane_stable_count = 0
+            return
+
+        self.lane_stable_count = 0
+        if now - self.state_enter_time >= self.finish_lane_loss_confirm_time:
+            rospy.loginfo(
+                "Endpoint confirmed after %.2f s without a valid lane.",
+                self.finish_lane_loss_confirm_time,
+            )
+            self._enter_terminal("FINISHED")
+
+    def _run_follow_to_second(self, now):
+        """Trigger the second right turn from confirmed lane loss near the junction."""
+        distance = self._segment_distance()
+        if self._lane_ready(now):
+            self.second_junction_loss_count = 0
+            if distance > self.second_junction_max_distance:
+                if not self._disable_line():
+                    self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+                    return
+                self._publish_stop()
+                self._enter_fault(
+                    "SECOND_JUNCTION_NOT_DETECTED: %.3f m" % distance
+                )
+            return
+
+        if distance < self.second_junction_arm_distance:
+            # A loss well before the expected junction is ordinary perception
+            # failure, not permission to turn into an arbitrary opening.
+            self.second_junction_loss_count = 0
+            self._begin_lane_recovery()
+            return
+
+        self.second_junction_loss_count += 1
+        self._publish_stop()
+        if self.second_junction_loss_count < self.second_junction_lost_frames:
+            return
+        if not self._disable_line():
+            self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+            return
+        rospy.loginfo(
+            "Second junction confirmed by %d lost-lane frames at %.3f m.",
+            self.second_junction_loss_count,
+            distance,
+        )
+        self._transition("SECOND_JUNCTION_FORWARD")
+
+    def _run_second_junction_forward(self, now):
+        elapsed = max(0.0, now - self.state_enter_time)
+        if elapsed >= self.second_junction_forward_duration:
+            self._publish_stop()
+            rospy.loginfo(
+                "Second-junction forward approach complete: %.2f s at %.2f m/s.",
+                self.second_junction_forward_duration,
+                self.second_junction_forward_speed,
+            )
+            self._transition("SECOND_RIGHT_MANEUVER", reset_segment=True)
+            return
+        self._publish_forward(self.second_junction_forward_speed)
+
+    def _complete_second_maneuver(self):
+        if self.stop_after_second_maneuver:
+            rospy.loginfo(
+                "Second-junction test stop reached; line following remains disabled."
+            )
+            self._enter_terminal("FINISHED")
+            return
+        self._begin_reacquire("FOLLOW_AFTER_SECOND")
+
+    def control_callback(self, _event):
+        with self.lock:
+            now = rospy.get_time()
+            self.distance_pub.publish(Float32(data=self._segment_distance()))
+
+            if self.state in self.TERMINAL_STATES:
+                self._publish_stop()
+                return
+
+            # Validate the complete route before requiring odometry or enabling
+            # either perception/control node. This guarantees an uncalibrated
+            # task fails as CONFIG_INVALID without ever permitting motion.
+            if self.state == "INIT":
+                config_error = self._configuration_error()
+                if config_error:
+                    self._enter_fault(config_error)
+                    return
+
+            if self.odom_fault_reason:
+                self._enter_fault(self.odom_fault_reason)
+                return
+            if not self._odom_fresh(now):
+                if now - self.state_enter_time > self.odom_timeout:
+                    self._enter_fault("ODOM_TIMEOUT")
+                else:
+                    self._publish_stop()
+                return
+
+            if self.state == "INIT":
+                self._publish_stop()
+                if not self._service_ready(self.sign_enable_service_name):
+                    if now - self.state_enter_time > self.sign_topic_timeout:
+                        self._enter_fault("SIGN_ENABLE_SERVICE_UNAVAILABLE")
+                    return
+                if not self._service_ready(self.line_enable_service_name):
+                    if now - self.state_enter_time > self.sign_topic_timeout:
+                        self._enter_fault("LINE_ENABLE_SERVICE_UNAVAILABLE")
+                    return
+                if not self._disable_line():
+                    self._enter_fault("LINE_DISABLE_SERVICE_FAILED")
+                    return
+                if not self._enable_sign():
+                    self._enter_fault("SIGN_ENABLE_SERVICE_FAILED")
+                    return
+                self.last_sign_time = now
+                self._transition("WAIT_SIGN")
+                return
+
+            if self.state == "WAIT_SIGN":
+                self._publish_stop()
+                if self.last_sign_time is None or now - self.last_sign_time > self.sign_topic_timeout:
+                    self._enter_fault("SIGN_TOPIC_TIMEOUT")
+                    return
+                if self.sign_candidate_count >= self.sign_confirmations:
+                    self.first_direction = self.sign_candidate
+                    self.direction_pub.publish(String(data=self.first_direction))
+                    if not self._disable_sign():
+                        self._enter_fault("SIGN_DISABLE_SERVICE_FAILED")
+                        return
+                    self._transition("RESTORE_CAMERA")
+                return
+
+            if self.state == "RESTORE_CAMERA":
+                self._publish_stop()
+                if now - self.state_enter_time >= self.camera_settle_time:
+                    self._begin_first_maneuver()
+                return
+
+            if self.state == "FIRST_MANEUVER":
+                target, angular = self._first_maneuver_parameters()
+                next_state = self.follow_state_after_first(self.first_direction)
+                self._run_maneuver(
+                    now,
+                    target,
+                    angular,
+                    lambda: self._begin_reacquire(next_state),
+                )
+                return
+
+            if self.state == "REACQUIRE_LINE":
+                self._run_reacquire(now, recovery=False)
+                return
+
+            if self.state == "WAIT_LANE_RECOVERY":
+                self._run_reacquire(now, recovery=True)
+                return
+
+            if self.state == "CONFIRM_FINISH":
+                self._run_finish_confirmation(now)
+                return
+
+            if self.state == "FOLLOW_DIRECT_FINISH":
+                self._run_finish_follow(now, self.distances["direct_finish"])
+                return
+
+            if self.state == "FOLLOW_TO_SECOND":
+                self._run_follow_to_second(now)
+                return
+
+            if self.state == "SECOND_JUNCTION_FORWARD":
+                self._run_second_junction_forward(now)
+                return
+
+            if self.state == "SECOND_RIGHT_MANEUVER":
+                self._run_maneuver(
+                    now,
+                    self.distances["second_right_turn"],
+                    -self.second_turn_angular_speed,
+                    self._complete_second_maneuver,
+                )
+                return
+
+            if self.state == "FOLLOW_AFTER_SECOND":
+                self._run_finish_follow(now, self.distances["after_second_finish"])
+                return
+
+            self._enter_fault("UNHANDLED_STATE: %s" % self.state)
+
+    def shutdown(self):
+        with self.lock:
+            self._disable_line()
+            self._disable_sign()
+            self._publish_stop()
+
+
+if __name__ == "__main__":
+    try:
+        TrafficLineStateMachine()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

traffic_line_task/srv/MarkDistance.srv

@@ -0,0 +1,6 @@
+string label
+---
+bool success
+float64 distance_m
+float64 yaw_rad
+string message

traffic_sign_recognition/.gitignore

@@ -0,0 +1,6 @@
+__pycache__/
+*.py[cod]
+
+# Device-specific RKNN binaries are deployed locally, not stored in Git.
+models/*.rknn
+!models/.gitkeep

traffic_sign_recognition/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(traffic_sign_recognition)
+
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  rospy
+  sensor_msgs
+  std_msgs
+  std_srvs
+)
+
+catkin_package(
+  CATKIN_DEPENDS cv_bridge rospy sensor_msgs std_msgs std_srvs
+)
+
+install(PROGRAMS
+  scripts/led_dataset_collector.py
+  scripts/traffic_sign_node
+  scripts/traffic_sign_node.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY config launch models
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)

traffic_sign_recognition/README.md

@@ -0,0 +1,50 @@
+# LED 指示牌识别（RK3588 NPU）
+
+这个包只订阅共享的 `/usb_cam/image_raw` 图像，不打开 `/dev/video0`，也不会改变相机的曝光、增益或白平衡。二维码和指示牌节点可以同时订阅同一原始图像话题，各自进行独立的软件预处理。
+
+USB 相机驱动应保持自动曝光、自动白平衡的正常原始画面。`config/traffic_sign.yaml` 中的 `brightness`、`contrast`、`saturation`、`gamma` 和 `lab_clahe` 只作用于本包的采集器和识别节点，不会影响二维码或其他相机订阅者。物理相机曝光是全局硬件设置，不能仅对某一个 ROS 节点生效。
+
+当前赛道流程在到达指示牌点时只使用指示牌识别，所以识别节点启动后会按同一配置文件中的 `led_*` 参数暂时切换相机到手动曝光 `50`；节点退出时会自动恢复自动曝光和默认色彩。恢复操作由节点外层启动脚本执行，不依赖 ROS/Python 的退出回调。不要在二维码或巡线仍依赖相机画面时启动该节点。
+
+`config/head_camera.yaml` 是复制到本包的 USB 相机 640×480 标定文件。每帧先按该文件去畸变，再水平翻转和执行 LED 彩色预处理；若相机话题分辨率不是 640×480，节点会跳过该帧，避免错误套用标定参数。
+
+## 模型
+
+将最终验证通过的 YOLOv5 RKNN 模型保存为：
+
+```text
+models/traffic_sign_direction.rknn
+```
+
+类别顺序必须为：`left`、`right`、`straight`、`stop`。模型输入固定为 640×640；节点兼容 YOLOv5 ONNX 导出的单一已解码输出，以及三尺度原始检测头输出。
+
+## 启动
+
+```bash
+source /opt/ros/noetic/setup.bash
+source ~/ucar_ws/devel/setup.bash
+roslaunch traffic_sign_recognition traffic_sign_recognition.launch
+```
+
+节点启动包装器会固定使用 `/home/ucar/venv3.9/bin/python3`，无需手动激活 venv。
+
+## ROS 接口
+
+| 话题 | 类型 | 含义 |
+| --- | --- | --- |
+| `/traffic_sign/direction` | `std_msgs/String` | `LEFT`、`RIGHT`、`STRAIGHT`、`STOP` 或 `NONE` |
+| `/traffic_sign/confidence` | `std_msgs/Float32` | 当前稳定结果的置信度；`NONE` 时为 `0.0` |
+| `/traffic_sign/debug_image` | `sensor_msgs/Image` | 标注检测框和稳定结果的调试图 |
+| `/traffic_sign_recognition/set_enabled` | `std_srvs/SetBool` | 启用识别与LED曝光，或停用识别并恢复自动曝光 |
+
+默认需在最近 5 帧中至少 4 帧识别为同一类别，且单帧置信度不低于 0.25，才会发布有效方向。可在启动时临时调整，例如 `object_threshold:=0.35`。
+
+## 采集训练数据
+
+先启动唯一的 USB 相机驱动，再启动采集器：
+
+```bash
+roslaunch traffic_sign_recognition led_dataset_collector.launch
+```
+
+采集器左侧显示原始帧，右侧显示去畸变、翻转和 LED 彩色预处理后的帧。当前实物测试已关闭 LAB-CLAHE，以免增强地面反光；训练采集与 NPU 推理会共用该设置。按 `1`、`2`、`3`、`4` 选择 `left`、`right`、`straight`、`stop`，按 `0` 选择 `negative`（无有效方向牌或干扰图），按 `s` 仅保存处理后的训练图到 `/home/ucar/traffic_sign_data/processed/<label>/`。`negative` 只用于创建空 YOLO 标注文件，不是模型的第五个类别；全红仍使用 `stop`。新采集的元数据保存到 `/home/ucar/traffic_sign_data/processed_metadata.csv`；此前已保存的原图与旧元数据保持不变。

traffic_sign_recognition/config/head_camera.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: head_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [461.2953681310527, 0, 294.6839234091084, 0, 463.0881041569399, 243.8141717345016, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.3382641806472924, 0.09294924180863037, -0.003794221327541872, 0.005385955042203554, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [368.5670471191406, 0, 288.7868961422464, 0, 0, 415.5361328125, 242.5790295740589, 0, 0, 0, 1, 0]

traffic_sign_recognition/config/traffic_sign.yaml

@@ -0,0 +1,45 @@
+# usb_cam remains the only process that opens /dev/video0.  While this LED
+# recognizer runs at the sign point, it may switch the shared camera to the
+# manual LED profile below, then restore automatic exposure on shutdown.
+image_topic: /usb_cam/image_raw
+enabled: true
+model_path: ""
+classes: [left, right, straight, stop]
+input_size: 640
+object_threshold: 0.25
+nms_threshold: 0.45
+
+# Preserve red LED colour.  Do not use the QR node's grayscale preprocessing.
+flip_horizontal: true
+# These are software-only adjustments made after subscribing to image_topic.
+# They never change /usb_cam/image_raw or the USB camera hardware controls.
+brightness: 0
+contrast: 1.0
+saturation: 1.0
+# Tested on the real LED sign: CLAHE brightens floor reflections and reduces
+# arrow contrast, so keep it disabled for both collection and NPU inference.
+lab_clahe: false
+clahe_clip_limit: 1.5
+clahe_tile_grid: [8, 8]
+gamma: 1.0
+
+# Hardware profile used only during LED-sign recognition.  This affects the
+# shared camera image temporarily, so start this node only at the sign point.
+camera_profile_enabled: true
+camera_device: /dev/video0
+led_exposure_auto: 1
+led_exposure_auto_priority: 0
+led_exposure_absolute: 50
+led_white_balance_auto: true
+restore_camera_on_shutdown: true
+restore_exposure_auto: 3
+restore_exposure_auto_priority: 0
+restore_white_balance_auto: true
+restore_brightness: 0
+restore_contrast: 50
+restore_saturation: 50
+restore_gamma: 300
+
+# A direction is accepted only after four matching detections in five frames.
+stability_window: 5
+stable_count: 4

traffic_sign_recognition/launch/led_dataset_collector.launch

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="config" default="$(find traffic_sign_recognition)/config/traffic_sign.yaml"/>
+  <arg name="calibration_file" default="$(find traffic_sign_recognition)/config/head_camera.yaml"/>
+  <arg name="data_root" default="/home/ucar/traffic_sign_data"/>
+
+  <node pkg="traffic_sign_recognition" type="led_dataset_collector.py" name="led_dataset_collector" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+    <param name="calibration_file" value="$(arg calibration_file)"/>
+    <param name="data_root" value="$(arg data_root)"/>
+  </node>
+</launch>

traffic_sign_recognition/launch/traffic_sign_recognition.launch

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="config" default="$(find traffic_sign_recognition)/config/traffic_sign.yaml"/>
+  <arg name="model_path" default="$(find traffic_sign_recognition)/models/traffic_sign_direction.rknn"/>
+  <arg name="calibration_file" default="$(find traffic_sign_recognition)/config/head_camera.yaml"/>
+  <arg name="object_threshold" default="0.25"/>
+  <arg name="enabled" default="true"/>
+
+  <node pkg="traffic_sign_recognition" type="traffic_sign_node" name="traffic_sign_recognition" output="screen">
+    <rosparam command="load" file="$(arg config)"/>
+    <param name="model_path" value="$(arg model_path)"/>
+    <param name="calibration_file" value="$(arg calibration_file)"/>
+    <param name="object_threshold" value="$(arg object_threshold)"/>
+    <param name="enabled" value="$(arg enabled)"/>
+  </node>
+</launch>

traffic_sign_recognition/models/.gitkeep

@@ -0,0 +1 @@
+

traffic_sign_recognition/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>traffic_sign_recognition</name>
+  <version>0.1.0</version>
+  <description>RK3588 NPU recognition of LED direction signs.</description>
+
+  <maintainer email="ucar@todo.todo">ucar</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+
+  <export/>
+</package>

traffic_sign_recognition/scripts/led_dataset_collector.py

@@ -0,0 +1,235 @@
+#!/usr/bin/env python3
+"""Interactive raw-frame collector for the LED direction-sign dataset.
+
+The collector never opens /dev/video0.  It consumes the common ROS image
+topic, previews the same colour-preserving preprocessing used by the future
+recogniser, and saves the processed training frame for every capture.
+"""
+
+from __future__ import print_function
+
+import csv
+import threading
+import time
+from pathlib import Path
+
+import cv2
+import numpy as np
+import rospy
+import yaml
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+
+
+CLASS_KEYS = {
+    ord("0"): "negative",
+    ord("1"): "left",
+    ord("2"): "right",
+    ord("3"): "straight",
+    ord("4"): "stop",
+}
+
+
+class LedDatasetCollector(object):
+    def __init__(self):
+        rospy.init_node("led_dataset_collector")
+        self._bridge = CvBridge()
+        self._package_dir = Path(__file__).resolve().parent.parent
+        calibration_default = self._package_dir / "config" / "head_camera.yaml"
+        self._calibration_file = Path(
+            rospy.get_param("~calibration_file", str(calibration_default))
+        ).expanduser()
+        self._camera_matrix, self._distortion, self._calibration_size = self._load_calibration()
+
+        self._flip_horizontal = bool(rospy.get_param("~flip_horizontal", True))
+        self._brightness = float(rospy.get_param("~brightness", 0.0))
+        self._contrast = float(rospy.get_param("~contrast", 1.0))
+        self._saturation = float(rospy.get_param("~saturation", 1.0))
+        self._use_lab_clahe = bool(rospy.get_param("~lab_clahe", True))
+        self._clahe = cv2.createCLAHE(
+            clipLimit=float(rospy.get_param("~clahe_clip_limit", 1.5)),
+            tileGridSize=tuple(rospy.get_param("~clahe_tile_grid", [8, 8])),
+        )
+        self._gamma = float(rospy.get_param("~gamma", 1.0))
+        self._camera_profile = rospy.get_param("~camera_profile", "usb-cam-auto-exposure-auto-white-balance")
+        self._data_root = Path(
+            rospy.get_param("~data_root", str(Path.home() / "traffic_sign_data"))
+        ).expanduser()
+        self._processed_root = self._data_root / "processed"
+        # Keep new processed-only records separate from the earlier raw-pair log.
+        self._metadata_path = self._data_root / "processed_metadata.csv"
+        for label in CLASS_KEYS.values():
+            (self._processed_root / label).mkdir(parents=True, exist_ok=True)
+
+        self._selected_label = "left"
+        self._lock = threading.Lock()
+        self._latest_raw = None
+        self._latest_processed = None
+        self._latest_stamp_ns = 0
+        self._write_metadata_header()
+
+        self._preview_pub = rospy.Publisher(
+            "/traffic_sign/collector/processed_image", Image, queue_size=1
+        )
+        image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
+        self._image_sub = rospy.Subscriber(image_topic, Image, self._image_callback, queue_size=1)
+        rospy.loginfo(
+            "LED collector ready: image_topic=%s processed_root=%s keys: 0=negative 1=left 2=right 3=straight 4=stop s=save q=quit",
+            image_topic,
+            self._processed_root,
+        )
+
+    def _load_calibration(self):
+        try:
+            with self._calibration_file.open("r", encoding="utf-8") as stream:
+                calibration = yaml.safe_load(stream)
+            width = int(calibration["image_width"])
+            height = int(calibration["image_height"])
+            matrix = np.asarray(calibration["camera_matrix"]["data"], dtype=np.float64).reshape(3, 3)
+            distortion = np.asarray(
+                calibration["distortion_coefficients"]["data"], dtype=np.float64
+            ).reshape(-1, 1)
+        except (OSError, KeyError, TypeError, ValueError, yaml.YAMLError) as error:
+            raise RuntimeError("Unable to load camera calibration %s: %s" % (self._calibration_file, error))
+        return matrix, distortion, (width, height)
+
+    def _write_metadata_header(self):
+        self._data_root.mkdir(parents=True, exist_ok=True)
+        if self._metadata_path.exists():
+            return
+        with self._metadata_path.open("w", newline="", encoding="utf-8") as stream:
+            csv.writer(stream).writerow(
+                [
+                    "processed_filename",
+                    "label",
+                    "saved_at_utc",
+                    "camera_profile",
+                    "calibration_file",
+                    "flip_horizontal",
+                    "lab_clahe",
+                    "clahe_clip_limit",
+                    "gamma",
+                ]
+            )
+
+    def _process(self, raw):
+        undistorted = cv2.undistort(raw, self._camera_matrix, self._distortion)
+        processed = cv2.flip(undistorted, 1) if self._flip_horizontal else undistorted
+        if self._contrast != 1.0 or self._brightness != 0.0:
+            processed = cv2.convertScaleAbs(
+                processed, alpha=self._contrast, beta=self._brightness
+            )
+        if self._saturation != 1.0:
+            hsv = cv2.cvtColor(processed, cv2.COLOR_BGR2HSV)
+            hsv[:, :, 1] = np.clip(
+                hsv[:, :, 1].astype(np.float32) * self._saturation, 0, 255
+            ).astype(np.uint8)
+            processed = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
+        if self._use_lab_clahe:
+            lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
+            lab[:, :, 0] = self._clahe.apply(lab[:, :, 0])
+            processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
+        if self._gamma != 1.0:
+            lookup = np.array(
+                [((value / 255.0) ** self._gamma) * 255.0 for value in range(256)], dtype=np.uint8
+            )
+            processed = cv2.LUT(processed, lookup)
+        return processed
+
+    def _image_callback(self, message):
+        try:
+            raw = self._bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except CvBridgeError as error:
+            rospy.logerr_throttle(5.0, "collector image conversion failed: %s", error)
+            return
+        width, height = self._calibration_size
+        if raw.shape[:2] != (height, width):
+            rospy.logwarn_throttle(
+                5.0,
+                "collector image size %dx%d differs from calibration %dx%d; frame skipped",
+                raw.shape[1], raw.shape[0], width, height,
+            )
+            return
+        processed = self._process(raw)
+        with self._lock:
+            self._latest_raw = raw.copy()
+            self._latest_processed = processed
+            self._latest_stamp_ns = message.header.stamp.to_nsec()
+        try:
+            self._preview_pub.publish(self._bridge.cv2_to_imgmsg(processed, encoding="bgr8"))
+        except CvBridgeError as error:
+            rospy.logerr_throttle(5.0, "collector preview publish failed: %s", error)
+
+    def _save_current_frame(self):
+        with self._lock:
+            if self._latest_processed is None:
+                rospy.logwarn("No camera frame received; nothing saved")
+                return
+            processed = self._latest_processed.copy()
+            stamp_ns = self._latest_stamp_ns or time.time_ns()
+        filename = "%s_%019d.png" % (self._selected_label, stamp_ns)
+        processed_destination = self._processed_root / self._selected_label / filename
+        if not cv2.imwrite(str(processed_destination), processed):
+            rospy.logerr("Failed to save processed frame: %s", processed_destination)
+            return
+        with self._metadata_path.open("a", newline="", encoding="utf-8") as stream:
+            csv.writer(stream).writerow(
+                [
+                    str(processed_destination.relative_to(self._data_root)),
+                    self._selected_label,
+                    time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
+                    self._camera_profile,
+                    str(self._calibration_file),
+                    self._flip_horizontal,
+                    self._use_lab_clahe,
+                    self._clahe.getClipLimit(),
+                    self._gamma,
+                ]
+            )
+        rospy.loginfo(
+            "Saved processed %s frame: %s",
+            self._selected_label,
+            processed_destination,
+        )
+
+    def _display(self, raw, processed):
+        preview = np.hstack((raw, processed))
+        cv2.putText(preview, "RAW", (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
+        offset = raw.shape[1]
+        cv2.putText(preview, "LED PREPROCESS", (offset + 12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
+        cv2.putText(
+            preview,
+            "label=%s | 0:negative 1:left 2:right 3:straight 4:stop | s:save q:quit" % self._selected_label,
+            (12, preview.shape[0] - 14),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.55,
+            (0, 255, 255),
+            2,
+        )
+        cv2.imshow("LED dataset collector", preview)
+
+    def run(self):
+        cv2.namedWindow("LED dataset collector", cv2.WINDOW_NORMAL)
+        while not rospy.is_shutdown():
+            with self._lock:
+                raw = None if self._latest_raw is None else self._latest_raw.copy()
+                processed = None if self._latest_processed is None else self._latest_processed.copy()
+            if raw is not None:
+                self._display(raw, processed)
+            key = cv2.waitKey(20) & 0xFF
+            if key in CLASS_KEYS:
+                self._selected_label = CLASS_KEYS[key]
+                rospy.loginfo("Collector label selected: %s", self._selected_label)
+            elif key in (ord("s"), ord("S")):
+                self._save_current_frame()
+            elif key in (ord("q"), ord("Q"), 27):
+                break
+        cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    try:
+        LedDatasetCollector().run()
+    except (RuntimeError, ValueError) as error:
+        rospy.logfatal("LED collector did not start: %s", error)
+        raise

traffic_sign_recognition/scripts/traffic_sign_node

@@ -0,0 +1,37 @@
+#!/usr/bin/env bash
+# Keep the NPU runtime isolated from the system Python used by ROS Noetic.
+set -uo pipefail
+
+script_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
+camera_device="/dev/video0"
+child_pid=""
+
+# This is deliberately in the outer process: rknnlite/rospy shutdown may not
+# run Python callbacks after roslaunch sends SIGINT.  The profile is global to
+# the USB camera, so always leave it in the normal automatic state on exit.
+restore_camera_profile() {
+  v4l2-ctl -d "$camera_device" --set-ctrl=exposure_auto=3 >/dev/null 2>&1 || true
+  v4l2-ctl -d "$camera_device" --set-ctrl=exposure_auto_priority=0 >/dev/null 2>&1 || true
+  v4l2-ctl -d "$camera_device" --set-ctrl=white_balance_temperature_auto=1 >/dev/null 2>&1 || true
+  v4l2-ctl -d "$camera_device" --set-ctrl=brightness=0 >/dev/null 2>&1 || true
+  v4l2-ctl -d "$camera_device" --set-ctrl=contrast=50 >/dev/null 2>&1 || true
+  v4l2-ctl -d "$camera_device" --set-ctrl=saturation=50 >/dev/null 2>&1 || true
+  v4l2-ctl -d "$camera_device" --set-ctrl=gamma=300 >/dev/null 2>&1 || true
+  echo "traffic_sign_node: camera profile restored to automatic exposure" >&2
+}
+
+forward_shutdown() {
+  if [[ -n "$child_pid" ]]; then
+    kill -INT "$child_pid" 2>/dev/null || true
+    wait "$child_pid" 2>/dev/null || true
+  fi
+  exit 0
+}
+
+trap restore_camera_profile EXIT
+trap forward_shutdown INT TERM
+
+/home/ucar/venv3.9/bin/python3 "$script_dir/traffic_sign_node.py" "$@" &
+child_pid=$!
+wait "$child_pid"
+exit $?

traffic_sign_recognition/scripts/traffic_sign_node.py

@@ -0,0 +1,516 @@
+#!/usr/bin/env python3
+"""Recognise LED direction signs with an RKNN YOLOv5 model.
+
+The node deliberately consumes the shared camera topic instead of opening a
+camera device.  Camera exposure, gain and white balance remain owned by the
+single camera-driver node.
+"""
+
+from __future__ import annotations
+
+import atexit
+from collections import Counter, deque
+import logging
+from pathlib import Path
+import subprocess
+import threading
+from typing import Iterable, Optional, Sequence, Tuple
+
+import cv2
+import numpy as np
+import rospy
+import yaml
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+from std_msgs.msg import Float32, String
+from std_srvs.srv import SetBool, SetBoolResponse
+
+try:
+    from rknnlite.api import RKNNLite
+except ImportError as error:  # pragma: no cover - depends on target hardware
+    raise RuntimeError(
+        "Unable to import RKNNLite. Start this node through the "
+        "traffic_sign_node wrapper so it uses /home/ucar/venv3.9."
+    ) from error
+
+# rknnlite 1.5.2 replaces Python's normal log level names (``DEBUG``,
+# ``INFO``...) with one-character variants.  ROS Noetic's logging config uses
+# the normal names, so restore them before rospy.init_node() configures logs.
+for _level, _name in (
+    (logging.CRITICAL, "CRITICAL"),
+    (logging.ERROR, "ERROR"),
+    (logging.WARNING, "WARNING"),
+    (logging.INFO, "INFO"),
+    (logging.DEBUG, "DEBUG"),
+    (logging.NOTSET, "NOTSET"),
+):
+    logging.addLevelName(_level, _name)
+
+
+DEFAULT_CLASSES = ("left", "right", "straight", "stop")
+DEFAULT_ANCHORS = np.array(
+    [[10, 13], [16, 30], [33, 23], [30, 61], [62, 45], [59, 119],
+     [116, 90], [156, 198], [373, 326]],
+    dtype=np.float32,
+)
+DEFAULT_MASKS = ((0, 1, 2), (3, 4, 5), (6, 7, 8))
+
+
+def sigmoid(values: np.ndarray) -> np.ndarray:
+    return 1.0 / (1.0 + np.exp(-values))
+
+
+def letterbox(image: np.ndarray, size: int) -> Tuple[np.ndarray, float, Tuple[float, float]]:
+    """Resize without stretching and return scale/padding for box restoration."""
+    height, width = image.shape[:2]
+    scale = min(float(size) / height, float(size) / width)
+    resized_width, resized_height = int(round(width * scale)), int(round(height * scale))
+    resized = cv2.resize(image, (resized_width, resized_height), interpolation=cv2.INTER_LINEAR)
+    pad_x = (size - resized_width) / 2.0
+    pad_y = (size - resized_height) / 2.0
+    bordered = cv2.copyMakeBorder(
+        resized,
+        int(round(pad_y - 0.1)),
+        int(round(pad_y + 0.1)),
+        int(round(pad_x - 0.1)),
+        int(round(pad_x + 0.1)),
+        cv2.BORDER_CONSTANT,
+        value=(114, 114, 114),
+    )
+    return bordered, scale, (pad_x, pad_y)
+
+
+def nms_boxes(boxes: np.ndarray, scores: np.ndarray, threshold: float) -> np.ndarray:
+    x1, y1, x2, y2 = boxes.T
+    areas = (x2 - x1) * (y2 - y1)
+    order = scores.argsort()[::-1]
+    keep = []
+    while order.size:
+        current = order[0]
+        keep.append(current)
+        if order.size == 1:
+            break
+        remaining = order[1:]
+        xx1 = np.maximum(x1[current], x1[remaining])
+        yy1 = np.maximum(y1[current], y1[remaining])
+        xx2 = np.minimum(x2[current], x2[remaining])
+        yy2 = np.minimum(y2[current], y2[remaining])
+        intersection = np.maximum(0.0, xx2 - xx1) * np.maximum(0.0, yy2 - yy1)
+        union = areas[current] + areas[remaining] - intersection
+        iou = intersection / np.maximum(union, 1e-6)
+        order = remaining[iou <= threshold]
+    return np.asarray(keep, dtype=np.int32)
+
+
+class TrafficSignRecognizer:
+    def __init__(self) -> None:
+        rospy.init_node("traffic_sign_recognition")
+        self._bridge = CvBridge()
+        self._state_lock = threading.RLock()
+        self._package_dir = Path(__file__).resolve().parent.parent
+        default_calibration = self._package_dir / "config" / "head_camera.yaml"
+        self._calibration_file = Path(
+            rospy.get_param("~calibration_file", str(default_calibration))
+        ).expanduser()
+        self._camera_matrix, self._distortion_coefficients, self._calibration_size = (
+            self._load_camera_calibration(self._calibration_file)
+        )
+        self._classes = tuple(rospy.get_param("~classes", list(DEFAULT_CLASSES)))
+        self._input_size = int(rospy.get_param("~input_size", 640))
+        self._object_threshold = float(rospy.get_param("~object_threshold", 0.70))
+        self._nms_threshold = float(rospy.get_param("~nms_threshold", 0.45))
+        self._flip_horizontal = bool(rospy.get_param("~flip_horizontal", True))
+        self._brightness = float(rospy.get_param("~brightness", 0.0))
+        self._contrast = float(rospy.get_param("~contrast", 1.0))
+        self._saturation = float(rospy.get_param("~saturation", 1.0))
+        self._lab_clahe = bool(rospy.get_param("~lab_clahe", True))
+        self._clahe = cv2.createCLAHE(
+            clipLimit=float(rospy.get_param("~clahe_clip_limit", 1.5)),
+            tileGridSize=tuple(rospy.get_param("~clahe_tile_grid", [8, 8])),
+        )
+        self._gamma = float(rospy.get_param("~gamma", 1.0))
+        self._stability_window = int(rospy.get_param("~stability_window", 5))
+        self._stable_count = int(rospy.get_param("~stable_count", 4))
+        if self._stable_count > self._stability_window:
+            raise ValueError("stable_count must not exceed stability_window")
+        self._history: deque[Optional[str]] = deque(maxlen=self._stability_window)
+        self._last_direction = "NONE"
+
+        default_model = self._package_dir / "models" / "traffic_sign_direction.rknn"
+        configured_model = Path(rospy.get_param("~model_path", str(default_model))).expanduser()
+        self._model_path = configured_model
+        if not self._model_path.is_file():
+            raise FileNotFoundError(
+                "RKNN model is missing: %s. Train and convert the LED sign model, then place "
+                "traffic_sign_direction.rknn in this package's models directory or set ~model_path."
+                % self._model_path
+            )
+
+        self._rknn = RKNNLite()
+        result = self._rknn.load_rknn(str(self._model_path))
+        if result != 0:
+            raise RuntimeError("RKNN model load failed with code %s: %s" % (result, self._model_path))
+        result = self._rknn.init_runtime()
+        if result != 0:
+            raise RuntimeError("RKNN runtime initialisation failed with code %s" % result)
+        self._camera_profile_enabled = bool(rospy.get_param("~camera_profile_enabled", False))
+        self._camera_device = str(rospy.get_param("~camera_device", "/dev/video0"))
+        self._restore_camera_on_shutdown = bool(
+            rospy.get_param("~restore_camera_on_shutdown", True)
+        )
+        # Cache controls now: ROS parameter access is not reliable once the
+        # ROS shutdown sequence has begun.
+        self._led_camera_controls = (
+            ("exposure_auto", rospy.get_param("~led_exposure_auto", 1)),
+            ("exposure_auto_priority", rospy.get_param("~led_exposure_auto_priority", 0)),
+            ("exposure_absolute", rospy.get_param("~led_exposure_absolute", 50)),
+            ("white_balance_temperature_auto", rospy.get_param("~led_white_balance_auto", True)),
+        )
+        self._restore_camera_controls = (
+            ("exposure_auto", rospy.get_param("~restore_exposure_auto", 3)),
+            ("exposure_auto_priority", rospy.get_param("~restore_exposure_auto_priority", 0)),
+            ("white_balance_temperature_auto", rospy.get_param("~restore_white_balance_auto", True)),
+            ("brightness", rospy.get_param("~restore_brightness", 0)),
+            ("contrast", rospy.get_param("~restore_contrast", 50)),
+            ("saturation", rospy.get_param("~restore_saturation", 50)),
+            ("gamma", rospy.get_param("~restore_gamma", 300)),
+        )
+        self._camera_profile_active = False
+        self._enabled = bool(rospy.get_param("~enabled", True))
+        if self._camera_profile_enabled and self._enabled:
+            self._apply_led_camera_profile()
+        rospy.on_shutdown(self._release_runtime)
+        rospy.on_shutdown(self._restore_camera_profile)
+        atexit.register(self._restore_camera_profile)
+
+        self._direction_pub = rospy.Publisher("/traffic_sign/direction", String, queue_size=1)
+        self._confidence_pub = rospy.Publisher("/traffic_sign/confidence", Float32, queue_size=1)
+        self._debug_pub = rospy.Publisher("/traffic_sign/debug_image", Image, queue_size=1)
+        self._enable_service = rospy.Service(
+            "~set_enabled", SetBool, self._set_enabled
+        )
+        image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
+        self._image_sub = rospy.Subscriber(image_topic, Image, self._image_callback, queue_size=1)
+        rospy.loginfo(
+            "traffic_sign_recognition ready: model=%s image_topic=%s calibration=%s classes=%s enabled=%s",
+            self._model_path,
+            image_topic,
+            self._calibration_file,
+            ",".join(self._classes),
+            self._enabled,
+        )
+
+    def _set_enabled(self, request) -> SetBoolResponse:
+        """Enable inference/LED exposure or restore the shared camera profile."""
+        requested = bool(request.data)
+        with self._state_lock:
+            if requested == self._enabled:
+                # A previous disable request may have stopped inference but
+                # failed midway through restoring V4L2 controls. Allow a
+                # repeated disable request (for example from FAULT handling)
+                # to retry that safety-critical restoration.
+                if not requested and self._camera_profile_active:
+                    try:
+                        self._restore_camera_profile(raise_on_error=True)
+                    except RuntimeError as error:
+                        return SetBoolResponse(success=False, message=str(error))
+                return SetBoolResponse(
+                    success=True,
+                    message="traffic sign recognition already %s"
+                    % ("enabled" if requested else "disabled"),
+                )
+
+            if requested:
+                try:
+                    if self._camera_profile_enabled:
+                        self._apply_led_camera_profile()
+                except RuntimeError as error:
+                    return SetBoolResponse(success=False, message=str(error))
+                self._history.clear()
+                self._last_direction = "NONE"
+                self._enabled = True
+                rospy.set_param("~enabled", True)
+                self._direction_pub.publish(String(data="NONE"))
+                self._confidence_pub.publish(Float32(data=0.0))
+                rospy.loginfo("Traffic sign recognition enabled.")
+                return SetBoolResponse(success=True, message="recognition enabled")
+
+            # Mark disabled before restoring exposure so no new callback can
+            # start inference using a half-restored camera frame.
+            self._enabled = False
+            self._history.clear()
+            self._last_direction = "NONE"
+            self._direction_pub.publish(String(data="NONE"))
+            self._confidence_pub.publish(Float32(data=0.0))
+            try:
+                self._restore_camera_profile(raise_on_error=True)
+            except RuntimeError as error:
+                return SetBoolResponse(success=False, message=str(error))
+            rospy.set_param("~enabled", False)
+            rospy.loginfo("Traffic sign recognition disabled; camera profile restored.")
+            return SetBoolResponse(success=True, message="recognition disabled")
+
+    def _release_runtime(self) -> None:
+        if getattr(self, "_rknn", None) is not None:
+            self._rknn.release()
+            self._rknn = None
+
+    def _set_camera_controls(self, controls: Sequence[Tuple[str, object]]) -> None:
+        """Set the shared USB camera controls without opening a second camera node."""
+        for name, value in controls:
+            command = [
+                "v4l2-ctl",
+                "-d",
+                self._camera_device,
+                "--set-ctrl=%s=%s" % (name, int(value) if isinstance(value, bool) else value),
+            ]
+            try:
+                subprocess.run(command, check=True, capture_output=True, text=True, timeout=3)
+            except (OSError, subprocess.SubprocessError) as error:
+                raise RuntimeError("Cannot set camera control %s: %s" % (name, error))
+
+    def _apply_led_camera_profile(self) -> None:
+        # Mark active first so a partially-applied V4L2 sequence can still be
+        # restored by a subsequent disable/fault request.
+        self._camera_profile_active = True
+        self._set_camera_controls(self._led_camera_controls)
+        rospy.loginfo("LED camera profile enabled on %s: manual exposure=%s", self._camera_device,
+                      dict(self._led_camera_controls)["exposure_absolute"])
+
+    def _restore_camera_profile(self, raise_on_error: bool = False) -> None:
+        if not getattr(self, "_camera_profile_active", False) or not self._restore_camera_on_shutdown:
+            return
+        try:
+            self._set_camera_controls(self._restore_camera_controls)
+            self._camera_profile_active = False
+            rospy.loginfo("Automatic USB camera profile restored on %s", self._camera_device)
+        except RuntimeError as error:
+            rospy.logerr("Unable to restore automatic camera profile: %s", error)
+            if raise_on_error:
+                raise
+
+    @staticmethod
+    def _load_camera_calibration(
+        calibration_file: Path,
+    ) -> Tuple[np.ndarray, np.ndarray, Tuple[int, int]]:
+        """Load the USB camera calibration and reject incomplete files early."""
+        try:
+            with calibration_file.open("r", encoding="utf-8") as stream:
+                calibration = yaml.safe_load(stream)
+            width = int(calibration["image_width"])
+            height = int(calibration["image_height"])
+            matrix = np.asarray(calibration["camera_matrix"]["data"], dtype=np.float64).reshape(3, 3)
+            coefficients = np.asarray(
+                calibration["distortion_coefficients"]["data"], dtype=np.float64
+            ).reshape(-1, 1)
+        except (OSError, KeyError, TypeError, ValueError, yaml.YAMLError) as error:
+            raise RuntimeError("Unable to load camera calibration %s: %s" % (calibration_file, error))
+        return matrix, coefficients, (width, height)
+
+    def _preprocess_camera_frame(self, frame: np.ndarray) -> np.ndarray:
+        """Preserve LED colour while applying the shared, versioned LED preprocessing."""
+        processed = cv2.flip(frame, 1) if self._flip_horizontal else frame.copy()
+        if self._contrast != 1.0 or self._brightness != 0.0:
+            processed = cv2.convertScaleAbs(
+                processed, alpha=self._contrast, beta=self._brightness
+            )
+        if self._saturation != 1.0:
+            hsv = cv2.cvtColor(processed, cv2.COLOR_BGR2HSV)
+            hsv[:, :, 1] = np.clip(
+                hsv[:, :, 1].astype(np.float32) * self._saturation, 0, 255
+            ).astype(np.uint8)
+            processed = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
+        if self._lab_clahe:
+            lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
+            lab[:, :, 0] = self._clahe.apply(lab[:, :, 0])
+            processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
+        if self._gamma != 1.0:
+            lookup = np.array(
+                [((value / 255.0) ** self._gamma) * 255.0 for value in range(256)], dtype=np.uint8
+            )
+            processed = cv2.LUT(processed, lookup)
+        return processed
+
+    def _decode_output(self, outputs: Sequence[np.ndarray]) -> Tuple[Optional[np.ndarray], Optional[np.ndarray], Optional[np.ndarray]]:
+        # ``export.py --include onnx`` creates YOLOv5's decoded output
+        # [batch, 25200, 5 + class_count].  Some RKNN-export pipelines expose
+        # the three raw detection heads instead, so keep support for both.
+        if len(outputs) == 1:
+            # RKNN Lite may retain a singleton batch dimension and/or append
+            # a singleton stride-alignment dimension: (1, 25200, 9, 1).
+            # Remove only dimensions of length one, yielding (25200, 9).
+            values = np.squeeze(np.asarray(outputs[0]))
+            if values.ndim == 2 and values.shape[0] == 5 + len(self._classes):
+                values = values.T
+            if values.ndim != 2 or values.shape[1] != 5 + len(self._classes):
+                raise RuntimeError(
+                    "Unsupported decoded YOLOv5 RKNN output shape: %s" % (values.shape,)
+                )
+
+            objectness = values[:, 4:5]
+            class_scores = values[:, 5:] * objectness
+            classes = np.argmax(class_scores, axis=1)
+            scores = np.max(class_scores, axis=1)
+            selected = scores >= self._object_threshold
+            if not np.any(selected):
+                return None, None, None
+
+            xywh = values[selected, :4]
+            boxes = np.concatenate((xywh[:, :2] - xywh[:, 2:] / 2.0,
+                                    xywh[:, :2] + xywh[:, 2:] / 2.0), axis=1)
+            return self._classwise_nms(boxes, classes[selected], scores[selected])
+
+        if len(outputs) != 3:
+            raise RuntimeError(
+                "Expected one decoded or three raw YOLOv5 RKNN outputs, got %d" % len(outputs)
+            )
+
+        all_boxes, all_classes, all_scores = [], [], []
+        for output, mask in zip(outputs, DEFAULT_MASKS):
+            values = np.asarray(output)
+            if values.ndim == 4 and values.shape[0] == 1:
+                values = values[0]
+            if values.ndim != 3:
+                raise RuntimeError("Unsupported RKNN output shape: %s" % (values.shape,))
+            if values.shape[0] % 3 == 0:
+                values = values.reshape(3, -1, values.shape[1], values.shape[2]).transpose(2, 3, 0, 1)
+            elif values.shape[-1] % 3 == 0:
+                values = values.reshape(values.shape[0], values.shape[1], 3, -1)
+            else:
+                raise RuntimeError("Cannot interpret RKNN output shape: %s" % (values.shape,))
+
+            grid_height, grid_width = values.shape[:2]
+            anchors = DEFAULT_ANCHORS[list(mask)]
+            confidence = sigmoid(values[..., 4:5])
+            class_probabilities = sigmoid(values[..., 5:])
+            class_scores = class_probabilities * confidence
+            classes = np.argmax(class_scores, axis=-1)
+            scores = np.max(class_scores, axis=-1)
+            selected = scores >= self._object_threshold
+            if not np.any(selected):
+                continue
+
+            grid_x, grid_y = np.meshgrid(np.arange(grid_width), np.arange(grid_height))
+            grid = np.stack((grid_x, grid_y), axis=-1)[..., None, :]
+            xy = (sigmoid(values[..., :2]) * 2.0 - 0.5 + grid) * (self._input_size / grid_height)
+            wh = (sigmoid(values[..., 2:4]) * 2.0) ** 2 * anchors[None, None, :, :]
+            xyxy = np.concatenate((xy - wh / 2.0, xy + wh / 2.0), axis=-1)
+            all_boxes.append(xyxy[selected])
+            all_classes.append(classes[selected])
+            all_scores.append(scores[selected])
+
+        if not all_boxes:
+            return None, None, None
+
+        boxes = np.concatenate(all_boxes)
+        classes = np.concatenate(all_classes)
+        scores = np.concatenate(all_scores)
+        return self._classwise_nms(boxes, classes, scores)
+
+    def _classwise_nms(
+        self,
+        boxes: np.ndarray,
+        classes: np.ndarray,
+        scores: np.ndarray,
+    ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+        """Suppress overlapping candidates independently for each class."""
+        kept_boxes, kept_classes, kept_scores = [], [], []
+        for class_index in np.unique(classes):
+            class_indices = np.where(classes == class_index)[0]
+            keep = nms_boxes(boxes[class_indices], scores[class_indices], self._nms_threshold)
+            kept_boxes.append(boxes[class_indices][keep])
+            kept_classes.append(classes[class_indices][keep])
+            kept_scores.append(scores[class_indices][keep])
+        return np.concatenate(kept_boxes), np.concatenate(kept_classes), np.concatenate(kept_scores)
+
+    def _stable_direction(self, candidate: Optional[str]) -> str:
+        self._history.append(candidate)
+        votes = Counter(value for value in self._history if value is not None)
+        if not votes:
+            return "NONE"
+        direction, count = votes.most_common(1)[0]
+        return direction.upper() if count >= self._stable_count else "NONE"
+
+    @staticmethod
+    def _restore_boxes(boxes: np.ndarray, scale: float, padding: Tuple[float, float], width: int, height: int) -> np.ndarray:
+        restored = boxes.copy()
+        restored[:, [0, 2]] = (restored[:, [0, 2]] - padding[0]) / scale
+        restored[:, [1, 3]] = (restored[:, [1, 3]] - padding[1]) / scale
+        restored[:, [0, 2]] = np.clip(restored[:, [0, 2]], 0, width - 1)
+        restored[:, [1, 3]] = np.clip(restored[:, [1, 3]], 0, height - 1)
+        return restored
+
+    def _image_callback(self, message: Image) -> None:
+        with self._state_lock:
+            if not self._enabled:
+                return
+            self._process_enabled_image(message)
+
+    def _process_enabled_image(self, message: Image) -> None:
+        try:
+            camera_frame = self._bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except CvBridgeError as error:
+            rospy.logerr_throttle(5.0, "traffic sign image conversion failed: %s", error)
+            return
+
+        expected_width, expected_height = self._calibration_size
+        if camera_frame.shape[:2] != (expected_height, expected_width):
+            rospy.logwarn_throttle(
+                5.0,
+                "traffic sign image size %dx%d differs from calibration %dx%d; frame skipped",
+                camera_frame.shape[1],
+                camera_frame.shape[0],
+                expected_width,
+                expected_height,
+            )
+            return
+
+        undistorted = cv2.undistort(camera_frame, self._camera_matrix, self._distortion_coefficients)
+        processed = self._preprocess_camera_frame(undistorted)
+        model_input, scale, padding = letterbox(processed, self._input_size)
+        model_input = cv2.cvtColor(model_input, cv2.COLOR_BGR2RGB)
+        try:
+            # The OpenCV image is HWC RGB.  State this explicitly instead of
+            # relying on RKNN Lite's default (which can be NCHW for ONNX
+            # models and yields near-zero detections with an HWC buffer).
+            outputs = self._rknn.inference(inputs=[model_input], data_format="nhwc")
+            boxes, classes, scores = self._decode_output(outputs)
+        except Exception as error:
+            rospy.logerr_throttle(5.0, "traffic sign inference failed: %s", error)
+            return
+
+        candidate, candidate_score = None, 0.0
+        debug = processed.copy()
+        if boxes is not None:
+            boxes = self._restore_boxes(boxes, scale, padding, debug.shape[1], debug.shape[0])
+            best_index = int(np.argmax(scores))
+            candidate = self._classes[int(classes[best_index])]
+            candidate_score = float(scores[best_index])
+            for box, class_index, score in zip(boxes, classes, scores):
+                label = self._classes[int(class_index)].upper()
+                x1, y1, x2, y2 = box.astype(int)
+                cv2.rectangle(debug, (x1, y1), (x2, y2), (0, 255, 0), 2)
+                cv2.putText(debug, "%s %.2f" % (label, score), (x1, max(20, y1 - 6)),
+                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
+
+        stable = self._stable_direction(candidate)
+        self._last_direction = stable
+        self._direction_pub.publish(String(data=stable))
+        self._confidence_pub.publish(Float32(data=candidate_score if stable != "NONE" else 0.0))
+        cv2.putText(debug, "STABLE: %s" % stable, (12, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
+                    (0, 255, 255), 2)
+        try:
+            self._debug_pub.publish(self._bridge.cv2_to_imgmsg(debug, encoding="bgr8"))
+        except CvBridgeError as error:
+            rospy.logerr_throttle(5.0, "traffic sign debug image publish failed: %s", error)
+
+    def run(self) -> None:
+        rospy.spin()
+
+
+if __name__ == "__main__":
+    try:
+        TrafficSignRecognizer().run()
+    except (RuntimeError, FileNotFoundError, ValueError) as error:
+        rospy.logfatal("traffic_sign_recognition did not start: %s", error)
+        raise

traffic_sign_recognition/tools/convert_onnx_to_rknn.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+"""Convert the exported YOLOv5 ONNX model to an INT8 RK3588 RKNN model.
+
+Run this script on an x86_64 Ubuntu 20.04 / Python 3.8 machine with
+RKNN Toolkit2 1.5.2 installed.  It is deliberately not run on the robot;
+the robot only needs RKNN Toolkit Lite2 to execute the resulting file.
+"""
+
+from __future__ import annotations
+
+import argparse
+import sys
+from pathlib import Path
+
+from rknn.api import RKNN
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--onnx", required=True, type=Path, help="Path to best.onnx")
+    parser.add_argument(
+        "--dataset",
+        required=True,
+        type=Path,
+        help="Text file containing absolute paths to representative PNG/JPG images",
+    )
+    parser.add_argument(
+        "--output",
+        type=Path,
+        default=Path("traffic_sign_direction.rknn"),
+        help="Output RKNN model path",
+    )
+    parser.add_argument(
+        "--no-quantize",
+        action="store_true",
+        help="Build an FP model for conversion diagnosis (larger and slower than INT8)",
+    )
+    return parser.parse_args()
+
+
+def check_inputs(onnx_path: Path, dataset_path: Path, needs_calibration: bool) -> None:
+    if not onnx_path.is_file():
+        raise FileNotFoundError("ONNX model not found: %s" % onnx_path)
+    if not needs_calibration:
+        return
+    if not dataset_path.is_file():
+        raise FileNotFoundError("Calibration dataset list not found: %s" % dataset_path)
+
+    images = [Path(line.strip()) for line in dataset_path.read_text(encoding="utf-8").splitlines() if line.strip()]
+    if len(images) < 20:
+        raise ValueError("Use at least 20 representative images for INT8 calibration; got %d" % len(images))
+    missing = [str(image) for image in images if not image.is_file()]
+    if missing:
+        raise FileNotFoundError("Missing image paths in %s, first: %s" % (dataset_path, missing[0]))
+    print("INT8 calibration images: %d" % len(images))
+
+
+def main() -> int:
+    args = parse_args()
+    onnx_path = args.onnx.expanduser().resolve()
+    dataset_path = args.dataset.expanduser().resolve()
+    output_path = args.output.expanduser().resolve()
+    quantize = not args.no_quantize
+    check_inputs(onnx_path, dataset_path, needs_calibration=quantize)
+    output_path.parent.mkdir(parents=True, exist_ok=True)
+
+    rknn = RKNN(verbose=True)
+    try:
+        # The ROS node supplies a 640x640 RGB uint8 image.  RKNN performs the
+        # equivalent of YOLOv5's /255 input normalization inside the graph.
+        result = rknn.config(
+            target_platform="rk3588",
+            mean_values=[[0, 0, 0]],
+            std_values=[[255, 255, 255]],
+            optimization_level=3,
+        )
+        if result != 0:
+            raise RuntimeError("rknn.config failed: %s" % result)
+        result = rknn.load_onnx(model=str(onnx_path))
+        if result != 0:
+            raise RuntimeError("rknn.load_onnx failed: %s" % result)
+        build_options = {"do_quantization": quantize}
+        if quantize:
+            build_options["dataset"] = str(dataset_path)
+        result = rknn.build(**build_options)
+        if result != 0:
+            raise RuntimeError("rknn.build failed: %s" % result)
+        result = rknn.export_rknn(str(output_path))
+        if result != 0:
+            raise RuntimeError("rknn.export_rknn failed: %s" % result)
+    finally:
+        rknn.release()
+
+    print("RKNN conversion complete (%s): %s" % ("INT8" if quantize else "FP", output_path))
+    return 0
+
+
+if __name__ == "__main__":
+    try:
+        raise SystemExit(main())
+    except (FileNotFoundError, RuntimeError, ValueError) as error:
+        print("ERROR: %s" % error, file=sys.stderr)
+        raise SystemExit(1)