From e77d8d446d4264019ab30a39c199abdc71ab1093 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Thu, 6 Feb 2025 18:44:26 -0600 Subject: [PATCH 01/14] Make simple simulation for pedestrian yielding --- .../onboard/planning/longitudinal_planning.py | 213 ++++++++++++++++-- 1 file changed, 200 insertions(+), 13 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 1f5347325..b3a94fe6f 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -5,6 +5,183 @@ from ...mathutils.transforms import vector_madd import time +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import math + +class Simulation: + """ + Simulation class to update positions of two rectangles (vehicle and pedestrian) + with velocities v1 and v2, performing collision detection at each time step. + Advances step-by-step on user key input and displays the plot continuously until + a collision is detected. + """ + def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0): + # Vehicle parameters with buffer adjustments. + self.x1 = x1 + 1.5 # Offset for buffer + self.y1 = y1 + self.w1 = 3.2 + 3.0 # Increase width with buffer + self.h1 = 1.7 + 1.0 # Increase height with buffer + self.t1 = t1 + + # Pedestrian parameters. + 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, set high enough + + 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): + appropriate_deceleration = 0.0 + relation = "None" + # None: No relation + # Yielding: Vehicle is yielding to pedestrian + # Stopping: Vehicle is stopping for pedestrian + + steps = int(self.total_time / self.dt) + 1 + + if is_displayed: + plt.ion() # Turn on interactive mode + fig, ax = plt.subplots(figsize=(10,5)) + ax.set_xlim(-5, 25) + ax.set_ylim(-5, 5) + ax.grid(True, linestyle='--', alpha=0.5) + ax.set_aspect('equal') + + for i in range(steps): + t_sim = i * self.dt + + # Update positions based on velocities. + self.x1 += self.v1[0] * self.dt + self.y1 += self.v1[1] * self.dt + self.x2 += self.v2[0] * self.dt + self.y2 += self.v2[1] * self.dt + + # Compute rectangle corners. + rect1 = self.get_corners(self.x1, self.y1, self.w1, self.h1, self.t1) + rect2 = self.get_corners(self.x2, self.y2, self.w2, self.h2, self.t2) + + # Perform collision detection. + collision = self.sat_collision(rect1, rect2) + + # Print simulation status. + # print(f"Time: {t_sim:.2f}s, Vehicle: ({self.x1:.2f}, {self.y1:.2f}), " + # f"Pedestrian: ({self.x2:.2f}, {self.y2:.2f}), Collision: {collision}") + + if is_displayed: + # Plot the current step. + ax.clear() + ax.set_xlim(-5, 20) + ax.set_ylim(-5, 5) + ax.grid(True, linestyle='--', alpha=0.5) + self.plot_rectangles(rect1, rect2, collision, ax) + ax.text(-4, 4.5, f"t = {t_sim:.1f}s", fontsize=12) + plt.draw() + + # Pause briefly to simulate real-time updating. + plt.pause(self.dt * 0.1) + + # Stop simulation if collision is detected. + if collision: + # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) + # ASSUMING DECELERATION IS 2.0 m/s^2 + minimum_distance = self.v1[0]**2 / (2 * 2.0) + appropriate_deceleration = self.v1[0]**2 / (2 * self.x1) + + print("Collision detected. Stopping simulation.") + print(f"Collision coordinates: ({self.x1:.1f}, {self.y1:.1f})") + print("Vehicle speed:", self.v1[0]) + print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") + print(f"Appropriate deceleration: {appropriate_deceleration:.1f}") + + if minimum_distance > self.x1: + relation = "Stopping" + else: + relation = "Yielding" + + break + + if is_displayed: + plt.ioff() + plt.show(block=True) + + return relation, appropriate_deceleration + def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: """Generates a longitudinal trajectory for a path with a @@ -171,28 +348,36 @@ def update(self, state : AllState): print("#### YIELDING PLANNING ####") - # Convert vehicle pose from vehicle to world coordinates - # xyhead_demo: vehicle start at [4, 5], ped walks [15, 2]~[15, 10] - # Is there relative pose of pedestrian or absolute pose of vehicle? - abs_x = vehicle.pose.x + state.start_vehicle_pose.x - abs_y = vehicle.pose.y + state.start_vehicle_pose.y + # Vehicle parameters. + x1, y1, t1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y, 0 + v1 = [curr_v, 0] # Vehicle speed vector - # TODO: Check if there are multiple pedestrians for n,a in state.agents.items(): - print("ped", a.pose.x, a.pose.y) - print("ego", abs_x, abs_y) - # TODO: Make logic for smooth deceleration and re-acceleration - # TEMPORARY: STOP WHEN WITHIN 10M OF PEDESTRIAN - if a.pose.x - abs_x < 10.0 and a.pose.x - abs_x > 0.0: - print("#### Yielding to",n) - should_brake = True + # Pedestrian parameters. + x2, y2, t2 = a.pose.x, a.pose.y, 0 + v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector + # Total simulation time + total_time = 10.0 + + # Create and run the simulation. + # start_time = time.time() + sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time) + + # sim.run(is_displayed=True) + relation, decel = sim.run(is_displayed=False) + print(f"Relation: {relation}") + print(f"Deceleration: {decel:.2f} m/s^2") + # print(f"Simulation took {time.time() - start_time:.3f} seconds.") break # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS # should_brake = True + # # UNCOMMENT NOT TO BRAKE + should_brake = False + #========================= should_accelerate = (not should_brake and curr_v < self.desired_speed) @@ -205,4 +390,6 @@ def update(self, state : AllState): else: traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + print(f"Simulation took {time.time() - start_time:.3f} seconds.") + return traj From 96b34a8942a92a4cd11bce8861fbc19ac2f00a40 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Thu, 6 Feb 2025 23:29:43 -0600 Subject: [PATCH 02/14] Make yielding logic and update simulation, upload test function --- .../onboard/planning/longitudinal_planning.py | 50 ++-- testing/test_collision_detection.py | 215 ++++++++++++++++++ 2 files changed, 252 insertions(+), 13 deletions(-) create mode 100644 testing/test_collision_detection.py diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index b3a94fe6f..86deb5e46 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -288,8 +288,9 @@ def __init__(self): self.route_progress = None self.t_last = None self.acceleration = 0.5 - self.desired_speed = 1.0 + self.desired_speed = 2.0 # default 1.0 self.deceleration = 2.0 + self.relation = "None" def state_inputs(self): return ['all'] @@ -322,8 +323,15 @@ def update(self, state : AllState): 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) #extract out a 10m segment of the route - route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) + # route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) + + decel = self.deceleration + prev_relation = self.relation + should_yield = False #parse the relations indicated should_brake = False @@ -349,34 +357,47 @@ def update(self, state : AllState): print("#### YIELDING PLANNING ####") # Vehicle parameters. - x1, y1, t1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y, 0 + x1, y1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y v1 = [curr_v, 0] # Vehicle speed vector for n,a in state.agents.items(): # Pedestrian parameters. - x2, y2, t2 = a.pose.x, a.pose.y, 0 + x2, y2 = a.pose.x, a.pose.y v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector # Total simulation time - total_time = 10.0 + if curr_v > 0.1: + total_time = min(10, lookahead_distance / curr_v) + else: + total_time = 10 + print(f"Total time: {total_time:.2f} seconds") # Create and run the simulation. - # start_time = time.time() - sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time) + sim = Simulation(x1, y1, 0, x2, y2, 0, v1, v2, total_time) - # sim.run(is_displayed=True) - relation, decel = sim.run(is_displayed=False) - print(f"Relation: {relation}") + self.relation, decel = sim.run() + print(f"Relation: {self.relation}") print(f"Deceleration: {decel:.2f} m/s^2") - # print(f"Simulation took {time.time() - start_time:.3f} seconds.") + + # relation: None, Yielding, Stopping + # Stopping => None + if prev_relation == "Stopping" and self.relation == "Yielding": + self.relation = "Stopping" break + if self.relation == "Yielding": + should_brake = True + should_yield = True + elif self.relation == "Stopping": + should_brake = True + should_yield = False + # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS # should_brake = True # # UNCOMMENT NOT TO BRAKE - should_brake = False + # should_brake = False #========================= @@ -385,8 +406,11 @@ 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) - elif should_brake: + elif should_brake and not should_yield: traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) + elif should_brake and should_yield: + print("Yielding with", decel, "m/s^2") + traj = longitudinal_brake(route_with_lookahead, decel, curr_v) else: traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) diff --git a/testing/test_collision_detection.py b/testing/test_collision_detection.py new file mode 100644 index 000000000..fb9af7337 --- /dev/null +++ b/testing/test_collision_detection.py @@ -0,0 +1,215 @@ +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import math +import time + +class Simulation: + """ + 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): + # Vehicle parameters with buffer adjustments. + self.x1 = x1 + 1.5 # Offset for buffer (remains constant) + self.y1 = y1 + self.w1 = 3.2 + 3.0 # Increase width with buffer + self.h1 = 1.7 + 1.0 # Increase height with buffer + self.t1 = t1 + + # Pedestrian parameters. + 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 + + 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): + appropriate_deceleration = 0.0 + relation = "None" + # None: No relation + # Yielding: Vehicle is yielding to pedestrian + # Stopping: Vehicle is stopping for pedestrian + + 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 + + if is_displayed: + plt.ion() # Turn on interactive mode + fig, ax = plt.subplots(figsize=(10,5)) + ax.set_xlim(-5, 25) + ax.set_ylim(-5, 5) + ax.grid(True, linestyle='--', alpha=0.5) + ax.set_aspect('equal') + + for i in range(steps): + t_sim = i * self.dt + + # Update local positions based on velocities. + current_x1 += self.v1[0] * self.dt + current_y1 += self.v1[1] * self.dt + current_x2 += self.v2[0] * self.dt + current_y2 += self.v2[1] * 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() + ax.set_xlim(-5, 20) + ax.set_ylim(-5, 5) + ax.grid(True, linestyle='--', alpha=0.5) + self.plot_rectangles(rect1, rect2, collision, ax) + + # Draw an additional rectangle (vehicle body) at the vehicle's center. + rect_vehiclebody = patches.Rectangle( + (current_x1 - 3.1, current_y1 - 0.85), + self.w1 - 3.0, + self.h1 - 1.0, + edgecolor='green', + fill=False, + linewidth=2, + linestyle='--' + ) + ax.add_patch(rect_vehiclebody) + + ax.text(-4, 4.5, f"t = {t_sim:.1f}s", fontsize=12) + plt.draw() + + # Pause briefly to simulate real-time updating. + plt.pause(self.dt * 0.1) + + # Stop simulation if collision is detected. + if collision: + # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) + # ASSUMING DECELERATION IS 2.0 m/s^2 + minimum_distance = self.v1[0]**2 / (2 * 2.0) + appropriate_deceleration = self.v1[0]**2 / (2 * current_x1) + + print("Collision detected. Stopping simulation.") + print(f"Collision coordinates: ({current_x1:.1f}, {current_y1:.1f})") + print("Vehicle speed:", self.v1[0]) + print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") + print(f"Appropriate deceleration: {appropriate_deceleration:.1f}") + + if minimum_distance > current_x1: + relation = "Stopping" + else: + relation = "Yielding" + break + + if is_displayed: + plt.ioff() + plt.show(block=True) + + return relation, appropriate_deceleration + + +if __name__ == "__main__": + # Vehicle parameters. + x1, y1, t1 = 0, 0, 0 + # Pedestrian parameters. + x2, y2, t2 = 9, -5, 0 + # Velocity vectors: [vx, vy] + v1 = [1.0, 0] # Vehicle speed vector + v2 = [0, 0.5] # Pedestrian speed vector + # Total simulation time + total_time = 10.0 + + # Create and run the simulation. + start_time = time.time() + sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time) + + relation, decel = sim.run(is_displayed=True) + # relation, decel = sim.run(is_displayed=False) + + print(f"Relation: {relation}") + print(f"Deceleration: {decel:.2f} m/s^2") + # print(f"Simulation took {time.time() - start_time:.3f} seconds.") \ No newline at end of file From 9074df0f52fa908b358a05037aa59f0d8b7f9d66 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 7 Feb 2025 00:17:21 -0600 Subject: [PATCH 03/14] Fixed the alignment between sim and collision detection --- .../onboard/planning/longitudinal_planning.py | 101 ++++++++++++------ scenes/xyhead_demo.yaml | 2 + testing/test_collision_detection.py | 54 ++++++---- 3 files changed, 104 insertions(+), 53 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 86deb5e46..74626dbf8 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -10,22 +10,34 @@ import matplotlib.patches as patches import math -class Simulation: +class CollosionDetector: """ Simulation class to update positions of two rectangles (vehicle and pedestrian) with velocities v1 and v2, performing collision detection at each time step. - Advances step-by-step on user key input and displays the plot continuously until - a collision is detected. + 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): - # Vehicle parameters with buffer adjustments. - self.x1 = x1 + 1.5 # Offset for buffer - self.y1 = y1 - self.w1 = 3.2 + 3.0 # Increase width with buffer - self.h1 = 1.7 + 1.0 # Increase height with buffer + + 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 + self.vehicle_buffer_y = 1.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 parameters. + # Pedestrian rectangle self.x2 = x2 self.y2 = y2 self.w2 = 0.5 @@ -37,7 +49,7 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0): self.v2 = v2 self.dt = 0.1 # seconds - self.total_time = total_time # seconds, set high enough + self.total_time = total_time # seconds def get_corners(self, x, y, w, h, theta): """ @@ -81,7 +93,7 @@ def project(self, rect, axis): def sat_collision(self, rect1, rect2): """ - Determines if two convex polygons (rectangles) collide using the + 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. """ @@ -115,6 +127,13 @@ def run(self, is_displayed=False): 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 + if is_displayed: plt.ion() # Turn on interactive mode fig, ax = plt.subplots(figsize=(10,5)) @@ -126,54 +145,66 @@ def run(self, is_displayed=False): for i in range(steps): t_sim = i * self.dt - # Update positions based on velocities. - self.x1 += self.v1[0] * self.dt - self.y1 += self.v1[1] * self.dt - self.x2 += self.v2[0] * self.dt - self.y2 += self.v2[1] * self.dt + # Update local positions based on velocities. + current_x1 += self.v1[0] * self.dt + current_y1 += self.v1[1] * self.dt + current_x2 += self.v2[0] * self.dt + current_y2 += self.v2[1] * self.dt - # Compute rectangle corners. - rect1 = self.get_corners(self.x1, self.y1, self.w1, self.h1, self.t1) - rect2 = self.get_corners(self.x2, self.y2, self.w2, self.h2, self.t2) + # 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) - # Print simulation status. - # print(f"Time: {t_sim:.2f}s, Vehicle: ({self.x1:.2f}, {self.y1:.2f}), " - # f"Pedestrian: ({self.x2:.2f}, {self.y2:.2f}), Collision: {collision}") - if is_displayed: # Plot the current step. ax.clear() - ax.set_xlim(-5, 20) + ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20) ax.set_ylim(-5, 5) ax.grid(True, linestyle='--', alpha=0.5) self.plot_rectangles(rect1, rect2, collision, ax) - ax.text(-4, 4.5, f"t = {t_sim:.1f}s", fontsize=12) + + # Draw an additional rectangle (vehicle body) at the vehicle's center. + rect_vehiclebody = patches.Rectangle( + (current_x1 - 3.1, current_y1 - 0.85), + self.w1 - 3.0, + self.h1 - 1.0, + edgecolor='green', + fill=False, + linewidth=2, + linestyle='--' + ) + ax.add_patch(rect_vehiclebody) + + ax.text(0, 5.5, f"t = {t_sim:.1f}s", fontsize=12) plt.draw() # Pause briefly to simulate real-time updating. - plt.pause(self.dt * 0.1) + plt.pause(self.dt * 0.05) # Stop simulation if collision is detected. if collision: # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) # ASSUMING DECELERATION IS 2.0 m/s^2 - minimum_distance = self.v1[0]**2 / (2 * 2.0) - appropriate_deceleration = self.v1[0]**2 / (2 * self.x1) + minimum_distance = self.v1[0]**2 / (2 * 2.0) + current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 + current_vehicle_y = current_y1 + current_vehicle_x_head = current_vehicle_x + self.vehicle_size_x + self.vehicle_buffer_x + current_vehicle_y_head = current_vehicle_y + appropriate_deceleration = self.v1[0]**2 / (2 * current_vehicle_x_head) print("Collision detected. Stopping simulation.") - print(f"Collision coordinates: ({self.x1:.1f}, {self.y1:.1f})") - print("Vehicle speed:", self.v1[0]) + print(f"Collision coordinates: ({current_vehicle_x:.1f}, {current_vehicle_y:.1f})") + print(f"Vehicle speed: {self.v1[0]:.1f}") print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - print(f"Appropriate deceleration: {appropriate_deceleration:.1f}") + print(f"Appropriate deceleration: {appropriate_deceleration:.2f}") - if minimum_distance > self.x1: + if minimum_distance > current_vehicle_x_head: relation = "Stopping" else: relation = "Yielding" - break if is_displayed: @@ -373,7 +404,9 @@ def update(self, state : AllState): print(f"Total time: {total_time:.2f} seconds") # Create and run the simulation. - sim = Simulation(x1, y1, 0, x2, y2, 0, v1, v2, total_time) + print(f"Vehicle: ({x1:.1f}, {y1:.1f})") + print(f"Pedestrian: ({x2:.1f}, {y2:.1f})") + sim = CollosionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time) self.relation, decel = sim.run() print(f"Relation: {self.relation}") diff --git a/scenes/xyhead_demo.yaml b/scenes/xyhead_demo.yaml index c6d97477a..2dbc6c267 100644 --- a/scenes/xyhead_demo.yaml +++ b/scenes/xyhead_demo.yaml @@ -3,7 +3,9 @@ agents: ped1: type: pedestrian position: [15.0, 2.0] + # position: [10.0, 2.0] nominal_velocity: 0.5 target: [15.0,10.0] + # target: [10.0,10.0] behavior: loop \ No newline at end of file diff --git a/testing/test_collision_detection.py b/testing/test_collision_detection.py index fb9af7337..6d518c7f9 100644 --- a/testing/test_collision_detection.py +++ b/testing/test_collision_detection.py @@ -12,14 +12,26 @@ class Simulation: local copies are used during simulation. """ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0): - # Vehicle parameters with buffer adjustments. - self.x1 = x1 + 1.5 # Offset for buffer (remains constant) - self.y1 = y1 - self.w1 = 3.2 + 3.0 # Increase width with buffer - self.h1 = 1.7 + 1.0 # Increase height with buffer + + 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 + self.vehicle_buffer_y = 1.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 parameters. + # Pedestrian rectangle self.x2 = x2 self.y2 = y2 self.w2 = 0.5 @@ -143,7 +155,7 @@ def run(self, is_displayed=False): if is_displayed: # Plot the current step. ax.clear() - ax.set_xlim(-5, 20) + ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20) ax.set_ylim(-5, 5) ax.grid(True, linestyle='--', alpha=0.5) self.plot_rectangles(rect1, rect2, collision, ax) @@ -160,26 +172,30 @@ def run(self, is_displayed=False): ) ax.add_patch(rect_vehiclebody) - ax.text(-4, 4.5, f"t = {t_sim:.1f}s", fontsize=12) + ax.text(0, 5.5, f"t = {t_sim:.1f}s", fontsize=12) plt.draw() # Pause briefly to simulate real-time updating. - plt.pause(self.dt * 0.1) + plt.pause(self.dt * 0.05) # Stop simulation if collision is detected. if collision: # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) # ASSUMING DECELERATION IS 2.0 m/s^2 - minimum_distance = self.v1[0]**2 / (2 * 2.0) - appropriate_deceleration = self.v1[0]**2 / (2 * current_x1) + minimum_distance = self.v1[0]**2 / (2 * 2.0) + current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 + current_vehicle_y = current_y1 + current_vehicle_x_head = current_vehicle_x + self.vehicle_size_x + self.vehicle_buffer_x + current_vehicle_y_head = current_vehicle_y + appropriate_deceleration = self.v1[0]**2 / (2 * current_vehicle_x_head) print("Collision detected. Stopping simulation.") - print(f"Collision coordinates: ({current_x1:.1f}, {current_y1:.1f})") - print("Vehicle speed:", self.v1[0]) + print(f"Collision coordinates: ({current_vehicle_x:.1f}, {current_vehicle_y:.1f})") + print(f"Vehicle speed: {self.v1[0]:.1f}") print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - print(f"Appropriate deceleration: {appropriate_deceleration:.1f}") + print(f"Appropriate deceleration: {appropriate_deceleration:.2f}") - if minimum_distance > current_x1: + if minimum_distance > current_vehicle_x_head: relation = "Stopping" else: relation = "Yielding" @@ -193,10 +209,10 @@ def run(self, is_displayed=False): if __name__ == "__main__": - # Vehicle parameters. - x1, y1, t1 = 0, 0, 0 - # Pedestrian parameters. - x2, y2, t2 = 9, -5, 0 + # Vehicle parameters. x, y, theta (angle in radians) + x1, y1, t1 = 6, 0, 0 + # Pedestrian parameters. x, y, theta (angle in radians) + x2, y2, t2 = 15, -5, 0 # Velocity vectors: [vx, vy] v1 = [1.0, 0] # Vehicle speed vector v2 = [0, 0.5] # Pedestrian speed vector From 06fa6e738d954fa0d1a8c18f5d86dd8ef6809838 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Fri, 7 Feb 2025 12:11:56 -0600 Subject: [PATCH 04/14] Fixed coordinate misalignment. There is still bug not to detect collision sometimes --- .../onboard/planning/longitudinal_planning.py | 48 ++++++++------- testing/test_collision_detection.py | 59 ++++++++++++------- 2 files changed, 65 insertions(+), 42 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 74626dbf8..95e2bd9b2 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -17,7 +17,7 @@ class CollosionDetector: 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): + def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, basic_deceleration=2.0, max_deceleration=8.0): self.vehicle_x = x1 self.vehicle_y = y1 @@ -50,6 +50,8 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0): self.dt = 0.1 # seconds self.total_time = total_time # seconds + self.basic_deceleration = basic_deceleration + self.max_deceleration = max_deceleration def get_corners(self, x, y, w, h, theta): """ @@ -119,7 +121,7 @@ def plot_rectangles(self, rect1, rect2, collision, ax): ax.set_title(f"Collision: {'Yes' if collision else 'No'}") def run(self, is_displayed=False): - appropriate_deceleration = 0.0 + output_deceleration = 0.0 relation = "None" # None: No relation # Yielding: Vehicle is yielding to pedestrian @@ -145,12 +147,6 @@ def run(self, is_displayed=False): for i in range(steps): t_sim = i * self.dt - # Update local positions based on velocities. - current_x1 += self.v1[0] * self.dt - current_y1 += self.v1[1] * self.dt - current_x2 += self.v2[0] * self.dt - current_y2 += self.v2[1] * 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) @@ -186,32 +182,42 @@ def run(self, is_displayed=False): # Stop simulation if collision is detected. if collision: + # Check if the vehicle will hit the pedestrian or can stop before hitting. # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) - # ASSUMING DECELERATION IS 2.0 m/s^2 - minimum_distance = self.v1[0]**2 / (2 * 2.0) + # Minimum distance required to stop before hitting with basic_deceleration + minimum_distance = self.v1[0]**2 / (2 * self.basic_deceleration) current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 current_vehicle_y = current_y1 - current_vehicle_x_head = current_vehicle_x + self.vehicle_size_x + self.vehicle_buffer_x - current_vehicle_y_head = current_vehicle_y - appropriate_deceleration = self.v1[0]**2 / (2 * current_vehicle_x_head) print("Collision detected. Stopping simulation.") print(f"Collision coordinates: ({current_vehicle_x:.1f}, {current_vehicle_y:.1f})") print(f"Vehicle speed: {self.v1[0]:.1f}") print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - print(f"Appropriate deceleration: {appropriate_deceleration:.2f}") - if minimum_distance > current_vehicle_x_head: + if minimum_distance > current_vehicle_x - self.vehicle_x: + print("Vehicle will hit the pedestrian!!!") relation = "Stopping" + # Deceleration to stop at the current position > basic_deceleration + output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x))) else: + print("Vehicle can yield. Speed down with:") + # Deceleration to stop at the current position < basic_deceleration + output_deceleration = self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x)) + print(f"Appropriate deceleration: {output_deceleration:.2f}") relation = "Yielding" break + # Update local positions based on velocities. + current_x1 += self.v1[0] * 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) - return relation, appropriate_deceleration + return relation, output_deceleration def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: @@ -321,6 +327,8 @@ def __init__(self): self.acceleration = 0.5 self.desired_speed = 2.0 # default 1.0 self.deceleration = 2.0 + + self.max_deceleration = 8.0 self.relation = "None" def state_inputs(self): @@ -354,7 +362,7 @@ def update(self, state : AllState): 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)) + lookahead_distance = max(5, 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 @@ -404,9 +412,9 @@ def update(self, state : AllState): print(f"Total time: {total_time:.2f} seconds") # Create and run the simulation. - print(f"Vehicle: ({x1:.1f}, {y1:.1f})") - print(f"Pedestrian: ({x2:.1f}, {y2:.1f})") - sim = CollosionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time) + 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}))") + sim = CollosionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time, self.deceleration, self.max_deceleration) self.relation, decel = sim.run() print(f"Relation: {self.relation}") diff --git a/testing/test_collision_detection.py b/testing/test_collision_detection.py index 6d518c7f9..9b85d72ff 100644 --- a/testing/test_collision_detection.py +++ b/testing/test_collision_detection.py @@ -11,7 +11,7 @@ class Simulation: 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): + def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, basic_deceleration=2.0, max_deceleration=8.0): self.vehicle_x = x1 self.vehicle_y = y1 @@ -44,6 +44,8 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0): self.dt = 0.1 # seconds self.total_time = total_time # seconds + self.basic_deceleration = basic_deceleration + self.max_deceleration = max_deceleration def get_corners(self, x, y, w, h, theta): """ @@ -113,7 +115,7 @@ def plot_rectangles(self, rect1, rect2, collision, ax): ax.set_title(f"Collision: {'Yes' if collision else 'No'}") def run(self, is_displayed=False): - appropriate_deceleration = 0.0 + output_deceleration = 0.0 relation = "None" # None: No relation # Yielding: Vehicle is yielding to pedestrian @@ -139,12 +141,6 @@ def run(self, is_displayed=False): for i in range(steps): t_sim = i * self.dt - # Update local positions based on velocities. - current_x1 += self.v1[0] * self.dt - current_y1 += self.v1[1] * self.dt - current_x2 += self.v2[0] * self.dt - current_y2 += self.v2[1] * 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) @@ -180,51 +176,70 @@ def run(self, is_displayed=False): # Stop simulation if collision is detected. if collision: + # Check if the vehicle will hit the pedestrian or can stop before hitting. # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) - # ASSUMING DECELERATION IS 2.0 m/s^2 - minimum_distance = self.v1[0]**2 / (2 * 2.0) + # Minimum distance required to stop before hitting with basic_deceleration + minimum_distance = self.v1[0]**2 / (2 * self.basic_deceleration) current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 current_vehicle_y = current_y1 - current_vehicle_x_head = current_vehicle_x + self.vehicle_size_x + self.vehicle_buffer_x - current_vehicle_y_head = current_vehicle_y - appropriate_deceleration = self.v1[0]**2 / (2 * current_vehicle_x_head) print("Collision detected. Stopping simulation.") print(f"Collision coordinates: ({current_vehicle_x:.1f}, {current_vehicle_y:.1f})") print(f"Vehicle speed: {self.v1[0]:.1f}") print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - print(f"Appropriate deceleration: {appropriate_deceleration:.2f}") - if minimum_distance > current_vehicle_x_head: + if minimum_distance > current_vehicle_x - self.vehicle_x: + print("Vehicle will hit the pedestrian!!!") relation = "Stopping" + # Deceleration to stop at the current position > basic_deceleration + output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x))) else: + print("Vehicle can yield. Speed down with:") + # Deceleration to stop at the current position < basic_deceleration + output_deceleration = self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x)) + print(f"Appropriate deceleration: {output_deceleration:.2f}") relation = "Yielding" break + # Update local positions based on velocities. + current_x1 += self.v1[0] * 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) - return relation, appropriate_deceleration + return relation, output_deceleration if __name__ == "__main__": # Vehicle parameters. x, y, theta (angle in radians) - x1, y1, t1 = 6, 0, 0 + x1, y1, t1 = 7.6, 5.0, 0 # Pedestrian parameters. x, y, theta (angle in radians) - x2, y2, t2 = 15, -5, 0 + x2, y2, t2 = 15.0, 5.8, 0 # Velocity vectors: [vx, vy] - v1 = [1.0, 0] # Vehicle speed vector + v1 = [0.6, 0] # Vehicle speed vector v2 = [0, 0.5] # Pedestrian speed vector # Total simulation time total_time = 10.0 + # Basic deceleration for the vehicle + # Output is decided based on this deceleration if the vehicle is about to hit + basic_deceleration = 2.0 + + # Max deceleration for the vehicle + # Return deceleration bigger than basic_deceleration when the vehicle will hit + max_deceleration = 8.0 + # Create and run the simulation. start_time = time.time() - sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time) + # Simulate with the above parameters: Whether to hit without decelerating + sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time, basic_deceleration, max_deceleration) - relation, decel = sim.run(is_displayed=True) - # relation, decel = sim.run(is_displayed=False) + # relation, decel = sim.run(is_displayed=True) + relation, decel = sim.run(is_displayed=False) print(f"Relation: {relation}") print(f"Deceleration: {decel:.2f} m/s^2") From 3de8182ad4aba17dcab9020f1a2887f59d1eec4b Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Sun, 9 Feb 2025 22:14:39 -0600 Subject: [PATCH 05/14] Modify buffer from 0.5m => 1m --- .../onboard/planning/longitudinal_planning.py | 37 ++++++++++-------- testing/test_collision_detection.py | 39 ++++++++++--------- 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 95e2bd9b2..aa5327478 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -28,7 +28,7 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, basic_decele self.vehicle_size_x = 3.2 self.vehicle_size_y = 1.7 self.vehicle_buffer_x = 3.0 - self.vehicle_buffer_y = 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) @@ -122,6 +122,7 @@ def plot_rectangles(self, rect1, rect2, collision, ax): def run(self, is_displayed=False): output_deceleration = 0.0 + collision_distance = -1 relation = "None" # None: No relation # Yielding: Vehicle is yielding to pedestrian @@ -139,10 +140,6 @@ def run(self, is_displayed=False): if is_displayed: plt.ion() # Turn on interactive mode fig, ax = plt.subplots(figsize=(10,5)) - ax.set_xlim(-5, 25) - ax.set_ylim(-5, 5) - ax.grid(True, linestyle='--', alpha=0.5) - ax.set_aspect('equal') for i in range(steps): t_sim = i * self.dt @@ -157,16 +154,17 @@ def run(self, is_displayed=False): if is_displayed: # Plot the current step. ax.clear() - ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20) - ax.set_ylim(-5, 5) - ax.grid(True, linestyle='--', alpha=0.5) 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 - 3.1, current_y1 - 0.85), - self.w1 - 3.0, - self.h1 - 1.0, + (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, @@ -174,7 +172,7 @@ def run(self, is_displayed=False): ) ax.add_patch(rect_vehiclebody) - ax.text(0, 5.5, f"t = {t_sim:.1f}s", fontsize=12) + 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. @@ -194,11 +192,13 @@ def run(self, is_displayed=False): print(f"Vehicle speed: {self.v1[0]:.1f}") print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - if minimum_distance > current_vehicle_x - self.vehicle_x: + collision_distance = current_vehicle_x - self.vehicle_x + + if minimum_distance > collision_distance: print("Vehicle will hit the pedestrian!!!") relation = "Stopping" # Deceleration to stop at the current position > basic_deceleration - output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x))) + output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (collision_distance))) else: print("Vehicle can yield. Speed down with:") # Deceleration to stop at the current position < basic_deceleration @@ -217,7 +217,7 @@ def run(self, is_displayed=False): plt.ioff() plt.show(block=True) - return relation, output_deceleration + return relation, output_deceleration, collision_distance def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: @@ -416,10 +416,14 @@ def update(self, state : AllState): print(f"Pedestrian: ({x2:.1f}, {y2:.1f}, ({v2[0]:.1f}, {v2[1]:.1f}))") sim = CollosionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time, self.deceleration, self.max_deceleration) - self.relation, decel = sim.run() + self.relation, decel, collision_distance = sim.run() print(f"Relation: {self.relation}") print(f"Deceleration: {decel:.2f} m/s^2") + # Update the lookahead distance based on the deceleration + if collision_distance > 0: + route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) + # relation: None, Yielding, Stopping # Stopping => None if prev_relation == "Stopping" and self.relation == "Yielding": @@ -450,7 +454,6 @@ def update(self, state : AllState): elif should_brake and not should_yield: traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) elif should_brake and should_yield: - print("Yielding with", decel, "m/s^2") traj = longitudinal_brake(route_with_lookahead, decel, curr_v) else: traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) diff --git a/testing/test_collision_detection.py b/testing/test_collision_detection.py index 9b85d72ff..2fc7d8473 100644 --- a/testing/test_collision_detection.py +++ b/testing/test_collision_detection.py @@ -22,7 +22,7 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, basic_decele self.vehicle_size_x = 3.2 self.vehicle_size_y = 1.7 self.vehicle_buffer_x = 3.0 - self.vehicle_buffer_y = 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) @@ -116,6 +116,7 @@ def plot_rectangles(self, rect1, rect2, collision, ax): def run(self, is_displayed=False): output_deceleration = 0.0 + collision_distance = -1 relation = "None" # None: No relation # Yielding: Vehicle is yielding to pedestrian @@ -133,10 +134,6 @@ def run(self, is_displayed=False): if is_displayed: plt.ion() # Turn on interactive mode fig, ax = plt.subplots(figsize=(10,5)) - ax.set_xlim(-5, 25) - ax.set_ylim(-5, 5) - ax.grid(True, linestyle='--', alpha=0.5) - ax.set_aspect('equal') for i in range(steps): t_sim = i * self.dt @@ -151,16 +148,17 @@ def run(self, is_displayed=False): if is_displayed: # Plot the current step. ax.clear() - ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20) - ax.set_ylim(-5, 5) - ax.grid(True, linestyle='--', alpha=0.5) 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 - 3.1, current_y1 - 0.85), - self.w1 - 3.0, - self.h1 - 1.0, + (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, @@ -168,7 +166,7 @@ def run(self, is_displayed=False): ) ax.add_patch(rect_vehiclebody) - ax.text(0, 5.5, f"t = {t_sim:.1f}s", fontsize=12) + 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. @@ -188,11 +186,13 @@ def run(self, is_displayed=False): print(f"Vehicle speed: {self.v1[0]:.1f}") print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - if minimum_distance > current_vehicle_x - self.vehicle_x: + collision_distance = current_vehicle_x - self.vehicle_x + + if minimum_distance > collision_distance: print("Vehicle will hit the pedestrian!!!") relation = "Stopping" # Deceleration to stop at the current position > basic_deceleration - output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x))) + output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (collision_distance))) else: print("Vehicle can yield. Speed down with:") # Deceleration to stop at the current position < basic_deceleration @@ -211,17 +211,17 @@ def run(self, is_displayed=False): plt.ioff() plt.show(block=True) - return relation, output_deceleration + return relation, output_deceleration, collision_distance if __name__ == "__main__": # Vehicle parameters. x, y, theta (angle in radians) - x1, y1, t1 = 7.6, 5.0, 0 + x1, y1, t1 = 7.5, 5.0, 0 # Pedestrian parameters. x, y, theta (angle in radians) x2, y2, t2 = 15.0, 5.8, 0 # Velocity vectors: [vx, vy] v1 = [0.6, 0] # Vehicle speed vector - v2 = [0, 0.5] # Pedestrian speed vector + v2 = [0, 0.25] # Pedestrian speed vector # Total simulation time total_time = 10.0 @@ -238,9 +238,10 @@ def run(self, is_displayed=False): # Simulate with the above parameters: Whether to hit without decelerating sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time, basic_deceleration, max_deceleration) - # relation, decel = sim.run(is_displayed=True) - relation, decel = sim.run(is_displayed=False) + relation, decel, distance = sim.run(is_displayed=True) + # relation, decel = sim.run(is_displayed=False) print(f"Relation: {relation}") print(f"Deceleration: {decel:.2f} m/s^2") + print(f"Distance: {distance:.2f} m") # print(f"Simulation took {time.time() - start_time:.3f} seconds.") \ No newline at end of file From 4ade5bbdd9740b876316d814745d55d894c56cc6 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Mon, 10 Feb 2025 00:59:41 -0600 Subject: [PATCH 06/14] Update longitudinal_planning.py --- .../onboard/planning/longitudinal_planning.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index aa5327478..28e396efd 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -293,6 +293,7 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) print("=====LONGITUDINAL BRAKE=====") print("path length: ", path.length()) + print("deceleration: ", deceleration) length = path.length() x0 = points[0][0] @@ -362,7 +363,7 @@ def update(self, state : AllState): 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(5, curr_v**2 / (2 * self.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 @@ -404,6 +405,7 @@ def update(self, state : AllState): # Pedestrian parameters. x2, y2 = a.pose.x, a.pose.y v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector + # Total simulation time if curr_v > 0.1: total_time = min(10, lookahead_distance / curr_v) @@ -421,11 +423,18 @@ def update(self, state : AllState): print(f"Deceleration: {decel:.2f} m/s^2") # Update the lookahead distance based on the deceleration - if collision_distance > 0: + if collision_distance >= 0: route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) # relation: None, Yielding, Stopping - # Stopping => None + # None: No need to speed down + # Yielding: Speed down but not to 0 m/s + # Stopping: Speed down to 0 m/s + # State transition: + # None => Yielding or Stopping + # Yielding => Stopping + # Stopping => nan + if prev_relation == "Stopping" and self.relation == "Yielding": self.relation = "Stopping" @@ -452,8 +461,10 @@ def update(self, state : AllState): if should_accelerate: traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v) elif should_brake and not should_yield: - traj = longitudinal_brake(route_with_lookahead, self.deceleration, curr_v) + # Stopping: 2.0 < Decel < 8.0 + traj = longitudinal_brake(route_with_lookahead, decel, curr_v) elif should_brake and should_yield: + # Yielding: 0.0 < Decel < 2.0 traj = longitudinal_brake(route_with_lookahead, decel, curr_v) else: traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) From a599e2ab6db71a0d12a46d148fe2598bf2b1599c Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Thu, 13 Feb 2025 00:40:32 -0600 Subject: [PATCH 07/14] Implement Part2.3: Keep slow until ped crossed --- .../onboard/planning/longitudinal_planning.py | 131 +++++----- testing/test_collision_detection.py | 247 ++---------------- 2 files changed, 85 insertions(+), 293 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index d880db88f..1299f178b 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -10,14 +10,14 @@ import matplotlib.patches as patches import math -class CollosionDetector: +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, basic_deceleration=2.0, max_deceleration=8.0): + 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 @@ -50,8 +50,13 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, basic_decele self.dt = 0.1 # seconds self.total_time = total_time # seconds - self.basic_deceleration = basic_deceleration - self.max_deceleration = max_deceleration + + 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): """ @@ -121,13 +126,7 @@ def plot_rectangles(self, rect1, rect2, collision, ax): ax.set_title(f"Collision: {'Yes' if collision else 'No'}") def run(self, is_displayed=False): - output_deceleration = 0.0 collision_distance = -1 - relation = "None" - # None: No relation - # Yielding: Vehicle is yielding to pedestrian - # Stopping: Vehicle is stopping for pedestrian - steps = int(self.total_time / self.dt) + 1 # Create local variables for positions; these will be updated @@ -136,6 +135,7 @@ def run(self, is_displayed=False): 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 @@ -180,35 +180,26 @@ def run(self, is_displayed=False): # Stop simulation if collision is detected. if collision: - # Check if the vehicle will hit the pedestrian or can stop before hitting. - # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) - # Minimum distance required to stop before hitting with basic_deceleration - minimum_distance = self.v1[0]**2 / (2 * self.basic_deceleration) - current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 - current_vehicle_y = current_y1 - - print("Collision detected. Stopping simulation.") - print(f"Collision coordinates: ({current_vehicle_x:.1f}, {current_vehicle_y:.1f})") - print(f"Vehicle speed: {self.v1[0]:.1f}") - print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - + 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 - - if minimum_distance > collision_distance: - print("Vehicle will hit the pedestrian!!!") - relation = "Stopping" - # Deceleration to stop at the current position > basic_deceleration - output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (collision_distance))) - else: - print("Vehicle can yield. Speed down with:") - # Deceleration to stop at the current position < basic_deceleration - output_deceleration = self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x)) - print(f"Appropriate deceleration: {output_deceleration:.2f}") - relation = "Yielding" 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 += self.v1[0] * self.dt + 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 @@ -217,7 +208,9 @@ def run(self, is_displayed=False): plt.ioff() plt.show(block=True) - return relation, output_deceleration, collision_distance + print("Collision distance:", collision_distance) + + return collision_distance def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: @@ -457,7 +450,6 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) print("=====LONGITUDINAL BRAKE=====") print("path length: ", path.length()) - print("deceleration: ", deceleration) length = path.length() x0 = points[0][0] @@ -494,9 +486,11 @@ def __init__(self): self.t_last = None self.acceleration = 0.5 self.desired_speed = 2.0 # default 1.0 - self.deceleration = 2.0 - self.max_deceleration = 8.0 + self.yield_speed = 0.5 + self.yield_deceleration = 0.5 + self.deceleration = 2.0 + self.max_deceleration = 8.0 self.relation = "None" def state_inputs(self): @@ -530,18 +524,15 @@ def update(self, state : AllState): 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)) + 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) - decel = self.deceleration - prev_relation = self.relation - should_yield = False - #parse the relations indicated should_brake = False + should_yield = False for r in state.relations: if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': @@ -583,30 +574,50 @@ def update(self, state : AllState): # 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}))") - sim = CollosionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time, self.deceleration, self.max_deceleration) - self.relation, decel, collision_distance = sim.run() - print(f"Relation: {self.relation}") - print(f"Deceleration: {decel:.2f} m/s^2") + # 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 + if collision_distance < 0: + print("No collision detected.") + self.relation = "None" - # Update the lookahead distance based on the deceleration - if collision_distance >= 0: - route_with_lookahead = route.trim(closest_parameter,closest_parameter + collision_distance) + # Collision detected + else: + # Update lookahead distance to pedestrian. + route_with_lookahead = route.trim(closest_parameter, closest_parameter + collision_distance) + + # Simulate if a collision will occur when the vehicle decelerate to yield speed. + sim.set_params(self.yield_speed, self.yield_deceleration * -1) + collision_distance_after_yield = sim.run() + + # Can yield to the pedestrian => Yielding + if collision_distance_after_yield < 0: + print("The vehicle can yield to the pedestrian.") + self.relation = "Yielding" + decel = self.yield_deceleration + + # Cannot yield to the pedestrian => Stopping + else: + print("The vehicle is Stopping.") + self.relation = "Stopping" + decel = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) + if decel > self.max_deceleration: + decel = self.max_deceleration + + print(f"Relation: {self.relation}") + print(f"Deceleration: {decel:.2f} m/s^2") # relation: None, Yielding, Stopping # None: No need to speed down # Yielding: Speed down but not to 0 m/s # Stopping: Speed down to 0 m/s - # State transition: - # None => Yielding or Stopping - # Yielding => Stopping - # Stopping => nan - - if prev_relation == "Stopping" and self.relation == "Yielding": - self.relation = "Stopping" break + if self.relation == "Yielding": should_brake = True should_yield = True @@ -628,11 +639,9 @@ def update(self, state : AllState): if should_accelerate: traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v) elif should_brake and not should_yield: - # Stopping: 2.0 < Decel < 8.0 traj = longitudinal_brake(route_with_lookahead, decel, curr_v) elif should_brake and should_yield: - # Yielding: 0.0 < Decel < 2.0 - traj = longitudinal_brake(route_with_lookahead, decel, curr_v) + traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.yield_deceleration, self.yield_speed, curr_v) else: traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) diff --git a/testing/test_collision_detection.py b/testing/test_collision_detection.py index 2fc7d8473..4a8c35c26 100644 --- a/testing/test_collision_detection.py +++ b/testing/test_collision_detection.py @@ -1,247 +1,30 @@ -import numpy as np -import matplotlib.pyplot as plt -import matplotlib.patches as patches -import math -import time - -class Simulation: - """ - 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, basic_deceleration=2.0, max_deceleration=8.0): - - 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 - 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.basic_deceleration = basic_deceleration - self.max_deceleration = max_deceleration - - 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): - output_deceleration = 0.0 - collision_distance = -1 - relation = "None" - # None: No relation - # Yielding: Vehicle is yielding to pedestrian - # Stopping: Vehicle is stopping for pedestrian - - 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 - - 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: - # Check if the vehicle will hit the pedestrian or can stop before hitting. - # Dmin = v^2 / (2 * a) => a = -v^2 / (2 * D) - # Minimum distance required to stop before hitting with basic_deceleration - minimum_distance = self.v1[0]**2 / (2 * self.basic_deceleration) - current_vehicle_x = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5 - current_vehicle_y = current_y1 - - print("Collision detected. Stopping simulation.") - print(f"Collision coordinates: ({current_vehicle_x:.1f}, {current_vehicle_y:.1f})") - print(f"Vehicle speed: {self.v1[0]:.1f}") - print(f"Minimum distance required to avoid collision: {minimum_distance:.1f}") - - collision_distance = current_vehicle_x - self.vehicle_x - - if minimum_distance > collision_distance: - print("Vehicle will hit the pedestrian!!!") - relation = "Stopping" - # Deceleration to stop at the current position > basic_deceleration - output_deceleration = min(self.max_deceleration, self.v1[0]**2 / (2 * (collision_distance))) - else: - print("Vehicle can yield. Speed down with:") - # Deceleration to stop at the current position < basic_deceleration - output_deceleration = self.v1[0]**2 / (2 * (current_vehicle_x - self.vehicle_x)) - print(f"Appropriate deceleration: {output_deceleration:.2f}") - relation = "Yielding" - break - - # Update local positions based on velocities. - current_x1 += self.v1[0] * 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) - - return relation, output_deceleration, collision_distance +#needed to import GEMstack from top level directory +import sys +import os +sys.path.append(os.getcwd()) +from GEMstack.onboard.planning.longitudinal_planning import CollisionDetector +import time if __name__ == "__main__": # Vehicle parameters. x, y, theta (angle in radians) - x1, y1, t1 = 7.5, 5.0, 0 + x1, y1, t1 = 4.0, 5.0, 0 # Pedestrian parameters. x, y, theta (angle in radians) - x2, y2, t2 = 15.0, 5.8, 0 + x2, y2, t2 = 15.0, 2.1, 0 # Velocity vectors: [vx, vy] - v1 = [0.6, 0] # Vehicle speed vector - v2 = [0, 0.25] # Pedestrian speed vector + v1 = [0.1, 0] # Vehicle speed vector + v2 = [0, 0.5] # Pedestrian speed vector # Total simulation time total_time = 10.0 - # Basic deceleration for the vehicle - # Output is decided based on this deceleration if the vehicle is about to hit - basic_deceleration = 2.0 - - # Max deceleration for the vehicle - # Return deceleration bigger than basic_deceleration when the vehicle will hit - max_deceleration = 8.0 + desired_speed = 0.5 + acceleration = -0.5 # Create and run the simulation. start_time = time.time() # Simulate with the above parameters: Whether to hit without decelerating - sim = Simulation(x1, y1, t1, x2, y2, t2, v1, v2, total_time, basic_deceleration, max_deceleration) - - relation, decel, distance = sim.run(is_displayed=True) - # relation, decel = sim.run(is_displayed=False) + sim = CollisionDetector(x1, y1, t1, x2, y2, t2, v1, v2, total_time, desired_speed, acceleration) - print(f"Relation: {relation}") - print(f"Deceleration: {decel:.2f} m/s^2") - print(f"Distance: {distance:.2f} m") + collision_distance = sim.run(is_displayed=True) + print(f"Collision distance: {collision_distance}") # print(f"Simulation took {time.time() - start_time:.3f} seconds.") \ No newline at end of file From 97e5e63f31a7f1cd5494d173e9ad6d28e37c345e Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Tue, 18 Feb 2025 18:19:07 -0600 Subject: [PATCH 08/14] Update longitudinal_planning.py --- .../onboard/planning/longitudinal_planning.py | 175 ++++++++++-------- 1 file changed, 102 insertions(+), 73 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 1299f178b..85ca9f9ac 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -27,7 +27,7 @@ def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, desired_spee # Vehicle parameters with buffer adjustments self.vehicle_size_x = 3.2 self.vehicle_size_y = 1.7 - self.vehicle_buffer_x = 3.0 + self.vehicle_buffer_x = 3.0 + 1.0 self.vehicle_buffer_y = 2.0 # Vehicle rectangle @@ -256,10 +256,10 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m 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("=====================================") + # print("new points: ", new_points) + # print("current index: ", cur_index) + # print("current speed: ", current_speed) # Information we will need: # Calculate how much time it would take to stop @@ -273,7 +273,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # 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") + # print("In case one") # put on the breaks # Calculate the next point in a special manner because of too-little time to stop @@ -285,7 +285,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # keep breaking until the next milestone in path if next_point[0] <= points[-1][0]: - print("continuing to next point") + # print("continuing to next point") delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) cur_time += delta_t_to_next_x cur_point = next_point @@ -306,7 +306,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # because the first if-statement covers for when we decelerating, # the only time current_speed < max_speed is when we are accelerating elif current_speed < max_speed: - print("In case two") + # print("In case two") # next point next_point = points[cur_index+1] # accelerate to max speed @@ -319,7 +319,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # if we would reach max speed after the next point, # just move to the next point and update the current speed and time if cur_point[0] + delta_x_to_max_speed >= next_point[0]: - print("go to next point") + # print("go to next point") # accelerate to max speed delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) cur_time += delta_t_to_next_x @@ -330,7 +330,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # 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") + # 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] @@ -343,17 +343,17 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m elif current_speed == max_speed: next_point = points[cur_index+1] # continue on with max speed - print("In case three") + # 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") + # 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") + # print("Continuing on to next point") cur_time += (next_point[0] - cur_point[0])/current_speed cur_point = next_point cur_index += 1 @@ -364,7 +364,7 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m # We need to hit the breaks next_point = points[cur_index+1] - print("In case four") + # 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 @@ -395,9 +395,9 @@ def longitudinal_plan(path : Path, acceleration : float, deceleration : float, m points = new_points times = new_times - print("[PLAN] Computed points:", points) - print("[TIME] Computed time:", times) - print("[Velocities] Computed velocities:", velocities) + # print("[PLAN] Computed points:", points) + # print("[TIME] Computed time:", times) + # print("[Velocities] Computed velocities:", velocities) #============================================= @@ -415,16 +415,16 @@ def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: # 0.5*a*t^2 + v0*t + x0 - x1 = 0 # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a - print("x0: ", x0) - print("x1: ", x1) - print("v: ", v) - print("a: ", a) + # print("x0: ", x0) + # print("x1: ", x1) + # print("v: ", v) + # print("a: ", a) t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a - print("t1: ", t1) - print("t2: ", t2) + # print("t1: ", t1) + # print("t2: ", t2) if math.isnan(t1): t1 = 0 if math.isnan(t2): t2 = 0 @@ -467,7 +467,7 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) new_points.append([x, 0]) velocities.append(current_speed - deceleration * t) points = new_points - print("[BRAKE] Computed points:", points) + # print("[BRAKE] Computed points:", points) #============================================= @@ -485,13 +485,14 @@ def __init__(self): self.route_progress = None self.t_last = None self.acceleration = 0.5 - self.desired_speed = 2.0 # default 1.0 + self.desired_speed = 1.0 # default 1.0 - self.yield_speed = 0.5 + # 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.relation = "None" def state_inputs(self): return ['all'] @@ -530,9 +531,12 @@ def update(self, state : AllState): #extract out a 10m segment of the route # route_with_lookahead = route.trim(closest_parameter,closest_parameter+10.0) - #parse the relations indicated + + # Default values should_brake = False - should_yield = False + desired_speed = self.desired_speed + accel = self.acceleration + decel = self.deceleration for r in state.relations: if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': @@ -555,6 +559,15 @@ def update(self, state : AllState): print("#### YIELDING PLANNING ####") # Vehicle parameters. + + + """ + TODO: Change the pose of vehicle and pedestrian according to the frames. + Simulation => World frame? + Vehicle => Relative frame? + """ + + x1, y1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y v1 = [curr_v, 0] # Vehicle speed vector @@ -565,8 +578,8 @@ def update(self, state : AllState): v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector # Total simulation time - if curr_v > 0.1: - total_time = min(10, lookahead_distance / curr_v) + 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") @@ -576,74 +589,90 @@ def update(self, state : AllState): 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) + sim = CollisionDetector(x1, y1, 0, x2, y2, 0, v1, v2, total_time, desired_speed, accel) collision_distance = sim.run() - # No collision detected + # No collision detected with acceleration to desired speed. if collision_distance < 0: print("No collision detected.") - self.relation = "None" - # Collision detected + # 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.") + # # Decide the deceleration based on the collision distance. + # brake_deceleration = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) + # if brake_deceleration > self.deceleration: + # if brake_deceleration > self.max_deceleration: + # brake_deceleration = self.max_deceleration + # decel = brake_deceleration if brake_deceleration > decel else decel + # should_brake = True + # break + # break + ############################################### + + print("Collision detected. Try to find yielding speed.") # Update lookahead distance to pedestrian. route_with_lookahead = route.trim(closest_parameter, closest_parameter + collision_distance) - # Simulate if a collision will occur when the vehicle decelerate to yield speed. - sim.set_params(self.yield_speed, self.yield_deceleration * -1) - collision_distance_after_yield = sim.run() - - # Can yield to the pedestrian => Yielding - if collision_distance_after_yield < 0: - print("The vehicle can yield to the pedestrian.") - self.relation = "Yielding" - decel = self.yield_deceleration - - # Cannot yield to the pedestrian => Stopping - else: + 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, accel) + # 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}") + desired_speed = v if v < desired_speed else desired_speed + decel = self.yield_deceleration if self.yield_deceleration > decel else decel + break + + # Collision detected with any yielding speed. + # => Brake to avoid collision. + if collision_distance_after_yield > 0: print("The vehicle is Stopping.") - self.relation = "Stopping" - decel = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) - if decel > self.max_deceleration: - decel = self.max_deceleration - - print(f"Relation: {self.relation}") - print(f"Deceleration: {decel:.2f} m/s^2") - - # relation: None, Yielding, Stopping - # None: No need to speed down - # Yielding: Speed down but not to 0 m/s - # Stopping: Speed down to 0 m/s + # 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 + decel = brake_deceleration if brake_deceleration > decel else decel + should_brake = True break - - - if self.relation == "Yielding": - should_brake = True - should_yield = True - elif self.relation == "Stopping": - should_brake = True - should_yield = False - + # # 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 #========================= + 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, self.deceleration, self.desired_speed, curr_v) - elif should_brake and not should_yield: + traj = longitudinal_plan(route_with_lookahead, accel, decel, desired_speed, curr_v) + elif should_brake: traj = longitudinal_brake(route_with_lookahead, decel, curr_v) - elif should_brake and should_yield: - traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.yield_deceleration, self.yield_speed, curr_v) else: - traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v) + traj = longitudinal_plan(route_with_lookahead, 0.0, decel, desired_speed, curr_v) print(f"Simulation took {time.time() - start_time:.3f} seconds.") From 844f035a56dff1f46325ffbb0641f44373f30a36 Mon Sep 17 00:00:00 2001 From: Patrick Wu Date: Wed, 19 Feb 2025 12:46:46 -0600 Subject: [PATCH 09/14] change the logic in part 2 --- .../onboard/planning/longitudinal_planning.py | 100 +++++++++++------- 1 file changed, 61 insertions(+), 39 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 4e59a9e0f..8add6b24a 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -10,7 +10,7 @@ import matplotlib.patches as patches import math -def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: +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 @@ -50,7 +50,7 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat # 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 + # 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: @@ -63,65 +63,87 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat print("Object is to the left of the vehicle and moving away") return False, 0.0 - if vehicle_front + vehicle_buffer_x >= pedestrian_back: + 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 - vehicle_buffer_x + 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 - - print(relative_v, distance) - if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - vehicle_buffer_y - yield_buffer_y - pedestrian_left) / abs(obj_v_y)): - # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle - print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") - return False, 0.0 - if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - vehicle_buffer_y - yield_buffer_y) / abs(obj_v_y)): - # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle - print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") - 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: - if obj_v_y < 0: - # The object is moving toward the right side of the vehicle - distance_to_pass = obj_y - (vehicle_right - vehicle_buffer_y - yield_buffer_y) + pedestrian_width / 2 - elif obj_v_y > 0: - # The object is moving toward the left side of the vehicle - distance_to_pass = (vehicle_left + vehicle_buffer_y + yield_buffer_y) - obj_y + pedestrian_width / 2 + print(relative_v, distance_with_buffer) - time_to_pass = distance_to_pass / abs(obj_v_y) + 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) - distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y + 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 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 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_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 + 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 - return True, [distance_to_move, time_to_pass] + # 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: - deceleration = relative_v ** 2 / (2 * distance) - if deceleration > max_deceleration: - return True, max_deceleration - if deceleration < min_deceleration: + if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: return False, 0.0 - return True, deceleration + + return True, [distance_to_move, time_to_pass] def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float, method : str) -> Trajectory: @@ -895,7 +917,7 @@ def update(self, state : AllState): #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) + 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] From 0ec9d23399d25b58ab8b49ee443dfb2a90b66824 Mon Sep 17 00:00:00 2001 From: GEM e4 user Date: Wed, 19 Feb 2025 14:20:28 -0600 Subject: [PATCH 10/14] Fixed to move GEM (GNSS, brake, steering are working) --- GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml | 3 ++- GEMstack/onboard/interface/gem_hardware.py | 4 +++- GEMstack/onboard/perception/state_estimation.py | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml index b9e719261..af509af66 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml +++ b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml @@ -2,4 +2,5 @@ ros_topics: top_lidar: /ouster/points front_camera: /oak/rgb/image_raw front_depth: /oak/stereo/image_raw - gnss: /novatel/inspva + gnss: /septentrio_gnss/insnavgeod + diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index fc6431f21..d50a38a0f 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -1,7 +1,7 @@ from .gem import * from ...utils import settings import math - +import time # ROS Headers import rospy from std_msgs.msg import String, Bool, Float32, Float64 @@ -178,6 +178,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva): else: def callback_with_gnss_reading(msg: INSNavGeod): pose = ObjectPose(ObjectFrameEnum.GLOBAL, + t = time.time(), x=msg.longitude, y=msg.latitude, z=msg.height, @@ -185,6 +186,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): roll=math.radians(msg.roll), pitch=math.radians(msg.pitch), ) + # print("@@@@@, POSE", pose.x, pose.y) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) diff --git a/GEMstack/onboard/perception/state_estimation.py b/GEMstack/onboard/perception/state_estimation.py index db96ad38f..4d19d4fa4 100644 --- a/GEMstack/onboard/perception/state_estimation.py +++ b/GEMstack/onboard/perception/state_estimation.py @@ -37,8 +37,8 @@ def state_outputs(self) -> List[str]: return ['vehicle'] def healthy(self): - return True - # return self.gnss_pose is not None TODO: fix + # return True + return self.gnss_pose is not None def update(self) -> VehicleState: if self.gnss_pose is None: From 94d17e1b2790b1221bcb95e98ce8edde5b44efac Mon Sep 17 00:00:00 2001 From: GEM e4 user Date: Wed, 19 Feb 2025 14:21:26 -0600 Subject: [PATCH 11/14] Adjust the parameters of vehicle xy temporalily (need to be fixed in state estimation --- .../onboard/planning/longitudinal_planning.py | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index d967d688e..bb2cbe44a 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -15,6 +15,7 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat # 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] @@ -838,8 +839,8 @@ class YieldTrajectoryPlanner(Component): def __init__(self): self.route_progress = None self.t_last = None - self.acceleration = 0.5 - self.desired_speed = 1.0 + self.acceleration = 0.75 # 0.5 is not enough to start + self.desired_speed = 3.0 # cannot got more than 1.5 m/s self.deceleration = 2.0 self.min_deceleration = 1.0 @@ -866,12 +867,17 @@ def update(self, state : AllState): 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_x = vehicle.pose.x * 20 + curr_y = vehicle.pose.y * 20 curr_v = vehicle.v - abs_x = curr_x + state.start_vehicle_pose.x - abs_y = curr_y + state.start_vehicle_pose.y + print("@@@@@ PLAN", curr_x, curr_y, curr_v) + + # abs_x = curr_x + state.start_vehicle_pose.x + # abs_y = curr_y + state.start_vehicle_pose.y + 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: @@ -895,6 +901,10 @@ def update(self, state : AllState): #get the object we are yielding to obj = state.agents[r.obj2] + + + print("@@@@@ PEESTRIAN", obj) + detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) if isinstance(deceleration, list): print("@@@@@ INPUT", deceleration) @@ -914,9 +924,10 @@ def update(self, state : AllState): should_yield = True print("should yield: ", should_yield) - + should_accelerate = (not should_yield 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, self.deceleration, self.desired_speed, curr_v, "dt") @@ -925,4 +936,5 @@ def update(self, state : AllState): else: traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") + print(traj) return traj From 60e3d20e01f47b2338d9f4bd69f98bcaef7f0f8a Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Sat, 22 Feb 2025 19:05:30 -0600 Subject: [PATCH 12/14] Fixed for test function --- .../onboard/planning/longitudinal_planning.py | 458 +++++++++--------- test_longitudinal_plan.py | 80 --- testing/test_longitudinal_plan.py | 16 +- 3 files changed, 239 insertions(+), 315 deletions(-) delete mode 100644 test_longitudinal_plan.py diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 4b382b0a4..050276d8e 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -1054,105 +1054,6 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) trajectory = Trajectory(path.frame,points,times,velocities) return trajectory -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.acceleration = 0.5 - self.desired_speed = 1.0 - self.deceleration = 2.0 - - self.min_deceleration = 1.0 - self.max_deceleration = 8.0 - - def state_inputs(self): - return ['all'] - - def state_outputs(self) -> List[str]: - return ['trajectory'] - - def rate(self): - return 10.0 - - def update(self, state : AllState): - 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 - - #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] - - 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, "milestone") - 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, "dt") - - return traj - - # 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 @@ -1163,14 +1064,11 @@ def update(self, state : AllState): # self.route_progress = None # self.t_last = None # self.acceleration = 0.5 -# self.desired_speed = 1.0 # default 1.0 +# self.desired_speed = 1.0 +# 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 # def state_inputs(self): # return ['all'] @@ -1197,161 +1095,263 @@ def update(self, state : AllState): # 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 + # #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) + +# should_yield = False +# yield_deceleration = 0.0 -# # Default values -# should_brake = False -# desired_speed = self.desired_speed -# accel = self.acceleration -# decel = self.deceleration +# print("Current Speed: ", curr_v) # for r in state.relations: # if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': -# #yielding to something, brake +# #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_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) -# #========================= -# """ -# 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. -# """ +# should_accelerate = (not should_yield and curr_v < self.desired_speed) -# print("#### YIELDING PLANNING ####") +# #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") +# 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, "dt") + +# return traj + + +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.acceleration = 0.5 + self.desired_speed = 1.0 # default 1.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 + + 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 -# # Vehicle parameters. + 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 + #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 -# """ -# TODO: Change the pose of vehicle and pedestrian according to the frames. -# Simulation => World frame? -# Vehicle => Relative frame? -# """ + 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) -# x1, y1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y -# v1 = [curr_v, 0] # Vehicle speed vector + # Default values + should_brake = False + desired_speed = self.desired_speed + accel = self.acceleration + decel = self.deceleration -# for n,a in state.agents.items(): + for r in state.relations: + if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': + #yielding to something, brake -# # Pedestrian parameters. -# x2, y2 = a.pose.x, a.pose.y -# v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector + #========================= + """ + 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. + """ -# # 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") + print("#### YIELDING PLANNING ####") -# # 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}))") + # Vehicle parameters. -# # 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, desired_speed, accel) -# collision_distance = sim.run() + + """ + TODO: Change the pose of vehicle and pedestrian according to the frames. + Simulation => World frame? + Vehicle => Relative frame? + """ + + + x1, y1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y + v1 = [curr_v, 0] # Vehicle speed vector + + for n,a in state.agents.items(): + + # Pedestrian parameters. + x2, y2 = a.pose.x, a.pose.y + v2 = [a.velocity[0], a.velocity[1]] # Pedestrian speed vector + + # 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, desired_speed, accel) + collision_distance = sim.run() -# # No collision detected with acceleration to desired speed. -# if collision_distance < 0: -# print("No collision detected.") - -# # 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.") -# # # Decide the deceleration based on the collision distance. -# # brake_deceleration = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) -# # if brake_deceleration > self.deceleration: -# # if brake_deceleration > self.max_deceleration: -# # brake_deceleration = self.max_deceleration -# # decel = brake_deceleration if brake_deceleration > decel else decel -# # should_brake = True -# # break -# # break -# ############################################### - -# print("Collision detected. Try to find yielding speed.") -# # Update lookahead distance to pedestrian. -# route_with_lookahead = route.trim(closest_parameter, closest_parameter + collision_distance) - -# 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, accel) -# # 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}") -# desired_speed = v if v < desired_speed else desired_speed -# decel = self.yield_deceleration if self.yield_deceleration > decel else decel -# break + # No collision detected with acceleration to desired speed. + if collision_distance < 0: + print("No collision detected.") + + # 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.") + # Decide the deceleration based on the collision distance. + brake_deceleration = max(self.deceleration, v1[0]**2 / (2 * (collision_distance))) + if brake_deceleration > self.deceleration: + if brake_deceleration > self.max_deceleration: + brake_deceleration = self.max_deceleration + decel = brake_deceleration if brake_deceleration > decel else decel + should_brake = True + break + break + ############################################### + + print("Collision detected. Try to find yielding speed.") + # Update lookahead distance to pedestrian. + route_with_lookahead = route.trim(closest_parameter, closest_parameter + collision_distance) + + 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, accel) + # 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}") + desired_speed = v if v < desired_speed else desired_speed + decel = self.yield_deceleration if self.yield_deceleration > decel else decel + 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 -# decel = brake_deceleration if brake_deceleration > decel else decel -# should_brake = True - -# 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 + decel = brake_deceleration if brake_deceleration > decel else decel + should_brake = True + + break -# # # UNCOMMENT TO BRAKE FOR ALL PEDESTRIANS -# # should_brake = True -# # desired_speed = 0.0 -# # decel = self.deceleration + # # 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 + # # UNCOMMENT NOT TO BRAKE + # should_brake = False + # desired_speed = self.desired_speed + # decel = self.deceleration -# #========================= + #========================= -# print(f"Desired speed: {desired_speed:.2f} m/s") -# print(f"Deceleration: {decel:.2f} m/s^2") + 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) + 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, accel, decel, desired_speed, curr_v) -# 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) + #choose whether to accelerate, brake, or keep at current velocity + if should_accelerate: + traj = longitudinal_plan(route_with_lookahead, accel, decel, desired_speed, curr_v, "dt") + 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, "dt") -# print(f"Simulation took {time.time() - start_time:.3f} seconds.") + print(f"Simulation took {time.time() - start_time:.3f} seconds.") -# return traj + return traj diff --git a/test_longitudinal_plan.py b/test_longitudinal_plan.py deleted file mode 100644 index 400f34fea..000000000 --- a/test_longitudinal_plan.py +++ /dev/null @@ -1,80 +0,0 @@ -#needed to import GEMstack from top level directory -import sys -import os -sys.path.append(os.getcwd()) - -from GEMstack.state import Path, ObjectFrameEnum -from GEMstack.onboard.planning.longitudinal_planning import longitudinal_plan,longitudinal_brake -import matplotlib.pyplot as plt - -def test_longitudinal_planning(): - test_path = Path(ObjectFrameEnum.START,[(0,0),(1,0),(2,0),(3,0),(4,0),(5,0),(6,0)]) - test_path2 = Path(ObjectFrameEnum.START,[(5,0),(6,1),(7,2),(9,4)]) - - test_traj = longitudinal_brake(test_path, 2.0, 0.0) - assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Braking from 0 m/s (should just stay still)") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - test_traj = longitudinal_brake(test_path, 2.0, 2.0) - assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Braking from 2 m/s") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 0.0) - assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Accelerating from 0 m/s") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - - test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0) - assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Accelerating from 2 m/s") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1) - assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Keeping constant velocity at 3.1 m/s") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0) - assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Too little time to stop, starting at 10 m/s") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - test_traj = longitudinal_brake(test_path, 2.0, 10.0) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Too little time to stop, braking at 10 m/s") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0) - plt.plot(test_traj.times,[p[0] for p in test_traj.points]) - plt.title("Nonuniform planning") - plt.xlabel('time') - plt.ylabel('position') - plt.show() - - - -if __name__ == '__main__': - test_longitudinal_planning() diff --git a/testing/test_longitudinal_plan.py b/testing/test_longitudinal_plan.py index ab16ca5e6..78896e893 100644 --- a/testing/test_longitudinal_plan.py +++ b/testing/test_longitudinal_plan.py @@ -6,7 +6,11 @@ from GEMstack.state import Path, ObjectFrameEnum from GEMstack.onboard.planning.longitudinal_planning import longitudinal_plan,longitudinal_brake import matplotlib.pyplot as plt - + + +mode = "milestone" # milestone, dt, dx + + def test_longitudinal_planning(): test_path = Path(ObjectFrameEnum.START,[(0,0),(1,0),(2,0),(3,0),(4,0),(5,0),(6,0)]) test_path2 = Path(ObjectFrameEnum.START,[(5,0),(6,1),(7,2),(9,4)]) @@ -39,7 +43,7 @@ def test_longitudinal_planning(): plt.ylabel('velocity') plt.show() - test_traj = longitudinal_plan(test_path, 1.1, 2.0, 3.0, 0.0) + test_traj = longitudinal_plan(test_path, 1.1, 2.0, 3.0, 0.0, mode) assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) plt.plot(test_traj.times,[p[0] for p in test_traj.points]) plt.title("Accelerating from 0 m/s") @@ -54,7 +58,7 @@ def test_longitudinal_planning(): plt.show() - test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0) + test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0, mode) assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) plt.plot(test_traj.times,[p[0] for p in test_traj.points]) plt.title("Accelerating from 2 m/s") @@ -69,7 +73,7 @@ def test_longitudinal_planning(): plt.show() - test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1) + test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1, mode) assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) plt.plot(test_traj.times,[p[0] for p in test_traj.points]) plt.title("Keeping constant velocity at 3.1 m/s") @@ -83,7 +87,7 @@ def test_longitudinal_planning(): plt.ylabel('velocity') plt.show() - test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0) + test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0, mode) assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) plt.plot(test_traj.times,[p[0] for p in test_traj.points]) plt.title("Too little time to stop, starting at 10 m/s") @@ -110,7 +114,7 @@ def test_longitudinal_planning(): plt.ylabel('velocity') plt.show() - test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0) + test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0, mode) plt.plot(test_traj.times,[p[0] for p in test_traj.points]) plt.title("Nonuniform planning") plt.xlabel('time') From 89b16df24148bb5a3b227d522b160a103950577f Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Sat, 22 Feb 2025 19:51:31 -0600 Subject: [PATCH 13/14] Added args for sim/real and milestine/dt/dx in launch --- .../onboard/planning/longitudinal_planning.py | 90 +++++++++++++------ launch/pedestrian_detection.yaml | 11 ++- 2 files changed, 75 insertions(+), 26 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 4f4008aad..5d9dbf1aa 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -10,6 +10,14 @@ import matplotlib.patches as patches import math + + +################################################################################ +########## Collisiong Detection ################################################ +################################################################################ + + + class CollisionDetector: """ Simulation class to update positions of two rectangles (vehicle and pedestrian) @@ -213,6 +221,7 @@ def run(self, is_displayed=False): 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.""" @@ -349,6 +358,14 @@ def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentStat return True, [distance_to_move, time_to_pass] + + +################################################################################ +########## 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 @@ -413,7 +430,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) @@ -1055,6 +1072,14 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) trajectory = Trajectory(path.frame,points,times,velocities) return trajectory + + +################################################################################ +########## Yield Trajectory Planner ############################################ +################################################################################ + + + # 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 @@ -1160,7 +1185,7 @@ class YieldTrajectoryPlanner(Component): the desired speed. """ - def __init__(self): + def __init__(self, mode : str = 'real', planner : str = 'milestone'): self.route_progress = None self.t_last = None self.acceleration = 0.75 # 0.5 is not enough to start @@ -1173,6 +1198,10 @@ def __init__(self): self.deceleration = 2.0 self.max_deceleration = 8.0 + # Planner mode + self.mode = mode + self.planner = planner + def state_inputs(self): return ['all'] @@ -1193,18 +1222,27 @@ 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 * 20 - curr_y = vehicle.pose.y * 20 + + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y curr_v = vehicle.v - print("@@@@@ PLAN", curr_x, curr_y, curr_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 + 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 + curr_v = vehicle.v + # print("@@@@@ PLAN", curr_x, curr_y, curr_v) + abs_x = curr_x + abs_y = curr_y + # print("@@@@@ PLAN", abs_x, abs_y) + ############################################### - # abs_x = curr_x + state.start_vehicle_pose.x - # abs_y = curr_y + state.start_vehicle_pose.y - 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: @@ -1255,7 +1293,7 @@ def update(self, state : AllState): """ - x1, y1 = vehicle.pose.x + state.start_vehicle_pose.x, vehicle.pose.y + state.start_vehicle_pose.y + x1, y1 = abs_x, abs_y v1 = [curr_v, 0] # Vehicle speed vector for n,a in state.agents.items(): @@ -1288,17 +1326,17 @@ def update(self, state : AllState): else: ############################################### - # UNCOMMENT NOT TO YIELD: JUST STOP FOR PART1 - 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.deceleration: - if brake_deceleration > self.max_deceleration: - brake_deceleration = self.max_deceleration - decel = brake_deceleration if brake_deceleration > decel else decel - should_brake = True - break - break + # # UNCOMMENT NOT TO YIELD: JUST STOP FOR PART1 + # 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.deceleration: + # if brake_deceleration > self.max_deceleration: + # brake_deceleration = self.max_deceleration + # decel = brake_deceleration if brake_deceleration > decel else decel + # should_brake = True + # break + # break ############################################### print("Collision detected. Try to find yielding speed.") @@ -1356,11 +1394,13 @@ def update(self, state : AllState): # 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, accel, decel, desired_speed, curr_v, "dt") + print(route_with_lookahead) + print(accel, decel, desired_speed, curr_v) + traj = longitudinal_plan(route_with_lookahead, accel, 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, "dt") + 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.") diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 98f49a9aa..524e109dc 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -19,7 +19,11 @@ drive: route_planning: type: StaticRoutePlanner args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] - motion_planning: longitudinal_planning.YieldTrajectoryPlanner + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + args: + mode: 'real' + planner: 'dt' # 'milestone', 'dt', or 'dx' trajectory_tracking: type: pure_pursuit.PurePursuitTrajectoryTracker print: False @@ -97,3 +101,8 @@ variants: # agent_detection : pedestrian_detection.FakePedestrianDetector2D agent_detection : OmniscientAgentDetector #this option reads agents from the simulator state_estimation : OmniscientStateEstimator + planning: + motion_planning: + type: longitudinal_planning.YieldTrajectoryPlanner + args: + mode: 'simulation' \ No newline at end of file From 91632e2d2d6a5dc8fadd5f51d3cd6684968141d6 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Sat, 22 Feb 2025 20:15:44 -0600 Subject: [PATCH 14/14] Modified coordinates among real/sim and perception --- .../onboard/planning/longitudinal_planning.py | 58 ++++++++++++++----- 1 file changed, 45 insertions(+), 13 deletions(-) diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py index 5d9dbf1aa..a75fb56c3 100644 --- a/GEMstack/onboard/planning/longitudinal_planning.py +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -216,7 +216,7 @@ def run(self, is_displayed=False): plt.ioff() plt.show(block=True) - print("Collision distance:", collision_distance) + # print("Collision distance:", collision_distance) return collision_distance @@ -1086,7 +1086,7 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) # the desired speed. # """ -# def __init__(self): +# def __init__(self, mode : str = 'real', planner : str = 'milestone'): # self.route_progress = None # self.t_last = None # self.acceleration = 0.5 @@ -1096,6 +1096,9 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) # self.min_deceleration = 1.0 # self.max_deceleration = 8.0 +# self.mode = mode +# self.planner = planner + # def state_inputs(self): # return ['all'] @@ -1124,6 +1127,22 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) # 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 +# curr_v = vehicle.v +# # 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 @@ -1146,6 +1165,10 @@ def longitudinal_brake(path : Path, deceleration : float, current_speed : float) # #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) @@ -1232,6 +1255,11 @@ def update(self, state : AllState): ############################################### # # 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 @@ -1284,24 +1312,30 @@ def update(self, state : AllState): print("#### YIELDING PLANNING ####") # Vehicle parameters. - - - """ - TODO: Change the pose of vehicle and pedestrian according to the frames. - Simulation => World frame? - Vehicle => Relative frame? - """ - - 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]) @@ -1394,8 +1428,6 @@ def update(self, state : AllState): # 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: - print(route_with_lookahead) - print(accel, decel, desired_speed, curr_v) traj = longitudinal_plan(route_with_lookahead, accel, decel, desired_speed, curr_v, self.planner) elif should_brake: traj = longitudinal_brake(route_with_lookahead, decel, curr_v)