Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
- Docs: plugin registry field tables list every field with a required column (README, Overview, Plugin Development)

### Fixed
- Execution / Visualizer: control no longer hard-requires a local planner — sensors→control stacks (`FollowTheGapController` with empty `c40_local_planner`) now recompute and apply commands in sync/async executers and Control **Step**
- Common: `TrajectoryTracker` initializes `path_s` from cumulative arc-length instead of re-projecting the reference through KD-tree Frenet conversion — closed tracks with `first==last` (e.g. bundled Yas Marina race line) no longer get non-monotonic `path_s` with `path_s[-1] == 0`
- Common: Frenet XY→SD picks the better adjacent segment around the nearest waypoint (and SD→XY brackets by arc-length) — on-path points after corners no longer pick up a huge false CTE from the previous segment
- Common / Planning: lattice sampling, replan end-of-track gates, and race lap detection use `TrajectoryTracker.track_end_s` (`path_s[-1]`) instead of the stale `path_s[-2]` workaround — avoids `IndexError` on 1-point paths and restores the final closed-track segment after the cumulative `path_s` fix
Expand Down
9 changes: 6 additions & 3 deletions avlite/c40_execution/c42_execution_strategy.py
Original file line number Diff line number Diff line change
Expand Up @@ -338,13 +338,16 @@ def _replan_step(self, sensors: SensorFrame) -> None:
def _control_step(self, sim_dt: float, sensors: SensorFrame) -> None:
"""Recompute control command into ``_last_cmd`` (no world integrate).

Uses the caller-supplied tick snapshot.
Uses the caller-supplied tick snapshot. Controllers that do not require
a plan (e.g. sensors→control Follow-the-Gap) run with ``plan=None``.
"""
if not self.controller or not self.local_planner:
if not self.controller:
return
if not self._can_actuate():
return
local_plan = self.local_planner.get_local_plan()
local_plan = (
self.local_planner.get_local_plan() if self.local_planner is not None else None
)
cmd = self.controller.control(
self.pm.ego_vehicle, local_plan, control_dt=sim_dt,
perception_model=self.pm, sensors=sensors,
Expand Down
1 change: 0 additions & 1 deletion avlite/c40_execution/c44_sync_executer.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,6 @@ def step(
do_control = (
call_control
and self.controller is not None
and self.local_planner is not None
and (
(not pace_control)
or (self.elapsed_sim_time - self.__controller_last_time >= control_dt)
Expand Down
2 changes: 1 addition & 1 deletion avlite/c40_execution/c45_async_threaded_executer.py
Original file line number Diff line number Diff line change
Expand Up @@ -242,7 +242,7 @@ def worker_control(self):
with self.lock_world:
if is_world_stack_capability_enabled(StackCapability.LOCALIZATION):
self.pm.ego_vehicle.copy_from(self.world.get_ego_state())
if self.controller and self.local_planner:
if self.controller:
self._control_step(self.sim_dt, self.world.get_sensor_frame())

# Free-run: sim and real share the same wall interval (start-of-iter stamps).
Expand Down
10 changes: 8 additions & 2 deletions avlite/plugins/p60_visualizer_tk/p67_stack_views.py
Original file line number Diff line number Diff line change
Expand Up @@ -610,11 +610,17 @@ def update_data(self):
self.controller_dropdown_menu["values"] = ("",) + tuple(ControlStrategy.registry.keys())

def step_control(self):
if not self.root.exec or not self.root.exec.controller or not self.root.exec.local_planner:
if not self.root.exec or not self.root.exec.controller:
return
# Sensors→control stacks (e.g. FollowTheGap) may omit the local planner.
plan = (
self.root.exec.local_planner.get_local_plan()
if self.root.exec.local_planner is not None
else None
)
cmd = self.root.exec.controller.control(
self.root.exec.ego_state,
self.root.exec.local_planner.get_local_plan(),
plan,
control_dt=self.root.setting.sim_dt.get(),
perception_model=self.root.exec.pm,
sensors=self.root.exec.world.get_sensor_frame(),
Expand Down
120 changes: 120 additions & 0 deletions test/c40_execution/test_c44_control_without_planner.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,120 @@
"""Sensors→control must run when no local planner is assembled."""

from __future__ import annotations

from dataclasses import dataclass, field
from types import SimpleNamespace
from typing import Optional

import numpy as np
import pytest

from avlite.c10_perception.c11_perception_model import EGO_AGENT_ID, EgoState, PerceptionModel
from avlite.c30_control.c31_control_model import AckermannControlCommand
from avlite.c30_control.c35_pure_pursuit import FollowTheGapController
from avlite.c30_control.c39_settings import ControlSettings
from avlite.c40_execution.c41_world_bridge import WorldBridge
from avlite.c40_execution.c44_sync_executer import SyncExecuter
from avlite.c40_execution.c49_settings import ExecutionSettings
from avlite.c50_common.c51_capabilities import StackCapability
from avlite.c50_common.c52_world_sensor_datatypes import SensorFrame
from avlite.c60_apps.c62_factory import executor_factory


@dataclass
class _PlantWorld(WorldBridge):
"""Minimal plant that records applied commands and serves LiDAR."""

ego_state: EgoState = field(
default_factory=lambda: EgoState(x=0.0, y=0.0, theta=0.0, velocity=0.0)
)
perception_model: Optional[PerceptionModel] = None
world_capabilities = frozenset()
stack_capabilities = frozenset({StackCapability.LOCALIZATION})
applied: list = field(default_factory=list)

def control_ego_state(self, cmd, dt: Optional[float] = 0.01):
self.applied.append(cmd)
# Crude integrate so factory smoke can observe motion.
self.ego_state.velocity = max(0.0, self.ego_state.velocity + float(cmd.acceleration) * float(dt or 0.01))
self.ego_state.x += self.ego_state.velocity * float(dt or 0.01)

def get_sensor_frame(self, agent_id: int = EGO_AGENT_ID) -> SensorFrame:
# Forward cone of returns so Follow-the-Gap can pick a gap.
angles = np.linspace(-0.8, 0.8, 40)
r = 12.0
pts = np.column_stack(
[
self.ego_state.x + r * np.cos(self.ego_state.theta + angles),
self.ego_state.y + r * np.sin(self.ego_state.theta + angles),
np.zeros_like(angles),
np.zeros_like(angles),
]
).astype(np.float32)
return SensorFrame(lidar=pts)


def test_sync_control_runs_without_local_planner():
"""Hard gate on local_planner previously skipped sensors→control entirely."""
seen: dict = {}

def control(ego, plan=None, control_dt=None, perception_model=None, sensors=None):
seen["plan"] = plan
seen["sensors"] = sensors
return AckermannControlCommand(steer=0.1, acceleration=1.0)

world = _PlantWorld()
exec_ = SyncExecuter(
perception_model=PerceptionModel(ego_vehicle=EgoState(x=0.0, y=0.0)),
world=world,
perception=None,
localization=None,
global_planner=None,
local_planner=None,
controller=SimpleNamespace(
world_requirements=frozenset(),
stack_requirements=frozenset({StackCapability.LOCALIZATION}),
stack_capabilities=frozenset({StackCapability.CONTROL}),
control=control,
reset=lambda: None,
),
)

exec_.step(sim_dt=0.05, control_dt=0.0, pace_control=True, pace_sim=True)

assert "sensors" in seen
assert seen["plan"] is None
assert exec_._last_cmd is not None
assert exec_._last_cmd.acceleration == pytest.approx(1.0)
assert len(world.applied) == 1


def test_follow_the_gap_factory_stack_actuates_without_local_planner():
"""Documented sensors→control composition must move the plant."""
# Keep world GT localization so _can_actuate passes with no localization module.
prev_caps = ExecutionSettings.c41_world_stack_capabilities
prev_cruise = ControlSettings.c35_cruise_velocity
try:
ExecutionSettings.c41_world_stack_capabilities = None # allow defaults / all
ControlSettings.c35_cruise_velocity = 5.0
ex = executor_factory(
local_planner_strategy_name="",
controller_strategy_name="FollowTheGapController",
global_planner_strategy_name="",
perception_strategy_name="",
localization_strategy_name="",
load_plugins=False,
)
assert ex.local_planner is None
assert isinstance(ex.controller, FollowTheGapController)

x0 = ex.world.get_ego_state().x
for _ in range(25):
ex.step(sim_dt=0.05, control_dt=0.0, replan_dt=1e9, perception_dt=1e9)

assert ex._last_cmd is not None
assert ex._last_cmd.acceleration != 0.0 or ex._last_cmd.steer != 0.0
assert ex.world.get_ego_state().x != pytest.approx(x0, abs=1e-6)
finally:
ExecutionSettings.c41_world_stack_capabilities = prev_caps
ControlSettings.c35_cruise_velocity = prev_cruise