From a0e7a15c2137c19c0c707c2cecde8f48b8e2cf17 Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Mon, 12 May 2025 18:17:48 -0500 Subject: [PATCH 01/11] adding everything required for planning integration --- GEMstack/onboard/perception/cone_detection.py | 79 ++- .../onboard/perception/parking_detection.py | 2 +- GEMstack/onboard/planning/astar.py | 267 ++++++++++ .../onboard/planning/longitudinal_planning.py | 486 ++++++++++++++++++ .../onboard/planning/parking_route_planner.py | 464 +++++++++++++++++ GEMstack/onboard/planning/pure_pursuit.py | 25 +- GEMstack/onboard/planning/reed_shepp.py | 473 +++++++++++++++++ .../planning/route_planning_component.py | 56 +- launch/parking_detection.yaml | 72 ++- 9 files changed, 1876 insertions(+), 48 deletions(-) create mode 100644 GEMstack/onboard/planning/astar.py create mode 100644 GEMstack/onboard/planning/longitudinal_planning.py create mode 100644 GEMstack/onboard/planning/parking_route_planner.py create mode 100644 GEMstack/onboard/planning/reed_shepp.py diff --git a/GEMstack/onboard/perception/cone_detection.py b/GEMstack/onboard/perception/cone_detection.py index 42b74bb6b..d0c13a31b 100644 --- a/GEMstack/onboard/perception/cone_detection.py +++ b/GEMstack/onboard/perception/cone_detection.py @@ -472,9 +472,12 @@ def save_sensor_data(self, vehicle: VehicleState, latest_image) -> None: class FakConeDetector(Component): def __init__(self, vehicle_interface: GEMInterface): + """ + Initializes the FakeConeDetector with two parking spots: + - One standard 2x2 rectangular spot + - One wider mouth for easier entry + """ self.vehicle_interface = vehicle_interface - self.times = [(5.0, 20.0), (30.0, 35.0)] - self.t_start = None def rate(self): return 4.0 @@ -486,23 +489,69 @@ def state_outputs(self): return ['obstacles'] def update(self, vehicle: VehicleState) -> Dict[str, Obstacle]: - if self.t_start is None: - self.t_start = self.vehicle_interface.time() - t = self.vehicle_interface.time() - self.t_start + """ + Called every simulation step, updates the timestamp and publishes the obstacle states. + """ + + current_time = self.vehicle_interface.time() + + # === Standard Parking Spot === + # cones_standard = { + # 'cone0': (5.0, 5.0, 0.5, 0.5), # Front Left + # 'cone1': (5.0, 7.0, 0.5, 0.5), # Back Left + # 'cone2': (7.0, 5.0, 0.5, 0.5), # Front Right + # 'cone3': (7.0, 7.0, 0.5, 0.5) # Back Right + # } + + # === Wider Mouth Parking Spot === + cones_wide = { + 'cone4': (6.0, 9.0, 0.5, 0.5), # Front Left (wide) + 'cone5': (10.0, 9.0, 0.5, 0.5), # Back Left + 'cone6': (3.0, 4.0, 0.5, 0.5), # Front Right (wide) + 'cone7': (7.0, 4.0, 0.5, 0.5) # Back Right + } + + # Populate the obstacle states res = {} - for time_range in self.times: - if t >= time_range[0] and t <= time_range[1]: - res['cone0'] = box_to_fake_obstacle((0, 0, 0, 0)) - rospy.loginfo("Detected a Cone (simulated)") - return res + for name, box in {**cones_wide}.items(): + res[name] = box_to_fake_obstacle(box, current_time) + # Update timestamp + for obstacle in res.values(): + obstacle.pose.t = current_time -def box_to_fake_obstacle(box): + rospy.loginfo(f"[FakeConeDetector] Simulated Two Parking Spots Detected") + return res + +def box_to_fake_obstacle(box, current_time): + """ + Helper function to create a fake obstacle (cone) from bounding box coordinates. + """ x, y, w, h = box - pose = ObjectPose(t=0, x=x + w / 2, y=y + h / 2, z=0, yaw=0, pitch=0, roll=0, frame=ObjectFrameEnum.CURRENT) - dims = (w, h, 0) - return Obstacle(pose=pose, dimensions=dims, outline=None, - material=ObstacleMaterialEnum.TRAFFIC_CONE, state=ObstacleStateEnum.STANDING, collidable=True) + pose = ObjectPose( + t=current_time, + x=x, + y=y, + z=0.0, + yaw=0.0, + pitch=0.0, + roll=0.0, + frame=ObjectFrameEnum.START + ) + + dims = (w, h, 1.0) + + new_obstacle = Obstacle( + pose=pose, + dimensions=dims, + outline=None, + material=ObstacleMaterialEnum.TRAFFIC_CONE, + collidable=True, + state=ObstacleStateEnum.STANDING + ) + print("Obstacles made") + return new_obstacle + if __name__ == '__main__': diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py index d6e66aed8..f0952544b 100644 --- a/GEMstack/onboard/perception/parking_detection.py +++ b/GEMstack/onboard/perception/parking_detection.py @@ -176,7 +176,7 @@ def update(self, cone_obstacles: Dict[str, Obstacle]): yaw=yaw, pitch=0.0, roll=0.0, - frame=ObjectFrameEnum.CURRENT + frame=ObjectFrameEnum.START ) new_obstacle = Obstacle( pose=obstacle_pose, diff --git a/GEMstack/onboard/planning/astar.py b/GEMstack/onboard/planning/astar.py new file mode 100644 index 000000000..2704a450b --- /dev/null +++ b/GEMstack/onboard/planning/astar.py @@ -0,0 +1,267 @@ +""" generic A-Star path searching algorithm (https://github.com/jrialland/python-astar/blob/master/tests/basic/test_basic.py) """ +# @TODO make it so we return the best path as discussed in class +# In class, we discussed that we should have a terminal function +# which approximates the cost_to_go and returns the path to the node +# with the least cost_to_go + terminal_cost + +from abc import ABC, abstractmethod +from typing import Callable, Dict, Iterable, Union, TypeVar, Generic +from math import inf as infinity +from operator import attrgetter +import heapq + +# introduce generic type +T = TypeVar("T") + + +################################################################################ +class SearchNode(Generic[T]): + """Representation of a search node""" + + __slots__ = ("data", "gscore", "fscore", "tscore", "closed", "came_from", "in_openset", "cache") + + def __init__( + self, data: T, gscore: float = infinity, fscore: float = infinity, tscore:float = infinity + ) -> None: + self.data = data + self.gscore = gscore + self.fscore = fscore + self.tscore = tscore + self.closed = False + self.in_openset = False + self.came_from: Union[None, SearchNode[T]] = None + self.cache: Any = None + + def __lt__(self, b: "SearchNode[T]") -> bool: + """Natural order is based on the fscore value & is used by heapq operations""" + return self.fscore < b.fscore + + +################################################################################ +class SearchNodeDict(Dict[T, SearchNode[T]]): + """A dict that returns a new SearchNode when a key is missing""" + + def __missing__(self, k) -> SearchNode[T]: + v = SearchNode(k) + self.__setitem__(k, v) + return v + + +################################################################################ +SNType = TypeVar("SNType", bound=SearchNode) + + +class OpenSet(Generic[SNType]): + def __init__(self) -> None: + self.heap: list[SNType] = [] + + def push(self, item: SNType) -> None: + item.in_openset = True + heapq.heappush(self.heap, item) + + def pop(self) -> SNType: + item = heapq.heappop(self.heap) + item.in_openset = False + return item + + def remove(self, item: SNType) -> None: + idx = self.heap.index(item) + item.in_openset = False + item = self.heap.pop() + if idx < len(self.heap): + self.heap[idx] = item + # Fix heap invariants + heapq._siftup(self.heap, idx) + heapq._siftdown(self.heap, 0, idx) + + def __len__(self) -> int: + return len(self.heap) + + +################################################################################* + + +class AStar(ABC, Generic[T]): + __slots__ = () + + @abstractmethod + def heuristic_cost_estimate(self, current: T, goal: T) -> float: + """ + Computes the estimated (rough) distance between a node and the goal. + The second parameter is always the goal. + + This method must be implemented in a subclass. + """ + raise NotImplementedError + + @abstractmethod + def terminal_cost_estimate(self, current: T, goal: T) -> float: + """Computes the estimated distance between a node and the goal. + This function is called after all iterations of A* have been run + and is used to determine the closest node to the goal found so far. + + This method must be implemented in a subclass. + + Args: + current (T): Current T + goal (T): goal T + + Returns: + float: _description_ + """ + raise NotImplementedError + + def distance_between(self, n1: T, n2: T) -> float: + """ + Gives the real distance between two adjacent nodes n1 and n2 (i.e n2 + belongs to the list of n1's neighbors). + n2 is guaranteed to belong to the list returned by the call to neighbors(n1). + + This method (or "path_distance_between") must be implemented in a subclass. + """ + raise NotImplementedError + + def path_distance_between(self, n1: SearchNode[T], n2: SearchNode[T]) -> float: + """ + Gives the real distance between the node n1 and its neighbor n2. + n2 is guaranteed to belong to the list returned by the call to + path_neighbors(n1). + + Calls "distance_between"`by default. + """ + return self.distance_between(n1.data, n2.data) + + def neighbors(self, node: T) -> Iterable[T]: + """ + For a given node, returns (or yields) the list of its neighbors. + + This method (or "path_neighbors") must be implemented in a subclass. + """ + raise NotImplementedError + + def path_neighbors(self, node: SearchNode[T]) -> Iterable[T]: + """ + For a given node, returns (or yields) the list of its reachable neighbors. + Calls "neighbors" by default. + """ + return self.neighbors(node.data) + + def _neighbors(self, current: SearchNode[T], search_nodes: SearchNodeDict[T]) -> Iterable[SearchNode]: + return (search_nodes[n] for n in self.path_neighbors(current)) + + def is_goal_reached(self, current: T, goal: T) -> bool: + """ + Returns true when we can consider that 'current' is the goal. + The default implementation simply compares `current == goal`, but this + method can be overwritten in a subclass to provide more refined checks. + """ + return current == goal + + def reconstruct_path(self, last: SearchNode, reversePath=False) -> Iterable[T]: + def _gen(): + current = last + while current: + yield current.data + current = current.came_from + + if reversePath: + return _gen() + else: + return reversed(list(_gen())) + + def astar( + self, start: T, goal: T, reversePath: bool = False, iterations: int = 5000 + ) -> Union[Iterable[T], None]: + if self.is_goal_reached(start, goal): + return [start] + + openSet: OpenSet[SearchNode[T]] = OpenSet() + searchNodes: SearchNodeDict[T] = SearchNodeDict() + startNode = searchNodes[start] = SearchNode( + start, gscore=0.0, fscore=self.heuristic_cost_estimate(start, goal) + ) + openSet.push(startNode) + bestNode = startNode + + iteration = 0 + + while openSet and iteration < iterations: + current = openSet.pop() + + if self.is_goal_reached(current.data, goal): + return self.reconstruct_path(current, reversePath) + + current.closed = True + + for neighbor in self._neighbors(current, searchNodes): + if neighbor.closed: + continue + + gscore = current.gscore + self.path_distance_between(current, neighbor) + + if gscore >= neighbor.gscore: + continue + + fscore = gscore + self.heuristic_cost_estimate( + neighbor.data, goal + ) + tscore = self.terminal_cost_estimate( + neighbor.data, goal + ) + + # print(f"Checking node: {neighbor.data} with tscore {tscore}") + if tscore < bestNode.tscore: + # print(f"Found a better node: {neighbor.data} with tscore {tscore}") + bestNode = neighbor + + if neighbor.in_openset: + if neighbor.fscore < fscore: + # the new path to this node isn't better + continue + + # we have to remove the item from the heap, as its score has changed + openSet.remove(neighbor) + + # update the node + neighbor.came_from = current + neighbor.gscore = gscore + neighbor.fscore = fscore + neighbor.tscore = tscore + + openSet.push(neighbor) + + iteration += 1 + + # print("Warning: A* search failed to find a path") + return self.reconstruct_path(bestNode, reversePath) + + +################################################################################ +U = TypeVar("U") + + +def find_path( + start: U, + goal: U, + neighbors_fnct: Callable[[U], Iterable[U]], + reversePath=False, + heuristic_cost_estimate_fnct: Callable[[U, U], float] = lambda a, b: infinity, + distance_between_fnct: Callable[[U, U], float] = lambda a, b: 1.0, + is_goal_reached_fnct: Callable[[U, U], bool] = lambda a, b: a == b, +) -> Union[Iterable[U], None]: + """A non-class version of the path finding algorithm""" + + class FindPath(AStar): + def heuristic_cost_estimate(self, current: U, goal: U) -> float: + return heuristic_cost_estimate_fnct(current, goal) # type: ignore + + def distance_between(self, n1: U, n2: U) -> float: + return distance_between_fnct(n1, n2) + + def neighbors(self, node) -> Iterable[U]: + return neighbors_fnct(node) # type: ignore + + def is_goal_reached(self, current: U, goal: U) -> bool: + return is_goal_reached_fnct(current, goal) + + return FindPath().astar(start, goal, reversePath) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py new file mode 100644 index 000000000..9a156d9ae --- /dev/null +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -0,0 +1,486 @@ +# File: GEMstack/onboard/planning/longitudinal_planning.py +from typing import List, Tuple +import math +from ..component import Component +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \ + ObjectFrameEnum +from ...utils import serialization, settings +from ...mathutils import transforms +import numpy as np + +DEBUG = False # Set to False to disable debug output + + +def generate_dense_points(points: List[Tuple[float, float]], density: int = 10) -> List[Tuple[float, float]]: + if not points: + return [] + if len(points) == 1: + return points.copy() + + dense_points = [points[0]] + for i in range(len(points) - 1): + p0 = points[i] + p1 = points[i + 1] + dx = p1[0] - p0[0] + dy = p1[1] - p0[1] + seg_length = math.hypot(dx, dy) + + n_interp = int(round(seg_length * density)) + + for j in range(1, n_interp + 1): + fraction = j / (n_interp + 1) + x_interp = p0[0] + fraction * dx + y_interp = p0[1] + fraction * dy + dense_points.append((x_interp, y_interp)) + + dense_points.append(p1) + + return dense_points + + +def compute_cumulative_distances(points: List[List[float]]) -> List[float]: + s_vals = [0.0] + for i in range(1, len(points)): + dx = points[i][0] - points[i - 1][0] + dy = points[i][1] - points[i - 1][1] + ds = math.hypot(dx, dy) + s_vals.append(s_vals[-1] + ds) + + if DEBUG: + print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) + + return s_vals + + +def longitudinal_plan(path, acceleration, deceleration, max_speed, current_speed): + path_normalized = path.arc_length_parameterize() + points = list(path_normalized.points) + dense_points = generate_dense_points(points) + s_vals = compute_cumulative_distances(dense_points) + L = s_vals[-1] # Total path length + stopping_distance = (current_speed ** 2) / (2 * deceleration) + + if DEBUG: + print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) + print("[DEBUG] longitudinal_plan: Total path length L =", L) + print("[DEBUG] longitudinal_plan: Braking distance needed =", stopping_distance) + + if stopping_distance > L: # Case where there is not enough stopping distance to stop before path ends (calls emergency brake) + return longitudinal_brake(path, deceleration, current_speed) + + if current_speed > max_speed: # Case where car is exceeding the max speed so we need to slow down (do initial slowdown) + if DEBUG: + print(f"[DEBUG] Handling case where current_speed ({current_speed:.2f}) > max_speed ({max_speed:.2f})") + + # Initial deceleration phase to reach max_speed + initial_decel_distance = (current_speed ** 2 - max_speed ** 2) / (2 * deceleration) + initial_decel_time = (current_speed - max_speed) / deceleration + remaining_distance = L - initial_decel_distance + + if DEBUG: + print( + f"[DEBUG] Phase 1 - Initial Decel: distance = {initial_decel_distance:.2f}, time = {initial_decel_time:.2f}") + print(f"[DEBUG] Remaining distance after reaching max_speed: {remaining_distance:.2f}") + + # Calculate final deceleration distance needed to stop from max_speed + final_decel_distance = (max_speed ** 2) / (2 * deceleration) + cruise_distance = remaining_distance - final_decel_distance + + if DEBUG: + print(f"[DEBUG] Phase 2 - Cruise: distance = {cruise_distance:.2f}") + print(f"[DEBUG] Phase 3 - Final Decel: distance = {final_decel_distance:.2f}") + + times = [] + for s in s_vals: + if s <= initial_decel_distance: # Phase 1: Initial deceleration to max_speed + v = math.sqrt(current_speed ** 2 - 2 * deceleration * s) + t = (current_speed - v) / deceleration + if DEBUG: # Print every 10m + print(f"[DEBUG] Initial Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") + + elif s <= initial_decel_distance + cruise_distance: # Phase 2: Cruise at max_speed + s_in_cruise = s - initial_decel_distance + t = initial_decel_time + s_in_cruise / max_speed + if DEBUG: # Print every 10m + print(f"[DEBUG] Cruise: s = {s:.2f}, v = {max_speed:.2f}, t = {t:.2f}") + + else: # Phase 3: Final deceleration to stop + s_in_final_decel = s - (initial_decel_distance + cruise_distance) + v = math.sqrt(max(max_speed ** 2 - 2 * deceleration * s_in_final_decel, 0.0)) + t = initial_decel_time + cruise_distance / max_speed + (max_speed - v) / deceleration + if DEBUG: # Print every 10m + print(f"[DEBUG] Final Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") + + times.append(t) + + if DEBUG: + print("[DEBUG] Trajectory complete: Three phases executed") + print(f"[DEBUG] Total time: {times[-1]:.2f}") + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + if acceleration <= 0: + if DEBUG: + print(f"[DEBUG] No acceleration allowed. Current speed: {current_speed:.2f}") + + # Pure deceleration phase + s_decel = (current_speed ** 2) / (2 * deceleration) + T_decel = current_speed / deceleration + + if DEBUG: + print(f"[DEBUG] Will maintain speed until s_decel: {s_decel:.2f}") + print(f"[DEBUG] Total deceleration time will be: {T_decel:.2f}") + + times = [] + for s in s_vals: + if s <= L - s_decel: # Maintain current speed until deceleration point + t_point = s / current_speed + if DEBUG: + print(f"[DEBUG] Constant Speed Phase: s = {s:.2f}, v = {current_speed:.2f}, t = {t_point:.2f}") + else: # Deceleration phase + s_in_decel = s - (L - s_decel) + v = math.sqrt(max(current_speed ** 2 - 2 * deceleration * s_in_decel, 0.0)) + t_point = (L - s_decel) / current_speed + (current_speed - v) / deceleration + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + # Determine max possible peak speed given distance + v_peak_possible = math.sqrt( + (2 * acceleration * deceleration * L + deceleration * current_speed ** 2) / (acceleration + deceleration)) + v_target = min(max_speed, v_peak_possible) + + if DEBUG: + print("[DEBUG] longitudinal_plan: v_peak_possible =", v_peak_possible, "v_target =", v_target) + + # Compute acceleration phase + s_accel = max(0.0, (v_target ** 2 - current_speed ** 2) / (2 * acceleration)) + t_accel = max(0.0, (v_target - current_speed) / acceleration) + + # Compute deceleration phase + s_decel = max(0.0, (v_target ** 2) / (2 * deceleration)) + t_decel = max(0.0, v_target / deceleration) + + # Compute cruise phase + s_cruise = max(0.0, L - s_accel - s_decel) + t_cruise = s_cruise / v_target if v_target > 0 else 0.0 + + if DEBUG: + print("[DEBUG] longitudinal_plan: s_accel =", s_accel, "t_accel =", t_accel) + print("[DEBUG] longitudinal_plan: s_decel =", s_decel, "t_decel =", t_decel) + print("[DEBUG] longitudinal_plan: s_cruise =", s_cruise, "t_cruise =", t_cruise) + + times = [] + for s in s_vals: + if s <= s_accel: # Acceleration phase + v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) + t_point = (v - current_speed) / acceleration + + if DEBUG: + print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + elif s <= s_accel + s_cruise: # Cruise phase + t_point = t_accel + (s - s_accel) / v_target + + if DEBUG: + print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + + else: # Deceleration phase + s_decel_phase = s - s_accel - s_cruise + v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) + t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + + if t_point < times[-1]: # Ensure time always increases + t_point = times[-1] + 0.01 # Small time correction step + + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + if DEBUG: + print("[DEBUG] longitudinal_plan: Final times =", times) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + +def longitudinal_brake(path: Path, deceleration: float, current_speed: float, + emergency_decel: float = 8.0) -> Trajectory: + # Vehicle already stopped - maintain position + if current_speed <= 0: + print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) + return Trajectory( + frame=path.frame, + points=[path.points[0]] * len(path.points), + times=[float(i) for i in range(len(path.points))] + ) + + # Get total path length + path_length = sum( + np.linalg.norm(np.array(path.points[i + 1]) - np.array(path.points[i])) + for i in range(len(path.points) - 1) + ) + + # Calculate stopping distance with normal deceleration + T_stop_normal = current_speed / deceleration + s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + + # Check if emergency braking is needed + if s_stop_normal > path_length: + if DEBUG: + print("[DEBUG] longitudinal_brake: Emergency braking needed!") + print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") + print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + + # Calculate emergency braking parameters + T_stop = current_speed / emergency_decel + s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") + print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") + + decel_to_use = emergency_decel + + else: + if DEBUG: + print("[DEBUG] longitudinal_brake: Normal braking sufficient") + T_stop = T_stop_normal + decel_to_use = deceleration + + # Generate time points (use more points for smoother trajectory) + num_points = max(len(path.points), 50) + times = np.linspace(0, T_stop, num_points) + + # Calculate distances at each time point using physics equation + distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) + + # Generate points along the path + points = [] + for d in distances: + if d <= path_length: + points.append(path.eval(d)) + else: + points.append(path.eval(d)) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") + print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + + return Trajectory(frame=path.frame, points=points, times=times.tolist()) + + times = [] + for s in s_vals: + if s <= s_accel: # Acceleration phase + v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) + t_point = (v - current_speed) / acceleration + + if DEBUG: + print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + elif s <= s_accel + s_cruise: # Cruise phase + t_point = t_accel + (s - s_accel) / v_target + + if DEBUG: + print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + + else: # Deceleration phase + s_decel_phase = s - s_accel - s_cruise + v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) + t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + + if t_point < times[-1]: # Ensure time always increases + t_point = times[-1] + 0.01 # Small time correction step + + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + if DEBUG: + print("[DEBUG] longitudinal_plan: Final times =", times) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + +def longitudinal_brake(path: Path, deceleration: float, current_speed: float, emergency_decel: float = 8.0) -> Trajectory: + # Vehicle already stopped - maintain position + if current_speed <= 0: + print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) + return Trajectory( + frame=path.frame, + points=[path.points[0]] * len(path.points), + times=[float(i) for i in range(len(path.points))] + ) + + # Get total path length + path_length = sum( + np.linalg.norm(np.array(path.points[i+1]) - np.array(path.points[i])) + for i in range(len(path.points)-1) + ) + + # Calculate stopping distance with normal deceleration + T_stop_normal = current_speed / deceleration + s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + + # Check if emergency braking is needed + if s_stop_normal > path_length: + if DEBUG: + print("[DEBUG] longitudinal_brake: Emergency braking needed!") + print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") + print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + + # Calculate emergency braking parameters + T_stop = current_speed / emergency_decel + s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") + print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") + + decel_to_use = emergency_decel + + else: + if DEBUG: + print("[DEBUG] longitudinal_brake: Normal braking sufficient") + T_stop = T_stop_normal + decel_to_use = deceleration + + # Generate time points (use more points for smoother trajectory) + num_points = max(len(path.points), 50) + times = np.linspace(0, T_stop, num_points) + + # Calculate distances at each time point using physics equation + distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) + + # Generate points along the path + points = [] + for d in distances: + if d <= path_length: + points.append(path.eval(d)) + else: + points.append(path.eval(d)) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") + print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + + return Trajectory(frame=path.frame, points=points, times=times.tolist()) + +class YieldTrajectoryPlanner(Component): + """Follows the given route. Brakes if the ego–vehicle must yield + (e.g. to a pedestrian) or if the end of the route is near; otherwise, + it accelerates (or cruises) toward a desired speed. + """ + + def __init__(self): + self.route_progress = None + self.t_last = None + self.acceleration = settings.get("run.drive.planning.motion_planning.largs.acceleration", 5) + self.desired_speed = settings.get("run.drive.planning.motion_planning.largs.desired_speed",2.0) + self.deceleration = settings.get("run.drive.planning.motion_planning.largs.deceleration", 2.0) + self.emergency_brake = settings.get("run.drive.planning.motion_planning.largs.emergency_brake", 8.0) + + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['trajectory'] + + def rate(self): + return 10.0 + + def update(self, state: AllState): + vehicle = state.vehicle # type: VehicleState + route = state.route # type: Route + t = state.t + + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: t =", t) + + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: dt =", dt) + + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + if DEBUG: + print(f"[DEBUG] YieldTrajectoryPlanner.update: Vehicle position = ({curr_x}, {curr_y}), speed = {curr_v}, ") + + # Determine progress along the route. + if self.route_progress is None: + self.route_progress = 0.0 + _, closest_parameter = route.closest_point_local( + [curr_x, curr_y], + (self.route_progress - 5.0, self.route_progress + 5.0) + ) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Closest parameter on route =", closest_parameter) + self.route_progress = closest_parameter + + # Extract a 10 m segment of the route for planning lookahead. + route_with_lookahead = route.trim(closest_parameter, closest_parameter + 10.0) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Route Lookahead =", route_with_lookahead) + + print("[DEBUG] state", state.relations) + # Check whether any yield relations (e.g. due to pedestrians) require braking. + stay_braking = False + pointSet = set() + for i in range(len(route_with_lookahead.points)): + if tuple(route_with_lookahead.points[i]) in pointSet: + stay_braking = True + break + pointSet.add(tuple(route_with_lookahead.points[i])) + + should_brake = any( + r.type == EntityRelationEnum.STOPPING_AT and r.obj1 == '' + for r in state.relations + ) + should_decelerate = any( + r.type == EntityRelationEnum.YIELDING and r.obj1 == '' + for r in state.relations + ) if should_brake == False else False + + should_accelerate = (not should_brake and not should_decelerate and curr_v < self.desired_speed) + + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: stay_braking =", stay_braking) + print("[DEBUG] YieldTrajectoryPlanner.update: should_brake =", should_brake) + print("[DEBUG] YieldTrajectoryPlanner.update: should_accelerate =", should_accelerate) + print("[DEBUG] YieldTrajectoryPlanner.update: should_decelerate =", should_decelerate) + + if stay_braking: + traj = longitudinal_brake(route_with_lookahead, 0.0, 0.0, 0.0) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake (stay braking).") + elif should_brake: + traj = longitudinal_brake(route_with_lookahead, self.emergency_brake, curr_v) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake.") + elif should_decelerate: + traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake.") + elif should_accelerate: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, + self.deceleration, self.desired_speed, curr_v) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_plan (accelerate).") + else: + # Maintain current speed if not accelerating or braking. + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + if DEBUG: + print( + "[DEBUG] YieldTrajectoryPlanner.update: Maintaining current speed with longitudinal_plan (0 accel).") + + self.t_last = t + 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 diff --git a/GEMstack/onboard/planning/parking_route_planner.py b/GEMstack/onboard/planning/parking_route_planner.py new file mode 100644 index 000000000..5924cf336 --- /dev/null +++ b/GEMstack/onboard/planning/parking_route_planner.py @@ -0,0 +1,464 @@ +from typing import List, Tuple, Union, Dict +from ..component import Component +from ...state import AllState, VehicleState, Path, Trajectory, Route, ObjectFrameEnum, AgentState, Obstacle, ObjectPose, PhysicalObject +from ...utils import serialization, settings +from ...mathutils.transforms import vector_madd +from GEMstack.mathutils.dubins import DubinsCar, SecondOrderDubinsCar, DubinsCarIntegrator +from ...mathutils.dynamics import IntegratorControlSpace +from ...mathutils import collisions +from .astar import AStar +from .longitudinal_planning import longitudinal_plan +from . import reed_shepp +from visualization_msgs.msg import Marker +from geometry_msgs.msg import Point +import rospy +import time + + + +import numpy as np +import math + + +def generate_action_set(max_vel, max_turn_rate): + return [ + (max_vel, -max_turn_rate), (max_vel, 0.0), (max_vel, max_turn_rate), + (-max_vel, -max_turn_rate), (-max_vel, 0.0), (-max_vel, max_turn_rate) + ] +class ParkingSolverFirstOrderDubins(AStar): + """sample use of the astar algorithm. In this exemple we work on a maze made of ascii characters, + and a 'node' is just a (x,y) tuple that represents a reachable position""" + + def __init__(self, vehicle=None, obstacles=None, actions=None): + self._obstacles = obstacles + + # Vehicle model + self._vehicle = None + + # settings + self.T = settings.get("run.drive.planning.route_planning_component.dubins.integrator.time_step", 1.5) + self.dt = settings.get("run.drive.planning.route_planning_component.dubins.integrator.dt", .25) + self.max_v = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_velocity", 0.75) + self.max_turn_rate = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_turn_rate", 0.3) + self.tolerance = settings.get("run.drive.planning.route_planning_component.dubins.tolerance", 0.5) + self.heuristic_string = settings.get("run.drive.planning.route_planning_component.astar.heuristic", "reeds_shepp") + + self.vehicle = DubinsCar() #x = (tx,ty,theta) and u = (fwd_velocity,turnRate). + self.vehicle_sim = DubinsCarIntegrator(self.vehicle, self.T, self.dt) + #@TODO create a more standardized way to define the actions + self._actions = generate_action_set(self.max_v, self.max_turn_rate) + + + @property + def vehicle(self): + return self._vehicle + + @vehicle.setter + def vehicle(self, vehicle): + self._vehicle = vehicle + + @property + def obstacles(self): + return self._obstacles + + @obstacles.setter + def obstacles(self, obstacles): + self._obstacles = obstacles + + @property + def actions(self): + return self._actions + + @actions.setter + def actions(self, actions): + # @TODO Add a validity checker for the actions with respect to the vehicle constraints + self._actions = actions + + def is_goal_reached(self, current: List[float], goal: List[float]): + # @TODO Currently, the threshold is just a random number, get rid of magic constants + # print(f"Current Pose: {current}") + # print(f"Goal Pose: {goal}") + # if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5 + # if np.abs(current[3] - goal[3]) > .25: return False + return np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5 + + def heuristic_cost_estimate(self, state_1, state_2): + """run the correct heuristic function based on the heuristic_string""" + + if self.heuristic_string == "reeds_shepp": + return self.reed_shepp(state_1, state_2) # Using turning radius of 1.0 + + elif self.heuristic_string == "approx_reeds_shepp": + return self.approx_reeds_shepp(state_1, state_2) # Using turning radius of 1.0 + + elif self.heuristic_string == "euclidian": + return self.euclidian_cost_esimate(state_1, state_2) + + else: + raise Exception(f"Unknown heuristic function: {self.heuristic_string}") + + def euclidian_cost_esimate(self, state_1, state_2): + """computes the 'direct' distance between two (x,y) tuples""" + # Extract position and orientation from states + (x1, y1, theta1, t) = state_1 + (x2, y2, theta2, t) = state_2 + + return math.hypot(x2 - x1, y2 - y1, theta2 - theta1) + + def approx_reeds_shepp(self, state_1, state_2): + # Extract position and orientation from states + (x1, y1, theta1, t1) = state_1 + (x2, y2, theta2, t2) = state_2 + + # Create start and goal configurations for Reeds-Shepp + start = (x1, y1, theta1) + goal = (x2, y2, theta2) + + # Calculate Reeds-Shepp path length + path_length_cost = path_length(start, goal, 1.0) # Using turning radius of 1.0 + + # # Add penalties for velocity and time differences + # velocity_penalty = abs(v2 - v1) * 0.1 + # time_penalty = abs(t2 - t1) * 0.01 + + return path_length_cost # + velocity_penalty + time_penalty + + def reed_shepp(self, state_1, state_2): + # Extract position and orientation from states + (x1, y1, theta1, t1) = state_1 + (x2, y2, theta2, t2) = state_2 + + # Create start and goal configurations for Reeds-Shepp + start = (x1, y1, theta1) + goal = (x2, y2, theta2) + + # Calculate Reeds-Shepp path length + path = reed_shepp.get_optimal_path(start, goal) + + return reed_shepp.path_length(path) # Using turning radius of 1.0 + + def terminal_cost_estimate(self, state_1, state_2): + """computes the 'direct' distance between two (x,y) tuples""" + # Extract position and orientation from states + return self.reed_shepp(state_1, state_2) + return self.approx_reeds_shepp(state_1, state_2) + + def distance_between(self, state_1, state_2): + """ + The states here are (x,y,theta,v,dtheta,t) + """ + (x1, y1, theta1, t) = state_1 + (x2, y2, theta2, t) = state_2 + + return math.hypot(x2 - x1, y2 - y1) + + def neighbors(self, node): + """ for a given configuration of the car in the maze, returns up to 4 adjacent(north,east,south,west) + nodes that can be reached (=any adjacent coordinate that is not a wall) + """ + print(f"[DEBUG] neighbors() called on node {node}") + neighbors = [] + # print(f"Node: {node}") + for control in self.actions: + next_state = self.vehicle_sim.nextState(node[:3], control) + # print(f"next state: {next_state}") + next_state = np.append(next_state, node[3] + self.vehicle_sim.T) + next_state = np.round(next_state, 3) + if self.is_valid_neighbor([next_state]): + # print(f"Accepted: {next_state}") + neighbors.append(tuple(next_state)) + else: + print(f"Rejected first order (collision): {next_state}") + return neighbors + + def is_valid_neighbor(self, path): + """check if any points along the path are in collision + with any of the known obstacles + @TODO We are not currently using the geometry of the vehicle, + define a geometry for the vehicle and then use polygon_itersects_polygon + @TODO Consider performing a linear search on the trajectory + and checking for collission on each of these configurations + + Args: + path (_type_): _description_ + """ + for obstacle in self.obstacles: + # print(f"Obstacle: {obstacle}") + for point in path: + vehicle_object = self.state_to_object(point) + # vehicle_polygon = vehicle_object.polygon_parent() + # print(f"Vehicle Polygon: {vehicle_polygon}") + # print(point) + # print(obstacle.polygon_parent()) + # print("====================================") + # print(f"Vehicle Object: {vehicle_object}") + # print(f"Vehicle Polygon: {vehicle_polygon}") + # print(f"Obstacle Polygon: {obstacle.polygon_parent()}") + # print(f"Obstacle: {obstacle}") + # print(f"Point: {point}") + # print(f"Obstacle Applied at current point: {obstacle.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=vehicle_object.pose)}") + # print(f"Obstacle Polygon at current point: {obstacle.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=vehicle_object.pose).polygon_parent()}") + #!!!!! the obstacle is in the world frame! + if collisions.polygon_intersects_polygon_2d(vehicle_object.polygon_parent(), obstacle.polygon_parent()): + # if collisions.polygon_intersects_polygon_2d(vehicle_object.polygon_parent(), + # obstacle.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=vehicle_object.pose).polygon_parent()): + #polygon_intersects_polygon_2d when we have the acutal car geometry + # raise Exception("Collision detected") + print("Collision detected") + return False + return True + + def state_to_object(self, state): + """Convert a state to a polygon. The state is of the form (x,y,theta,v,dtheta,t) + + Args: + state (_type_): _description_ + + Returns: + _type_: _description_ + """ + x = state[0] + y = state[1] + theta = state[2] + t = state[3] + + pose = ObjectPose(frame=ObjectFrameEnum.CURRENT,t=t, x=x,y=y,z=0,yaw=theta) + + temp_obj = PhysicalObject(pose=pose, + dimensions=self.vehicle.to_object().dimensions, + outline=self.vehicle.to_object().outline) + + return temp_obj + + +class ParkingPlanner(): + """_summary_ + + Args: + Component (_type_): _description_ + """ + def __init__(self): + # self.route_progress = None + # self.t_last = None + + # self.mode = settings.get("planning.longitudinal_plan.mode") + # self.planner = settings.get("planning.longitudinal_plan.planner") + # self.acceleration = settings.get("planning.longitudinal_plan.acceleration") + # self.deceleration = settings.get("planning.longitudinal_plan.deceleration") + # self.desired_speed = settings.get("planning.longitudinal_plan.desired_speed") + # self.yield_deceleration = settings.get("planning.longitudinal_plan.yield_deceleration") + + # Get the model of the car we are going to be using + # Get the obstacles + # Define the functions we need + # Create the Astar object + # self.planner = ParkingSolverSecondOrderDubins() + self.planner = ParkingSolverFirstOrderDubins() + + self.iterations = settings.get("run.drive.planning.route_planning_component.astar.iterations", 200) + + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['route'] + + def rate(self): + return 1.0 + + def vehicle_state_to_second_order(self, vehicle_state: VehicleState) -> Tuple[float, float]: + """Takes a vehicle state and outputs the state of a second order dubins car + + Args: + vehicle_state (VehicleState): _description_ + + Returns: + Tuple[float, float]: _description_ + """ + vehicle_state.pose = vehicle_state.pose.to_frame() + x = vehicle_state.pose.x + y = vehicle_state.pose.y + theta = vehicle_state.pose.yaw # check that this is correct + v = vehicle_state.v + dtheta = vehicle_state.heading_rate + t = 0 + + return (x,y,theta,v,dtheta,t) + + def vehicle_state_to_first_order(self, vehicle_state: VehicleState) -> Tuple[float, float]: + """Takes a vehicle state and outputs the state of a second order dubins car + + Args: + vehicle_state (VehicleState): _description_ + + Returns: + Tuple[float, float]: _description_ + """ + x = vehicle_state.pose.x + y = vehicle_state.pose.y + theta = vehicle_state.pose.yaw # check that this is correct + v = vehicle_state.v + dtheta = vehicle_state.heading_rate + t = 0 + + return (x,y,theta,t) + + def pose_to_first_order(self, pose: ObjectPose) -> Tuple[float, float]: + """Takes a vehicle state and outputs the state of a second order dubins car + + Args: + vehicle_state (VehicleState): _description_ + + Returns: + Tuple[float, float]: _description_ + """ + x = pose.x + y = pose.y + theta = pose.yaw # check that this is correct + t = 0 + + return (x,y,theta,t) + + def update(self, state : AllState) -> Route: + """_summary_ + + Args: + state (AllState): _description_ + + Returns: + Trajectory: _description_ + """ + vehicle = state.vehicle # type: VehicleState + print(f"Vehicle {vehicle}") + obstacles = state.obstacles # type: Dict[str, Obstacle] + agents = state.agents # type: Dict[str, AgentState] + all_obstacles = {**agents, **obstacles} + # print(f"Obstacles {obstacles}") + print(f"Agents {agents}") + goal_pose = state.goal + assert goal_pose.frame == ObjectFrameEnum.CURRENT + print("Checking VALUE: ", state.start_vehicle_pose) + print("Checking VALUE: ", state.vehicle) + start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) + # start_pose = state.vehicle.pose.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) + # start_cur = VehicleState.zero() + # start_cur.pose = start_pose # goal.pose = goal_pose + # goal.v = 0 + print("Checking VALUE: ", start_state) + + + + + # # Need to parse and create second order dubin car states + # start_state = self.vehicle_state_to_dynamics(vehicle) + # goal_state = self.vehicle_state_to_dynamics(goal) + + # Need to parse and create second order dubin car states + start = self.vehicle_state_to_first_order(start_state) + # goal = self.vehicle_state_to_first_order(goal_state) + goal = self.pose_to_first_order(goal_pose) + + # Update the planner + self.planner.obstacles = list(all_obstacles.values()) + self.planner.vehicle = vehicle + + # Compute the new trajectory and return it + print(f"Iteartions ------------------------------ {self.iterations}") + res = list(self.planner.astar(start, goal, reversePath=False, iterations=self.iterations)) + # points = [state[:2] for state in res] # change here to return the theta as well + points = [] + for state in res: + points.append((state[0], state[1])) + times = [state[3] for state in res] + path = Path(frame=vehicle.pose.frame, points=points) + traj = Trajectory(path.frame,points,times) + # print("===========================") + # print(f"Points: {points}") + # print(f"Times: {times}") + route = Path(frame=vehicle.pose.frame, points=points) + # traj = longitudinal_plan(route, 2, -2, 10, vehicle.v, "milestone") + print(traj) + return route + + + from rospy.exceptions import ROSInitException + + def create_trajectory_line_marker(self, traj: Trajectory, frame_id="map", marker_id=0, color=(0.0, 0.0, 1.0, 1.0)) -> Marker: + marker = Marker() + marker.header.frame_id = frame_id + + try: + marker.header.stamp = rospy.Time.now() + except ROSInitException: + # fallback to zero time (safe default in RViz) + marker.header.stamp = rospy.Time(0) + + marker.ns = "trajectory" + marker.id = marker_id + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + + marker.scale.x = 0.1 # Line width + marker.color.r, marker.color.g, marker.color.b, marker.color.a = color + + for pt in traj.points: + p = Point(x=pt[0], y=pt[1], z=pt[2] if len(pt) > 2 else 0.0) + marker.points.append(p) + + return marker + + + def visualize_trajectory(self, traj: Trajectory, frame_id="vehicle"): + + self.marker_pub = rospy.Publisher("trajectory_markers", Marker, queue_size=1, latch=True) + rospy.sleep(0.5) # ensure publisher is ready + + line_marker = self.create_trajectory_line_marker(traj, frame_id=frame_id) + self.marker_pub.publish(line_marker) + + def visualize_trajectory_animated(self, traj: Trajectory, frame_id="vehicle"): + """Animates a moving marker along the trajectory in RViz.""" + marker_pub = rospy.Publisher("animated_trajectory_marker", Marker, queue_size=1) + rate = rospy.Rate(20) # 20 Hz for smooth animation + + # Create a red sphere marker + marker = Marker() + marker.header.frame_id = frame_id + marker.ns = "animated_trajectory" + marker.id = 100 + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.scale.x = 0.4 + marker.scale.y = 0.4 + marker.scale.z = 0.4 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + + try: + start_ros_time = rospy.Time.now().to_sec() + except rospy.ROSInitException: + print("[WARN] Cannot animate trajectory — ROS node not initialized.") + return + + t_start = traj.times[0] + t_end = traj.times[-1] + + while not rospy.is_shutdown(): + t_now = rospy.Time.now().to_sec() + t = t_now - start_ros_time + t_start + + if t > t_end: + break + + pos = traj.eval(t) + marker.header.stamp = rospy.Time.now() + marker.pose.position.x = pos[0] + marker.pose.position.y = pos[1] + marker.pose.position.z = pos[2] if len(pos) > 2 else 0.0 + + marker_pub.publish(marker) + rate.sleep() + + print("[INFO] Animated trajectory complete.") diff --git a/GEMstack/onboard/planning/pure_pursuit.py b/GEMstack/onboard/planning/pure_pursuit.py index e4b6f4f4e..e4cb0f48b 100644 --- a/GEMstack/onboard/planning/pure_pursuit.py +++ b/GEMstack/onboard/planning/pure_pursuit.py @@ -2,7 +2,7 @@ from ...utils import settings from ...mathutils import transforms from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions -from ...state.vehicle import VehicleState,ObjectFrameEnum +from ...state.vehicle import VehicleState,ObjectFrameEnum, VehicleGearEnum from ...state.trajectory import Path,Trajectory,compute_headings from ...knowledge.vehicle.geometry import front2steer from ..interface.gem import GEMVehicleCommand @@ -105,9 +105,13 @@ def compute(self, state : VehicleState, component : Component = None): else: desired_x,desired_y = self.path.eval(des_parameter) desired_yaw = np.arctan2(desired_y-curr_y,desired_x-curr_x) - #print("Desired point",(desired_x,desired_y)," with lookahead distance",self.look_ahead + self.look_ahead_scale * speed) - #print("Current yaw",curr_yaw,"desired yaw",desired_yaw) - + + print("Desired point",(desired_x,desired_y)," with lookahead distance",self.look_ahead + self.look_ahead_scale * speed) + print("Current x",curr_x,"Current y",curr_y) + print("Current yaw",curr_yaw,"Desired yaw",desired_yaw) + print("Parameter : ", self.current_path_parameter) + print("Last index : ", self.path.domain()[1]) + # distance between the desired point and the vehicle L = transforms.vector2_dist((desired_x,desired_y),(curr_x,curr_y)) @@ -142,6 +146,7 @@ def compute(self, state : VehicleState, component : Component = None): #past the end, just stop desired_speed = 0.0 feedforward_accel = -2.0 + f_delta = 0 else: if self.desired_speed_source=='path': current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) @@ -165,6 +170,17 @@ def compute(self, state : VehicleState, component : Component = None): else: #decay speed when crosstrack error is high desired_speed *= np.exp(-abs(ct_error)*0.4) + + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + if component is not None: + component.debug_event('Past the end of trajectory') + #past the end, just stop + desired_speed = 0.0 + feedforward_accel = -2.0 + f_delta = 0 + + + if desired_speed > self.speed_limit: desired_speed = self.speed_limit output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel) @@ -205,6 +221,7 @@ def state_outputs(self): return [] def update(self, vehicle : VehicleState, trajectory: Trajectory): + vehicle.gear = 1 self.pure_pursuit.set_path(trajectory) accel,wheel_angle = self.pure_pursuit.compute(vehicle, self) #print("Desired wheel angle",wheel_angle) diff --git a/GEMstack/onboard/planning/reed_shepp.py b/GEMstack/onboard/planning/reed_shepp.py new file mode 100644 index 000000000..8d5496e65 --- /dev/null +++ b/GEMstack/onboard/planning/reed_shepp.py @@ -0,0 +1,473 @@ +""" +https://github.com/nathanlct/reeds-shepp-curves + +Implementation of the optimal path formulas given in the following paper: + +OPTIMAL PATHS FOR A CAR THAT GOES BOTH FORWARDS AND BACKWARDS +J. A. REEDS AND L. A. SHEPP + +notes: there are some typos in the formulas given in the paper; +some formulas have been adapted (cf http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c) + +Each of the 12 functions (each representing 4 of the 48 possible words) +have 3 arguments x, y and phi, the goal position and angle (in degrees) of the +object given it starts at position (0, 0) and angle 0, and returns the +corresponding path (if it exists) as a list of PathElements (or an empty list). + +(actually there are less than 48 possible words but this code is not optimized) + +MIT License + +Copyright (c) 2019 Nathan Lichtlé + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +""" + +import math +from enum import Enum +from dataclasses import dataclass, replace + +def M(theta): + """ + Return the angle phi = theta mod (2 pi) such that -pi <= theta < pi. + """ + theta = theta % (2*math.pi) + if theta < -math.pi: return theta + 2*math.pi + if theta >= math.pi: return theta - 2*math.pi + return theta + +def R(x, y): + """ + Return the polar coordinates (r, theta) of the point (x, y). + """ + r = math.sqrt(x*x + y*y) + theta = math.atan2(y, x) + return r, theta + +def change_of_basis(p1, p2): + """ + Given p1 = (x1, y1, theta1) and p2 = (x2, y2, theta2) represented in a + coordinate system with origin (0, 0) and rotation 0 (in degrees), return + the position and rotation of p2 in the coordinate system which origin + (x1, y1) and rotation theta1. + """ + theta1 = deg2rad(p1[2]) + dx = p2[0] - p1[0] + dy = p2[1] - p1[1] + new_x = dx * math.cos(theta1) + dy * math.sin(theta1) + new_y = -dx * math.sin(theta1) + dy * math.cos(theta1) + new_theta = p2[2] - p1[2] + return new_x, new_y, new_theta + +def rad2deg(rad): + return 180 * rad / math.pi + +def deg2rad(deg): + return math.pi * deg / 180 + +def sign(x): + return 1 if x >= 0 else -1 + +class Steering(Enum): + LEFT = -1 + RIGHT = 1 + STRAIGHT = 0 + + +class Gear(Enum): + FORWARD = 1 + BACKWARD = -1 + + +@dataclass(eq=True) +class PathElement: + param: float + steering: Steering + gear: Gear + + @classmethod + def create(cls, param: float, steering: Steering, gear: Gear): + if param >= 0: + return cls(param, steering, gear) + else: + return cls(-param, steering, gear).reverse_gear() + + def __repr__(self): + s = "{ Steering: " + self.steering.name + "\tGear: " + self.gear.name \ + + "\tdistance: " + str(round(self.param, 2)) + " }" + return s + + def reverse_steering(self): + steering = Steering(-self.steering.value) + return replace(self, steering=steering) + + def reverse_gear(self): + gear = Gear(-self.gear.value) + return replace(self, gear=gear) + + +def path_length(path): + """ + this one's obvious + """ + return sum([e.param for e in path]) + + +def get_optimal_path(start, end): + """ + Return the shortest path from start to end among those that exist + """ + paths = get_all_paths(start, end) + return min(paths, key=path_length) + + +def get_all_paths(start, end): + """ + Return a list of all the paths from start to end generated by the + 12 functions and their variants + """ + path_fns = [path1, path2, path3, path4, path5, path6, \ + path7, path8, path9, path10, path11, path12] + paths = [] + + # get coordinates of end in the set of axis where start is (0,0,0) + x, y, theta = change_of_basis(start, end) + + for get_path in path_fns: + # get the four variants for each path type, cf article + paths.append(get_path(x, y, theta)) + paths.append(timeflip(get_path(-x, y, -theta))) + paths.append(reflect(get_path(x, -y, -theta))) + paths.append(reflect(timeflip(get_path(-x, -y, theta)))) + + # remove path elements that have parameter 0 + for i in range(len(paths)): + paths[i] = list(filter(lambda e: e.param != 0, paths[i])) + + # remove empty paths + paths = list(filter(None, paths)) + + return paths + + +def timeflip(path): + """ + timeflip transform described around the end of the article + """ + new_path = [e.reverse_gear() for e in path] + return new_path + + +def reflect(path): + """ + reflect transform described around the end of the article + """ + new_path = [e.reverse_steering() for e in path] + return new_path + + +def path1(x, y, phi): + """ + Formula 8.1: CSC (same turns) + """ + phi = deg2rad(phi) + path = [] + + u, t = R(x - math.sin(phi), y - 1 + math.cos(phi)) + v = M(phi - t) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.FORWARD)) + path.append(PathElement.create(v, Steering.LEFT, Gear.FORWARD)) + + return path + + +def path2(x, y, phi): + """ + Formula 8.2: CSC (opposite turns) + """ + phi = M(deg2rad(phi)) + path = [] + + rho, t1 = R(x + math.sin(phi), y - 1 - math.cos(phi)) + + if rho * rho >= 4: + u = math.sqrt(rho * rho - 4) + t = M(t1 + math.atan2(2, u)) + v = M(t - phi) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.FORWARD)) + path.append(PathElement.create(v, Steering.RIGHT, Gear.FORWARD)) + + return path + + +def path3(x, y, phi): + """ + Formula 8.3: C|C|C + """ + phi = deg2rad(phi) + path = [] + + xi = x - math.sin(phi) + eta = y - 1 + math.cos(phi) + rho, theta = R(xi, eta) + + if rho <= 4: + A = math.acos(rho / 4) + t = M(theta + math.pi/2 + A) + u = M(math.pi - 2*A) + v = M(phi - t - u) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.RIGHT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.LEFT, Gear.FORWARD)) + + return path + + +def path4(x, y, phi): + """ + Formula 8.4 (1): C|CC + """ + phi = deg2rad(phi) + path = [] + + xi = x - math.sin(phi) + eta = y - 1 + math.cos(phi) + rho, theta = R(xi, eta) + + if rho <= 4: + A = math.acos(rho / 4) + t = M(theta + math.pi/2 + A) + u = M(math.pi - 2*A) + v = M(t + u - phi) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.RIGHT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.LEFT, Gear.BACKWARD)) + + return path + + +def path5(x, y, phi): + """ + Formula 8.4 (2): CC|C + """ + phi = deg2rad(phi) + path = [] + + xi = x - math.sin(phi) + eta = y - 1 + math.cos(phi) + rho, theta = R(xi, eta) + + if rho <= 4: + u = math.acos(1 - rho*rho/8) + A = math.asin(2 * math.sin(u) / rho) + t = M(theta + math.pi/2 - A) + v = M(t - u - phi) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.RIGHT, Gear.FORWARD)) + path.append(PathElement.create(v, Steering.LEFT, Gear.BACKWARD)) + + return path + + +def path6(x, y, phi): + """ + Formula 8.7: CCu|CuC + """ + phi = deg2rad(phi) + path = [] + + xi = x + math.sin(phi) + eta = y - 1 - math.cos(phi) + rho, theta = R(xi, eta) + + if rho <= 4: + if rho <= 2: + A = math.acos((rho + 2) / 4) + t = M(theta + math.pi/2 + A) + u = M(A) + v = M(phi - t + 2*u) + else: + A = math.acos((rho - 2) / 4) + t = M(theta + math.pi/2 - A) + u = M(math.pi - A) + v = M(phi - t + 2*u) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.RIGHT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.LEFT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.RIGHT, Gear.BACKWARD)) + + return path + + +def path7(x, y, phi): + """ + Formula 8.8: C|CuCu|C + """ + phi = deg2rad(phi) + path = [] + + xi = x + math.sin(phi) + eta = y - 1 - math.cos(phi) + rho, theta = R(xi, eta) + u1 = (20 - rho*rho) / 16 + + if rho <= 6 and 0 <= u1 <= 1: + u = math.acos(u1) + A = math.asin(2 * math.sin(u) / rho) + t = M(theta + math.pi/2 + A) + v = M(t - phi) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.RIGHT, Gear.BACKWARD)) + path.append(PathElement.create(u, Steering.LEFT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.RIGHT, Gear.FORWARD)) + + return path + + +def path8(x, y, phi): + """ + Formula 8.9 (1): C|C[pi/2]SC + """ + phi = deg2rad(phi) + path = [] + + xi = x - math.sin(phi) + eta = y - 1 + math.cos(phi) + rho, theta = R(xi, eta) + + if rho >= 2: + u = math.sqrt(rho*rho - 4) - 2 + A = math.atan2(2, u+2) + t = M(theta + math.pi/2 + A) + v = M(t - phi + math.pi/2) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(math.pi/2, Steering.RIGHT, Gear.BACKWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.LEFT, Gear.BACKWARD)) + + return path + + +def path9(x, y, phi): + """ + Formula 8.9 (2): CSC[pi/2]|C + """ + phi = deg2rad(phi) + path = [] + + xi = x - math.sin(phi) + eta = y - 1 + math.cos(phi) + rho, theta = R(xi, eta) + + if rho >= 2: + u = math.sqrt(rho*rho - 4) - 2 + A = math.atan2(u+2, 2) + t = M(theta + math.pi/2 - A) + v = M(t - phi - math.pi/2) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.FORWARD)) + path.append(PathElement.create(math.pi/2, Steering.RIGHT, Gear.FORWARD)) + path.append(PathElement.create(v, Steering.LEFT, Gear.BACKWARD)) + + return path + + +def path10(x, y, phi): + """ + Formula 8.10 (1): C|C[pi/2]SC + """ + phi = deg2rad(phi) + path = [] + + xi = x + math.sin(phi) + eta = y - 1 - math.cos(phi) + rho, theta = R(xi, eta) + + if rho >= 2: + t = M(theta + math.pi/2) + u = rho - 2 + v = M(phi - t - math.pi/2) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(math.pi/2, Steering.RIGHT, Gear.BACKWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.RIGHT, Gear.BACKWARD)) + + return path + + +def path11(x, y, phi): + """ + Formula 8.10 (2): CSC[pi/2]|C + """ + phi = deg2rad(phi) + path = [] + + xi = x + math.sin(phi) + eta = y - 1 - math.cos(phi) + rho, theta = R(xi, eta) + + if rho >= 2: + t = M(theta) + u = rho - 2 + v = M(phi - t - math.pi/2) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.FORWARD)) + path.append(PathElement.create(math.pi/2, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(v, Steering.RIGHT, Gear.BACKWARD)) + + return path + + +def path12(x, y, phi): + """ + Formula 8.11: C|C[pi/2]SC[pi/2]|C + """ + phi = deg2rad(phi) + path = [] + + xi = x + math.sin(phi) + eta = y - 1 - math.cos(phi) + rho, theta = R(xi, eta) + + if rho >= 4: + u = math.sqrt(rho*rho - 4) - 4 + A = math.atan2(2, u+4) + t = M(theta + math.pi/2 + A) + v = M(t - phi) + + path.append(PathElement.create(t, Steering.LEFT, Gear.FORWARD)) + path.append(PathElement.create(math.pi/2, Steering.RIGHT, Gear.BACKWARD)) + path.append(PathElement.create(u, Steering.STRAIGHT, Gear.BACKWARD)) + path.append(PathElement.create(math.pi/2, Steering.LEFT, Gear.BACKWARD)) + path.append(PathElement.create(v, Steering.RIGHT, Gear.FORWARD)) + + return path \ No newline at end of file diff --git a/GEMstack/onboard/planning/route_planning_component.py b/GEMstack/onboard/planning/route_planning_component.py index 3eb85d4e0..4b3dc535f 100644 --- a/GEMstack/onboard/planning/route_planning_component.py +++ b/GEMstack/onboard/planning/route_planning_component.py @@ -6,7 +6,10 @@ from GEMstack.state.agent import AgentState from GEMstack.state.all import AllState from GEMstack.state.physical_object import ObjectFrameEnum -from GEMstack.state.route import PlannerEnum, Route +from GEMstack.state.route import PlannerEnum, Route, Path +from .parking_route_planner import ParkingPlanner +from .longitudinal_planning import longitudinal_plan + @@ -14,8 +17,11 @@ class RoutePlanningComponent(Component): """Reads a route from disk and returns it as the desired route.""" def __init__(self): print("Route Planning Component init") - self.planner = None self.route = None + self.planner = None + self.compute_parking_route = False + self.done_computing = False + self.already_computed = False def state_inputs(self): return ["all"] @@ -27,21 +33,41 @@ def rate(self): return 10.0 def update(self, state: AllState): - # print("Route Planner's mission:", state.mission_plan.planner_type.value) - # print("type of mission plan:", type(PlannerEnum.RRT_STAR)) - # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value) - # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.PARKING.value) - # print("Mission plan:", state.mission_plan) - # print("Vehicle x:", state.vehicle.pose.x) - # print("Vehicle y:", state.vehicle.pose.y) - # print("Vehicle yaw:", state.vehicle.pose.yaw) - if state.mission_plan.type.name == "PARKING": - print("I am in PARKING mode") - # Return a route after doing some processing based on mission plan REMOVE ONCE OTHER PLANNERS ARE IMPLEMENTED + + if state.mission_plan.type.name == "PARKING" and not self.compute_parking_route: + print("I am in BRAKING mode") + + desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y), + (state.vehicle.pose.x, state.vehicle.pose.y)] + desired_path = Path(state.vehicle.pose.frame, desired_points) + + desired_path = desired_path.to_frame(state.vehicle.pose.frame, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) + + self.compute_parking_route = True + + return desired_path + elif state.mission_plan.type.name == "PARKING" and self.compute_parking_route and not self.done_computing: + print("I am in PARKING mode") + state.vehicle.pose.yaw = 0 # needed this to avoid a weird error in the parking planner + + if not self.already_computed: + self.planner = ParkingPlanner() + self.done_computing = True + self.route = self.planner.update(state) + self.route = self.route.to_frame(ObjectFrameEnum.START, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) + self.planner.visualize_trajectory(self.route) + self.already_computed = True + + return self.route + elif state.mission_plan.type.name == "SCANNING": print("I am in SCANNING mode") - else: - print("Unknown mode") + desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y), + (state.vehicle.pose.x + 1, state.vehicle.pose.y)] + desired_path = Path(ObjectFrameEnum.START, desired_points) + + desired_path = desired_path.to_frame(state.vehicle.pose.frame, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) + return desired_path return self.route \ No newline at end of file diff --git a/launch/parking_detection.yaml b/launch/parking_detection.yaml index 4745bdc12..10f495a83 100644 --- a/launch/parking_detection.yaml +++ b/launch/parking_detection.yaml @@ -14,19 +14,19 @@ drive: perception: state_estimation : GNSSStateEstimator # agent_detection : cone_detection.ConeDetector3D - obstacle_detection : - type: cone_detection.ConeDetector3D - args: - camera_name: front_right #[front, front_right] - camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + # obstacle_detection : + # type: cone_detection.ConeDetector3D + # args: + # camera_name: front_right #[front, front_right] + # camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml - # optional overrides - enable_tracking: False - visualize_2d: False - use_cyl_roi: False - save_data: False - orientation: False - use_start_frame: False + # # optional overrides + # enable_tracking: False + # visualize_2d: False + # use_cyl_roi: False + # save_data: False + # orientation: False + # use_start_frame: False parking_detection: type: parking_detection.ParkingSpotsDetector3D args: @@ -84,12 +84,58 @@ variants: camera_name: front_right #[front, front_right] camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml visualize_3d: True + planning: + _mission_planner: + type: parking_component.ParkingSim + # _route_planner: + # type: route_planning_component.RoutePlanningComponent + + fake_sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + visualization: !include "klampt_visualization.yaml" + drive: + perception: + state_estimation : OmniscientStateEstimator + obstacle_detection : + type: cone_detection.FakConeDetector + # agent_detection : cone_detection_simple.ConeDetectorSimple3D + parking_detection: + type: parking_detection.ParkingSpotsDetector3D + args: + camera_name: front_right #[front, front_right] + camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + visualize_3d: True planning: _mission_planner: type: parking_component.ParkingSim _route_planner: type: route_planning_component.RoutePlanningComponent - + dubins: + integrator: + timestep: 1 + dt: .25 + max_velocity: 0.75 + max_turning_rate: 0.3 + tolerance: 0.25 + astar: + iterations: 200 + heuristic: reeds_shepp + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + # type: yield_spline_planner.SplinePlanner + largs: + acceleration: 5 + deceleration: 2 + desired_speed: 2 + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + sim: run: mode: simulation From ac2a80d91ee9ba1a7244c8d510f1951966b922b5 Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Mon, 12 May 2025 19:23:53 -0500 Subject: [PATCH 02/11] fix for simulator --- GEMstack/knowledge/defaults/current.yaml | 4 ++-- GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index c886288be..d18e37711 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -14,10 +14,10 @@ control: pure_pursuit: lookahead: 2.0 lookahead_scale: 3.0 - crosstrack_gain: 1.0 + crosstrack_gain: 0.5 desired_speed: trajectory longitudinal_control: - pid_p: 1.0 + pid_p: 1.5 pid_i: 0.1 pid_d: 0.0 diff --git a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml index d20d2c357..06ad3415b 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml +++ b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml @@ -12,7 +12,7 @@ max_accelerator_power: #Watts. Power at max accelerator pedal, by gear - 10000.0 max_accelerator_power_reverse: 10000.0 #Watts. Power (backwards) in reverse gear -acceleration_model : kris_v1 +acceleration_model : hang_v1 accelerator_active_range : [0.2, 1.0] #range of accelerator pedal where output acceleration is not flat brake_active_range : [0,1] #range of brake pedal where output deceleration is not flat From b47ca380b22d715eee9903347b772042c30bace3 Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Mon, 12 May 2025 20:16:44 -0500 Subject: [PATCH 03/11] renaming parking goal for clarity + adding on board launch functionality --- .../knowledge/defaults/computation_graph.yaml | 2 +- GEMstack/onboard/perception/parking_detection.py | 2 +- GEMstack/onboard/planning/parking_component.py | 8 ++------ .../onboard/planning/parking_route_planner.py | 16 ++++++++-------- .../onboard/planning/route_planning_component.py | 8 ++++---- GEMstack/state/all.py | 2 +- 6 files changed, 17 insertions(+), 21 deletions(-) diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index 418b0b512..fb58e6009 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -23,7 +23,7 @@ components: outputs: vehicle_lane - parking_detection: inputs: obstacles - outputs: [goal, obstacles] + outputs: [parking_goal, obstacles] - sign_detection: inputs: [vehicle, roadgraph] outputs: roadgraph.signs diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py index f0952544b..334e0238e 100644 --- a/GEMstack/onboard/perception/parking_detection.py +++ b/GEMstack/onboard/perception/parking_detection.py @@ -76,7 +76,7 @@ def state_inputs(self) -> list: return ['obstacles'] def state_outputs(self) -> list: - return ['goal', 'obstacles'] + return ['parking_goal', 'obstacles'] def visualize(self, diff --git a/GEMstack/onboard/planning/parking_component.py b/GEMstack/onboard/planning/parking_component.py index 6860fd357..6f2ba139e 100644 --- a/GEMstack/onboard/planning/parking_component.py +++ b/GEMstack/onboard/planning/parking_component.py @@ -24,14 +24,10 @@ def update(self, state: AllState): # Calculate elapsed time since initialization. elapsed_time = time.time() - self.start_time - # Reading goal from state - # print(f"\n AllState (parking goal): {state.goal} \n") - # print(f"AllState (parking obstacles): {state.obstacles} \n") - # After a goal is detected, change the mission plan to use PARKING. - if state.goal: + if state.parking_goal: print("\n Parking goal available. Entering PARKING mode......") - mission_plan = MissionObjective(PlannerEnum.PARKING, state.goal) + mission_plan = MissionObjective(PlannerEnum.PARKING, state.parking_goal) else: print("\n Entering SCANNING mode......") mission_plan = MissionObjective(PlannerEnum.SCANNING) diff --git a/GEMstack/onboard/planning/parking_route_planner.py b/GEMstack/onboard/planning/parking_route_planner.py index 5924cf336..80641a7e0 100644 --- a/GEMstack/onboard/planning/parking_route_planner.py +++ b/GEMstack/onboard/planning/parking_route_planner.py @@ -36,12 +36,12 @@ def __init__(self, vehicle=None, obstacles=None, actions=None): self._vehicle = None # settings - self.T = settings.get("run.drive.planning.route_planning_component.dubins.integrator.time_step", 1.5) - self.dt = settings.get("run.drive.planning.route_planning_component.dubins.integrator.dt", .25) - self.max_v = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_velocity", 0.75) - self.max_turn_rate = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_turn_rate", 0.3) - self.tolerance = settings.get("run.drive.planning.route_planning_component.dubins.tolerance", 0.5) - self.heuristic_string = settings.get("run.drive.planning.route_planning_component.astar.heuristic", "reeds_shepp") + self.T = settings.get("run.drive.planning._route_planner.dubins.integrator.time_step", 1.5) + self.dt = settings.get("run.drive.planning._route_planner.dubins.integrator.dt", .25) + self.max_v = settings.get("run.drive.planning._route_planner.dubins.actions.max_velocity", 0.75) + self.max_turn_rate = settings.get("run.drive.planning._route_planner.dubins.actions.max_turn_rate", 0.3) + self.tolerance = settings.get("run.drive.planning._route_planner.dubins.tolerance", 0.5) + self.heuristic_string = settings.get("run.drive.planning._route_planner.astar.heuristic", "reeds_shepp") self.vehicle = DubinsCar() #x = (tx,ty,theta) and u = (fwd_velocity,turnRate). self.vehicle_sim = DubinsCarIntegrator(self.vehicle, self.T, self.dt) @@ -255,7 +255,7 @@ def __init__(self): # self.planner = ParkingSolverSecondOrderDubins() self.planner = ParkingSolverFirstOrderDubins() - self.iterations = settings.get("run.drive.planning.route_planning_component.astar.iterations", 200) + self.iterations = settings.get("run.drive.planning._route_planner.astar.iterations", 200) def state_inputs(self): return ['all'] @@ -335,7 +335,7 @@ def update(self, state : AllState) -> Route: all_obstacles = {**agents, **obstacles} # print(f"Obstacles {obstacles}") print(f"Agents {agents}") - goal_pose = state.goal + goal_pose = state.parking_goal assert goal_pose.frame == ObjectFrameEnum.CURRENT print("Checking VALUE: ", state.start_vehicle_pose) print("Checking VALUE: ", state.vehicle) diff --git a/GEMstack/onboard/planning/route_planning_component.py b/GEMstack/onboard/planning/route_planning_component.py index 4b3dc535f..090f25e77 100644 --- a/GEMstack/onboard/planning/route_planning_component.py +++ b/GEMstack/onboard/planning/route_planning_component.py @@ -35,7 +35,7 @@ def rate(self): def update(self, state: AllState): if state.mission_plan.type.name == "PARKING" and not self.compute_parking_route: - print("I am in BRAKING mode") + # print("I am in BRAKING mode") desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y), (state.vehicle.pose.x, state.vehicle.pose.y)] @@ -48,7 +48,7 @@ def update(self, state: AllState): return desired_path elif state.mission_plan.type.name == "PARKING" and self.compute_parking_route and not self.done_computing: - print("I am in PARKING mode") + # print("I am in PARKING mode") state.vehicle.pose.yaw = 0 # needed this to avoid a weird error in the parking planner if not self.already_computed: @@ -58,11 +58,11 @@ def update(self, state: AllState): self.route = self.route.to_frame(ObjectFrameEnum.START, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose) self.planner.visualize_trajectory(self.route) self.already_computed = True - + self.route.points = self.route.points[:-2] # temporary fix to avoid going too far into the parking space, next perception PR will resolve return self.route elif state.mission_plan.type.name == "SCANNING": - print("I am in SCANNING mode") + # print("I am in SCANNING mode") desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y), (state.vehicle.pose.x + 1, state.vehicle.pose.y)] desired_path = Path(ObjectFrameEnum.START, desired_points) diff --git a/GEMstack/state/all.py b/GEMstack/state/all.py index 4fb863ed5..d639b23cb 100644 --- a/GEMstack/state/all.py +++ b/GEMstack/state/all.py @@ -24,7 +24,7 @@ class AllState(SceneState): agent_intents : Dict[str,AgentIntentMixture] = field(default_factory=dict) relations : List[EntityRelation] = field(default_factory=list) predicates : PredicateValues = field(default_factory=PredicateValues) - goal: ObjectPose = None + parking_goal: ObjectPose = None # planner-output state mission : MissionObjective = field(default_factory=MissionObjective) From f8b9dfc06c1e9d4d318425de80a02931db7466ab Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Mon, 12 May 2025 20:20:36 -0500 Subject: [PATCH 04/11] updating launch --- launch/parking_detection.yaml | 51 +++++++++++++++++++++++++---------- 1 file changed, 37 insertions(+), 14 deletions(-) diff --git a/launch/parking_detection.yaml b/launch/parking_detection.yaml index 10f495a83..e4e35b2b0 100644 --- a/launch/parking_detection.yaml +++ b/launch/parking_detection.yaml @@ -13,20 +13,19 @@ recovery: drive: perception: state_estimation : GNSSStateEstimator - # agent_detection : cone_detection.ConeDetector3D - # obstacle_detection : - # type: cone_detection.ConeDetector3D - # args: - # camera_name: front_right #[front, front_right] - # camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + obstacle_detection : + type: cone_detection.ConeDetector3D + # args: + # camera_name: front_right #[front, front_right] + # camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml - # # optional overrides - # enable_tracking: False - # visualize_2d: False - # use_cyl_roi: False - # save_data: False - # orientation: False - # use_start_frame: False + # # optional overrides + # enable_tracking: False + # visualize_2d: False + # use_cyl_roi: False + # save_data: False + # orientation: False + # use_start_frame: False parking_detection: type: parking_detection.ParkingSpotsDetector3D args: @@ -37,6 +36,30 @@ drive: planning: _mission_planner: type: parking_component.ParkingSim + _route_planner: + type: route_planning_component.RoutePlanningComponent + dubins: + integrator: + timestep: 1 + dt: .25 + max_velocity: 0.75 + max_turning_rate: 0.3 + tolerance: 0.25 + astar: + iterations: 20000 + heuristic: reeds_shepp + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + # type: yield_spline_planner.SplinePlanner + largs: + acceleration: 5 + deceleration: 2 + desired_speed: 2 + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + + #usually can keep this constant computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" @@ -123,7 +146,7 @@ variants: max_turning_rate: 0.3 tolerance: 0.25 astar: - iterations: 200 + iterations: 20000 heuristic: reeds_shepp motion_planning: type: longitudinal_planning.YieldTrajectoryPlanner From 6507a37b044c33ccfd7a593335ce56bae2e9a587 Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Mon, 12 May 2025 20:22:14 -0500 Subject: [PATCH 05/11] revert cone_detection to avoid merge conflicts --- GEMstack/onboard/perception/cone_detection.py | 81 ++++--------------- 1 file changed, 16 insertions(+), 65 deletions(-) diff --git a/GEMstack/onboard/perception/cone_detection.py b/GEMstack/onboard/perception/cone_detection.py index d0c13a31b..ab83d03df 100644 --- a/GEMstack/onboard/perception/cone_detection.py +++ b/GEMstack/onboard/perception/cone_detection.py @@ -472,12 +472,9 @@ def save_sensor_data(self, vehicle: VehicleState, latest_image) -> None: class FakConeDetector(Component): def __init__(self, vehicle_interface: GEMInterface): - """ - Initializes the FakeConeDetector with two parking spots: - - One standard 2x2 rectangular spot - - One wider mouth for easier entry - """ self.vehicle_interface = vehicle_interface + self.times = [(5.0, 20.0), (30.0, 35.0)] + self.t_start = None def rate(self): return 4.0 @@ -489,70 +486,24 @@ def state_outputs(self): return ['obstacles'] def update(self, vehicle: VehicleState) -> Dict[str, Obstacle]: - """ - Called every simulation step, updates the timestamp and publishes the obstacle states. - """ - - current_time = self.vehicle_interface.time() - - # === Standard Parking Spot === - # cones_standard = { - # 'cone0': (5.0, 5.0, 0.5, 0.5), # Front Left - # 'cone1': (5.0, 7.0, 0.5, 0.5), # Back Left - # 'cone2': (7.0, 5.0, 0.5, 0.5), # Front Right - # 'cone3': (7.0, 7.0, 0.5, 0.5) # Back Right - # } - - # === Wider Mouth Parking Spot === - cones_wide = { - 'cone4': (6.0, 9.0, 0.5, 0.5), # Front Left (wide) - 'cone5': (10.0, 9.0, 0.5, 0.5), # Back Left - 'cone6': (3.0, 4.0, 0.5, 0.5), # Front Right (wide) - 'cone7': (7.0, 4.0, 0.5, 0.5) # Back Right - } - - # Populate the obstacle states + if self.t_start is None: + self.t_start = self.vehicle_interface.time() + t = self.vehicle_interface.time() - self.t_start res = {} - for name, box in {**cones_wide}.items(): - res[name] = box_to_fake_obstacle(box, current_time) - - # Update timestamp - for obstacle in res.values(): - obstacle.pose.t = current_time - - rospy.loginfo(f"[FakeConeDetector] Simulated Two Parking Spots Detected") + for time_range in self.times: + if t >= time_range[0] and t <= time_range[1]: + res['cone0'] = box_to_fake_obstacle((0, 0, 0, 0)) + rospy.loginfo("Detected a Cone (simulated)") return res -def box_to_fake_obstacle(box, current_time): - """ - Helper function to create a fake obstacle (cone) from bounding box coordinates. - """ - x, y, w, h = box - pose = ObjectPose( - t=current_time, - x=x, - y=y, - z=0.0, - yaw=0.0, - pitch=0.0, - roll=0.0, - frame=ObjectFrameEnum.START - ) - - dims = (w, h, 1.0) - - new_obstacle = Obstacle( - pose=pose, - dimensions=dims, - outline=None, - material=ObstacleMaterialEnum.TRAFFIC_CONE, - collidable=True, - state=ObstacleStateEnum.STANDING - ) - print("Obstacles made") - return new_obstacle +def box_to_fake_obstacle(box): + x, y, w, h = box + pose = ObjectPose(t=0, x=x + w / 2, y=y + h / 2, z=0, yaw=0, pitch=0, roll=0, frame=ObjectFrameEnum.CURRENT) + dims = (w, h, 0) + return Obstacle(pose=pose, dimensions=dims, outline=None, + material=ObstacleMaterialEnum.TRAFFIC_CONE, state=ObstacleStateEnum.STANDING, collidable=True) if __name__ == '__main__': - pass + pass \ No newline at end of file From cb4e5975126fd59ed29cd41823742cf61c9fee1e Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Mon, 12 May 2025 20:31:32 -0500 Subject: [PATCH 06/11] comment + print statement clean up --- .../onboard/planning/parking_route_planner.py | 113 +----------------- 1 file changed, 4 insertions(+), 109 deletions(-) diff --git a/GEMstack/onboard/planning/parking_route_planner.py b/GEMstack/onboard/planning/parking_route_planner.py index 80641a7e0..ce363bdec 100644 --- a/GEMstack/onboard/planning/parking_route_planner.py +++ b/GEMstack/onboard/planning/parking_route_planner.py @@ -45,7 +45,6 @@ def __init__(self, vehicle=None, obstacles=None, actions=None): self.vehicle = DubinsCar() #x = (tx,ty,theta) and u = (fwd_velocity,turnRate). self.vehicle_sim = DubinsCarIntegrator(self.vehicle, self.T, self.dt) - #@TODO create a more standardized way to define the actions self._actions = generate_action_set(self.max_v, self.max_turn_rate) @@ -75,11 +74,6 @@ def actions(self, actions): self._actions = actions def is_goal_reached(self, current: List[float], goal: List[float]): - # @TODO Currently, the threshold is just a random number, get rid of magic constants - # print(f"Current Pose: {current}") - # print(f"Goal Pose: {goal}") - # if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5 - # if np.abs(current[3] - goal[3]) > .25: return False return np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5 def heuristic_cost_estimate(self, state_1, state_2): @@ -186,25 +180,8 @@ def is_valid_neighbor(self, path): # print(f"Obstacle: {obstacle}") for point in path: vehicle_object = self.state_to_object(point) - # vehicle_polygon = vehicle_object.polygon_parent() - # print(f"Vehicle Polygon: {vehicle_polygon}") - # print(point) - # print(obstacle.polygon_parent()) - # print("====================================") - # print(f"Vehicle Object: {vehicle_object}") - # print(f"Vehicle Polygon: {vehicle_polygon}") - # print(f"Obstacle Polygon: {obstacle.polygon_parent()}") - # print(f"Obstacle: {obstacle}") - # print(f"Point: {point}") - # print(f"Obstacle Applied at current point: {obstacle.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=vehicle_object.pose)}") - # print(f"Obstacle Polygon at current point: {obstacle.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=vehicle_object.pose).polygon_parent()}") - #!!!!! the obstacle is in the world frame! if collisions.polygon_intersects_polygon_2d(vehicle_object.polygon_parent(), obstacle.polygon_parent()): - # if collisions.polygon_intersects_polygon_2d(vehicle_object.polygon_parent(), - # obstacle.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=vehicle_object.pose).polygon_parent()): - #polygon_intersects_polygon_2d when we have the acutal car geometry - # raise Exception("Collision detected") - print("Collision detected") + # print("Collision detected") return False return True @@ -238,21 +215,7 @@ class ParkingPlanner(): Component (_type_): _description_ """ def __init__(self): - # self.route_progress = None - # self.t_last = None - - # self.mode = settings.get("planning.longitudinal_plan.mode") - # self.planner = settings.get("planning.longitudinal_plan.planner") - # self.acceleration = settings.get("planning.longitudinal_plan.acceleration") - # self.deceleration = settings.get("planning.longitudinal_plan.deceleration") - # self.desired_speed = settings.get("planning.longitudinal_plan.desired_speed") - # self.yield_deceleration = settings.get("planning.longitudinal_plan.yield_deceleration") - - # Get the model of the car we are going to be using - # Get the obstacles - # Define the functions we need - # Create the Astar object - # self.planner = ParkingSolverSecondOrderDubins() + self.planner = ParkingSolverFirstOrderDubins() self.iterations = settings.get("run.drive.planning._route_planner.astar.iterations", 200) @@ -329,33 +292,16 @@ def update(self, state : AllState) -> Route: Trajectory: _description_ """ vehicle = state.vehicle # type: VehicleState - print(f"Vehicle {vehicle}") obstacles = state.obstacles # type: Dict[str, Obstacle] agents = state.agents # type: Dict[str, AgentState] all_obstacles = {**agents, **obstacles} - # print(f"Obstacles {obstacles}") - print(f"Agents {agents}") goal_pose = state.parking_goal assert goal_pose.frame == ObjectFrameEnum.CURRENT - print("Checking VALUE: ", state.start_vehicle_pose) - print("Checking VALUE: ", state.vehicle) - start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) - # start_pose = state.vehicle.pose.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) - # start_cur = VehicleState.zero() - # start_cur.pose = start_pose # goal.pose = goal_pose - # goal.v = 0 - print("Checking VALUE: ", start_state) - - + start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) - # # Need to parse and create second order dubin car states - # start_state = self.vehicle_state_to_dynamics(vehicle) - # goal_state = self.vehicle_state_to_dynamics(goal) - # Need to parse and create second order dubin car states start = self.vehicle_state_to_first_order(start_state) - # goal = self.vehicle_state_to_first_order(goal_state) goal = self.pose_to_first_order(goal_pose) # Update the planner @@ -371,13 +317,8 @@ def update(self, state : AllState) -> Route: points.append((state[0], state[1])) times = [state[3] for state in res] path = Path(frame=vehicle.pose.frame, points=points) - traj = Trajectory(path.frame,points,times) - # print("===========================") - # print(f"Points: {points}") - # print(f"Times: {times}") + route = Path(frame=vehicle.pose.frame, points=points) - # traj = longitudinal_plan(route, 2, -2, 10, vehicle.v, "milestone") - print(traj) return route @@ -416,49 +357,3 @@ def visualize_trajectory(self, traj: Trajectory, frame_id="vehicle"): line_marker = self.create_trajectory_line_marker(traj, frame_id=frame_id) self.marker_pub.publish(line_marker) - def visualize_trajectory_animated(self, traj: Trajectory, frame_id="vehicle"): - """Animates a moving marker along the trajectory in RViz.""" - marker_pub = rospy.Publisher("animated_trajectory_marker", Marker, queue_size=1) - rate = rospy.Rate(20) # 20 Hz for smooth animation - - # Create a red sphere marker - marker = Marker() - marker.header.frame_id = frame_id - marker.ns = "animated_trajectory" - marker.id = 100 - marker.type = Marker.SPHERE - marker.action = Marker.ADD - marker.scale.x = 0.4 - marker.scale.y = 0.4 - marker.scale.z = 0.4 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 1.0 - - try: - start_ros_time = rospy.Time.now().to_sec() - except rospy.ROSInitException: - print("[WARN] Cannot animate trajectory — ROS node not initialized.") - return - - t_start = traj.times[0] - t_end = traj.times[-1] - - while not rospy.is_shutdown(): - t_now = rospy.Time.now().to_sec() - t = t_now - start_ros_time + t_start - - if t > t_end: - break - - pos = traj.eval(t) - marker.header.stamp = rospy.Time.now() - marker.pose.position.x = pos[0] - marker.pose.position.y = pos[1] - marker.pose.position.z = pos[2] if len(pos) > 2 else 0.0 - - marker_pub.publish(marker) - rate.sleep() - - print("[INFO] Animated trajectory complete.") From 45b22d518bf1d09724b37289ed52906cec44654e Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Tue, 13 May 2025 20:40:44 -0500 Subject: [PATCH 07/11] reverting back to "goal" --- GEMstack/knowledge/defaults/computation_graph.yaml | 2 +- GEMstack/onboard/perception/parking_detection.py | 2 +- GEMstack/onboard/planning/parking_component.py | 4 ++-- GEMstack/onboard/planning/parking_route_planner.py | 2 +- GEMstack/state/all.py | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index fb58e6009..418b0b512 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -23,7 +23,7 @@ components: outputs: vehicle_lane - parking_detection: inputs: obstacles - outputs: [parking_goal, obstacles] + outputs: [goal, obstacles] - sign_detection: inputs: [vehicle, roadgraph] outputs: roadgraph.signs diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py index 334e0238e..f0952544b 100644 --- a/GEMstack/onboard/perception/parking_detection.py +++ b/GEMstack/onboard/perception/parking_detection.py @@ -76,7 +76,7 @@ def state_inputs(self) -> list: return ['obstacles'] def state_outputs(self) -> list: - return ['parking_goal', 'obstacles'] + return ['goal', 'obstacles'] def visualize(self, diff --git a/GEMstack/onboard/planning/parking_component.py b/GEMstack/onboard/planning/parking_component.py index 6f2ba139e..df2e6ea9f 100644 --- a/GEMstack/onboard/planning/parking_component.py +++ b/GEMstack/onboard/planning/parking_component.py @@ -25,9 +25,9 @@ def update(self, state: AllState): elapsed_time = time.time() - self.start_time # After a goal is detected, change the mission plan to use PARKING. - if state.parking_goal: + if state.goal: print("\n Parking goal available. Entering PARKING mode......") - mission_plan = MissionObjective(PlannerEnum.PARKING, state.parking_goal) + mission_plan = MissionObjective(PlannerEnum.PARKING, state.goal) else: print("\n Entering SCANNING mode......") mission_plan = MissionObjective(PlannerEnum.SCANNING) diff --git a/GEMstack/onboard/planning/parking_route_planner.py b/GEMstack/onboard/planning/parking_route_planner.py index ce363bdec..e8ea41be5 100644 --- a/GEMstack/onboard/planning/parking_route_planner.py +++ b/GEMstack/onboard/planning/parking_route_planner.py @@ -295,7 +295,7 @@ def update(self, state : AllState) -> Route: obstacles = state.obstacles # type: Dict[str, Obstacle] agents = state.agents # type: Dict[str, AgentState] all_obstacles = {**agents, **obstacles} - goal_pose = state.parking_goal + goal_pose = state.goal assert goal_pose.frame == ObjectFrameEnum.CURRENT start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) diff --git a/GEMstack/state/all.py b/GEMstack/state/all.py index d639b23cb..4fb863ed5 100644 --- a/GEMstack/state/all.py +++ b/GEMstack/state/all.py @@ -24,7 +24,7 @@ class AllState(SceneState): agent_intents : Dict[str,AgentIntentMixture] = field(default_factory=dict) relations : List[EntityRelation] = field(default_factory=list) predicates : PredicateValues = field(default_factory=PredicateValues) - parking_goal: ObjectPose = None + goal: ObjectPose = None # planner-output state mission : MissionObjective = field(default_factory=MissionObjective) From 882ec74dc5f7dc5cacf615643393effcf578f8f4 Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Wed, 14 May 2025 00:07:44 -0500 Subject: [PATCH 08/11] small updates to showcase scanning functionality in simulation --- .../knowledge/defaults/computation_graph.yaml | 2 +- .../onboard/perception/parking_detection.py | 23 ++++++++++++++----- .../onboard/planning/parking_route_planner.py | 4 ++-- launch/parking_detection.yaml | 2 +- 4 files changed, 21 insertions(+), 10 deletions(-) diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index 418b0b512..04c6c8fdb 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -22,7 +22,7 @@ components: inputs: [vehicle, roadgraph] outputs: vehicle_lane - parking_detection: - inputs: obstacles + inputs: all outputs: [goal, obstacles] - sign_detection: inputs: [vehicle, roadgraph] diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py index f0952544b..a08b001c6 100644 --- a/GEMstack/onboard/perception/parking_detection.py +++ b/GEMstack/onboard/perception/parking_detection.py @@ -4,7 +4,7 @@ from sensor_msgs.msg import PointCloud2 from typing import Dict from ..component import Component -from ...state import ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, ObstacleStateEnum +from ...state import ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, ObstacleStateEnum, AllState from ..interface.gem import GEMInterface from .utils.constants import * from .utils.detection_utils import * @@ -73,7 +73,7 @@ def rate(self) -> float: return 10.0 # Hz def state_inputs(self) -> list: - return ['obstacles'] + return ['all'] def state_outputs(self) -> list: return ['goal', 'obstacles'] @@ -134,13 +134,13 @@ def visualize(self, self.pub_cones_centers_pc2.publish(ros_cones_centers_pc2) - def update(self, cone_obstacles: Dict[str, Obstacle]): + def update(self, state: AllState): # Initial variables goal_parking_spot = None parking_obstacles_pose = [] parking_obstacles_dim = [] ordered_cone_ground_centers_2D = [] - + cone_obstacles = state.obstacles # Populate cone points cone_pts_3D = [] for cone in cone_obstacles.values(): @@ -198,8 +198,19 @@ def update(self, cone_obstacles: Dict[str, Obstacle]): yaw=yaw, pitch=0.0, roll=0.0, - frame=ObjectFrameEnum.CURRENT + frame=ObjectFrameEnum.START ) + DISTANCE_THRESHOLD = 12.0 + if self.euclidean_distance((x,y), state) > DISTANCE_THRESHOLD: # we are not close enough to the parking spot + return None + new_state = [goal_pose, parking_obstacles] - return new_state \ No newline at end of file + return new_state + + def euclidean_distance(self, point, state): + x, y = point + vehicle_x = state.vehicle.pose.x + vehicle_y = state.vehicle.pose.y + distance = np.sqrt((x - vehicle_x) ** 2 + (y - vehicle_y) ** 2) + return distance \ No newline at end of file diff --git a/GEMstack/onboard/planning/parking_route_planner.py b/GEMstack/onboard/planning/parking_route_planner.py index e8ea41be5..b8bde3917 100644 --- a/GEMstack/onboard/planning/parking_route_planner.py +++ b/GEMstack/onboard/planning/parking_route_planner.py @@ -296,9 +296,9 @@ def update(self, state : AllState) -> Route: agents = state.agents # type: Dict[str, AgentState] all_obstacles = {**agents, **obstacles} goal_pose = state.goal - assert goal_pose.frame == ObjectFrameEnum.CURRENT + assert goal_pose.frame == ObjectFrameEnum.START - start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) + start_state = state.vehicle.to_frame(ObjectFrameEnum.START, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose) start = self.vehicle_state_to_first_order(start_state) diff --git a/launch/parking_detection.yaml b/launch/parking_detection.yaml index e4e35b2b0..3f690bc10 100644 --- a/launch/parking_detection.yaml +++ b/launch/parking_detection.yaml @@ -146,7 +146,7 @@ variants: max_turning_rate: 0.3 tolerance: 0.25 astar: - iterations: 20000 + iterations: 500 heuristic: reeds_shepp motion_planning: type: longitudinal_planning.YieldTrajectoryPlanner From d6da61b5af1348aa9a76c78bf05ff491485b3dea Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Thu, 15 May 2025 01:32:06 -0500 Subject: [PATCH 09/11] updating controls --- GEMstack/knowledge/defaults/current.yaml | 2 +- GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index d18e37711..149080f58 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -17,7 +17,7 @@ control: crosstrack_gain: 0.5 desired_speed: trajectory longitudinal_control: - pid_p: 1.5 + pid_p: 1.0 pid_i: 0.1 pid_d: 0.0 diff --git a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml index 06ad3415b..d20d2c357 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml +++ b/GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml @@ -12,7 +12,7 @@ max_accelerator_power: #Watts. Power at max accelerator pedal, by gear - 10000.0 max_accelerator_power_reverse: 10000.0 #Watts. Power (backwards) in reverse gear -acceleration_model : hang_v1 +acceleration_model : kris_v1 accelerator_active_range : [0.2, 1.0] #range of accelerator pedal where output acceleration is not flat brake_active_range : [0,1] #range of brake pedal where output deceleration is not flat From 73140c40c4eeac97e80f71ce2be4345554d6c273 Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Thu, 15 May 2025 09:52:43 -0500 Subject: [PATCH 10/11] fixing a conflict --- .../onboard/planning/longitudinal_planning.py | 1569 ++++++++++++----- .../planning/parking_motion_planning.py | 486 +++++ launch/parking_detection.yaml | 2 +- 3 files changed, 1659 insertions(+), 398 deletions(-) create mode 100644 GEMstack/onboard/planning/parking_motion_planning.py diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 9a156d9ae..b28177d46 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,486 +1,1261 @@ -# File: GEMstack/onboard/planning/longitudinal_planning.py -from typing import List, Tuple -import math +from typing import List, Tuple, Union from ..component import Component -from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \ - ObjectFrameEnum -from ...utils import serialization, settings -from ...mathutils import transforms +from ...state import ( + AllState, + VehicleState, + EntityRelation, + EntityRelationEnum, + Path, + Trajectory, + Route, + ObjectFrameEnum, + AgentState, + MissionEnum, +) +from ...utils import serialization +from ...mathutils.transforms import vector_madd +from ...mathutils.quadratic_equation import quad_root + + +import time import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import math +from scipy.optimize import minimize + + +# Global variables +PEDESTRIAN_LENGTH = 0.5 +PEDESTRIAN_WIDTH = 0.5 + +VEHICLE_LENGTH = 3.5 +VEHICLE_WIDTH = 2 + +VEHICLE_BUFFER_X = 3.0 +VEHICLE_BUFFER_Y = 1.5 + +YIELD_BUFFER_Y = 1.0 +V_MAX = 5 +COMFORT_DECELERATION = 1.5 + + +def detect_collision( + curr_x: float, + curr_y: float, + curr_v: float, + obj: AgentState, + min_deceleration: float, + max_deceleration: float, + acceleration: float, + max_speed: float, +) -> Tuple[bool, Union[float, List[float]]]: + """Detects if a collision will occur with the given object and return deceleration to avoid it.""" + + # Get the object's position and velocity + obj_x = obj.pose.x + obj_y = obj.pose.y + obj_v_x = obj.velocity[0] + obj_v_y = obj.velocity[1] + + if obj.pose.frame == ObjectFrameEnum.CURRENT: + # Simulation: Current + obj_x = obj.pose.x + curr_x + obj_y = obj.pose.y + curr_y + print("PEDESTRIAN", obj_x, obj_y) + + vehicle_front = curr_x + VEHICLE_LENGTH + vehicle_back = curr_x + vehicle_left = curr_y + VEHICLE_WIDTH / 2 + vehicle_right = curr_y - VEHICLE_WIDTH / 2 + + pedestrian_front = obj_x + PEDESTRIAN_LENGTH / 2 + pedestrian_back = obj_x - PEDESTRIAN_LENGTH / 2 + pedestrian_left = obj_y + PEDESTRIAN_WIDTH / 2 + pedestrian_right = obj_y - PEDESTRIAN_WIDTH / 2 + + # Check if the object is in front of the vehicle + if vehicle_front > pedestrian_back: + if vehicle_back > pedestrian_front: + # The object is behind the vehicle + print("Object is behind the vehicle") + return False, 0.0 + if ( + vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left + or vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right + ): + # The object is to the side of the vehicle + print("Object is to the side of the vehicle") + return False, 0.0 + # The object overlaps with the vehicle's buffer + return True, max_deceleration + + if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left and obj_v_y <= 0: + # The object is to the right of the vehicle and moving away + print("Object is to the right of the vehicle and moving away") + return False, 0.0 + + if vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right and obj_v_y >= 0: + # The object is to the left of the vehicle and moving away + print("Object is to the left of the vehicle and moving away") + return False, 0.0 + + if vehicle_front + VEHICLE_BUFFER_X >= pedestrian_back and ( + vehicle_right - VEHICLE_BUFFER_Y <= pedestrian_left + and vehicle_left + VEHICLE_BUFFER_Y >= pedestrian_right + ): + # The object is in front of the vehicle and within the buffer + print("Object is in front of the vehicle and within the buffer") + return True, max_deceleration + + # Calculate the deceleration needed to avoid a collision + print("Object is in front of the vehicle and outside the buffer") + distance = pedestrian_back - vehicle_front + distance_with_buffer = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X + + relative_v = curr_v - obj_v_x + if relative_v <= 0: + return False, 0.0 + + if obj_v_y == 0: + # The object is in front of the vehicle blocking it + deceleration = relative_v**2 / (2 * distance_with_buffer) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 + + return True, deceleration + + print(relative_v, distance_with_buffer) + + if obj_v_y > 0: + # The object is to the right of the vehicle and moving towards it + time_to_get_close = ( + vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER_Y - pedestrian_left + ) / abs(obj_v_y) + time_to_pass = ( + vehicle_left + VEHICLE_BUFFER_Y + YIELD_BUFFER_Y - pedestrian_right + ) / abs(obj_v_y) + else: + # The object is to the left of the vehicle and moving towards it + time_to_get_close = ( + pedestrian_right - vehicle_left - VEHICLE_BUFFER_Y - YIELD_BUFFER_Y + ) / abs(obj_v_y) + time_to_pass = ( + pedestrian_left - vehicle_right + VEHICLE_BUFFER_Y + YIELD_BUFFER_Y + ) / abs(obj_v_y) + + time_to_accel_to_max_speed = (max_speed - curr_v) / acceleration + distance_to_accel_to_max_speed = ( + (max_speed + curr_v - 2 * obj_v_x) * time_to_accel_to_max_speed / 2 + ) # area of trapezoid + + if distance_to_accel_to_max_speed > distance_with_buffer: + # The object will reach the buffer before reaching max speed + time_to_buffer_when_accel = ( + -relative_v + + (relative_v * relative_v + 2 * distance_with_buffer * acceleration) ** 0.5 + ) / acceleration + else: + # The object will reach the buffer after reaching max speed + time_to_buffer_when_accel = time_to_accel_to_max_speed + ( + distance_with_buffer - distance_to_accel_to_max_speed + ) / (max_speed - obj_v_x) + + if distance_to_accel_to_max_speed > distance: + # We will collide before reaching max speed + time_to_collide_when_accel = ( + -relative_v + (relative_v * relative_v + 2 * distance * acceleration) ** 0.5 + ) / acceleration + else: + # We will collide after reaching max speed + time_to_collide_when_accel = time_to_accel_to_max_speed + ( + distance - distance_to_accel_to_max_speed + ) / (max_speed - obj_v_x) + + if time_to_get_close > time_to_collide_when_accel: + # We can do normal driving and will pass the object before it gets in our way + print( + "We can do normal driving and will pass the object before it gets in our way" + ) + return False, 0.0 -DEBUG = False # Set to False to disable debug output + if vehicle_front + VEHICLE_BUFFER_X >= pedestrian_back: + # We cannot move pass the pedestrian before it reaches the buffer from side + return True, max_deceleration + if time_to_pass < time_to_buffer_when_accel: + # The object will pass through our front before we drive normally and reach it + print( + "The object will pass through our front before we drive normally and reach it" + ) + return False, 0.0 + + distance_to_move = distance_with_buffer + time_to_pass * obj_v_x + + if curr_v**2 / (2 * distance_to_move) >= COMFORT_DECELERATION: + return True, curr_v**2 / (2 * distance_to_move) + + print("Calculating cruising speed") + return True, [distance_to_move, time_to_pass] + + +def detect_collision_analytical( + r_pedestrain_x: float, + r_pedestrain_y: float, + p_vehicle_left_y_after_t: float, + p_vehicle_right_y_after_t: float, + lateral_buffer: float, +) -> Union[bool, str]: + """Detects if a collision will occur with the given object and return deceleration to avoid it. Analytical""" + if r_pedestrain_x < 0 and abs(r_pedestrain_y) > lateral_buffer: + return False + elif r_pedestrain_x < 0: + return "max" + if ( + r_pedestrain_y >= p_vehicle_left_y_after_t + and r_pedestrain_y <= p_vehicle_right_y_after_t + ): + return True + + return False + + +def get_minimum_deceleration_for_collision_avoidance( + curr_x: float, + curr_y: float, + curr_v: float, + obj: AgentState, + min_deceleration: float, + max_deceleration: float, +) -> Tuple[bool, float]: + """Detects if a collision will occur with the given object and return deceleration to avoid it. Via Optimization""" + + # Get the object's position and velocity + obj_x = obj.pose.x + obj_y = obj.pose.y + obj_v_x = obj.velocity[0] + obj_v_y = obj.velocity[1] + + if obj.pose.frame == ObjectFrameEnum.CURRENT: + obj_x = obj.pose.x + curr_x + obj_y = obj.pose.y + curr_y + + obj_x = obj_x - curr_x + obj_y = obj_y - curr_y + + curr_x = curr_x - curr_x + curr_y = curr_y - curr_y + + vehicle_front = curr_x + VEHICLE_LENGTH + VEHICLE_BUFFER_X + vehicle_back = curr_x + vehicle_left = curr_y - VEHICLE_WIDTH / 2 + vehicle_right = curr_y + VEHICLE_WIDTH / 2 + + r_vehicle_front = vehicle_front - vehicle_front + r_vehicle_back = vehicle_back - vehicle_front + r_vehicle_left = vehicle_left - VEHICLE_BUFFER_Y + r_vehicle_right = vehicle_right + VEHICLE_BUFFER_Y + r_vehicle_v_x = curr_v + r_vehicle_v_y = 0 + + r_pedestrain_x = obj_x - vehicle_front + r_pedestrain_y = -obj_y + r_pedestrain_v_x = obj_v_x + r_pedestrain_v_y = -obj_v_y + + r_velocity_x_from_vehicle = r_vehicle_v_x - r_pedestrain_v_x + r_velocity_y_from_vehicle = r_vehicle_v_y - r_pedestrain_v_y + + t_to_r_pedestrain_x = (r_pedestrain_x - r_vehicle_front) / r_velocity_x_from_vehicle + + p_vehicle_left_y_after_t = ( + r_vehicle_left + r_velocity_y_from_vehicle * t_to_r_pedestrain_x + ) + p_vehicle_right_y_after_t = ( + r_vehicle_right + r_velocity_y_from_vehicle * t_to_r_pedestrain_x + ) -def generate_dense_points(points: List[Tuple[float, float]], density: int = 10) -> List[Tuple[float, float]]: - if not points: - return [] - if len(points) == 1: - return points.copy() + collision_flag = detect_collision_analytical( + r_pedestrain_x, + r_pedestrain_y, + p_vehicle_left_y_after_t, + p_vehicle_right_y_after_t, + VEHICLE_BUFFER_Y, + ) + if collision_flag == False: + print( + "No collision", + curr_x, + curr_y, + r_pedestrain_x, + r_pedestrain_y, + r_vehicle_left, + r_vehicle_right, + p_vehicle_left_y_after_t, + p_vehicle_right_y_after_t, + ) + return 0.0, r_pedestrain_x + elif collision_flag == "max": + return max_deceleration, r_pedestrain_x + + print( + "Collision", + curr_x, + curr_y, + r_pedestrain_x, + r_pedestrain_y, + r_vehicle_left, + r_vehicle_right, + p_vehicle_left_y_after_t, + p_vehicle_right_y_after_t, + ) - dense_points = [points[0]] - for i in range(len(points) - 1): - p0 = points[i] - p1 = points[i + 1] - dx = p1[0] - p0[0] - dy = p1[1] - p0[1] - seg_length = math.hypot(dx, dy) + minimum_deceleration = None + if abs(r_velocity_y_from_vehicle) > 0.1: + if r_velocity_y_from_vehicle > 0.1: + # Vehicle Left would be used to yield + r_pedestrain_y_temp = r_pedestrain_y + abs(r_vehicle_left) + elif r_velocity_y_from_vehicle < -0.1: + # Vehicle Right would be used to yield + r_pedestrain_y_temp = r_pedestrain_y - abs(r_vehicle_right) + + softest_accleration = ( + 2 + * r_velocity_y_from_vehicle + * ( + r_velocity_y_from_vehicle * r_pedestrain_x + - r_velocity_x_from_vehicle * r_pedestrain_y_temp + ) + / r_pedestrain_y_temp**2 + ) + peak_y = ( + -(r_velocity_x_from_vehicle * r_velocity_y_from_vehicle) + / softest_accleration + ) + # if the peak is within the position of the pedestrian, + # then it indicates the path had already collided with the pedestrian, + # and so the softest acceleration should be the one the peak of the path is the same as the pedestrain's x position + # and the vehicle should be stopped exactly before the pedestrain's x position + if abs(peak_y) > abs(r_pedestrain_y_temp): + minimum_deceleration = abs(softest_accleration) + # else: the vehicle should be stopped exactly before the pedestrain's x position the same case as the pedestrain barely move laterally + if minimum_deceleration is None: + minimum_deceleration = r_velocity_x_from_vehicle**2 / (2 * r_pedestrain_x) + + print("calculated minimum deceleration: ", minimum_deceleration) + + if minimum_deceleration < min_deceleration: + return 0.0, r_pedestrain_x + else: + return ( + max(min(minimum_deceleration, max_deceleration), min_deceleration), + r_pedestrain_x, + ) - n_interp = int(round(seg_length * density)) - for j in range(1, n_interp + 1): - fraction = j / (n_interp + 1) - x_interp = p0[0] + fraction * dx - y_interp = p0[1] + fraction * dy - dense_points.append((x_interp, y_interp)) +################################################################################ +########## Longitudinal Planning ############################################### +################################################################################ - dense_points.append(p1) - return dense_points +def longitudinal_plan( + path: Path, + acceleration: float, + deceleration: float, + max_speed: float, + current_speed: float, + method: str, +) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ -def compute_cumulative_distances(points: List[List[float]]) -> List[float]: - s_vals = [0.0] - for i in range(1, len(points)): - dx = points[i][0] - points[i - 1][0] - dy = points[i][1] - points[i - 1][1] - ds = math.hypot(dx, dy) - s_vals.append(s_vals[-1] + ds) + if method == "milestone": + return longitudinal_plan_milestone( + path, acceleration, deceleration, max_speed, current_speed + ) + elif method == "dt": + return longitudinal_plan_dt( + path, acceleration, deceleration, max_speed, current_speed + ) + elif method == "dx": + return longitudinal_plan_dx( + path, acceleration, deceleration, max_speed, current_speed + ) + else: + raise NotImplementedError( + "Invalid method, only milestone, dt, adn dx are implemented." + ) - if DEBUG: - print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) - return s_vals +def longitudinal_plan_milestone( + path: Path, + acceleration: float, + deceleration: float, + max_speed: float, + current_speed: float, +) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. + + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ + # Extrapolation factor for the points + factor = 5.0 + new_points = [] + for idx, point in enumerate(path.points[:-1]): + next_point = path.points[idx + 1] + if point[0] == next_point[0]: + break + xarange = np.arange( + point[0], next_point[0], (next_point[0] - point[0]) / factor + ) + if point[1] == next_point[1]: + yarange = [point[1]] * len(xarange) + else: + yarange = np.arange( + point[1], next_point[1], (next_point[1] - point[1]) / factor + ) + print(yarange) + for x, y in zip(xarange, yarange): + new_points.append((x, y)) + new_points.append(path.points[-1]) + print("new points", new_points) + path = Path(path.frame, new_points) -def longitudinal_plan(path, acceleration, deceleration, max_speed, current_speed): path_normalized = path.arc_length_parameterize() - points = list(path_normalized.points) - dense_points = generate_dense_points(points) - s_vals = compute_cumulative_distances(dense_points) - L = s_vals[-1] # Total path length - stopping_distance = (current_speed ** 2) / (2 * deceleration) - - if DEBUG: - print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) - print("[DEBUG] longitudinal_plan: Total path length L =", L) - print("[DEBUG] longitudinal_plan: Braking distance needed =", stopping_distance) - - if stopping_distance > L: # Case where there is not enough stopping distance to stop before path ends (calls emergency brake) - return longitudinal_brake(path, deceleration, current_speed) - - if current_speed > max_speed: # Case where car is exceeding the max speed so we need to slow down (do initial slowdown) - if DEBUG: - print(f"[DEBUG] Handling case where current_speed ({current_speed:.2f}) > max_speed ({max_speed:.2f})") - - # Initial deceleration phase to reach max_speed - initial_decel_distance = (current_speed ** 2 - max_speed ** 2) / (2 * deceleration) - initial_decel_time = (current_speed - max_speed) / deceleration - remaining_distance = L - initial_decel_distance - - if DEBUG: - print( - f"[DEBUG] Phase 1 - Initial Decel: distance = {initial_decel_distance:.2f}, time = {initial_decel_time:.2f}") - print(f"[DEBUG] Remaining distance after reaching max_speed: {remaining_distance:.2f}") - - # Calculate final deceleration distance needed to stop from max_speed - final_decel_distance = (max_speed ** 2) / (2 * deceleration) - cruise_distance = remaining_distance - final_decel_distance - - if DEBUG: - print(f"[DEBUG] Phase 2 - Cruise: distance = {cruise_distance:.2f}") - print(f"[DEBUG] Phase 3 - Final Decel: distance = {final_decel_distance:.2f}") - - times = [] - for s in s_vals: - if s <= initial_decel_distance: # Phase 1: Initial deceleration to max_speed - v = math.sqrt(current_speed ** 2 - 2 * deceleration * s) - t = (current_speed - v) / deceleration - if DEBUG: # Print every 10m - print(f"[DEBUG] Initial Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") - - elif s <= initial_decel_distance + cruise_distance: # Phase 2: Cruise at max_speed - s_in_cruise = s - initial_decel_distance - t = initial_decel_time + s_in_cruise / max_speed - if DEBUG: # Print every 10m - print(f"[DEBUG] Cruise: s = {s:.2f}, v = {max_speed:.2f}, t = {t:.2f}") - - else: # Phase 3: Final deceleration to stop - s_in_final_decel = s - (initial_decel_distance + cruise_distance) - v = math.sqrt(max(max_speed ** 2 - 2 * deceleration * s_in_final_decel, 0.0)) - t = initial_decel_time + cruise_distance / max_speed + (max_speed - v) / deceleration - if DEBUG: # Print every 10m - print(f"[DEBUG] Final Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") - - times.append(t) - - if DEBUG: - print("[DEBUG] Trajectory complete: Three phases executed") - print(f"[DEBUG] Total time: {times[-1]:.2f}") - - return Trajectory(frame=path.frame, points=dense_points, times=times) - - if acceleration <= 0: - if DEBUG: - print(f"[DEBUG] No acceleration allowed. Current speed: {current_speed:.2f}") - - # Pure deceleration phase - s_decel = (current_speed ** 2) / (2 * deceleration) - T_decel = current_speed / deceleration - - if DEBUG: - print(f"[DEBUG] Will maintain speed until s_decel: {s_decel:.2f}") - print(f"[DEBUG] Total deceleration time will be: {T_decel:.2f}") - - times = [] - for s in s_vals: - if s <= L - s_decel: # Maintain current speed until deceleration point - t_point = s / current_speed - if DEBUG: - print(f"[DEBUG] Constant Speed Phase: s = {s:.2f}, v = {current_speed:.2f}, t = {t_point:.2f}") - else: # Deceleration phase - s_in_decel = s - (L - s_decel) - v = math.sqrt(max(current_speed ** 2 - 2 * deceleration * s_in_decel, 0.0)) - t_point = (L - s_decel) / current_speed + (current_speed - v) / deceleration - if DEBUG: - print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") - - times.append(t_point) - - return Trajectory(frame=path.frame, points=dense_points, times=times) - - # Determine max possible peak speed given distance - v_peak_possible = math.sqrt( - (2 * acceleration * deceleration * L + deceleration * current_speed ** 2) / (acceleration + deceleration)) - v_target = min(max_speed, v_peak_possible) - - if DEBUG: - print("[DEBUG] longitudinal_plan: v_peak_possible =", v_peak_possible, "v_target =", v_target) - - # Compute acceleration phase - s_accel = max(0.0, (v_target ** 2 - current_speed ** 2) / (2 * acceleration)) - t_accel = max(0.0, (v_target - current_speed) / acceleration) - - # Compute deceleration phase - s_decel = max(0.0, (v_target ** 2) / (2 * deceleration)) - t_decel = max(0.0, v_target / deceleration) - - # Compute cruise phase - s_cruise = max(0.0, L - s_accel - s_decel) - t_cruise = s_cruise / v_target if v_target > 0 else 0.0 - - if DEBUG: - print("[DEBUG] longitudinal_plan: s_accel =", s_accel, "t_accel =", t_accel) - print("[DEBUG] longitudinal_plan: s_decel =", s_decel, "t_decel =", t_decel) - print("[DEBUG] longitudinal_plan: s_cruise =", s_cruise, "t_cruise =", t_cruise) + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + # ============================================= + + print("-----LONGITUDINAL PLAN-----") + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed / deceleration + min_delta_x_stop = ( + current_speed * min_delta_t_stop - 0.5 * deceleration * min_delta_t_stop**2 + ) - times = [] - for s in s_vals: - if s <= s_accel: # Acceleration phase - v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) - t_point = (v - current_speed) / acceleration + assert min_delta_x_stop >= 0 + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0]: + # put on the breaks + + # Calculate the next point in a special manner because of too-little time to stop + if cur_index == len(points) - 1: + # the next point in this instance would be when we stop + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = points[cur_index + 1] + + # keep breaking until the next milestone in path + if next_point[0] <= points[-1][0]: + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, -deceleration + ) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration * delta_t_to_next_x + cur_index += 1 + else: + # continue to the point in which we would stop (current_velocity = 0) + # update to the next point + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, -deceleration + ) + cur_point = next_point + cur_time += delta_t_to_next_x + # current_speed would not be exactly zero error would be less than 1e-4 but prefer to just set to zero + # current_speed -= delta_t_to_next_x*deceleration + current_speed = 0 + assert current_speed == 0 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + # next point + next_point = points[cur_index + 1] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed) / acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = ( + current_speed * delta_t_to_max_speed + + 0.5 * acceleration * delta_t_to_max_speed**2 + ) + + delta_t_to_stop_from_max_speed = max_speed / deceleration + delta_x_to_stop_from_max_speed = ( + max_speed * delta_t_to_stop_from_max_speed + - 0.5 * deceleration * delta_t_to_stop_from_max_speed**2 + ) + + delta_t_to_next_point = compute_time_to_x( + cur_point[0], next_point[0], current_speed, acceleration + ) + velocity_at_next_point = ( + current_speed + delta_t_to_next_point * acceleration + ) + time_to_stop_from_next_point = velocity_at_next_point / deceleration + delta_x_to_stop_from_next_point = ( + velocity_at_next_point * time_to_stop_from_next_point + - 0.5 * deceleration * time_to_stop_from_next_point**2 + ) + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if ( + next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] + and cur_point[0] + delta_x_to_max_speed >= next_point[0] + ): + # ("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, acceleration + ) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x * acceleration + cur_index += 1 + + # This is the case where we would need to start breaking before reaching + # top speed and before the next point (i.e. triangle shape velocity) + elif ( + cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed + >= points[-1][0] + ): + # Add a new point at the point where we should start breaking + delta_t_to_next_x = compute_time_triangle( + cur_point[0], + points[-1][0], + current_speed, + 0, + acceleration, + deceleration, + ) + next_x = ( + cur_point[0] + + current_speed * delta_t_to_next_x + + 0.5 * acceleration * delta_t_to_next_x**2 + ) + cur_time += delta_t_to_next_x + cur_point = [next_x, 0] + current_speed += delta_t_to_next_x * acceleration + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index + 1] + # continue on with max speed + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + cur_time += ( + points[-1][0] - min_delta_x_stop - cur_point[0] + ) / current_speed + cur_point = [points[-1][0] - min_delta_x_stop, 0] + current_speed = max_speed + else: + # Continue on to next point + cur_time += (next_point[0] - cur_point[0]) / current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + + next_point = points[cur_index + 1] + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed) / deceleration + delta_x_to_max_speed = ( + current_speed * delta_t_to_max_speed + - 0.5 * deceleration * delta_t_to_max_speed**2 + ) + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, -deceleration + ) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x * deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") - if DEBUG: - print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) - elif s <= s_accel + s_cruise: # Cruise phase - t_point = t_accel + (s - s_accel) / v_target + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) - if DEBUG: - print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + # ============================================= - else: # Deceleration phase - s_decel_phase = s - s_accel - s_cruise - v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) - t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + trajectory = Trajectory(path.frame, points, times) + return trajectory - if t_point < times[-1]: # Ensure time always increases - t_point = times[-1] + 0.01 # Small time correction step - if DEBUG: - print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") +def compute_time_to_x(x0: float, x1: float, v: float, a: float) -> float: + """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 + with constant acceleration a. I am assuming that we will always have a solution by settings + discriminant equal to zero, i'm not sure if this is an issue.""" - times.append(t_point) + """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" - if DEBUG: - print("[DEBUG] longitudinal_plan: Final times =", times) + t1 = (-v + max(0, (v**2 - 2 * a * (x0 - x1))) ** 0.5) / a + t2 = (-v - max(0, (v**2 - 2 * a * (x0 - x1))) ** 0.5) / a - return Trajectory(frame=path.frame, points=dense_points, times=times) + if math.isnan(t1): + t1 = 0 + if math.isnan(t2): + t2 = 0 + valid_times = [n for n in [t1, t2] if n > 0] + if valid_times: + return min(valid_times) + else: + return 0.0 -def longitudinal_brake(path: Path, deceleration: float, current_speed: float, - emergency_decel: float = 8.0) -> Trajectory: - # Vehicle already stopped - maintain position - if current_speed <= 0: - print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) - return Trajectory( - frame=path.frame, - points=[path.points[0]] * len(path.points), - times=[float(i) for i in range(len(path.points))] - ) - # Get total path length - path_length = sum( - np.linalg.norm(np.array(path.points[i + 1]) - np.array(path.points[i])) - for i in range(len(path.points) - 1) +def compute_time_triangle( + x0: float, xf: float, v0: float, vf: float, acceleration: float, deceleration: float +) -> float: + """ + Compute the time to go from current point assuming we are accelerating to the point at which + we would need to start breaking in order to reach the final point with velocity 0. + """ + roots = quad_root( + 0.5 * acceleration + + acceleration**2 / deceleration + - 0.5 * acceleration**2 / deceleration, + v0 + 2 * acceleration * v0 / deceleration - acceleration * v0 / deceleration, + x0 - xf + v0**2 / deceleration - 0.5 * v0**2 / deceleration, ) + t1 = max(roots) + assert t1 > 0 + return t1 + + +def solve_for_v_peak( + v0: float, acceleration: float, deceleration: float, total_length: float +) -> float: + + if acceleration <= 0 or deceleration <= 0: + raise ValueError("Acceleration and deceleration cant be negative") + + # Formuala: (v_peak^2 - v0^2)/(2*a) + v_peak^2/(2*d) = total_length + numerator = deceleration * v0**2 + 2 * acceleration * deceleration * total_length + denominator = acceleration + deceleration + v_peak_sq = numerator / denominator - # Calculate stopping distance with normal deceleration - T_stop_normal = current_speed / deceleration - s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + if v_peak_sq < 0: + return 0.0 - # Check if emergency braking is needed - if s_stop_normal > path_length: - if DEBUG: - print("[DEBUG] longitudinal_brake: Emergency braking needed!") - print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") - print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + return math.sqrt(v_peak_sq) - # Calculate emergency braking parameters - T_stop = current_speed / emergency_decel - s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") - print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") +def compute_dynamic_dt(acceleration, speed, k=0.01, a_min=0.5): + position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent + return np.sqrt(2 * position_step / max(acceleration, a_min)) - decel_to_use = emergency_decel +def longitudinal_plan_dt( + path, + acceleration: float, + deceleration: float, + max_speed: float, + current_speed: float, +): + # 1 parametrizatiom. + path_norm = path.arc_length_parameterize(speed=1.0) + total_length = path.length() + + # ------------------- + # If the path is too short, just return the path for preventing sudden halt of simulation + if total_length < 0.05: + points = [p for p in path_norm.points] + times = [t for t in path_norm.times] + return Trajectory(path.frame, points, times) + # ------------------- + + # 2. Compute distances for d_accel,d_decel + if max_speed > current_speed: + d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) else: - if DEBUG: - print("[DEBUG] longitudinal_brake: Normal braking sufficient") - T_stop = T_stop_normal - decel_to_use = deceleration - - # Generate time points (use more points for smoother trajectory) - num_points = max(len(path.points), 50) - times = np.linspace(0, T_stop, num_points) - - # Calculate distances at each time point using physics equation - distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) - - # Generate points along the path - points = [] - for d in distances: - if d <= path_length: - points.append(path.eval(d)) - else: - points.append(path.eval(d)) + d_accel = 0.0 # Already at or above max_speed - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") - print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + d_decel = (max_speed**2) / (2 * deceleration) - return Trajectory(frame=path.frame, points=points, times=times.tolist()) + # 3. trapezoidal or triangle? + if d_accel + d_decel <= total_length: + t_accel = ( + (max_speed - current_speed) / acceleration + if max_speed > current_speed + else 0.0 + ) + t_decel = max_speed / deceleration + d_cruise = total_length - d_accel - d_decel + t_cruise = d_cruise / max_speed if max_speed != 0 else 0.0 + t_final = t_accel + t_cruise + t_decel + profile_type = "trapezoidal" + else: + # Triangular profile: not enough distance to reach max_speed so we will calculate peak speed. + peak_speed = solve_for_v_peak( + current_speed, acceleration, deceleration, total_length + ) + # choose the min just in case + peak_speed = min(peak_speed, max_speed) + t_accel = ( + (peak_speed - current_speed) / acceleration + if peak_speed > current_speed + else 0.0 + ) + t_decel = peak_speed / deceleration + t_final = t_accel + t_decel + profile_type = "triangular" + t = 0 times = [] - for s in s_vals: - if s <= s_accel: # Acceleration phase - v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) - t_point = (v - current_speed) / acceleration + s_vals = [] + velocities = [] # for graphing and debugging purposes + + num_time_steps = 0 + speed = current_speed + while t < t_final: + times.append(t) + velocities.append(speed) + if profile_type == "trapezoidal": + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + speed = current_speed + acceleration * t + elif t < t_accel + t_cruise: + # Cruise phase. + s = d_accel + max_speed * (t - t_accel) + else: + # Deceleration phase. + t_decel_phase = t - (t_accel + t_cruise) + s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase) ** 2 + speed = speed - deceleration * (t_decel - t_decel_phase) + else: # Triangular profile. + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + speed = current_speed + acceleration * t + else: + t_decel_phase = t - t_accel + s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + s = ( + s_accel + + peak_speed * t_decel_phase + - 0.5 * deceleration * t_decel_phase**2 + ) + speed = speed - deceleration * t_decel_phase + + s_vals.append(min(s, total_length)) + if s >= total_length: + break + + dt = compute_dynamic_dt( + acceleration if t < t_accel else deceleration, current_speed + ) + t = t + dt - if DEBUG: - print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + num_time_steps += 1 - elif s <= s_accel + s_cruise: # Cruise phase - t_point = t_accel + (s - s_accel) / v_target + # Compute trajectory points + points = [path_norm.eval(s) for s in s_vals] + print("Number of time steps is --------------------", num_time_steps) - if DEBUG: - print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + trajectory = Trajectory(path_norm.frame, points, list(times)) + return trajectory - else: # Deceleration phase - s_decel_phase = s - s_accel - s_cruise - v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) - t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration - if t_point < times[-1]: # Ensure time always increases - t_point = times[-1] + 0.01 # Small time correction step - - if DEBUG: - print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") +def longitudinal_plan_dx( + path: Path, + acceleration: float, + deceleration: float, + max_speed: float, + current_speed: float, +) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. - times.append(t_point) + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ + path_normalized = path.arc_length_parameterize() + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + + # ============================================= + # Adjust these two numbers to choose between computation speed or smoothness + rq = 0.1 # Smaller, smoother + multi = 5 # Larger, smoother + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + acc = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + print("new points: ", new_points) + print("current index: ", cur_index) + print("current speed: ", current_speed) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed / deceleration + min_delta_x_stop = ( + current_speed * min_delta_t_stop - 0.5 * deceleration * min_delta_t_stop**2 + ) + assert min_delta_x_stop >= 0 + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0] - 0.0001: + acc = deceleration + flag = 1 + print("In case one") + # put on the breaks + # Calculate the next point in a special manner because of too-little time to stop + if cur_index >= len(points) - 1: + # the next point in this instance would be when we stop + print(1) + if min_delta_x_stop < rq * acc: + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = (cur_point[0] + (min_delta_x_stop / (acc * multi)), 0) + flag = 0 + else: + print(2) + next_point = points[cur_index + 1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + + # keep breaking until the next milestone in path + print("continuing to next point") + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, -deceleration + ) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration * delta_t_to_next_x + if flag: + cur_index += 1 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + print("In case two") + print(current_speed) + acc = acceleration + flag = 1 + # next point + next_point = points[cur_index + 1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed) / acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = ( + current_speed * delta_t_to_max_speed + + 0.5 * acceleration * delta_t_to_max_speed**2 + ) + + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + print("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, acceleration + ) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x * acceleration + if flag: + cur_index += 1 + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index + 1] + # continue on with max speed + print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + print("Adding new point to start decelerating") + cur_time += ( + points[-1][0] - min_delta_x_stop - cur_point[0] + ) / current_speed + cur_point = [points[-1][0] - min_delta_x_stop, 0] + current_speed = max_speed + else: + # Continue on to next point + print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0]) / current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + acc = deceleration + flag = 1 + # next point + next_point = points[cur_index + 1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed) / deceleration + delta_x_to_max_speed = ( + current_speed * delta_t_to_max_speed + - 0.5 * deceleration * delta_t_to_max_speed**2 + ) + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x( + cur_point[0], next_point[0], current_speed, -deceleration + ) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x * deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed - if DEBUG: - print("[DEBUG] longitudinal_plan: Final times =", times) + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") - return Trajectory(frame=path.frame, points=dense_points, times=times) + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) -def longitudinal_brake(path: Path, deceleration: float, current_speed: float, emergency_decel: float = 8.0) -> Trajectory: - # Vehicle already stopped - maintain position - if current_speed <= 0: - print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) - return Trajectory( - frame=path.frame, - points=[path.points[0]] * len(path.points), - times=[float(i) for i in range(len(path.points))] - ) + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) - # Get total path length - path_length = sum( - np.linalg.norm(np.array(path.points[i+1]) - np.array(path.points[i])) - for i in range(len(path.points)-1) - ) + # ============================================= - # Calculate stopping distance with normal deceleration - T_stop_normal = current_speed / deceleration - s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) - - # Check if emergency braking is needed - if s_stop_normal > path_length: - if DEBUG: - print("[DEBUG] longitudinal_brake: Emergency braking needed!") - print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") - print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") - - # Calculate emergency braking parameters - T_stop = current_speed / emergency_decel - s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) - - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") - print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") - - decel_to_use = emergency_decel - - else: - if DEBUG: - print("[DEBUG] longitudinal_brake: Normal braking sufficient") - T_stop = T_stop_normal - decel_to_use = deceleration - - # Generate time points (use more points for smoother trajectory) - num_points = max(len(path.points), 50) - times = np.linspace(0, T_stop, num_points) - - # Calculate distances at each time point using physics equation - distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) - - # Generate points along the path - points = [] - for d in distances: - if d <= path_length: - points.append(path.eval(d)) + trajectory = Trajectory(path.frame, points, times) + return trajectory + + +def longitudinal_brake( + path: Path, deceleration: float, current_speed: float +) -> Trajectory: + """Generates a longitudinal trajectory for braking along a path.""" + path_normalized = path.arc_length_parameterize() + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + + # ============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("path length: ", path.length()) + length = path.length() + + x0 = points[0][0] + t_stop = current_speed / deceleration + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + + new_points = [] + velocities = [] + + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 else: - points.append(path.eval(d)) + x = x_stop + new_points.append([x, 0]) + velocities.append(current_speed - deceleration * t) + points = new_points + print("[BRAKE] Computed points:", points) + + # ============================================= + + trajectory = Trajectory(path.frame, points, times) + return trajectory + - if DEBUG: - print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") - print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") +################################################################################ +########## Yield Trajectory Planner ############################################ +################################################################################ - return Trajectory(frame=path.frame, points=points, times=times.tolist()) class YieldTrajectoryPlanner(Component): - """Follows the given route. Brakes if the ego–vehicle must yield - (e.g. to a pedestrian) or if the end of the route is near; otherwise, - it accelerates (or cruises) toward a desired speed. + """Follows the given route. Brakes if you have to yield or + you are at the end of the route, otherwise accelerates to + the desired speed. """ - def __init__(self): + def __init__( + self, + mode: str = "real", + params: dict = {"planner": "dt", "desired_speed": 1.0, "acceleration": 0.5}, + ): self.route_progress = None self.t_last = None - self.acceleration = settings.get("run.drive.planning.motion_planning.largs.acceleration", 5) - self.desired_speed = settings.get("run.drive.planning.motion_planning.largs.desired_speed",2.0) - self.deceleration = settings.get("run.drive.planning.motion_planning.largs.deceleration", 2.0) - self.emergency_brake = settings.get("run.drive.planning.motion_planning.largs.emergency_brake", 8.0) + self.acceleration = 1.0 + self.desired_speed = 1.0 + self.deceleration = 2.0 + + self.min_deceleration = 1.0 + self.max_deceleration = 8.0 + + self.mode = mode + self.planner = params["planner"] + self.mission = None def state_inputs(self): - return ['all'] + return ["all"] def state_outputs(self) -> List[str]: - return ['trajectory'] + return ["trajectory"] def rate(self): return 10.0 def update(self, state: AllState): + start_time = time.time() + if self.mission == None: + self.mission = state.mission.type + vehicle = state.vehicle # type: VehicleState route = state.route # type: Route t = state.t - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: t =", t) - if self.t_last is None: self.t_last = t dt = t - self.t_last - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: dt =", dt) + # Position in vehicle frame (Start (0,0) to (15,0)) curr_x = vehicle.pose.x curr_y = vehicle.pose.y curr_v = vehicle.v - if DEBUG: - print(f"[DEBUG] YieldTrajectoryPlanner.update: Vehicle position = ({curr_x}, {curr_y}), speed = {curr_v}, ") - # Determine progress along the route. + abs_x = curr_x + state.start_vehicle_pose.x + abs_y = curr_y + state.start_vehicle_pose.y + + if self.mode == "real": + abs_x = curr_x + abs_y = curr_y + ############################################### + + if state.mission.type == MissionEnum.IDLE: + return Trajectory( + times=[0, 0], frame=ObjectFrameEnum.START, points=[[0, 0]] + ) + + # figure out where we are on the route + if state.mission.type!=self.mission: + self.route_progress = None + self.mission = state.mission.type + if self.route_progress is None: self.route_progress = 0.0 - _, closest_parameter = route.closest_point_local( - [curr_x, curr_y], - (self.route_progress - 5.0, self.route_progress + 5.0) + closest_dist, closest_parameter = state.route.closest_point_local( + (curr_x, curr_y), [self.route_progress - 5.0, self.route_progress + 5.0] ) - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: Closest parameter on route =", closest_parameter) self.route_progress = closest_parameter - # Extract a 10 m segment of the route for planning lookahead. - route_with_lookahead = route.trim(closest_parameter, closest_parameter + 10.0) - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: Route Lookahead =", route_with_lookahead) - - print("[DEBUG] state", state.relations) - # Check whether any yield relations (e.g. due to pedestrians) require braking. - stay_braking = False - pointSet = set() - for i in range(len(route_with_lookahead.points)): - if tuple(route_with_lookahead.points[i]) in pointSet: - stay_braking = True - break - pointSet.add(tuple(route_with_lookahead.points[i])) - - should_brake = any( - r.type == EntityRelationEnum.STOPPING_AT and r.obj1 == '' - for r in state.relations - ) - should_decelerate = any( - r.type == EntityRelationEnum.YIELDING and r.obj1 == '' - for r in state.relations - ) if should_brake == False else False - - should_accelerate = (not should_brake and not should_decelerate and curr_v < self.desired_speed) - - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: stay_braking =", stay_braking) - print("[DEBUG] YieldTrajectoryPlanner.update: should_brake =", should_brake) - print("[DEBUG] YieldTrajectoryPlanner.update: should_accelerate =", should_accelerate) - print("[DEBUG] YieldTrajectoryPlanner.update: should_decelerate =", should_decelerate) - - if stay_braking: - traj = longitudinal_brake(route_with_lookahead, 0.0, 0.0, 0.0) - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake (stay braking).") - elif should_brake: - traj = longitudinal_brake(route_with_lookahead, self.emergency_brake, curr_v) - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake.") - elif should_decelerate: - traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake.") - elif should_accelerate: - traj = longitudinal_plan(route_with_lookahead, self.acceleration, - self.deceleration, self.desired_speed, curr_v) - if DEBUG: - print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_plan (accelerate).") + route_to_end = route.trim(closest_parameter, len(route.points) - 1) + + should_yield = False + yield_deceleration = 0.0 + + for r in state.relations: + if r.type == EntityRelationEnum.YIELDING and r.obj1 == "": + # get the object we are yielding to + obj = state.agents[r.obj2] + + detected, deceleration = detect_collision( + abs_x, + abs_y, + curr_v, + obj, + self.min_deceleration, + self.max_deceleration, + self.acceleration, + self.desired_speed, + ) + if isinstance(deceleration, list): + print("@@@@@ INPUT", deceleration) + time_collision = deceleration[1] + distance_collision = deceleration[0] + b = 3 * time_collision - 2 * curr_v + c = curr_v**2 - 3 * distance_collision + desired_speed = (-b + (b**2 - 4 * c) ** 0.5) / 2 + deceleration = 1.5 + print("@@@@@ YIELDING", desired_speed) + route_yield = route.trim( + closest_parameter, closest_parameter + distance_collision + ) + traj = longitudinal_plan( + route_yield, + self.acceleration, + deceleration, + desired_speed, + curr_v, + self.planner, + ) + return traj + else: + if detected and deceleration > 0: + yield_deceleration = deceleration + should_yield = True + + print("should yield: ", should_yield) + + should_accelerate = not should_yield and curr_v < self.desired_speed + + # choose whether to accelerate, brake, or keep at current velocity + if should_accelerate: + traj = longitudinal_plan( + route_to_end, + self.acceleration, + self.deceleration, + self.desired_speed, + curr_v, + self.planner, + ) + elif should_yield: + traj = longitudinal_brake(route_to_end, yield_deceleration, curr_v) else: - # Maintain current speed if not accelerating or braking. - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) - if DEBUG: - print( - "[DEBUG] YieldTrajectoryPlanner.update: Maintaining current speed with longitudinal_plan (0 accel).") - - self.t_last = t - 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 + traj = longitudinal_plan( + route_to_end, + 0.0, + self.deceleration, + self.desired_speed, + curr_v, + self.planner, + ) + + return traj \ No newline at end of file diff --git a/GEMstack/onboard/planning/parking_motion_planning.py b/GEMstack/onboard/planning/parking_motion_planning.py new file mode 100644 index 000000000..9a156d9ae --- /dev/null +++ b/GEMstack/onboard/planning/parking_motion_planning.py @@ -0,0 +1,486 @@ +# File: GEMstack/onboard/planning/longitudinal_planning.py +from typing import List, Tuple +import math +from ..component import Component +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \ + ObjectFrameEnum +from ...utils import serialization, settings +from ...mathutils import transforms +import numpy as np + +DEBUG = False # Set to False to disable debug output + + +def generate_dense_points(points: List[Tuple[float, float]], density: int = 10) -> List[Tuple[float, float]]: + if not points: + return [] + if len(points) == 1: + return points.copy() + + dense_points = [points[0]] + for i in range(len(points) - 1): + p0 = points[i] + p1 = points[i + 1] + dx = p1[0] - p0[0] + dy = p1[1] - p0[1] + seg_length = math.hypot(dx, dy) + + n_interp = int(round(seg_length * density)) + + for j in range(1, n_interp + 1): + fraction = j / (n_interp + 1) + x_interp = p0[0] + fraction * dx + y_interp = p0[1] + fraction * dy + dense_points.append((x_interp, y_interp)) + + dense_points.append(p1) + + return dense_points + + +def compute_cumulative_distances(points: List[List[float]]) -> List[float]: + s_vals = [0.0] + for i in range(1, len(points)): + dx = points[i][0] - points[i - 1][0] + dy = points[i][1] - points[i - 1][1] + ds = math.hypot(dx, dy) + s_vals.append(s_vals[-1] + ds) + + if DEBUG: + print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) + + return s_vals + + +def longitudinal_plan(path, acceleration, deceleration, max_speed, current_speed): + path_normalized = path.arc_length_parameterize() + points = list(path_normalized.points) + dense_points = generate_dense_points(points) + s_vals = compute_cumulative_distances(dense_points) + L = s_vals[-1] # Total path length + stopping_distance = (current_speed ** 2) / (2 * deceleration) + + if DEBUG: + print("[DEBUG] compute_cumulative_distances: s_vals =", s_vals) + print("[DEBUG] longitudinal_plan: Total path length L =", L) + print("[DEBUG] longitudinal_plan: Braking distance needed =", stopping_distance) + + if stopping_distance > L: # Case where there is not enough stopping distance to stop before path ends (calls emergency brake) + return longitudinal_brake(path, deceleration, current_speed) + + if current_speed > max_speed: # Case where car is exceeding the max speed so we need to slow down (do initial slowdown) + if DEBUG: + print(f"[DEBUG] Handling case where current_speed ({current_speed:.2f}) > max_speed ({max_speed:.2f})") + + # Initial deceleration phase to reach max_speed + initial_decel_distance = (current_speed ** 2 - max_speed ** 2) / (2 * deceleration) + initial_decel_time = (current_speed - max_speed) / deceleration + remaining_distance = L - initial_decel_distance + + if DEBUG: + print( + f"[DEBUG] Phase 1 - Initial Decel: distance = {initial_decel_distance:.2f}, time = {initial_decel_time:.2f}") + print(f"[DEBUG] Remaining distance after reaching max_speed: {remaining_distance:.2f}") + + # Calculate final deceleration distance needed to stop from max_speed + final_decel_distance = (max_speed ** 2) / (2 * deceleration) + cruise_distance = remaining_distance - final_decel_distance + + if DEBUG: + print(f"[DEBUG] Phase 2 - Cruise: distance = {cruise_distance:.2f}") + print(f"[DEBUG] Phase 3 - Final Decel: distance = {final_decel_distance:.2f}") + + times = [] + for s in s_vals: + if s <= initial_decel_distance: # Phase 1: Initial deceleration to max_speed + v = math.sqrt(current_speed ** 2 - 2 * deceleration * s) + t = (current_speed - v) / deceleration + if DEBUG: # Print every 10m + print(f"[DEBUG] Initial Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") + + elif s <= initial_decel_distance + cruise_distance: # Phase 2: Cruise at max_speed + s_in_cruise = s - initial_decel_distance + t = initial_decel_time + s_in_cruise / max_speed + if DEBUG: # Print every 10m + print(f"[DEBUG] Cruise: s = {s:.2f}, v = {max_speed:.2f}, t = {t:.2f}") + + else: # Phase 3: Final deceleration to stop + s_in_final_decel = s - (initial_decel_distance + cruise_distance) + v = math.sqrt(max(max_speed ** 2 - 2 * deceleration * s_in_final_decel, 0.0)) + t = initial_decel_time + cruise_distance / max_speed + (max_speed - v) / deceleration + if DEBUG: # Print every 10m + print(f"[DEBUG] Final Decel: s = {s:.2f}, v = {v:.2f}, t = {t:.2f}") + + times.append(t) + + if DEBUG: + print("[DEBUG] Trajectory complete: Three phases executed") + print(f"[DEBUG] Total time: {times[-1]:.2f}") + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + if acceleration <= 0: + if DEBUG: + print(f"[DEBUG] No acceleration allowed. Current speed: {current_speed:.2f}") + + # Pure deceleration phase + s_decel = (current_speed ** 2) / (2 * deceleration) + T_decel = current_speed / deceleration + + if DEBUG: + print(f"[DEBUG] Will maintain speed until s_decel: {s_decel:.2f}") + print(f"[DEBUG] Total deceleration time will be: {T_decel:.2f}") + + times = [] + for s in s_vals: + if s <= L - s_decel: # Maintain current speed until deceleration point + t_point = s / current_speed + if DEBUG: + print(f"[DEBUG] Constant Speed Phase: s = {s:.2f}, v = {current_speed:.2f}, t = {t_point:.2f}") + else: # Deceleration phase + s_in_decel = s - (L - s_decel) + v = math.sqrt(max(current_speed ** 2 - 2 * deceleration * s_in_decel, 0.0)) + t_point = (L - s_decel) / current_speed + (current_speed - v) / deceleration + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + # Determine max possible peak speed given distance + v_peak_possible = math.sqrt( + (2 * acceleration * deceleration * L + deceleration * current_speed ** 2) / (acceleration + deceleration)) + v_target = min(max_speed, v_peak_possible) + + if DEBUG: + print("[DEBUG] longitudinal_plan: v_peak_possible =", v_peak_possible, "v_target =", v_target) + + # Compute acceleration phase + s_accel = max(0.0, (v_target ** 2 - current_speed ** 2) / (2 * acceleration)) + t_accel = max(0.0, (v_target - current_speed) / acceleration) + + # Compute deceleration phase + s_decel = max(0.0, (v_target ** 2) / (2 * deceleration)) + t_decel = max(0.0, v_target / deceleration) + + # Compute cruise phase + s_cruise = max(0.0, L - s_accel - s_decel) + t_cruise = s_cruise / v_target if v_target > 0 else 0.0 + + if DEBUG: + print("[DEBUG] longitudinal_plan: s_accel =", s_accel, "t_accel =", t_accel) + print("[DEBUG] longitudinal_plan: s_decel =", s_decel, "t_decel =", t_decel) + print("[DEBUG] longitudinal_plan: s_cruise =", s_cruise, "t_cruise =", t_cruise) + + times = [] + for s in s_vals: + if s <= s_accel: # Acceleration phase + v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) + t_point = (v - current_speed) / acceleration + + if DEBUG: + print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + elif s <= s_accel + s_cruise: # Cruise phase + t_point = t_accel + (s - s_accel) / v_target + + if DEBUG: + print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + + else: # Deceleration phase + s_decel_phase = s - s_accel - s_cruise + v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) + t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + + if t_point < times[-1]: # Ensure time always increases + t_point = times[-1] + 0.01 # Small time correction step + + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + if DEBUG: + print("[DEBUG] longitudinal_plan: Final times =", times) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + + +def longitudinal_brake(path: Path, deceleration: float, current_speed: float, + emergency_decel: float = 8.0) -> Trajectory: + # Vehicle already stopped - maintain position + if current_speed <= 0: + print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) + return Trajectory( + frame=path.frame, + points=[path.points[0]] * len(path.points), + times=[float(i) for i in range(len(path.points))] + ) + + # Get total path length + path_length = sum( + np.linalg.norm(np.array(path.points[i + 1]) - np.array(path.points[i])) + for i in range(len(path.points) - 1) + ) + + # Calculate stopping distance with normal deceleration + T_stop_normal = current_speed / deceleration + s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + + # Check if emergency braking is needed + if s_stop_normal > path_length: + if DEBUG: + print("[DEBUG] longitudinal_brake: Emergency braking needed!") + print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") + print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + + # Calculate emergency braking parameters + T_stop = current_speed / emergency_decel + s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") + print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") + + decel_to_use = emergency_decel + + else: + if DEBUG: + print("[DEBUG] longitudinal_brake: Normal braking sufficient") + T_stop = T_stop_normal + decel_to_use = deceleration + + # Generate time points (use more points for smoother trajectory) + num_points = max(len(path.points), 50) + times = np.linspace(0, T_stop, num_points) + + # Calculate distances at each time point using physics equation + distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) + + # Generate points along the path + points = [] + for d in distances: + if d <= path_length: + points.append(path.eval(d)) + else: + points.append(path.eval(d)) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") + print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + + return Trajectory(frame=path.frame, points=points, times=times.tolist()) + + times = [] + for s in s_vals: + if s <= s_accel: # Acceleration phase + v = math.sqrt(current_speed ** 2 + 2 * acceleration * s) + t_point = (v - current_speed) / acceleration + + if DEBUG: + print(f"[DEBUG] Acceleration Phase: s = {s:.2f}, v = {v:.2f}, t = {t_point:.2f}") + + elif s <= s_accel + s_cruise: # Cruise phase + t_point = t_accel + (s - s_accel) / v_target + + if DEBUG: + print(f"[DEBUG] Cruise Phase: s = {s:.2f}, t = {t_point:.2f}") + + else: # Deceleration phase + s_decel_phase = s - s_accel - s_cruise + v_decel = math.sqrt(max(v_target ** 2 - 2 * deceleration * s_decel_phase, 0.0)) + t_point = t_accel + t_cruise + (v_target - v_decel) / deceleration + + if t_point < times[-1]: # Ensure time always increases + t_point = times[-1] + 0.01 # Small time correction step + + if DEBUG: + print(f"[DEBUG] Deceleration Phase: s = {s:.2f}, v = {v_decel:.2f}, t = {t_point:.2f}") + + times.append(t_point) + + if DEBUG: + print("[DEBUG] longitudinal_plan: Final times =", times) + + return Trajectory(frame=path.frame, points=dense_points, times=times) + +def longitudinal_brake(path: Path, deceleration: float, current_speed: float, emergency_decel: float = 8.0) -> Trajectory: + # Vehicle already stopped - maintain position + if current_speed <= 0: + print("[DEBUG] longitudinal_brake: Zero velocity case! ", [path.points[0]] * len(path.points)) + return Trajectory( + frame=path.frame, + points=[path.points[0]] * len(path.points), + times=[float(i) for i in range(len(path.points))] + ) + + # Get total path length + path_length = sum( + np.linalg.norm(np.array(path.points[i+1]) - np.array(path.points[i])) + for i in range(len(path.points)-1) + ) + + # Calculate stopping distance with normal deceleration + T_stop_normal = current_speed / deceleration + s_stop_normal = current_speed * T_stop_normal - 0.5 * deceleration * (T_stop_normal ** 2) + + # Check if emergency braking is needed + if s_stop_normal > path_length: + if DEBUG: + print("[DEBUG] longitudinal_brake: Emergency braking needed!") + print(f"[DEBUG] longitudinal_brake: Normal stopping distance: {s_stop_normal:.2f}m") + print(f"[DEBUG] longitudinal_brake: Available distance: {path_length:.2f}m") + + # Calculate emergency braking parameters + T_stop = current_speed / emergency_decel + s_stop = current_speed * T_stop - 0.5 * emergency_decel * (T_stop ** 2) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Emergency stopping distance: {s_stop:.2f}m") + print(f"[DEBUG] longitudinal_brake: Emergency stopping time: {T_stop:.2f}s") + + decel_to_use = emergency_decel + + else: + if DEBUG: + print("[DEBUG] longitudinal_brake: Normal braking sufficient") + T_stop = T_stop_normal + decel_to_use = deceleration + + # Generate time points (use more points for smoother trajectory) + num_points = max(len(path.points), 50) + times = np.linspace(0, T_stop, num_points) + + # Calculate distances at each time point using physics equation + distances = current_speed * times - 0.5 * decel_to_use * (times ** 2) + + # Generate points along the path + points = [] + for d in distances: + if d <= path_length: + points.append(path.eval(d)) + else: + points.append(path.eval(d)) + + if DEBUG: + print(f"[DEBUG] longitudinal_brake: Using deceleration of {decel_to_use:.2f} m/s²") + print(f"[DEBUG] longitudinal_brake: Final stopping time: {T_stop:.2f}s") + + return Trajectory(frame=path.frame, points=points, times=times.tolist()) + +class YieldTrajectoryPlanner(Component): + """Follows the given route. Brakes if the ego–vehicle must yield + (e.g. to a pedestrian) or if the end of the route is near; otherwise, + it accelerates (or cruises) toward a desired speed. + """ + + def __init__(self): + self.route_progress = None + self.t_last = None + self.acceleration = settings.get("run.drive.planning.motion_planning.largs.acceleration", 5) + self.desired_speed = settings.get("run.drive.planning.motion_planning.largs.desired_speed",2.0) + self.deceleration = settings.get("run.drive.planning.motion_planning.largs.deceleration", 2.0) + self.emergency_brake = settings.get("run.drive.planning.motion_planning.largs.emergency_brake", 8.0) + + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['trajectory'] + + def rate(self): + return 10.0 + + def update(self, state: AllState): + vehicle = state.vehicle # type: VehicleState + route = state.route # type: Route + t = state.t + + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: t =", t) + + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: dt =", dt) + + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + if DEBUG: + print(f"[DEBUG] YieldTrajectoryPlanner.update: Vehicle position = ({curr_x}, {curr_y}), speed = {curr_v}, ") + + # Determine progress along the route. + if self.route_progress is None: + self.route_progress = 0.0 + _, closest_parameter = route.closest_point_local( + [curr_x, curr_y], + (self.route_progress - 5.0, self.route_progress + 5.0) + ) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Closest parameter on route =", closest_parameter) + self.route_progress = closest_parameter + + # Extract a 10 m segment of the route for planning lookahead. + route_with_lookahead = route.trim(closest_parameter, closest_parameter + 10.0) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Route Lookahead =", route_with_lookahead) + + print("[DEBUG] state", state.relations) + # Check whether any yield relations (e.g. due to pedestrians) require braking. + stay_braking = False + pointSet = set() + for i in range(len(route_with_lookahead.points)): + if tuple(route_with_lookahead.points[i]) in pointSet: + stay_braking = True + break + pointSet.add(tuple(route_with_lookahead.points[i])) + + should_brake = any( + r.type == EntityRelationEnum.STOPPING_AT and r.obj1 == '' + for r in state.relations + ) + should_decelerate = any( + r.type == EntityRelationEnum.YIELDING and r.obj1 == '' + for r in state.relations + ) if should_brake == False else False + + should_accelerate = (not should_brake and not should_decelerate and curr_v < self.desired_speed) + + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: stay_braking =", stay_braking) + print("[DEBUG] YieldTrajectoryPlanner.update: should_brake =", should_brake) + print("[DEBUG] YieldTrajectoryPlanner.update: should_accelerate =", should_accelerate) + print("[DEBUG] YieldTrajectoryPlanner.update: should_decelerate =", should_decelerate) + + if stay_braking: + traj = longitudinal_brake(route_with_lookahead, 0.0, 0.0, 0.0) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake (stay braking).") + elif should_brake: + traj = longitudinal_brake(route_with_lookahead, self.emergency_brake, curr_v) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake.") + elif should_decelerate: + traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_brake.") + elif should_accelerate: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, + self.deceleration, self.desired_speed, curr_v) + if DEBUG: + print("[DEBUG] YieldTrajectoryPlanner.update: Using longitudinal_plan (accelerate).") + else: + # Maintain current speed if not accelerating or braking. + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + if DEBUG: + print( + "[DEBUG] YieldTrajectoryPlanner.update: Maintaining current speed with longitudinal_plan (0 accel).") + + self.t_last = t + 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 diff --git a/launch/parking_detection.yaml b/launch/parking_detection.yaml index 3f690bc10..611d51f8d 100644 --- a/launch/parking_detection.yaml +++ b/launch/parking_detection.yaml @@ -149,7 +149,7 @@ variants: iterations: 500 heuristic: reeds_shepp motion_planning: - type: longitudinal_planning.YieldTrajectoryPlanner + type: parking_motion_planning.YieldTrajectoryPlanner # type: yield_spline_planner.SplinePlanner largs: acceleration: 5 From c5072d91c94e6db069171c32157066a51273601e Mon Sep 17 00:00:00 2001 From: Aadarsh Date: Thu, 15 May 2025 10:12:04 -0500 Subject: [PATCH 11/11] resolving issues on s2025 --- GEMstack/onboard/perception/parking_detection.py | 4 ++-- GEMstack/onboard/planning/route_planning_component.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/GEMstack/onboard/perception/parking_detection.py b/GEMstack/onboard/perception/parking_detection.py index 0ff8ceacd..76a3c9e06 100644 --- a/GEMstack/onboard/perception/parking_detection.py +++ b/GEMstack/onboard/perception/parking_detection.py @@ -240,7 +240,7 @@ def update(self, state: AllState): material=ObstacleMaterialEnum.BARRIER, collidable=True ) - parking_obstacles[obstacle_id] = new_obstacle + parking_obstacles[f"parking_obstacle_{obstacle_id}"] = new_obstacle obstacle_id += 1 # Constructing goal pose @@ -256,7 +256,7 @@ def update(self, state: AllState): frame=ObjectFrameEnum.START ) - DISTANCE_THRESHOLD = 12.0 + DISTANCE_THRESHOLD = 8.0 if self.euclidean_distance((x,y), state) > DISTANCE_THRESHOLD: # we are not close enough to the parking spot return None diff --git a/GEMstack/onboard/planning/route_planning_component.py b/GEMstack/onboard/planning/route_planning_component.py index 4838ab3a0..ef3bf5c07 100644 --- a/GEMstack/onboard/planning/route_planning_component.py +++ b/GEMstack/onboard/planning/route_planning_component.py @@ -13,8 +13,8 @@ from GEMstack.state.vehicle import VehicleState from GEMstack.state.intent import VehicleIntentEnum -from .planner import optimized_kinodynamic_rrt_planning -from .map_utils import load_pgm_to_occupancy_grid +# from .planner import optimized_kinodynamic_rrt_planning +# from .map_utils import load_pgm_to_occupancy_grid from .rrt_star import RRTStar from typing import List from ..component import Component @@ -26,7 +26,7 @@ import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge -from .occupancy_grid2 import OccupancyGrid2 +# from .occupancy_grid2 import OccupancyGrid2 import cv2