avlite-executer-ROS2/p41_ros_sync.py at main · AV-Lab/avlite-executer-ROS2 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
"""ROSExecuter state sync helpers (no rclpy imports — safe for unit tests)."""
from __future__ import annotations

from typing import TYPE_CHECKING, Any

from avlite.c10_perception.c11_perception_model import EgoState
from avlite import TrajectoryTracker

if TYPE_CHECKING:
    from avlite.c10_perception.c11_perception_model import PerceptionModel
    from avlite.plugins.avlite_executer_ROS2.topic_mirror_bridge import TopicMirrorBridge


def apply_worker_ego_state(
    src: EgoState,
    ego_state: EgoState,
    pm: "PerceptionModel",
    world: "TopicMirrorBridge",
) -> None:
    """Copy worker localization into main-process ego mirrors."""
    ego_state.x = src.x
    ego_state.y = src.y
    ego_state.theta = src.theta
    ego_state.velocity = src.velocity
    world.ego_state.x = src.x
    world.ego_state.y = src.y
    world.ego_state.theta = src.theta
    world.ego_state.velocity = src.velocity
    pm.ego_vehicle.x = src.x
    pm.ego_vehicle.y = src.y
    pm.ego_vehicle.theta = src.theta
    pm.ego_vehicle.velocity = src.velocity


def should_mirror_plan(plan_stamp: float, last_synced_stamp: float, has_plan: bool) -> bool:
    return has_plan and plan_stamp > 0 and plan_stamp != last_synced_stamp


def should_sync_proxy_on_ego(ego_stamp: float, last_synced_stamp: float) -> bool:
    """True when worker localization has updated since the last proxy sync."""
    return ego_stamp > 0 and ego_stamp != last_synced_stamp


def ego_pose_changed(
    src: EgoState,
    dst: EgoState,
    *,
    eps: float = 1e-6,
) -> bool:
    """True when src and dst differ in pose or velocity."""
    return (
        abs(src.x - dst.x) > eps
        or abs(src.y - dst.y) > eps
        or abs(src.theta - dst.theta) > eps
        or abs(src.velocity - dst.velocity) > eps
    )


def align_plan_to_ego(
    plan: TrajectoryTracker | None,
    ego: EgoState,
    min_wp: int | None = None,
) -> None:
    """Forward-align a mirrored trajectory to the synced ego pose."""
    if plan is not None and plan.path:
        plan.update_waypoint_by_xy_forward(ego.x, ego.y, min_wp=min_wp)


def sync_display_from_world(
    ego_state: EgoState,
    pm: "PerceptionModel",
    world: Any,
) -> None:
    """Copy in-process world ego into display mirrors (authoritative for UI)."""
    ego = world.get_ego_state()
    ego_state.x = ego.x
    ego_state.y = ego.y
    ego_state.theta = ego.theta
    ego_state.velocity = ego.velocity
    pm.ego_vehicle.x = ego.x
    pm.ego_vehicle.y = ego.y
    pm.ego_vehicle.theta = ego.theta
    pm.ego_vehicle.velocity = ego.velocity


def sync_proxy_planner_location(planner: Any, ego: EgoState) -> None:
    """Update proxy planner localization bookkeeping for UI display only."""
    if planner is None:
        return
    planner.location_xy = (ego.x, ego.y)
    global_tj = getattr(planner, "global_trajectory", None)
    if global_tj is not None and global_tj.path:
        s, d = global_tj.convert_xy_to_sd(ego.x, ego.y)
        planner.location_sd = (s, d)
    if (
        planner.traversed_x
        and planner.traversed_x[-1] == ego.x
        and planner.traversed_y[-1] == ego.y
    ):
        return
    planner.traversed_x.append(ego.x)
    planner.traversed_y.append(ego.y)
    if global_tj is not None and global_tj.path:
        s, d = global_tj.convert_xy_to_sd(ego.x, ego.y)
        planner.traversed_s.append(s)
        planner.traversed_d.append(d)