From 82d68b66a15a960ab7285e4aca93c698f3bb9762 Mon Sep 17 00:00:00 2001 From: Henry-YiW Date: Mon, 24 Feb 2025 11:59:53 -0600 Subject: [PATCH 01/11] get_minimum_deceleration_for_collision_avoidance tested --- GEMstack/onboard/planning/longitudinal_planning.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 79abfc62f..21bc4054e 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -211,7 +211,7 @@ def get_minimum_deceleration_for_collision_avoidance(curr_x: float, curr_y: floa # 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 =x abs(softest_accleration) + 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) @@ -1012,11 +1012,16 @@ def update(self, state : AllState): if should_accelerate: traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, "milestone") elif should_yield: - desired_speed = math.sqrt(-2 * yield_deceleration * r_pedestrain_x + curr_v**2) - desired_speed = max(desired_speed, 0) + desired_speed_squared = -2 * yield_deceleration * r_pedestrain_x + curr_v**2 + desired_speed = math.sqrt(max(desired_speed_squared, 0)) + print('desired speed', desired_speed) + print('yield deceleration', yield_deceleration) + print('r_pedestrain_x', r_pedestrain_x) + print('curr_v', curr_v) + desired_speed = 0 if desired_speed < 0 else desired_speed # traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) if desired_speed > 0: - traj = longitudinal_plan(route_with_lookahead, 0, yield_deceleration, desired_speed, curr_v, "dt") + traj = longitudinal_plan(route_with_lookahead, 0.1, yield_deceleration, desired_speed, curr_v, "dt") else: traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) else: From a75f3559714f09992ce22194a4d682f8d02b0157 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Tue, 25 Feb 2025 01:32:41 -0600 Subject: [PATCH 02/11] Transfered logic into pedestrian_yield_logic.py --- .../knowledge/defaults/computation_graph.yaml | 2 +- GEMstack/knowledge/defaults/current.yaml | 11 + .../onboard/planning/longitudinal_planning.py | 653 ++++-------------- .../planning/pedestrian_yield_logic.py | 607 +++++++++++++++- GEMstack/state/relations.py | 3 + launch/pedestrian_detection.yaml | 18 +- 6 files changed, 769 insertions(+), 525 deletions(-) diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index b6d5c92f7..683dc0c7b 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -31,7 +31,7 @@ components: inputs: [vehicle, roadgraph, agents] outputs: agent_intents - relations_estimation: - inputs: [vehicle, roadgraph, agents, obstacles] + inputs: all outputs: relations - predicate_evaluation: inputs: [vehicle, roadgraph, agents, obstacles] diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 12a9f55c5..1a135b4ba 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -20,6 +20,17 @@ control: pid_p: 1.0 pid_i: 0.05 pid_d: 0.0 +planning: + longitudinal_plan: + mode: 'real' + planner: 'dt' # 'milestone', 'dt', or 'dx' + desired_speed: 1.0 + acceleration: 0.5 + max_deceleration: 6.0 + deceleration: 2.0 + min_deceleration: 0.5 + yield_deceleration: 0.5 + #configure the simulator, if using simulator: diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 61ad9240d..debbba809 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,10 +1,9 @@ from typing import List, Tuple, Union from ..component import Component from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState -from ...utils import serialization +from ...utils import serialization, settings from ...mathutils.transforms import vector_madd -import time import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches @@ -17,211 +16,6 @@ ################################################################################ - -class CollisionDetector: - """ - Simulation class to update positions of two rectangles (vehicle and pedestrian) - with velocities v1 and v2, performing collision detection at each time step. - All functions remain within the class, and variables defined in __init__ remain unchanged; - local copies are used during simulation. - """ - def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, desired_speed=2.0, acceleration=0.5): - - self.vehicle_x = x1 - self.vehicle_y = y1 - self.pedestrian_x = x2 - self.pedestrian_y = y2 - - # Vehicle parameters with buffer adjustments - self.vehicle_size_x = 3.2 - self.vehicle_size_y = 1.7 - self.vehicle_buffer_x = 3.0 + 1.0 - self.vehicle_buffer_y = 2.0 - - # Vehicle rectangle - self.x1 = self.vehicle_x + (self.vehicle_size_x + self.vehicle_buffer_x)*0.5 # Offset for buffer (remains constant) - self.y1 = self.vehicle_y - self.w1 = self.vehicle_size_x + self.vehicle_buffer_x # Increase width with buffer - self.h1 = self.vehicle_size_y + self.vehicle_buffer_y # Increase height with buffer - self.t1 = t1 - - # Pedestrian rectangle - self.x2 = x2 - self.y2 = y2 - self.w2 = 0.5 - self.h2 = 0.5 - self.t2 = t2 - - # Velocities are expected as [vx, vy] - self.v1 = v1 - self.v2 = v2 - - self.dt = 0.1 # seconds - self.total_time = total_time # seconds - - self.desired_speed = desired_speed - self.acceleration = acceleration - - def set_params(self, speed, acceleration): - self.desired_speed = speed - self.acceleration = acceleration - - def get_corners(self, x, y, w, h, theta): - """ - Returns the 4 corners of a rotated rectangle. - (x, y): center of rectangle - w, h: width and height of rectangle - theta: rotation angle in radians - """ - cos_t = math.cos(theta) - sin_t = math.sin(theta) - hw, hh = w / 2.0, h / 2.0 - - corners = np.array([ - [ hw * cos_t - hh * sin_t, hw * sin_t + hh * cos_t], - [-hw * cos_t - hh * sin_t, -hw * sin_t + hh * cos_t], - [-hw * cos_t + hh * sin_t, -hw * sin_t - hh * cos_t], - [ hw * cos_t + hh * sin_t, hw * sin_t - hh * cos_t] - ]) - - corners[:, 0] += x - corners[:, 1] += y - - return corners - - def get_axes(self, rect): - axes = [] - for i in range(len(rect)): - p1 = rect[i] - p2 = rect[(i + 1) % len(rect)] - edge = p2 - p1 - normal = np.array([-edge[1], edge[0]]) - norm = np.linalg.norm(normal) - if norm: - normal /= norm - axes.append(normal) - return axes - - def project(self, rect, axis): - dots = np.dot(rect, axis) - return np.min(dots), np.max(dots) - - def sat_collision(self, rect1, rect2): - """ - Determines if two convex polygons (rectangles) collide using the - Separating Axis Theorem (SAT). - rect1 and rect2 are numpy arrays of shape (4,2) representing their corners. - """ - axes1 = self.get_axes(rect1) - axes2 = self.get_axes(rect2) - - for axis in axes1 + axes2: - min1, max1 = self.project(rect1, axis) - min2, max2 = self.project(rect2, axis) - if max1 < min2 or max2 < min1: - return False - return True - - def plot_rectangles(self, rect1, rect2, collision, ax): - """ - Plot two rectangles on a provided axis and indicate collision by color. - """ - color = 'red' if collision else 'blue' - for rect in [rect1, rect2]: - polygon = patches.Polygon(rect, closed=True, edgecolor=color, fill=False, linewidth=2) - ax.add_patch(polygon) - ax.scatter(rect[:, 0], rect[:, 1], color=color, zorder=3) - ax.set_title(f"Collision: {'Yes' if collision else 'No'}") - - def run(self, is_displayed=False): - collision_distance = -1 - steps = int(self.total_time / self.dt) + 1 - - # Create local variables for positions; these will be updated - # without modifying the __init__ attributes. - current_x1 = self.x1 - current_y1 = self.y1 - current_x2 = self.x2 - current_y2 = self.y2 - current_v1 = self.v1[0] - - if is_displayed: - plt.ion() # Turn on interactive mode - fig, ax = plt.subplots(figsize=(10,5)) - - for i in range(steps): - t_sim = i * self.dt - - # Compute rectangle corners using the local positional variables. - rect1 = self.get_corners(current_x1, current_y1, self.w1, self.h1, self.t1) - rect2 = self.get_corners(current_x2, current_y2, self.w2, self.h2, self.t2) - - # Perform collision detection. - collision = self.sat_collision(rect1, rect2) - - if is_displayed: - # Plot the current step. - ax.clear() - self.plot_rectangles(rect1, rect2, collision, ax) - ax.set_aspect('equal') - ax.grid(True, linestyle='--', alpha=0.5) - ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20) - ax.set_ylim(self.vehicle_y -5, self.vehicle_y +5) - - # Draw an additional rectangle (vehicle body) at the vehicle's center. - rect_vehiclebody = patches.Rectangle( - (current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x)*0.5, current_y1 - self.vehicle_size_y * 0.5), - self.w1 - self.vehicle_buffer_x, - self.h1 - self.vehicle_buffer_y, - edgecolor='green', - fill=False, - linewidth=2, - linestyle='--' - ) - ax.add_patch(rect_vehiclebody) - - ax.text(0.01, 0.99, f"t = {t_sim:.1f}s", fontsize=12, transform=ax.transAxes, verticalalignment='top') - plt.draw() - - # Pause briefly to simulate real-time updating. - plt.pause(self.dt * 0.05) - - # Stop simulation if collision is detected. - if collision: - current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 - current_vehicle_y = current_y1 - collision_distance = current_vehicle_x - self.vehicle_x - break - - # Update the vehicle's speed if it is not at the desired speed. - next_v = current_v1 + self.acceleration * self.dt - # Accelerating: Check if the vehicle is about to exceed the desired speed. - if next_v > self.desired_speed and current_v1 <= self.desired_speed: - current_v1 = self.desired_speed - # Decelerating: Check if the vehicle is about to fall below the desired speed. - elif next_v < self.desired_speed and current_v1 >= self.desired_speed: - current_v1 = self.desired_speed - else: - current_v1 = next_v - - current_v1 = 0.0 if current_v1 < 0.0 else current_v1 - - # Update local positions based on velocities. - current_x1 += current_v1 * self.dt - current_y1 += self.v1[1] * self.dt - current_x2 += self.v2[0] * self.dt - current_y2 += self.v2[1] * self.dt - - if is_displayed: - plt.ioff() - plt.show(block=True) - - # print("Collision distance:", collision_distance) - - return collision_distance - - - 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, float]: """Detects if a collision will occur with the given object and return deceleration to avoid it.""" @@ -1180,134 +974,9 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) ################################################################################ -########################## -##### Patrick's Code ##### -########################## - -class YieldTrajectoryPlanner(Component): - """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, mode : str = 'real', params : dict = {"planner": "dt", "desired_speed": 1.0, "acceleration": 0.5}): - self.route_progress = None - self.t_last = None - self.acceleration = params["acceleration"] - self.desired_speed = params["desired_speed"] - self.deceleration = 2.0 - - self.min_deceleration = 1.0 - self.max_deceleration = 8.0 - - self.mode = mode - self.planner = params["planner"] - - 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): - start_time = time.time() - - vehicle = state.vehicle # type: VehicleState - route = state.route # type: Route - t = state.t - - if self.t_last is None: - self.t_last = t - dt = t - self.t_last - - # Position in vehicle frame (Start (0,0) to (15,0)) - curr_x = vehicle.pose.x - curr_y = vehicle.pose.y - curr_v = vehicle.v - - abs_x = curr_x + state.start_vehicle_pose.x - abs_y = curr_y + state.start_vehicle_pose.y - - ############################################### - # print("@@@@@ VEHICLE STATE @@@@@") - # print(vehicle) - # print("@@@@@@@@@@@@@@@@@@@@@@@@@") - - if self.mode == 'real': - # Position in vehicle frame (Start (0,0) to (15,0)) - # curr_x = vehicle.pose.x * 20 - # curr_y = vehicle.pose.y * 20 - # print("@@@@@ PLAN", curr_x, curr_y, curr_v) - abs_x = curr_x - abs_y = curr_y - # print("@@@@@ PLAN", abs_x, abs_y) - ############################################### - - #figure out where we are on the route - if self.route_progress is None: - self.route_progress = 0.0 - closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) - self.route_progress = closest_parameter - - lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) - route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) - print("Lookahead distance:", lookahead_distance) - - route_to_end = route.trim(closest_parameter, len(route.points) - 1) - - should_yield = False - yield_deceleration = 0.0 - - print("Current Speed: ", curr_v) - - 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] - - if self.mode == 'real': - obj.pose.x = obj.pose.x + curr_x - obj.pose.y = obj.pose.y + curr_y - - 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_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) - traj = longitudinal_plan(route_with_lookahead, self.acceleration, deceleration, desired_speed, curr_v) - return traj - else: - if detected and deceleration > 0: - yield_deceleration = max(deceleration, yield_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_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, self.planner) - elif should_yield: - traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) - else: - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, self.planner) - - return traj - - -# ######################## -# ##### Yudai's Code ##### -# ######################## +# ########################## +# ##### Patrick's Code ##### +# ########################## # class YieldTrajectoryPlanner(Component): # """Follows the given route. Brakes if you have to yield or @@ -1315,20 +984,16 @@ def update(self, state : AllState): # the desired speed. # """ -# def __init__(self, mode : str = 'real', params : dict = {}): +# 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 = params["acceleration"] # self.desired_speed = params["desired_speed"] +# self.deceleration = 2.0 -# # Yielding parameters -# # Yielding speed [..., 1.0, 0.8, ..., 0.2] -# self.yield_speed = [v for v in np.arange(self.desired_speed, 0.1, -0.25)] -# self.yield_deceleration = 0.5 -# self.deceleration = 2.0 -# self.max_deceleration = 8.0 +# self.min_deceleration = 1.0 +# self.max_deceleration = 8.0 -# # Planner mode # self.mode = mode # self.planner = params["planner"] @@ -1352,7 +1017,7 @@ def update(self, state : AllState): # self.t_last = t # dt = t - self.t_last - +# # Position in vehicle frame (Start (0,0) to (15,0)) # curr_x = vehicle.pose.x # curr_y = vehicle.pose.y # curr_v = vehicle.v @@ -1361,8 +1026,6 @@ def update(self, state : AllState): # abs_y = curr_y + state.start_vehicle_pose.y # ############################################### -# # # TODO: Fix the coordinate conversion of other files - # # print("@@@@@ VEHICLE STATE @@@@@") # # print(vehicle) # # print("@@@@@@@@@@@@@@@@@@@@@@@@@") @@ -1377,188 +1040,160 @@ def update(self, state : AllState): # # print("@@@@@ PLAN", abs_x, abs_y) # ############################################### - # #figure out where we are on the route # if self.route_progress is None: # self.route_progress = 0.0 # closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) # self.route_progress = closest_parameter -# lookahead_distance = max(10, curr_v**2 / (2 * self.yield_deceleration)) +# lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) # route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) # print("Lookahead distance:", lookahead_distance) -# #extract out a 10m segment of the route -# # route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) +# route_to_end = route.trim(closest_parameter, len(route.points) - 1) -# # Default values -# should_brake = False -# input_values = [{"decel": self.deceleration, "desired_speed": self.desired_speed, "collision_distance": lookahead_distance}] +# should_yield = False +# yield_deceleration = 0.0 -# print(state.relations) +# print("Current Speed: ", curr_v) # for r in state.relations: # if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': -# #yielding to something, brake - -# #========================= -# """ -# Collision detection: -# - Compute the lookahead distance required to avoid collision using: -# d = v^2/(2*a) -# - For many steps along the route (using a resolution that adapts if the -# planner runs too slowly), simulate the vehicle's future positions. -# - If a pedestrian is detected within 3m longitudinal and 1m lateral buffer, -# determine the distance-to-collision. Then compute the required deceleration: -# a = -(v^2)/(2*d_collision) -# - For distant crossing pedestrians, apply a gentle deceleration based on the -# perception-estimated pedestrian velocity. -# """ - -# print("#### YIELDING PLANNING ####") - -# # Vehicle parameters. -# x1, y1 = abs_x, abs_y -# v1 = [curr_v, 0] # Vehicle speed vector - -# for n,a in state.agents.items(): - -# """ -# class ObjectFrameEnum(Enum): -# START = 0 #position / yaw in m / radians relative to starting pose of vehicle -# CURRENT = 1 #position / yaw in m / radians relative to current pose of vehicle -# GLOBAL = 2 #position in longitude / latitude, yaw=heading in radians with respect to true north (used in GNSS) -# ABSOLUTE_CARTESIAN = 3 #position / yaw in m / radians relative to a global cartesian reference frame (used in simulation) -# """ -# # print("@@@@@ AGENT STATE @@@@@") -# # print(a) -# # print("@@@@@@@@@@@@@@@@@@@@@@@") - -# # Pedestrian parameters. -# x2, y2 = a.pose.x, a.pose.y -# v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector - -# if self.mode == 'real': -# x2 = a.pose.x + curr_x -# y2 = a.pose.y + curr_y - -# # Total simulation time -# if v1[0] > 0.1: -# total_time = min(10, lookahead_distance / v1[0]) -# else: -# total_time = 10 -# print(f"Total time: {total_time:.2f} seconds") - -# # Create and run the simulation. -# print(f"Vehicle: ({x1:.1f}, {y1:.1f}, ({v1[0]:.1f}, {v1[1]:.1f}))") -# print(f"Pedestrian: ({x2:.1f}, {y2:.1f}, ({v2[0]:.1f}, {v2[1]:.1f}))") - -# # Simulate if a collision will occur when the vehicle accelerate to desired speed. -# sim = CollisionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time, self.desired_speed, self.acceleration) -# collision_distance = sim.run() - -# # No collision detected with acceleration to desired speed. -# if collision_distance < 0: -# print("No collision detected.") -# input_values.append({"decel": self.deceleration, "desired_speed": self.desired_speed, "collision_distance": collision_distance}) -# continue - -# # Collision detected with acceleration to desired speed. -# # => Check if the vehicle can yield to the pedestrian with deceleration. -# else: - -# ############################################### -# # # UNCOMMENT NOT TO YIELD: JUST STOP FOR PART1 -# # print("The vehicle is Stopping.") -# # print("@@@@@", a.pose.x) - -# # # Update the collision distance. -# # if lookahead_distance_to_pedestrian > collision_distance: -# # lookahead_distance_to_pedestrian = collision_distance - -# # # Decide the deceleration based on the collision distance. -# # # To stop perfectly, assume the vehicle is running at the desired speed. -# # brake_deceleration = max(self.deceleration, desired_speed**2 / (2 * (collision_distance))) -# # if brake_deceleration > self.max_deceleration: -# # brake_deceleration = self.max_deceleration - -# # if brake_deceleration > decel: -# # decel = brake_deceleration if brake_deceleration > decel else decel -# # should_brake = True -# # break -# ############################################### - -# print("Collision detected. Try to find yielding speed.") - -# collision_distance_after_yield = -1 - -# # Simulate with different yield speeds. -# # Try to maximize the yield speed to avoid collision. -# for v in self.yield_speed: -# # Simulate if the vehicle can yield to the pedestrian with acceleration to yielding speed. -# if v > v1[0]: -# sim.set_params(v, self.acceleration) -# # Simulate if the vehicle can yield to the pedestrian with deceleration to yielding speed. -# else: -# sim.set_params(v, self.yield_deceleration * -1.0) -# collision_distance_after_yield = sim.run() -# if collision_distance_after_yield < 0: -# print(f"Yielding at speed: {v}") -# input_values.append({"decel": self.yield_deceleration, "desired_speed": v, "collision_distance": collision_distance}) -# break - -# # Collision detected with any yielding speed. -# # => Brake to avoid collision. -# if collision_distance_after_yield >= 0: -# print("The vehicle is Stopping.") -# # Decide the deceleration based on the collision distance. -# brake_deceleration = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) -# if brake_deceleration > self.max_deceleration: -# brake_deceleration = self.max_deceleration -# input_values.append({"decel": brake_deceleration, "desired_speed": v1[0], "collision_distance": collision_distance}) -# should_brake = True - -# # You need to break state.relation loop to avoid unnecessary computation. -# break - -# # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS -# # should_brake = True -# # desired_speed = 0.0 -# # decel = self.deceleration - -# # # UNCOMMENT NOT TO BRAKE -# # should_brake = False -# # desired_speed = self.desired_speed -# # decel = self.deceleration - -# #========================= - -# # Choose the maximum deceleration from input_values. -# print("Input values:", input_values) -# decel = min(input_values, key=lambda x: x['desired_speed'])['decel'] -# desired_speed = min(input_values, key=lambda x: x['desired_speed'])['desired_speed'] -# collision_distance = min(input_values, key=lambda x: x['desired_speed'])['collision_distance'] - -# # Update the lookahead distance to pedestrian. -# route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) - -# print(f"Desired speed: {desired_speed:.2f} m/s") -# print(f"Deceleration: {decel:.2f} m/s^2") - -# should_accelerate = (not should_brake and curr_v < self.desired_speed) - -# # traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v, "milestone") +# #get the object we are yielding to +# obj = state.agents[r.obj2] + +# if self.mode == 'real': +# obj.pose.x = obj.pose.x + curr_x +# obj.pose.y = obj.pose.y + curr_y + +# 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_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) +# traj = longitudinal_plan(route_with_lookahead, self.acceleration, deceleration, desired_speed, curr_v) +# return traj +# else: +# if detected and deceleration > 0: +# yield_deceleration = max(deceleration, yield_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_with_lookahead, self.acceleration, decel, desired_speed, curr_v, self.planner) -# elif should_brake: -# traj = longitudinal_brake(route_with_lookahead, decel, curr_v) +# traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, self.planner) +# elif should_yield: +# traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) # else: -# traj = longitudinal_plan(route_with_lookahead, 0.0, decel, desired_speed, curr_v, self.planner) - -# print(f"Simulation took {time.time() - start_time:.3f} seconds.") +# traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, self.planner) # return traj + +######################################################## +##### Yudai's Code Using pedestrian_yield_logic.py ##### +######################################################## +class YieldTrajectoryPlanner(Component): + """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): + 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") + + 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 self.t_last is None: + self.t_last = t + dt = t - self.t_last + print("Elapsed time:", int(dt*1000), "ms") + self.t_last = t + + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + + #figure out where we are on the route + if self.route_progress is None: + self.route_progress = 0.0 + closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) + self.route_progress = closest_parameter + + lookahead_distance = max(10, curr_v**2 / (2 * self.yield_deceleration)) + print("Lookahead distance:", lookahead_distance) + + # Default values + should_brake = False + input_values = [{"decel": self.deceleration, "desired_speed": self.desired_speed, "collision_distance": lookahead_distance}] + + for r in state.relations: + if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': + input_values.append({"decel": r.yield_decel, "desired_speed": r.yield_speed, "collision_distance": r.yield_dist}) + + print("Input values:", input_values) + # Choose minimum desired speed and extract deceleration and collision distance from input_values. + desired_speed = min(input_values, key=lambda x: x['desired_speed'])['desired_speed'] + decel = min(input_values, key=lambda x: x['desired_speed'])['decel'] + collision_distance = min(input_values, key=lambda x: x['desired_speed'])['collision_distance'] + if desired_speed == 0.0: + should_brake = True + + # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS + # should_brake = True + # desired_speed = 0.0 + # decel = self.deceleration + + # # UNCOMMENT NOT TO BRAKE + # should_brake = False + # desired_speed = self.desired_speed + # decel = self.deceleration + + # Update the lookahead distance to pedestrian. + route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) + + print(f"Desired speed: {desired_speed:.2f} m/s") + print(f"Deceleration: {decel:.2f} m/s^2") + + should_accelerate = (not should_brake and curr_v < self.desired_speed) + + #choose whether to accelerate, brake, or keep at current velocity + if should_accelerate: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, decel, desired_speed, curr_v, self.planner) + elif should_brake: + traj = longitudinal_brake(route_with_lookahead, decel, curr_v) + else: + traj = longitudinal_plan(route_with_lookahead, 0.0, decel, desired_speed, curr_v, self.planner) + + return traj # ######################## diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py index 2567c0093..9c223b8c5 100644 --- a/GEMstack/onboard/planning/pedestrian_yield_logic.py +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -1,22 +1,617 @@ -from ...state import AgentState,AgentEnum,EntityRelation,EntityRelationEnum +from ...state import AllState,VehicleState,AgentState,AgentEnum,EntityRelation,EntityRelationEnum,ObjectFrameEnum from ..component import Component -from typing import List,Dict +from typing import List,Dict,Union,Tuple +from scipy.optimize import minimize +import numpy as np +import math +import matplotlib.pyplot as plt +import matplotlib.patches as patches +from ...utils import settings + +################################################################################ +########## Collisiong Detection ################################################ +################################################################################ + +class CollisionDetector: + """ + Simulation class to update positions of two rectangles (vehicle and pedestrian) + with velocities v1 and v2, performing collision detection at each time step. + All functions remain within the class, and variables defined in __init__ remain unchanged; + local copies are used during simulation. + """ + def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, desired_speed=2.0, acceleration=0.5): + + self.vehicle_x = x1 + self.vehicle_y = y1 + self.pedestrian_x = x2 + self.pedestrian_y = y2 + + # Vehicle parameters with buffer adjustments + self.vehicle_size_x = 3.2 + self.vehicle_size_y = 1.7 + self.vehicle_buffer_x = 3.0 + 1.0 + self.vehicle_buffer_y = 2.0 + + # Vehicle rectangle + self.x1 = self.vehicle_x + (self.vehicle_size_x + self.vehicle_buffer_x)*0.5 # Offset for buffer (remains constant) + self.y1 = self.vehicle_y + self.w1 = self.vehicle_size_x + self.vehicle_buffer_x # Increase width with buffer + self.h1 = self.vehicle_size_y + self.vehicle_buffer_y # Increase height with buffer + self.t1 = t1 + + # Pedestrian rectangle + self.x2 = x2 + self.y2 = y2 + self.w2 = 0.5 + self.h2 = 0.5 + self.t2 = t2 + + # Velocities are expected as [vx, vy] + self.v1 = v1 + self.v2 = v2 + + self.dt = 0.1 # seconds + self.total_time = total_time # seconds + + self.desired_speed = desired_speed + self.acceleration = acceleration + + def set_params(self, speed, acceleration): + self.desired_speed = speed + self.acceleration = acceleration + + def get_corners(self, x, y, w, h, theta): + """ + Returns the 4 corners of a rotated rectangle. + (x, y): center of rectangle + w, h: width and height of rectangle + theta: rotation angle in radians + """ + cos_t = math.cos(theta) + sin_t = math.sin(theta) + hw, hh = w / 2.0, h / 2.0 + + corners = np.array([ + [ hw * cos_t - hh * sin_t, hw * sin_t + hh * cos_t], + [-hw * cos_t - hh * sin_t, -hw * sin_t + hh * cos_t], + [-hw * cos_t + hh * sin_t, -hw * sin_t - hh * cos_t], + [ hw * cos_t + hh * sin_t, hw * sin_t - hh * cos_t] + ]) + + corners[:, 0] += x + corners[:, 1] += y + + return corners + + def get_axes(self, rect): + axes = [] + for i in range(len(rect)): + p1 = rect[i] + p2 = rect[(i + 1) % len(rect)] + edge = p2 - p1 + normal = np.array([-edge[1], edge[0]]) + norm = np.linalg.norm(normal) + if norm: + normal /= norm + axes.append(normal) + return axes + + def project(self, rect, axis): + dots = np.dot(rect, axis) + return np.min(dots), np.max(dots) + + def sat_collision(self, rect1, rect2): + """ + Determines if two convex polygons (rectangles) collide using the + Separating Axis Theorem (SAT). + rect1 and rect2 are numpy arrays of shape (4,2) representing their corners. + """ + axes1 = self.get_axes(rect1) + axes2 = self.get_axes(rect2) + + for axis in axes1 + axes2: + min1, max1 = self.project(rect1, axis) + min2, max2 = self.project(rect2, axis) + if max1 < min2 or max2 < min1: + return False + return True + + def plot_rectangles(self, rect1, rect2, collision, ax): + """ + Plot two rectangles on a provided axis and indicate collision by color. + """ + color = 'red' if collision else 'blue' + for rect in [rect1, rect2]: + polygon = patches.Polygon(rect, closed=True, edgecolor=color, fill=False, linewidth=2) + ax.add_patch(polygon) + ax.scatter(rect[:, 0], rect[:, 1], color=color, zorder=3) + ax.set_title(f"Collision: {'Yes' if collision else 'No'}") + + def run(self, is_displayed=False): + collision_distance = -1 + steps = int(self.total_time / self.dt) + 1 + + # Create local variables for positions; these will be updated + # without modifying the __init__ attributes. + current_x1 = self.x1 + current_y1 = self.y1 + current_x2 = self.x2 + current_y2 = self.y2 + current_v1 = self.v1[0] + + if is_displayed: + plt.ion() # Turn on interactive mode + fig, ax = plt.subplots(figsize=(10,5)) + + for i in range(steps): + t_sim = i * self.dt + + # Compute rectangle corners using the local positional variables. + rect1 = self.get_corners(current_x1, current_y1, self.w1, self.h1, self.t1) + rect2 = self.get_corners(current_x2, current_y2, self.w2, self.h2, self.t2) + + # Perform collision detection. + collision = self.sat_collision(rect1, rect2) + + if is_displayed: + # Plot the current step. + ax.clear() + self.plot_rectangles(rect1, rect2, collision, ax) + ax.set_aspect('equal') + ax.grid(True, linestyle='--', alpha=0.5) + ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20) + ax.set_ylim(self.vehicle_y -5, self.vehicle_y +5) + + # Draw an additional rectangle (vehicle body) at the vehicle's center. + rect_vehiclebody = patches.Rectangle( + (current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x)*0.5, current_y1 - self.vehicle_size_y * 0.5), + self.w1 - self.vehicle_buffer_x, + self.h1 - self.vehicle_buffer_y, + edgecolor='green', + fill=False, + linewidth=2, + linestyle='--' + ) + ax.add_patch(rect_vehiclebody) + + ax.text(0.01, 0.99, f"t = {t_sim:.1f}s", fontsize=12, transform=ax.transAxes, verticalalignment='top') + plt.draw() + + # Pause briefly to simulate real-time updating. + plt.pause(self.dt * 0.05) + + # Stop simulation if collision is detected. + if collision: + current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 + current_vehicle_y = current_y1 + collision_distance = current_vehicle_x - self.vehicle_x + break + + # Update the vehicle's speed if it is not at the desired speed. + next_v = current_v1 + self.acceleration * self.dt + # Accelerating: Check if the vehicle is about to exceed the desired speed. + if next_v > self.desired_speed and current_v1 <= self.desired_speed: + current_v1 = self.desired_speed + # Decelerating: Check if the vehicle is about to fall below the desired speed. + elif next_v < self.desired_speed and current_v1 >= self.desired_speed: + current_v1 = self.desired_speed + else: + current_v1 = next_v + + current_v1 = 0.0 if current_v1 < 0.0 else current_v1 + + # Update local positions based on velocities. + current_x1 += current_v1 * self.dt + current_y1 += self.v1[1] * self.dt + current_x2 += self.v2[0] * self.dt + current_y2 += self.v2[1] * self.dt + + if is_displayed: + plt.ioff() + plt.show(block=True) + + # print("Collision distance:", collision_distance) + + return collision_distance + + + +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, 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_x = 0.0 + obj_y = obj.pose.y + obj_v_x = obj.velocity[0] + obj_v_y = obj.velocity[1] + + 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 + + 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 + + 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_y + + # if curr_v**2/2*distance_to_pass >= 1.5: + # return True, curr_v**2/2*distance_to_pass + time_to_max_v = (5 - curr_v)/0.5 + + if time_to_max_v > time_to_pass: + if curr_v*time_to_pass + 0.25*time_to_pass**2 > distance_to_move: + return False, 0.0 + else: + if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: + return False, 0.0 + + + 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] + + + vehicle_length = 3 + vehicle_width = 2 + + vehicle_buffer_x = 3.0 + vehicle_buffer_y = 1.0 + + 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 + + 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) + yaw = None + minimum_deceleration = None + if yaw is 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 pestrain, + # 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) + else: + pass + + print("calculatedminimum_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 + + +################################################################################ +########## Pedestrian Yielder ################################################## +################################################################################ + class PedestrianYielder(Component): """Yields for all pedestrians in the scene. Result is stored in the relations graph. """ + def __init__(self, mode : str = 'real', params : dict = {}): + # Planner mode + self.mode = mode + self.planner = params["planner"] + self.acceleration = params["acceleration"] + self.desired_speed = params["desired_speed"] + + # Update current.yaml settings with the given parameters in yaml + settings.set("planning.longitudinal_plan.mode", self.mode) + settings.set("planning.longitudinal_plan.planner", self.planner) + settings.set("planning.longitudinal_plan.acceleration", self.acceleration) + settings.set("planning.longitudinal_plan.desired_speed", self.desired_speed) + + self.min_deceleration = settings.get("planning.longitudinal_plan.min_deceleration") + self.max_deceleration = settings.get("planning.longitudinal_plan.max_deceleration") + self.deceleration = settings.get("planning.longitudinal_plan.deceleration") + self.yield_deceleration = settings.get("planning.longitudinal_plan.yield_deceleration") + def rate(self): return None def state_inputs(self): - return ['agents'] + return ['all'] def state_outputs(self): return ['relations'] - def update(self,agents : Dict[str,AgentState]) -> List[EntityRelation]: + def update(self, state : AllState) -> List[EntityRelation]: res = [] + vehicle = state.vehicle + agents = state.agents + + # Position in vehicle frame (Start (0,0) to (15,0)) + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + + # Yielding speed [..., 1.0, 0.8, ..., 0.2] + yield_speed = [v for v in np.arange(self.desired_speed, 0.1, -0.25)] + for n,a in agents.items(): if a.type == AgentEnum.PEDESTRIAN: - #relation: ego-vehicle yields to pedestrian - res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n)) + + ##### Henry's Code ##### + # output_decel, output_dist = get_minimum_deceleration_for_collision_avoidance(curr_x, curr_y, curr_v, a, self.min_deceleration, self.max_deceleration) + # output_speed = None + + ##### Yudai's Code ##### + x1, y1 = curr_x, curr_y + v1 = [curr_v, 0] # Vehicle speed vector + x2, y2 = a.pose.x, a.pose.y + v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector + """ + class ObjectFrameEnum(Enum): + START = 0 #position / yaw in m / radians relative to starting pose of vehicle + CURRENT = 1 #position / yaw in m / radians relative to current pose of vehicle + GLOBAL = 2 #position in longitude / latitude, yaw=heading in radians with respect to true north (used in GNSS) + ABSOLUTE_CARTESIAN = 3 #position / yaw in m / radians relative to a global cartesian reference frame (used in simulation) + """ + # If the pedestrian's frame is ABSOLUTE, convert the pedestrian's position to START. + if a.pose.frame == ObjectFrameEnum.ABSOLUTE_CARTESIAN: + x1 = curr_x + state.start_vehicle_pose.x + y1 = curr_y + state.start_vehicle_pose.y + + # If the pedestrian's frame is CURRENT, convert the pedestrian's position to START. + if a.pose.frame == ObjectFrameEnum.CURRENT: + x2 = a.pose.x + curr_x + y2 = a.pose.y + curr_y + + # Create and run the simulation. + print(f"Vehicle: ({x1:.1f}, {y1:.1f}, ({v1[0]:.1f}, {v1[1]:.1f}))") + print(f"Pedestrian: ({x2:.1f}, {y2:.1f}, ({v2[0]:.1f}, {v2[1]:.1f}))") + + # Simulate if a collision will occur when the vehicle accelerate to desired speed. + sim = CollisionDetector(x1, y1, 0, x2, y2, 0, v1, v2, 10.0, self.desired_speed, self.acceleration) + collision_distance = sim.run() + + # No collision detected with acceleration to desired speed. + if collision_distance < 0: + print("No collision detected.") + output_decel = self.deceleration + output_speed = self.desired_speed + output_dist = collision_distance + + # Collision detected with acceleration to desired speed. + # => Check if the vehicle can yield to the pedestrian with deceleration. + else: + + ############################################### + # # UNCOMMENT NOT TO YIELD: JUST STOP FOR PART1 + # # Decide the deceleration based on the collision distance. + # # To stop perfectly, assume the vehicle is running at the desired speed. + # brake_deceleration = max(self.deceleration, self.desired_speed**2 / (2 * (collision_distance))) + # if brake_deceleration > self.max_deceleration: + # brake_deceleration = self.max_deceleration + # if brake_deceleration > self.deceleration: + # print("The vehicle is Stopping.") + # output_decel = brake_deceleration + # output_dist = collision_distance + # output_speed = 0.0 + # res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n, + # yield_decel=round(output_decel, 2), + # yield_dist=round(output_dist, 2), + # yield_speed=round(output_speed, 2))) + # continue + ############################################### + + print("Collision detected. Try to find yielding speed.") + + collision_distance_after_yield = -1 + + # Simulate with different yield speeds. + # Try to maximize the yield speed to avoid collision. + for v in yield_speed: + # Simulate if the vehicle can yield to the pedestrian with acceleration to yielding speed. + if v > v1[0]: + sim.set_params(v, self.acceleration) + # Simulate if the vehicle can yield to the pedestrian with deceleration to yielding speed. + else: + sim.set_params(v, self.yield_deceleration * -1.0) + collision_distance_after_yield = sim.run() + if collision_distance_after_yield < 0: + print(f"Yielding at speed: {v}") + output_decel = self.yield_deceleration + output_dist = collision_distance + output_speed = v + break + + # Collision detected with any yielding speed. + # => Brake to avoid collision. + if collision_distance_after_yield >= 0: + print("The vehicle is Stopping.") + # Decide the deceleration based on the collision distance. + brake_deceleration = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) + if brake_deceleration > self.max_deceleration: + brake_deceleration = self.max_deceleration + output_decel = brake_deceleration + output_dist = collision_distance + output_speed = 0.0 + should_brake = True + + """ + yield_decel : float # Deceleration at which obj1 yields to obj2 + yield_dist : float # Distance at which obj1 yields to obj2 + yield_speed : float # Speed at which obj1 yields to obj2 + """ + res.append(EntityRelation(type=EntityRelationEnum.YIELDING, obj1='', obj2=n, + yield_decel=round(output_decel, 2), + yield_dist=round(output_dist, 2), + yield_speed=round(output_speed, 2))) + + # res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n)) + return res diff --git a/GEMstack/state/relations.py b/GEMstack/state/relations.py index 465d4e657..c24e1bc22 100644 --- a/GEMstack/state/relations.py +++ b/GEMstack/state/relations.py @@ -23,6 +23,9 @@ class EntityRelation: type : EntityRelationEnum obj1 : str # Named object in the scene. '' indicates ego-vehicle obj2 : str # Named object in the scene. '' indicates ego-vehicle + yield_dist : float # Distance at which obj1 yields to obj2 + yield_speed : float # Speed at which obj1 yields to obj2 + yield_decel : float # Deceleration at which obj1 yields to obj2 class EntityRelationGraph: diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 62dd8f202..239dcebe8 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -15,12 +15,8 @@ drive: agent_detection : pedestrian_detection.PedestrianDetector2D perception_normalization : StandardPerceptionNormalizer planning: - relations_estimation: pedestrian_yield_logic.PedestrianYielder - route_planning: - type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] - motion_planning: - type: longitudinal_planning.YieldTrajectoryPlanner + relations_estimation: + type: pedestrian_yield_logic.PedestrianYielder args: mode: 'real' params: { @@ -28,6 +24,10 @@ drive: 'desired_speed': 1.0, # m/s, 1.5 m/s seems max speed? Feb24 'acceleration': 0.75 # m/s2, 0.5 is not enough to start moving. Feb24 } + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] + motion_planning: longitudinal_planning.YieldTrajectoryPlanner trajectory_tracking: type: pure_pursuit.PurePursuitTrajectoryTracker print: False @@ -106,7 +106,7 @@ variants: agent_detection : OmniscientAgentDetector #this option reads agents from the simulator state_estimation : OmniscientStateEstimator planning: - motion_planning: - type: longitudinal_planning.YieldTrajectoryPlanner + relations_estimation: + type: pedestrian_yield_logic.PedestrianYielder args: - mode: 'simulation' \ No newline at end of file + mode: 'sim' \ No newline at end of file From af020c0c042deae202b417e6790dece1a7edd732 Mon Sep 17 00:00:00 2001 From: Animesh Singh Date: Tue, 25 Feb 2025 10:51:57 -0600 Subject: [PATCH 03/11] Removing conflict flags --- GEMstack/onboard/interface/gem_hardware.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 341096f00..2bac121bc 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -2,10 +2,7 @@ from ...utils import settings import math import time -<<<<<<< HEAD -======= ->>>>>>> s2025 # ROS Headers import rospy from std_msgs.msg import String, Bool, Float32, Float64 From 12663fe432999c3c2d24a65f757cbcaddf51f842 Mon Sep 17 00:00:00 2001 From: joe-hung Date: Tue, 25 Feb 2025 19:49:05 -0600 Subject: [PATCH 04/11] desired speeds and real speeds mismatch issues debugging --- .../onboard/planning/longitudinal_planning.py | 43 ++++++++++++------- 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 21bc4054e..d6573c993 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -211,7 +211,7 @@ def get_minimum_deceleration_for_collision_avoidance(curr_x: float, curr_y: floa # 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) + 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) @@ -256,23 +256,37 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration 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() + # Extrapolation factor for the points + factor = 2.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) + for x, y in zip(xarange, yarange): + new_points.append((x, y)) - #TODO: actually do something to points and times + new_points.append(path.points[-1]) + path = Path(path.frame, new_points) + + path_normalized = path.arc_length_parameterize() points = [p for p in path_normalized.points] times = [t for t in path_normalized.times] #============================================= print("-----LONGITUDINAL PLAN-----") - print("path length: ", path.length()) + #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] @@ -290,7 +304,7 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration new_points.append(cur_point) new_times.append(cur_time) velocities.append(current_speed) - print("=====================================") + # print("=====================================") # print("new points: ", new_points) # print("current index: ", cur_index) # print("current speed: ", current_speed) @@ -304,12 +318,10 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration # print(min_delta_x_stop) 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]: - print("In case one") # put on the breaks # Calculate the next point in a special manner because of too-little time to stop @@ -1010,7 +1022,7 @@ def update(self, state : AllState): #choose whether to accelerate, brake, or keep at current velocity if should_accelerate: - traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, "milestone") + traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, "dt") elif should_yield: desired_speed_squared = -2 * yield_deceleration * r_pedestrain_x + curr_v**2 desired_speed = math.sqrt(max(desired_speed_squared, 0)) @@ -1020,11 +1032,12 @@ def update(self, state : AllState): print('curr_v', curr_v) desired_speed = 0 if desired_speed < 0 else desired_speed # traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) - if desired_speed > 0: - traj = longitudinal_plan(route_with_lookahead, 0.1, yield_deceleration, desired_speed, curr_v, "dt") - else: - traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + # if desired_speed > 0: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, yield_deceleration, desired_speed, curr_v, "milestone") + # else: + # traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + # print('Got brake trajectory') else: - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "milestone") return traj \ No newline at end of file From 47006ee3ed5470144529a4e0e9e17b39cf801631 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 26 Feb 2025 21:09:41 -0600 Subject: [PATCH 05/11] Transfered yield algorithms --- GEMstack/knowledge/defaults/current.yaml | 3 +- GEMstack/onboard/planning/README.md | 50 ++ .../onboard/planning/longitudinal_planning.py | 573 ++------------- .../planning/pedestrian_yield_logic.py | 650 +++++++++--------- launch/pedestrian_detection.yaml | 9 +- testing/test_collision_detection.py | 10 +- .../test_yield_logic_analyic.ipynb | 0 7 files changed, 441 insertions(+), 854 deletions(-) create mode 100644 GEMstack/onboard/planning/README.md rename GEMstack/onboard/planning/testing.ipynb => testing/test_yield_logic_analyic.ipynb (100%) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index b3fb6b13a..eb89bbd9d 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -22,7 +22,8 @@ control: pid_d: 0.0 planning: longitudinal_plan: - mode: 'real' + mode: 'real' # 'real' or 'sim' + yielder: 'expart' # 'expart', 'analytic', or 'simulation' planner: 'dt' # 'milestone', 'dt', or 'dx' desired_speed: 1.0 acceleration: 0.5 diff --git a/GEMstack/onboard/planning/README.md b/GEMstack/onboard/planning/README.md new file mode 100644 index 000000000..8072f2434 --- /dev/null +++ b/GEMstack/onboard/planning/README.md @@ -0,0 +1,50 @@ +# Planning-A HW1 Document + +This document serves as an overview for integrating pedestrian yielder into longitudinal planner. It describes how parameters from `pedetrian_detecion.yaml` are used in the planning system along with references to the `longitudinal_planning.py` script and the `pedestrian_yield_logic.py` script. + +Details on the algorithm and demos are availavle at the link below:
+https://drive.google.com/drive/folders/1aSOWIrqXjf9-j6i1S_MJksATMyyoP31_?usp=sharing + + +## Contribution +Each member contributed to: +### longitudinal_planning.py Contributions + +| Algorithm | Contributor | +| :--------- | :-------------- | +| milestone | Simon (sk106) | +| dt | Rohit (srm17) | +| dx | Hansen (hl58) | + +### pedestrian_yield_logic.py Contributions + +| Algorithm | Contributor | +| :--------- | :------------------------------------------ | +| expert | Patrick (bohaowu2), Animesh (animesh8) | +| analytic | Henry (weigang2) | +| simulation | Yudai (yyamada2) | + + +## Configuration Details +The `pedetrian_detecion.yaml` file includes: +``` +args: + mode: 'real' + params: { + 'yielder': 'expart', # 'expart', 'analytic', or 'simulation' + 'planner': 'milestone', # 'milestone', 'dt', or 'dx' + 'desired_speed': 1.0, # m/s + 'acceleration': 0.75 # m/s2 + } +``` + + +## Usage +### pedetrian_detecion.yaml +Algorithm can be chosen from the above in this file. Also contains parameters for the logic. +### Execution: Simulator +`python3 main.py --variant=fake_sim launch/pedestrian_detection.yaml` +### Testing +- `testing/test_longitudinal_plan.py` +- `testing/test_yield_logic_analytic.ipynb` +- `testing/test_collision_detection.py` diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 38c8dc2c7..1603ae0f1 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -5,253 +5,14 @@ from ...mathutils.transforms import vector_madd from ...mathutils.quadratic_equation import quad_root - 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 - - 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 - - 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) - - 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 - ################################################################################ ########## Longitudinal Planning ############################################### ################################################################################ - 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 @@ -272,7 +33,9 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m else: raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") - +######################## +##### Simon's Code ##### +######################## 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. @@ -537,6 +300,10 @@ def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceler return t1 + +######################## +##### Rohit's Code ##### +######################## def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: if acceleration <= 0 or deceleration <= 0: @@ -696,12 +463,14 @@ def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_spe # print("T FInal ----------------------------", t_final) # points = [path_norm.eval(s) for s in s_vals] - - trajectory = Trajectory(path_norm.frame, points, list(times),velocities=velocities) return trajectory + +######################### +##### Hansen's Code ##### +######################### 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. @@ -949,29 +718,25 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) ########## Yield Trajectory Planner ############################################ ################################################################################ - -########################## -##### Patrick's Code ##### -########################## - +##################################################### +##### Patrick, Animesh, Henry, and Yudai's Code ##### +##################################################### class YieldTrajectoryPlanner(Component): """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, mode : str = 'real', params : dict = {"planner": "dt", "desired_speed": 1.0, "acceleration": 0.5}): + def __init__(self): self.route_progress = None self.t_last = None - self.acceleration = params["acceleration"] - self.desired_speed = params["desired_speed"] - self.deceleration = 2.0 - - self.min_deceleration = 1.0 - self.max_deceleration = 8.0 - self.mode = mode - self.planner = params["planner"] + 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") def state_inputs(self): return ['all'] @@ -983,8 +748,6 @@ def rate(self): return 10.0 def update(self, state : AllState): - start_time = time.time() - vehicle = state.vehicle # type: VehicleState route = state.route # type: Route t = state.t @@ -992,286 +755,58 @@ def update(self, state : AllState): if self.t_last is None: self.t_last = t dt = t - self.t_last - - # Position in vehicle frame (Start (0,0) to (15,0)) + print("Elapsed time:", int(dt*1000), "ms") + self.t_last = t + curr_x = vehicle.pose.x curr_y = vehicle.pose.y curr_v = vehicle.v - abs_x = curr_x + state.start_vehicle_pose.x - abs_y = curr_y + state.start_vehicle_pose.y - - ############################################### - # print("@@@@@ VEHICLE STATE @@@@@") - # print(vehicle) - # print("@@@@@@@@@@@@@@@@@@@@@@@@@") - - if self.mode == 'real': - abs_x = curr_x - abs_y = curr_y - ############################################### - #figure out where we are on the route if self.route_progress is None: self.route_progress = 0.0 closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) self.route_progress = closest_parameter - lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) - route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) + # Compute the lookahead distance + lookahead_distance = max(10, curr_v**2 / (2 * self.yield_deceleration)) print("Lookahead distance:", lookahead_distance) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) - route_to_end = route.trim(closest_parameter, len(route.points) - 1) - - should_yield = False - yield_deceleration = 0.0 - - print("Current Speed: ", curr_v) + # Default values + should_brake = False + input_values = [{"decel": self.deceleration, "desired_speed": self.desired_speed, "collision_distance": lookahead_distance}] + # Extract yielding information from all YIELDING agents. 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) + input_values.append({"decel": r.yield_decel, "desired_speed": r.yield_speed, "collision_distance": r.yield_dist}) + + print("Input values:", input_values) + # Choose minimum desired speed and extract deceleration and collision distance from input_values. + valid_input_values = [val for val in input_values if val['desired_speed'] is not None] + desired_speed = min(valid_input_values, key=lambda x: x['desired_speed'])['desired_speed'] if not None else self.desired_speed + decel = min(valid_input_values, key=lambda x: x['desired_speed'])['decel'] if not None else self.deceleration + # If desired_speed is 0, we should brake to stop. + if desired_speed == 0.0: + should_brake = True + else: + collision_distance = min(valid_input_values, key=lambda x: x['desired_speed'])['collision_distance'] if not None else lookahead_distance + # Update the lookahead distance to pedestrian. + route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) + print(f"Collision distance: {collision_distance:.2f} m") + + print(f"Desired speed: {desired_speed:.2f} m/s") + print(f"Deceleration: {decel:.2f} m/s^2") - should_accelerate = (not should_yield and curr_v < self.desired_speed) + should_accelerate = (not should_brake 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) + traj = longitudinal_plan(route_with_lookahead, self.acceleration, decel, desired_speed, curr_v, self.planner) + elif should_brake: + traj = longitudinal_brake(route_with_lookahead, decel, curr_v) else: - traj = longitudinal_plan(route_to_end, 0.0, self.deceleration, self.desired_speed, curr_v, self.planner) + traj = longitudinal_plan(route_with_lookahead, 0.0, decel, desired_speed, curr_v, self.planner) return traj - -# ######################################################## -# ##### Yudai's Code Using pedestrian_yield_logic.py ##### -# ######################################################## -# class YieldTrajectoryPlanner(Component): -# """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): -# 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") - -# 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 self.t_last is None: -# self.t_last = t -# dt = t - self.t_last -# print("Elapsed time:", int(dt*1000), "ms") -# self.t_last = t - -# curr_x = vehicle.pose.x -# curr_y = vehicle.pose.y -# curr_v = vehicle.v - -# #figure out where we are on the route -# if self.route_progress is None: -# self.route_progress = 0.0 -# closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) -# self.route_progress = closest_parameter - -# lookahead_distance = max(10, curr_v**2 / (2 * self.yield_deceleration)) -# print("Lookahead distance:", lookahead_distance) - -# # Default values -# should_brake = False -# input_values = [{"decel": self.deceleration, "desired_speed": self.desired_speed, "collision_distance": lookahead_distance}] - -# for r in state.relations: -# if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': -# input_values.append({"decel": r.yield_decel, "desired_speed": r.yield_speed, "collision_distance": r.yield_dist}) - -# print("Input values:", input_values) -# # Choose minimum desired speed and extract deceleration and collision distance from input_values. -# desired_speed = min(input_values, key=lambda x: x['desired_speed'])['desired_speed'] -# decel = min(input_values, key=lambda x: x['desired_speed'])['decel'] -# collision_distance = min(input_values, key=lambda x: x['desired_speed'])['collision_distance'] -# if desired_speed == 0.0: -# should_brake = True - -# # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS -# # should_brake = True -# # desired_speed = 0.0 -# # decel = self.deceleration - -# # # UNCOMMENT NOT TO BRAKE -# # should_brake = False -# # desired_speed = self.desired_speed -# # decel = self.deceleration - -# # Update the lookahead distance to pedestrian. -# route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) - -# print(f"Desired speed: {desired_speed:.2f} m/s") -# print(f"Deceleration: {decel:.2f} m/s^2") - -# should_accelerate = (not should_brake and curr_v < self.desired_speed) - -# #choose whether to accelerate, brake, or keep at current velocity -# if should_accelerate: -# traj = longitudinal_plan(route_with_lookahead, self.acceleration, decel, desired_speed, curr_v, self.planner) -# elif should_brake: -# traj = longitudinal_brake(route_with_lookahead, decel, curr_v) -# else: -# traj = longitudinal_plan(route_with_lookahead, 0.0, decel, desired_speed, curr_v, self.planner) - -# return traj - - -# ######################## -# ##### Henry's Code ##### -# ######################## -# class YieldTrajectoryPlanner(Component): -# """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, mode : str = 'real', params : dict = {}): -# self.route_progress = None -# self.t_last = None -# self.acceleration = params["acceleration"] -# self.desired_speed = params["desired_speed"] -# self.deceleration = 2.0 - -# self.min_deceleration = 1.0 -# self.max_deceleration = 8.0 - -# self.mode = mode -# self.planner = params["planner"] - -# 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): -# start_time = time.time() - -# vehicle = state.vehicle # type: VehicleState -# route = state.route # type: Route -# t = state.t - -# if self.t_last is None: -# self.t_last = t -# dt = t - self.t_last - -# # Position in vehicle frame (Start (0,0) to (15,0)) -# curr_x = vehicle.pose.x -# curr_y = vehicle.pose.y -# curr_v = vehicle.v - -# abs_x = curr_x + state.start_vehicle_pose.x -# abs_y = curr_y + state.start_vehicle_pose.y - - # ############################################### - # # print("@@@@@ VEHICLE STATE @@@@@") - # # print(vehicle) - # # print("@@@@@@@@@@@@@@@@@@@@@@@@@") - - # if self.mode == 'real': - # # Position in vehicle frame (Start (0,0) to (15,0)) - # # curr_x = vehicle.pose.x * 20 - # # curr_y = vehicle.pose.y * 20 - # # print("@@@@@ PLAN", curr_x, curr_y, curr_v) - # abs_x = curr_x - # abs_y = curr_y - # # print("@@@@@ PLAN", abs_x, abs_y) - # ############################################### - -# #figure out where we are on the route -# if self.route_progress is None: -# self.route_progress = 0.0 -# closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) -# self.route_progress = closest_parameter - -# lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) -# route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) -# print("Lookahead distance:", lookahead_distance) - -# route_to_end = route.trim(closest_parameter, len(route.points) - 1) - -# should_yield = False -# yield_deceleration = 0.0 - -# print("Current Speed: ", curr_v) - -# 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] - -# deceleration, r_pedestrain_x = get_minimum_deceleration_for_collision_avoidance(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) -# print("deceleration: ", deceleration) -# if deceleration > 0: -# yield_deceleration = max(deceleration, yield_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_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, self.planner) -# elif should_yield: -# desired_speed = math.sqrt(-2 * yield_deceleration * r_pedestrain_x + curr_v**2) -# desired_speed = max(desired_speed, 0) -# # traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) -# if desired_speed > 0: -# traj = longitudinal_plan(route_with_lookahead, 0, yield_deceleration, desired_speed, curr_v, self.planner) -# else: -# traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) -# else: -# traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, self.planner) - -# return traj diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py index 9c223b8c5..ba86314af 100644 --- a/GEMstack/onboard/planning/pedestrian_yield_logic.py +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -12,6 +12,221 @@ ########## Collisiong Detection ################################################ ################################################################################ +###################################### +##### Patrick and Animesh's Code ##### +###################################### +# 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 +COMFORT_DECELERATION = 1.5 + +def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj_x: float, obj_y: float, obj_v_x: float, obj_v_y: float, 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.""" + + 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 + + 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] + +######################## +##### Henry's Code ##### +######################## +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_x: float, obj_y: float, obj_v_x: float, obj_v_y: float, 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""" + + 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 + + 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) + + 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 + +######################## +##### Yudai's Code ##### +######################## class CollisionDetector: """ Simulation class to update positions of two rectangles (vehicle and pedestrian) @@ -214,251 +429,78 @@ def run(self, is_displayed=False): return collision_distance +def calculate_yielding_parameters(curr_x, curr_y, curr_v, a_x, a_y, a_v, desired_speed, acceleration, deceleration, max_deceleration, yield_deceleration): + """ + Calculate yielding parameters using Yudai's simulation code. + Returns a tuple (output_decel, output_dist, output_speed). + """ + # Simulate if a collision will occur when the vehicle accelerates to the desired speed. + sim = CollisionDetector(curr_x, curr_y, 0, a_x, a_y, 0, curr_v, a_v, total_time=10.0, desired_speed=desired_speed, acceleration=acceleration) + collision_distance = sim.run() -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, 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_x = 0.0 - obj_y = obj.pose.y - obj_v_x = obj.velocity[0] - obj_v_y = obj.velocity[1] - - 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 - - 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 - - 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_y - - # if curr_v**2/2*distance_to_pass >= 1.5: - # return True, curr_v**2/2*distance_to_pass - time_to_max_v = (5 - curr_v)/0.5 + # No collision detected: use default deceleration and desired speed. + if collision_distance < 0: + print("No collision detected.") + output_decel = deceleration + output_speed = desired_speed + output_dist = collision_distance - if time_to_max_v > time_to_pass: - if curr_v*time_to_pass + 0.25*time_to_pass**2 > distance_to_move: - return False, 0.0 + # Collision detected: try to find a yielding speed. else: - if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: - return False, 0.0 - - - return True, [distance_to_move, time_to_pass] - - + print("Collision detected. Try to find yielding speed.") + output_decel = None + output_speed = None + collision_distance_after_yield = -1 + + # Generate yielding speeds from desired speed down to a low speed. + yield_speed = [v for v in np.arange(desired_speed, 0.1, -0.25)] + for v in yield_speed: + # If trying to accelerate (v > current vehicle speed) + if v > curr_v[0]: + sim.set_params(v, acceleration) + # Otherwise apply deceleration to yield + else: + sim.set_params(v, yield_deceleration * -1.0) + collision_distance_after_yield = sim.run() + if collision_distance_after_yield < 0: + print(f"Yielding at speed: {v}") + output_decel = yield_deceleration + output_dist = collision_distance + output_speed = v + break -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 + # Collision detected for any yielding speed: brake to avoid collision. + if collision_distance_after_yield >= 0: + print("The vehicle is Stopping.") + brake_deceleration = max(deceleration, curr_v[0]**2 / (2 * (collision_distance))) + if brake_deceleration > max_deceleration: + brake_deceleration = max_deceleration + output_decel = brake_deceleration + output_dist = collision_distance + output_speed = 0.0 + return output_decel, output_dist, output_speed -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] - vehicle_length = 3 - vehicle_width = 2 - vehicle_buffer_x = 3.0 - vehicle_buffer_y = 1.0 - 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 - 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) - yaw = None - minimum_deceleration = None - if yaw is 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 pestrain, - # 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) - else: - pass - print("calculatedminimum_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 - ################################################################################ ########## Pedestrian Yielder ################################################## ################################################################################ - class PedestrianYielder(Component): """Yields for all pedestrians in the scene. @@ -467,12 +509,14 @@ class PedestrianYielder(Component): def __init__(self, mode : str = 'real', params : dict = {}): # Planner mode self.mode = mode + self.yielder = params["yielder"] self.planner = params["planner"] self.acceleration = params["acceleration"] self.desired_speed = params["desired_speed"] - # Update current.yaml settings with the given parameters in yaml + # Update current.yaml settings with the given parameters in pedestrian_detection.yaml settings.set("planning.longitudinal_plan.mode", self.mode) + settings.set("planning.longitudinal_plan.yielder", self.yielder) settings.set("planning.longitudinal_plan.planner", self.planner) settings.set("planning.longitudinal_plan.acceleration", self.acceleration) settings.set("planning.longitudinal_plan.desired_speed", self.desired_speed) @@ -497,120 +541,80 @@ def update(self, state : AllState) -> List[EntityRelation]: curr_x = vehicle.pose.x curr_y = vehicle.pose.y curr_v = vehicle.v - - # Yielding speed [..., 1.0, 0.8, ..., 0.2] - yield_speed = [v for v in np.arange(self.desired_speed, 0.1, -0.25)] for n,a in agents.items(): if a.type == AgentEnum.PEDESTRIAN: - - ##### Henry's Code ##### - # output_decel, output_dist = get_minimum_deceleration_for_collision_avoidance(curr_x, curr_y, curr_v, a, self.min_deceleration, self.max_deceleration) - # output_speed = None - - ##### Yudai's Code ##### - x1, y1 = curr_x, curr_y - v1 = [curr_v, 0] # Vehicle speed vector - x2, y2 = a.pose.x, a.pose.y - v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector """ - class ObjectFrameEnum(Enum): - START = 0 #position / yaw in m / radians relative to starting pose of vehicle - CURRENT = 1 #position / yaw in m / radians relative to current pose of vehicle - GLOBAL = 2 #position in longitude / latitude, yaw=heading in radians with respect to true north (used in GNSS) - ABSOLUTE_CARTESIAN = 3 #position / yaw in m / radians relative to a global cartesian reference frame (used in simulation) + yield_decel : float # Deceleration at which obj1 yields to obj2 + yield_dist : float # Distance at which obj1 yields to obj2 + yield_speed : float # Speed at which obj1 yields to obj2 """ - # If the pedestrian's frame is ABSOLUTE, convert the pedestrian's position to START. - if a.pose.frame == ObjectFrameEnum.ABSOLUTE_CARTESIAN: - x1 = curr_x + state.start_vehicle_pose.x - y1 = curr_y + state.start_vehicle_pose.y + output_decel, output_dist, output_speed = None, None, None - # If the pedestrian's frame is CURRENT, convert the pedestrian's position to START. - if a.pose.frame == ObjectFrameEnum.CURRENT: - x2 = a.pose.x + curr_x - y2 = a.pose.y + curr_y + # Get the pedestrian's position and velocity + a_x, a_y = a.pose.x, a.pose.y + a_v_x, a_v_y = a.velocity[0], a.velocity[1] # Pedestrian speed vector - # Create and run the simulation. - print(f"Vehicle: ({x1:.1f}, {y1:.1f}, ({v1[0]:.1f}, {v1[1]:.1f}))") - print(f"Pedestrian: ({x2:.1f}, {y2:.1f}, ({v2[0]:.1f}, {v2[1]:.1f}))") + # If the pedestrian's frame is ABSOLUTE, convert the vehicle's frame to ABSOLUTE. + if a.pose.frame == ObjectFrameEnum.ABSOLUTE_CARTESIAN: + curr_x = curr_x + state.start_vehicle_pose.x + curr_y = curr_y + state.start_vehicle_pose.y + + # If the pedestrian's frame is CURRENT, convert the pedestrian's frame to START. + elif a.pose.frame == ObjectFrameEnum.CURRENT: + a_x = a.pose.x + curr_x + a_y = a.pose.y + curr_y + a_v_x = a_v_x - curr_v + + ########################## + ##### Yielding Part ###### + ########################## + + # Switch between different yielder methods + if self.yielder == 'expart': + ###################################### + ##### Patrick and Animesh's Code ##### + ###################################### + detected, deceleration = detect_collision(curr_x, curr_y, curr_v, a_x, a_y, a_v_x, a_v_y, 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 + output_speed = (-b + (b**2 - 4*c)**0.5)/2 + output_decel = 1.5 + output_dist = distance_collision + else: + if detected and deceleration > 0: + output_speed = 0 # Brake to Stop + output_decel = deceleration + output_dist = None + + elif self.yielder == 'analytic': + ######################### + ##### Henry's Code ###### + ######################### + deceleration, output_dist = get_minimum_deceleration_for_collision_avoidance(curr_x, curr_y, curr_v, a_x, a_y, a_v_x, a_v_y, self.min_deceleration, self.max_deceleration) + if deceleration > 0: + output_decel = deceleration + output_speed = math.sqrt(-2 * output_decel * output_dist + curr_v**2) + output_speed = max(output_speed, 0) + + elif self.yielder == 'simulation': + ######################## + ##### Yudai's Code ##### + ######################## + output_decel, output_dist, output_speed = calculate_yielding_parameters(curr_x, curr_y, [curr_v, 0], a_x, a_y, [a_v_x, a_v_y], self.desired_speed, self.acceleration, self.deceleration, self.max_deceleration, self.yield_deceleration) - # Simulate if a collision will occur when the vehicle accelerate to desired speed. - sim = CollisionDetector(x1, y1, 0, x2, y2, 0, v1, v2, 10.0, self.desired_speed, self.acceleration) - collision_distance = sim.run() - - # No collision detected with acceleration to desired speed. - if collision_distance < 0: - print("No collision detected.") - output_decel = self.deceleration - output_speed = self.desired_speed - output_dist = collision_distance - - # Collision detected with acceleration to desired speed. - # => Check if the vehicle can yield to the pedestrian with deceleration. else: + raise ValueError(f"Yielder {self.yielder} is not supported.") - ############################################### - # # UNCOMMENT NOT TO YIELD: JUST STOP FOR PART1 - # # Decide the deceleration based on the collision distance. - # # To stop perfectly, assume the vehicle is running at the desired speed. - # brake_deceleration = max(self.deceleration, self.desired_speed**2 / (2 * (collision_distance))) - # if brake_deceleration > self.max_deceleration: - # brake_deceleration = self.max_deceleration - # if brake_deceleration > self.deceleration: - # print("The vehicle is Stopping.") - # output_decel = brake_deceleration - # output_dist = collision_distance - # output_speed = 0.0 - # res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n, - # yield_decel=round(output_decel, 2), - # yield_dist=round(output_dist, 2), - # yield_speed=round(output_speed, 2))) - # continue - ############################################### - - print("Collision detected. Try to find yielding speed.") - - collision_distance_after_yield = -1 - - # Simulate with different yield speeds. - # Try to maximize the yield speed to avoid collision. - for v in yield_speed: - # Simulate if the vehicle can yield to the pedestrian with acceleration to yielding speed. - if v > v1[0]: - sim.set_params(v, self.acceleration) - # Simulate if the vehicle can yield to the pedestrian with deceleration to yielding speed. - else: - sim.set_params(v, self.yield_deceleration * -1.0) - collision_distance_after_yield = sim.run() - if collision_distance_after_yield < 0: - print(f"Yielding at speed: {v}") - output_decel = self.yield_deceleration - output_dist = collision_distance - output_speed = v - break - - # Collision detected with any yielding speed. - # => Brake to avoid collision. - if collision_distance_after_yield >= 0: - print("The vehicle is Stopping.") - # Decide the deceleration based on the collision distance. - brake_deceleration = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) - if brake_deceleration > self.max_deceleration: - brake_deceleration = self.max_deceleration - output_decel = brake_deceleration - output_dist = collision_distance - output_speed = 0.0 - should_brake = True - - """ - yield_decel : float # Deceleration at which obj1 yields to obj2 - yield_dist : float # Distance at which obj1 yields to obj2 - yield_speed : float # Speed at which obj1 yields to obj2 - """ + # Add the relation to the list res.append(EntityRelation(type=EntityRelationEnum.YIELDING, obj1='', obj2=n, - yield_decel=round(output_decel, 2), - yield_dist=round(output_dist, 2), - yield_speed=round(output_speed, 2))) + yield_decel=output_decel, + yield_dist=output_dist, + yield_speed=output_speed)) # res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n)) diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index ac1e3227d..27e31a212 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -20,7 +20,8 @@ drive: args: mode: 'real' params: { - 'planner': 'dt', # 'milestone', 'dt', or 'dx' + 'yielder': 'expart', # 'expart', 'analytic', or 'simulation' + 'planner': 'milestone', # 'milestone', 'dt', or 'dx' 'desired_speed': 1.0, # m/s, 1.5 m/s seems max speed? Feb24 'acceleration': 0.75 # m/s2, 0.5 is not enough to start moving. Feb24 } @@ -84,7 +85,11 @@ variants: drive: perception: state_estimation : OmniscientStateEstimator - + planning: + relations_estimation: + type: pedestrian_yield_logic.PedestrianYielder + args: + mode: 'sim' fake_real: run: diff --git a/testing/test_collision_detection.py b/testing/test_collision_detection.py index e4e1fe8ab..cf0f45ea1 100644 --- a/testing/test_collision_detection.py +++ b/testing/test_collision_detection.py @@ -4,18 +4,10 @@ sys.path.append(os.getcwd()) import time -from typing import List, Tuple, Union -from ..component import Component -from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState -from ...utils import serialization -from ...mathutils.transforms import vector_madd - -import time import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches import math -from scipy.optimize import minimize ################################################################################ @@ -230,7 +222,7 @@ def run(self, is_displayed=False): # Pedestrian parameters. x, y, theta (angle in radians) x2, y2, t2 = 15.0, 2.1, 0 # Velocity vectors: [vx, vy] - v1 = [0.1, 0] # Vehicle speed vector + v1 = [1.0, 0] # Vehicle speed vector v2 = [0, 0.5] # Pedestrian speed vector # Total simulation time total_time = 10.0 diff --git a/GEMstack/onboard/planning/testing.ipynb b/testing/test_yield_logic_analyic.ipynb similarity index 100% rename from GEMstack/onboard/planning/testing.ipynb rename to testing/test_yield_logic_analyic.ipynb From a4a43c39fe174ded98093baa12af9848e1865519 Mon Sep 17 00:00:00 2001 From: Simon Date: Wed, 26 Feb 2025 21:30:31 -0600 Subject: [PATCH 06/11] Cleaned up comments --- GEMstack/mathutils/quadratic_equation.py | 10 +++++++ .../onboard/planning/longitudinal_planning.py | 28 +++---------------- 2 files changed, 14 insertions(+), 24 deletions(-) diff --git a/GEMstack/mathutils/quadratic_equation.py b/GEMstack/mathutils/quadratic_equation.py index e229b63a9..6580fbec9 100644 --- a/GEMstack/mathutils/quadratic_equation.py +++ b/GEMstack/mathutils/quadratic_equation.py @@ -1,6 +1,16 @@ import math def quad_root(a : float, b : float, c : float) -> float: + """Calculates the roots of a quadratic equation + + Args: + a (float): coefficient of x^2 + b (float): coefficient of x + c (float): constant term + + Returns: + float: returns all valid roots of the quadratic equation + """ x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 1603ae0f1..3ba8e9f6b 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -59,12 +59,10 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration 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) path_normalized = path.arc_length_parameterize() @@ -92,27 +90,20 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration 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" + # to the new points. However, we also want to add "switch 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) - # print("current position: ", cur_point) # Information we will need: - # Calculate how much time it would take to stop - # Calculate how much distance it would take to stop + # 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 # print(min_delta_x_stop) 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 @@ -147,7 +138,6 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration # 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") # next point next_point = points[cur_index+1] # accelerate to max speed @@ -168,7 +158,7 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration # 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") + # 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 @@ -179,13 +169,8 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration # 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]: - # print(delta_x_to_max_speed) - # print(delta_x_to_stop_from_max_speed) # Add a new point at the point where we should start breaking - # print("Adding new point to start breaking") delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) - # print(delta_t_to_next_x) - #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) 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] @@ -194,7 +179,6 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration # 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] @@ -207,17 +191,14 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration 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 @@ -228,7 +209,6 @@ def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration # We need to hit the breaks next_point = points[cur_index+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 From fd76adb9f89de28dbb9b2885b19e56c1ac2fe750 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Wed, 26 Feb 2025 21:53:20 -0600 Subject: [PATCH 07/11] some code cleanup --- GEMstack/onboard/planning/pedestrian_yield_logic.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py index ba86314af..419845d4a 100644 --- a/GEMstack/onboard/planning/pedestrian_yield_logic.py +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -29,7 +29,7 @@ COMFORT_DECELERATION = 1.5 def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj_x: float, obj_y: float, obj_v_x: float, obj_v_y: float, 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.""" + """Detects if a collision will occur with the given object and return deceleration to avoid it or info for computing cruising speed""" vehicle_front = curr_x + VEHICLE_LENGTH vehicle_back = curr_x @@ -88,8 +88,6 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj_x: float, 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) From d922b49575825602022f8b2d44d0e9dde635d7c9 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 26 Feb 2025 21:54:58 -0600 Subject: [PATCH 08/11] Fix typo --- GEMstack/knowledge/defaults/current.yaml | 2 +- GEMstack/onboard/planning/README.md | 2 +- GEMstack/onboard/planning/pedestrian_yield_logic.py | 2 +- launch/pedestrian_detection.yaml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index eb89bbd9d..1a947ef54 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -23,7 +23,7 @@ control: planning: longitudinal_plan: mode: 'real' # 'real' or 'sim' - yielder: 'expart' # 'expart', 'analytic', or 'simulation' + yielder: 'expert' # 'expert', 'analytic', or 'simulation' planner: 'dt' # 'milestone', 'dt', or 'dx' desired_speed: 1.0 acceleration: 0.5 diff --git a/GEMstack/onboard/planning/README.md b/GEMstack/onboard/planning/README.md index 8072f2434..5efb4fced 100644 --- a/GEMstack/onboard/planning/README.md +++ b/GEMstack/onboard/planning/README.md @@ -31,7 +31,7 @@ The `pedetrian_detecion.yaml` file includes: args: mode: 'real' params: { - 'yielder': 'expart', # 'expart', 'analytic', or 'simulation' + 'yielder': 'expert', # 'expert', 'analytic', or 'simulation' 'planner': 'milestone', # 'milestone', 'dt', or 'dx' 'desired_speed': 1.0, # m/s 'acceleration': 0.75 # m/s2 diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py index ba86314af..0bc28754d 100644 --- a/GEMstack/onboard/planning/pedestrian_yield_logic.py +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -571,7 +571,7 @@ def update(self, state : AllState) -> List[EntityRelation]: ########################## # Switch between different yielder methods - if self.yielder == 'expart': + if self.yielder == 'expert': ###################################### ##### Patrick and Animesh's Code ##### ###################################### diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 27e31a212..4b59cc3a3 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -20,7 +20,7 @@ drive: args: mode: 'real' params: { - 'yielder': 'expart', # 'expart', 'analytic', or 'simulation' + 'yielder': 'expert', # 'expert', 'analytic', or 'simulation' 'planner': 'milestone', # 'milestone', 'dt', or 'dx' 'desired_speed': 1.0, # m/s, 1.5 m/s seems max speed? Feb24 'acceleration': 0.75 # m/s2, 0.5 is not enough to start moving. Feb24 From f2029d5e2545df797e117cb05a163e151e0335c9 Mon Sep 17 00:00:00 2001 From: Henry-YiW Date: Wed, 26 Feb 2025 22:10:10 -0600 Subject: [PATCH 09/11] codes cleaned --- .../onboard/planning/longitudinal_planning.py | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index d6573c993..aed1daf6a 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -126,11 +126,11 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat return True, deceleration -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]: +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, longitudinal_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 'maintain' + elif r_pedestrain_x < 0 and abs(r_pedestrain_x) < longitudinal_buffer: return 'max' if r_pedestrain_y >= p_vehicle_left_y_after_t and r_pedestrain_y <= p_vehicle_right_y_after_t: return True @@ -185,10 +185,12 @@ def get_minimum_deceleration_for_collision_avoidance(curr_x: float, curr_y: floa 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 - 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) + 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, vehicle_buffer_x) 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 == 'maintain': + return -1.0, r_pedestrain_x elif collision_flag == 'max': return max_deceleration, r_pedestrain_x @@ -1015,6 +1017,9 @@ def update(self, state : AllState): if deceleration > 0: yield_deceleration = max(deceleration, yield_deceleration) should_yield = True + elif deceleration < 0: + should_accelerate = False + should_yield = False print("should yield: ", should_yield) @@ -1033,11 +1038,11 @@ def update(self, state : AllState): desired_speed = 0 if desired_speed < 0 else desired_speed # traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) # if desired_speed > 0: - traj = longitudinal_plan(route_with_lookahead, self.acceleration, yield_deceleration, desired_speed, curr_v, "milestone") + # traj = longitudinal_plan(route_with_lookahead, self.acceleration, yield_deceleration, desired_speed, curr_v, "milestone") # else: - # traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) # print('Got brake trajectory') else: - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "milestone") + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") return traj \ No newline at end of file From 97f7521b012b5412dde65a10c892dcafa7b16457 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 26 Feb 2025 22:22:49 -0600 Subject: [PATCH 10/11] Clean dt code --- .../onboard/planning/longitudinal_planning.py | 55 +------------------ 1 file changed, 1 insertion(+), 54 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 3ba8e9f6b..f2657c67d 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -389,60 +389,7 @@ def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_spe # Compute trajectory points points = [path_norm.eval(s) for s in s_vals] - print("Number of time steps is --------------------", num_time_steps) - - # return Trajectory(path_norm.frame, points, times) - - - # # Plot: update a single window - # import matplotlib.pyplot as plt - # plt.figure("Distance vs Time") - # plt.clf() # Clear the current figure - # plt.plot(times, s_vals) - # plt.xlabel("Time (s)") - # plt.ylabel("Distance (m)") - # plt.title("Distance vs Time") - # plt.draw() - # plt.pause(0.001) - - - - # 4. Create a time grid. - # dt = 0.1 # adjust based on computation - # times = np.arange(0, t_final + dt, dt) - # num_time_steps = 0 - - # # 5. Compute the distance s(t) for each time step. - # s_vals = [] - # for t in times: - # if profile_type == "trapezoidal": - # if t < t_accel: - # # Acceleration phase. - # s = current_speed * t + 0.5 * acceleration * t**2 - # 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) - # # Compute the remaining distance using the deceleration equation. - # s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 - # else: # Triangular profile. - # if t < t_accel: - # # Acceleration phase. - # s = current_speed * t + 0.5 * acceleration * t**2 - # 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 - # num_time_steps +=1 - - # # should not exceed total path length - # s_vals.append(min(s, total_length)) - # print("NUmber of time steps -----------",num_time_steps) - # print("T FInal ----------------------------", t_final) - # points = [path_norm.eval(s) for s in s_vals] - + trajectory = Trajectory(path_norm.frame, points, list(times),velocities=velocities) return trajectory From 40743a6e736ba99e526800d6d93dc54e955774a8 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Wed, 26 Feb 2025 23:13:57 -0600 Subject: [PATCH 11/11] Update pedestrian_yield_logic.py --- .../planning/pedestrian_yield_logic.py | 94 ++++++++++--------- 1 file changed, 50 insertions(+), 44 deletions(-) diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py index 66b0714ba..4f1936fa2 100644 --- a/GEMstack/onboard/planning/pedestrian_yield_logic.py +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -139,6 +139,7 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj_x: float, ######################## ##### Henry's Code ##### ######################## + 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: @@ -154,21 +155,27 @@ def detect_collision_analytical(r_pedestrain_x: float, r_pedestrain_y: float, p_ def get_minimum_deceleration_for_collision_avoidance(curr_x: float, curr_y: float, curr_v: float, obj_x: float, obj_y: float, obj_v_x: float, obj_v_y: float, 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""" + vehicle_length = 3 + vehicle_width = 2 + + vehicle_buffer_x = 3.0 + vehicle_buffer_y = 1.0 + 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_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 + 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_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 @@ -185,42 +192,43 @@ def get_minimum_deceleration_for_collision_avoidance(curr_x: float, curr_y: floa 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 - 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) + 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 + return 0.0 elif collision_flag == 'max': - return max_deceleration, r_pedestrain_x + return max_deceleration 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) - + yaw = None 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 + if yaw is 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 pestrain, + # 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) else: - return max(min(minimum_deceleration, max_deceleration), min_deceleration), r_pedestrain_x + pass + + print("calculatedminimum_deceleration: ", minimum_deceleration) + return max(min(minimum_deceleration, max_deceleration), min_deceleration) + ######################## ##### Yudai's Code ##### @@ -240,10 +248,10 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, desired_spee self.pedestrian_y = y2 # Vehicle parameters with buffer adjustments - self.vehicle_size_x = 3.2 - self.vehicle_size_y = 1.7 - self.vehicle_buffer_x = 3.0 + 1.0 - self.vehicle_buffer_y = 2.0 + self.vehicle_size_x = VEHICLE_LENGTH + self.vehicle_size_y = VEHICLE_WIDTH + self.vehicle_buffer_x = VEHICLE_BUFFER_X + self.vehicle_buffer_y = VEHICLE_BUFFER_Y * 2.0 # Double the buffer for both sides # Vehicle rectangle self.x1 = self.vehicle_x + (self.vehicle_size_x + self.vehicle_buffer_x)*0.5 # Offset for buffer (remains constant) @@ -593,12 +601,12 @@ def update(self, state : AllState) -> List[EntityRelation]: ######################### ##### Henry's Code ###### ######################### - deceleration, output_dist = get_minimum_deceleration_for_collision_avoidance(curr_x, curr_y, curr_v, a_x, a_y, a_v_x, a_v_y, self.min_deceleration, self.max_deceleration) + deceleration = get_minimum_deceleration_for_collision_avoidance(curr_x, curr_y, curr_v, a_x, a_y, a_v_x, a_v_y, self.min_deceleration, self.max_deceleration) if deceleration > 0: output_decel = deceleration - output_speed = math.sqrt(-2 * output_decel * output_dist + curr_v**2) - output_speed = max(output_speed, 0) - + output_speed = 0 + output_dist = None + elif self.yielder == 'simulation': ######################## ##### Yudai's Code ##### @@ -614,6 +622,4 @@ def update(self, state : AllState) -> List[EntityRelation]: yield_dist=output_dist, yield_speed=output_speed)) - # res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n)) - return res