Synced trajectory planners different use cases

Author: brijesh2709

GEMstack/onboard/planning/trajectory_planning_component.py

@@ -683,4 +683,69 @@ def update(self, state: AllState):
         if DEBUG:
             print('[DEBUG] Current Velocity of the Car: LOOK!', curr_v, self.desired_speed)
             print("[DEBUG] YieldTrajectoryPlanner.update: Returning trajectory with", len(traj.points), "points.")
+        return traj
+    
+
+
+class SplinePlanner(Component):
+    """Follows route by smoothing coarse waypoints into a quintic spline."""
+    def __init__(self):
+        super().__init__()
+        self.route_progress = None
+        self.t_last         = None
+
+        # how far ahead to plan (m), and sampling speed & dt
+        self.lookahead_dist = 10.0
+        self.v_des          = 2.0
+        self.dt             = 0.02
+
+        # the spline engine
+        self._spline = QuinticHermiteSplinePlanner(self.v_des, self.dt)
+
+    def state_inputs(self):
+        return ['all']
+
+    def state_outputs(self):
+        return ['trajectory']
+
+    def rate(self):
+        return 10.0  # Hz
+
+    def update(self, state: AllState) -> Trajectory:
+        t = state.t
+        if self.t_last is None:
+            self.t_last = t
+
+        # keep route_progress up to date
+        veh = state.vehicle
+        curr = np.array([veh.pose.x, veh.pose.y])
+
+        if self.route_progress is None:
+            self.route_progress = 0.0
+        _, new_param = state.route.closest_point_local(
+            curr.tolist(),
+            (self.route_progress - 5.0,
+             self.route_progress + 5.0)
+        )
+        self.route_progress = new_param
+
+        # extract a look-ahead segment from the route
+        seg: Path = state.route.trim(
+            self.route_progress,
+            self.route_progress + self.lookahead_dist
+        )
+
+        pts_raw: List[List[float]] = [
+            list(pt) for pt in seg.points
+        ]
+
+        spline_pts, spline_times = self._spline.build(pts_raw)
+
+        traj = Trajectory(
+            frame  = seg.frame,
+            points = spline_pts.tolist(),
+            times  = spline_times.tolist()
+        )
+
+        self.t_last = t
         return traj