diff --git a/dash/Dashboard.py b/dash/Dashboard.py index 657e5812..6779a93b 100644 --- a/dash/Dashboard.py +++ b/dash/Dashboard.py @@ -118,7 +118,8 @@ def run_dash_process(self, traffic_state_sharr, debug_shdata): self.plot_vehicle_sd(traj, cand) #behavior tree self.tree_msg.delete("1.0", "end") - self.tree_msg.insert("1.0", btree_snapshot) + if btree_snapshot: + self.tree_msg.insert("1.0", btree_snapshot) else: #vehicles without planner: self.plot_cartesian_chart(vid, vehicles, pedestrians) diff --git a/requirements/RequirementViolationEvents.py b/requirements/RequirementViolationEvents.py index bce836c6..5265fea3 100644 --- a/requirements/RequirementViolationEvents.py +++ b/requirements/RequirementViolationEvents.py @@ -60,6 +60,21 @@ def __init__(self, agent_id, vid): # That reassignment is necessary for the update to work in multiprocessing agent_collisions[agent_id] = collision_state +class CollisionWithPedestrian(UnmetRequirement): + def __init__(self, agent_id, pid): + collision_state = agent_collisions[agent_id] + + if pid not in collision_state or agent_ticks[agent_id] - 5 > collision_state[pid]: + self.raise_it(agent_id, { + 'colliderId': pid, + 'message': 'v' + str(agent_id) + ' bounding box overlapped with the pedestrian agent p' + str(pid), + }) + + collision_state[pid] = agent_ticks[agent_id] + + # That reassignment is necessary for the update to work in multiprocessing + agent_collisions[agent_id] = collision_state + class GlobalTick: def __init__(self): global global_tick diff --git a/requirements/RequirementsChecker.py b/requirements/RequirementsChecker.py index b7a141e7..0c5e58a7 100644 --- a/requirements/RequirementsChecker.py +++ b/requirements/RequirementsChecker.py @@ -3,7 +3,7 @@ from math import cos, radians, sin from Actor import ActorSimState -from requirements.RequirementViolationEvents import CollisionWithVehicle, GoalOvershot, ScenarioCompletion, ScenarioEnd +from requirements.RequirementViolationEvents import CollisionWithVehicle, CollisionWithPedestrian, GoalOvershot, ScenarioCompletion, ScenarioEnd from sv.SDVTrafficState import * class RequirementsChecker: @@ -23,6 +23,9 @@ def analyze(self, traffic_state:TrafficState): self.vehicles.update(traffic_state.traffic_vehicles) self.vehicles.update(traffic_state.traffic_vehicles_orp) + self.pedestrians = {} + self.pedestrians.update(traffic_state.pedestrians) + for condition in self.conditions: condition(traffic_state) @@ -65,8 +68,16 @@ def detect_collisions(self, traffic_state:TrafficState): vehicle_box = self.calculate_rectangular_bounding_box(vehicle) if self.do_polygons_intersect(ego_box, vehicle_box): CollisionWithVehicle(ego_vehicle.id, vid) + + for pid, pedestrian in self.pedestrians.items(): + if pedestrian.sim_state not in [ActorSimState.ACTIVE, ActorSimState.ACTIVE.value]: + continue + + pedestrian_pos = [pedestrian.state.x, pedestrian.state.y] - + if self.check_circle_rectangle_collision(pedestrian_pos, ego_box, pedestrian.PEDESTRIAN_RADIUS): + CollisionWithPedestrian(ego_vehicle.id, pid) + def detect_goal_overshot(self, traffic_state:TrafficState): """ Checks if the vehicle has reached or passed the goal point in the frenet frame. """ @@ -102,6 +113,11 @@ def detect_goal_overshot(self, traffic_state:TrafficState): if self.goal_ends_simulation: ScenarioEnd() raise ScenarioCompletion() + + def forced_exit(self): + #store data upon forced exit + ScenarioEnd() + raise ScenarioCompletion def do_polygons_intersect(self, polygon_a, polygon_b): """ @@ -159,6 +175,19 @@ def do_polygons_intersect(self, polygon_a, polygon_b): return False; return True + + def check_circle_rectangle_collision(self, centre, rectangle, radius): + """ + checks if the circular bounding box of a pedestrian intersects with the bounding box of a vehicle + """ + centre_x, centre_y = centre + + closest_x = np.clip(centre_x, np.min(rectangle[:, 0]), np.max(rectangle[:, 0])) + closest_y = np.clip(centre_y, np.min(rectangle[:, 1]), np.max(rectangle[:, 1])) + + distance = np.sqrt((centre_x - closest_x)**2 + (centre_y - closest_y)**2) + + return distance < radius def rotate(self, center_x, center_y, x, y, degree_theta): radian_theta = radians(degree_theta) diff --git a/sp/Pedestrian.py b/sp/Pedestrian.py index 182440e1..3e9fd60c 100644 --- a/sp/Pedestrian.py +++ b/sp/Pedestrian.py @@ -30,7 +30,9 @@ class Pedestrian(Actor): EP_TYPE = 3 SP_TYPE = 4 - PEDESTRIAN_RADIUS = 0.2 + # Source NCAP: https://cdn.euroncap.com/media/58226/euro-ncap-aeb-vru-test-protocol-v303.pdf + # Pedestrian dimensions (width: 0.5 m, length: 0.6 m) approximated by a circle with radius 0.27 m + PEDESTRIAN_RADIUS = 0.27 def __init__(self, id, name='', start_state=[0.0,0.0,0.0, 0.0,0.0,0.0], yaw=0.0): super().__init__(id, name, start_state, yaw=yaw)