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 158bff41c..1a947ef54 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -20,6 +20,18 @@ control: pid_p: 0.8 pid_i: 0.03 pid_d: 0.0 +planning: + longitudinal_plan: + mode: 'real' # 'real' or 'sim' + yielder: 'expert' # 'expert', 'analytic', or 'simulation' + 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/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/README.md b/GEMstack/onboard/planning/README.md new file mode 100644 index 000000000..5efb4fced --- /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': 'expert', # 'expert', '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 b45d3eb74..f2657c67d 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1,258 +1,18 @@ 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 from ...mathutils.quadratic_equation import quad_root - -import time import numpy as np -import matplotlib.pyplot as plt -import matplotlib.patches as patches import math -from scipy.optimize import minimize - - -# Global variables -PEDESTRIAN_LENGTH = 0.5 -PEDESTRIAN_WIDTH = 0.5 - -VEHICLE_LENGTH = 3.5 -VEHICLE_WIDTH = 2 - -VEHICLE_BUFFER_X = 3.0 -VEHICLE_BUFFER_Y = 1.5 - -YIELD_BUFFER_Y = 1.0 -V_MAX = 5 -COMFORT_DECELERATION = 1.5 - - -def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float, acceleration: float, max_speed: float) -> Tuple[bool, Union[float, List[float]]]: - """Detects if a collision will occur with the given object and return deceleration to avoid it.""" - - # Get the object's position and velocity - obj_x = obj.pose.x - obj_y = obj.pose.y - obj_v_x = obj.velocity[0] - obj_v_y = obj.velocity[1] - - if obj.pose.frame == ObjectFrameEnum.CURRENT: - # Simulation: Current - obj_x = obj.pose.x + curr_x - obj_y = obj.pose.y + curr_y - print("PEDESTRIAN", obj_x, obj_y) - - - vehicle_front = curr_x + VEHICLE_LENGTH - vehicle_back = curr_x - vehicle_left = curr_y + VEHICLE_WIDTH / 2 - vehicle_right = curr_y - VEHICLE_WIDTH / 2 - - pedestrian_front = obj_x + PEDESTRIAN_LENGTH / 2 - pedestrian_back = obj_x - PEDESTRIAN_LENGTH / 2 - pedestrian_left = obj_y + PEDESTRIAN_WIDTH / 2 - pedestrian_right = obj_y - PEDESTRIAN_WIDTH / 2 - - # Check if the object is in front of the vehicle - if vehicle_front > pedestrian_back: - if vehicle_back > pedestrian_front: - # The object is behind the vehicle - print("Object is behind the vehicle") - return False, 0.0 - if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left or vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right: - # The object is to the side of the vehicle - print("Object is to the side of the vehicle") - return False, 0.0 - # The object overlaps with the vehicle's buffer - return True, max_deceleration - - if vehicle_right - VEHICLE_BUFFER_Y > pedestrian_left and obj_v_y <= 0: - # The object is to the right of the vehicle and moving away - print("Object is to the right of the vehicle and moving away") - return False, 0.0 - - if vehicle_left + VEHICLE_BUFFER_Y < pedestrian_right and obj_v_y >= 0: - # The object is to the left of the vehicle and moving away - print("Object is to the left of the vehicle and moving away") - return False, 0.0 - - if vehicle_front + VEHICLE_BUFFER_X >= pedestrian_back and (vehicle_right - VEHICLE_BUFFER_Y <= pedestrian_left and vehicle_left + VEHICLE_BUFFER_Y >= pedestrian_right): - # The object is in front of the vehicle and within the buffer - print("Object is in front of the vehicle and within the buffer") - return True, max_deceleration - - # Calculate the deceleration needed to avoid a collision - print("Object is in front of the vehicle and outside the buffer") - distance = pedestrian_back - vehicle_front - distance_with_buffer = pedestrian_back - vehicle_front - VEHICLE_BUFFER_X - - relative_v = curr_v - obj_v_x - if relative_v <= 0: - return False, 0.0 - - if obj_v_y == 0: - # The object is in front of the vehicle blocking it - deceleration = relative_v ** 2 / (2 * distance_with_buffer) - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: - return False, 0.0 - - return True, deceleration - - print(relative_v, distance_with_buffer) - - if obj_v_y > 0: - # The object is to the right of the vehicle and moving towards it - time_to_get_close = (vehicle_right - VEHICLE_BUFFER_Y - YIELD_BUFFER_Y - pedestrian_left) / abs(obj_v_y) - time_to_pass = (vehicle_left + VEHICLE_BUFFER_Y + YIELD_BUFFER_Y - pedestrian_right) / abs(obj_v_y) - else: - # The object is to the left of the vehicle and moving towards it - time_to_get_close = (pedestrian_right - vehicle_left - VEHICLE_BUFFER_Y - YIELD_BUFFER_Y) / abs(obj_v_y) - time_to_pass = (pedestrian_left - vehicle_right + VEHICLE_BUFFER_Y + YIELD_BUFFER_Y) / abs(obj_v_y) - - time_to_accel_to_max_speed = (max_speed - curr_v) / acceleration - distance_to_accel_to_max_speed = (max_speed + curr_v - 2 * obj_v_x) * time_to_accel_to_max_speed / 2 #area of trapezoid - - if distance_to_accel_to_max_speed > distance_with_buffer: - # The object will reach the buffer before reaching max speed - time_to_buffer_when_accel = (-relative_v + (relative_v * relative_v + 2 * distance_with_buffer * acceleration) ** 0.5) / acceleration - else: - # The object will reach the buffer after reaching max speed - time_to_buffer_when_accel = time_to_accel_to_max_speed + (distance_with_buffer - distance_to_accel_to_max_speed) / (max_speed - obj_v_x) - - if distance_to_accel_to_max_speed > distance: - # We will collide before reaching max speed - time_to_collide_when_accel = (-relative_v + (relative_v * relative_v + 2 * distance * acceleration) ** 0.5) / acceleration - else: - # We will collide after reaching max speed - time_to_collide_when_accel = time_to_accel_to_max_speed + (distance - distance_to_accel_to_max_speed) / (max_speed - obj_v_x) - - if time_to_get_close > time_to_collide_when_accel: - # We can do normal driving and will pass the object before it gets in our way - print("We can do normal driving and will pass the object before it gets in our way") - return False, 0.0 - - 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 @@ -273,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. @@ -297,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() @@ -330,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 @@ -385,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 @@ -406,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 @@ -417,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] @@ -432,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] @@ -445,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 @@ -466,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 @@ -538,6 +280,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: @@ -643,66 +389,15 @@ 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 + +######################### +##### 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. @@ -950,29 +645,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'] @@ -984,8 +675,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 @@ -993,445 +682,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") - should_accelerate = (not should_yield and curr_v < self.desired_speed) + 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_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 ##### -# ######################## - -# 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"] - -# # 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 - -# # Planner mode -# 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 - - -# 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 - -# ############################################### -# # # TODO: Fix the coordinate conversion of other files - -# # 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.yield_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) - - -# # Default values -# should_brake = False -# input_values = [{"decel": self.deceleration, "desired_speed": self.desired_speed, "collision_distance": lookahead_distance}] - -# print(state.relations) - -# 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") -# #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) - -# print(f"Simulation took {time.time() - start_time:.3f} seconds.") - -# 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 2567c0093..4f1936fa2 100644 --- a/GEMstack/onboard/planning/pedestrian_yield_logic.py +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -1,22 +1,625 @@ -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 ################################################ +################################################################################ + +###################################### +##### 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 or info for computing cruising speed""" + + 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 + + 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""" + + 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 + elif collision_flag == 'max': + 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 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) + return max(min(minimum_deceleration, max_deceleration), min_deceleration) + + +######################## +##### Yudai's Code ##### +######################## +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 = 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) + 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 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() + + # 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 + + # Collision detected: try to find a yielding speed. + else: + 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 + + # 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 + + + + + + + + + + + + + + + + +################################################################################ +########## 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.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 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) + + 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 + 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)) + """ + 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 + """ + output_decel, output_dist, output_speed = None, None, None + + # 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 + + # 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 == 'expert': + ###################################### + ##### 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 = 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 = 0 + output_dist = None + + 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) + + else: + raise ValueError(f"Yielder {self.yielder} is not supported.") + + # Add the relation to the list + res.append(EntityRelation(type=EntityRelationEnum.YIELDING, obj1='', obj2=n, + yield_decel=output_decel, + yield_dist=output_dist, + yield_speed=output_speed)) + 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 edb0dde47..4b59cc3a3 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -15,19 +15,20 @@ 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: { - 'planner': 'dt', # 'milestone', 'dt', or 'dx' + '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 } + 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 @@ -84,7 +85,11 @@ variants: drive: perception: state_estimation : OmniscientStateEstimator - + planning: + relations_estimation: + type: pedestrian_yield_logic.PedestrianYielder + args: + mode: 'sim' fake_real: run: @@ -108,7 +113,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 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