# 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.