巡线行走

概述 / Overview

"巡线行走"是 MDCS Clumsy 最基础的运动原语：给一个起点 (srcX, srcY) 和终点 (dstX, dstY)，AGV 沿直线段从起点驶到终点。它由 `SteeringLineFollowing`（正向）和 `SteeringLineFollowingReverse`（反向）两个 `MovementDefinition` 子类实现，所有其它复杂动作（取放货、绕障、双车联动）都是在它之上组合而成。

"Line following" is the most basic Clumsy motion primitive: given (srcX, srcY) and (dstX, dstY), the AGV runs a straight segment between them. It's implemented by `SteeringLineFollowing` (forward) and `SteeringLineFollowingReverse` (reverse), both `MovementDefinition` subclasses. Every higher action (pick/place, obstacle avoidance, twin-car coordination) composes on top.

文件 / Files: `D:\src\Clumsy\ClumsyCore\Legacy\SteeringLineFollowing.cs`、`SteeringLineFollowingReverse.cs`。

使用场景 / When to use

站点对点的直线行驶（无需绕障 / 无需精对位）。
取放货的 粗对位阶段（开工位识别之前的逼近）。
反向出库（用 `Reverse` 变体）。
简单调度的最小可用配置。

Point-to-point straight driving on uncongested aisles.
Coarse-approach stage before station-aware pickup.
Reverse withdraw (the `Reverse` variant).
Minimum config for a quick-running scheduler.

关键字段 / Key fields

字段 / Field	类型	含义 / Meaning
`srcX`, `srcY`	`double`	起点 / segment start (mm)
`dstX`, `dstY`	`double`	终点 / segment end (mm)
`basespeed`	`int`	巡航速度 / cruise speed (mm/s)
`lookAhead`	`float`	纯跟踪前瞻距离 / pure-pursuit look-ahead (mm)
`dFactor`	`float`	横向偏差转化为转向的增益 / lateral-error → steer gain
`slowdown`	`bool`	临近终点时降速 / decelerate near goal (true by default)
`finSpeed`	`int`	终点最低速度（slowdown 触发后）/ final speed when slowdown active
`slowdownDist`	`float`	进入减速段的距离阈值 / decel-zone size (mm)

`SteeringLineFollowingReverse` 字段完全一致；语义为车头朝向不变、车体后退到目标。 `SteeringLineFollowingReverse` has the same fields; the car drives in reverse while keeping its heading unchanged.

工作机制 / How it works

1. `MovementDefinition.Get()` 是一个 可枚举的 bool 序列，每个 tick 返回一次 `true`（表示继续），直到完成时返回 `false` / 退出枚举。 2. `DriveTask.WaitDriveTask(IEnumerable<bool>)` 把这个序列每隔 `Configuration.conf.DriveTaskInterval` ms 调一次，并阻塞调用方直到序列结束。 3. 每个 tick 内部：

  - 读当前位姿 `(x, y, th)`（来自 Detour，已订阅）。
  - 计算前瞻点（沿目标线段往前 `lookAhead`）。
  - 用纯跟踪法求转向角；`vCmd` 取 `basespeed`（若进入减速段则衰减到 `finSpeed`）。
  - 写到 Medulla 上位 IO。

4. 当车体距 `(dstX, dstY)` 小于阈值（默认 50 mm），返回 `false` 结束。

1. `MovementDefinition.Get()` returns an `IEnumerable<bool>` ticked once per control cycle; `true` = keep going, end-of-iteration = done. 2. `DriveTask.WaitDriveTask(...)` ticks at `Configuration.conf.DriveTaskInterval` ms and blocks the caller until the iteration ends. 3. Each tick: read pose → compute look-ahead point → pure-pursuit steer → write `vCmd` / `steerCmd`. 4. Exits when within ~50 mm of `(dstX, dstY)`.

调用样例 / Example

public void DriveStraight(double sx, double sy, double dx, double dy)
{
    Queue(async () =>
    {
        DriveTask.WaitDriveTask(new SteeringLineFollowing
        {
            srcX = sx, srcY = sy,
            dstX = dx, dstY = dy,
            basespeed = 600,
            lookAhead = 500,
            dFactor   = 1.2f,
            slowdown  = true,
            finSpeed  = 100
        }.Follow());
    });
}

调试要点 / Tuning

现象 / Symptom	调整 / Adjustment
路径偏移 / Drifts off the line	增大 `dFactor`（默认 1.2），但太大会震荡
在终点震荡 / Oscillates at goal	启用 `slowdown` 并增大 `slowdownDist`
终点冲过 / Overshoots	降低 `basespeed` 或减小 `finSpeed`
弯道内切 / Cuts inside corner (短线段段拼接时)	减小 `lookAhead`
慢速时跟不准 / Poor tracking at low speed	`dFactor` 与速度成反比；低速时增大
反向跑歪 / Reverse drifts	检查后向激光雷达视野；可能未启用，导致姿态误差累积

与其它 Movement 的衔接 / Composing with other Movements

取货 / 放货前的接近 — 后接 `AutoFetchGood` / `AutoShelfFetching_ManyLegs`。
绕障 — 见绕障行走。
反向出库 — 用 `SteeringLineFollowingReverse`，搭配同样的字段。
长路径 — SimpleComposer 调度会把长路径切成若干段 LineFollowing 拼接，每段一个 `srcX/Y → dstX/Y`。

注意 / Caveat

LineFollowing 是 几何意义上的直线段跟踪，与车型无关。但具体 Movement 内部隐含假设：阿克曼 / 差速车型可以 转向即改向。对全向车应改用 `OmniLineFollowing`（见全向车适配案例），它允许车身朝向独立于运动方向。

`SteeringLineFollowing` is a geometric line-segment tracker but implicitly assumes Ackermann / differential kinematics. For omni vehicles use `OmniLineFollowing` (heading independent of motion direction) — see the omni case study.