From 030b36eed7deb60fc4e7be5e4c0f89deee959d91 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Fri, 29 Oct 2021 11:52:04 +0200 Subject: [PATCH] Fixed collision detection for behavior agent --- CHANGELOG.md | 1 + .../carla/agents/navigation/basic_agent.py | 28 ++++++---- .../carla/agents/navigation/behavior_agent.py | 56 ------------------- 3 files changed, 17 insertions(+), 68 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ea039eae0..2f8be76d9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ * Added missing dependency `libomp5` to Release.Dockerfile * Added physical simulation to vehicle doors, capable of opening and closing * Fixed global route planner crash when being used at maps without lane markings + * Improved collision detection of the Python agents * Added the new VehicleLightStage to the Traffic Manager to dynamically update the vehicle lights. * Added two new examples to PythonAPI/util: Conversion of OpenStreetMaps to OpenDRIVE maps `osm_to_xodr.py` and Extraction of map spawn points `extract_spawn_points.py` diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index addbc43f7..d3112e212 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -15,7 +15,7 @@ from shapely.geometry import Polygon from agents.navigation.local_planner import LocalPlanner from agents.navigation.global_route_planner import GlobalRoutePlanner -from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location +from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location, compute_distance class BasicAgent(object): @@ -167,7 +167,7 @@ class BasicAgent(object): # Check for possible vehicle obstacles max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed - affected_by_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) + affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) if affected_by_vehicle: hazard_detected = True @@ -249,7 +249,7 @@ class BasicAgent(object): return (False, None) - def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None): + def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, lane_offset=0): """ Method to check if there is a vehicle in front of the agent blocking its path. @@ -259,7 +259,7 @@ class BasicAgent(object): If None, the base threshold value is used """ if self._ignore_vehicles: - return (False, None) + return (False, None, -1) if not vehicle_list: vehicle_list = self._world.get_actors().filter("*vehicle*") @@ -270,6 +270,10 @@ class BasicAgent(object): ego_transform = self._vehicle.get_transform() ego_wpt = self._map.get_waypoint(self._vehicle.get_location()) + # Get the right offset + if ego_wpt.lane_id < 0 and lane_offset != 0: + lane_offset *= -1 + # Get the transform of the front of the ego ego_forward_vector = ego_transform.get_forward_vector() ego_extent = self._vehicle.bounding_box.extent.x @@ -286,11 +290,11 @@ class BasicAgent(object): # Simplified version for outside junctions if not ego_wpt.is_junction or not target_wpt.is_junction: - if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id: + if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset: next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0] if not next_wpt: continue - if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id: + if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset: continue target_forward_vector = target_transform.get_forward_vector() @@ -301,8 +305,8 @@ class BasicAgent(object): y=target_extent * target_forward_vector.y, ) - if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [0, 90]): - return (True, target_vehicle) + if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [low_angle_th, up_angle_th]): + return (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location)) # Waypoints aren't reliable, check the proximity of the vehicle to the route else: @@ -327,7 +331,7 @@ class BasicAgent(object): if len(route_bb) < 3: # 2 points don't create a polygon, nothing to check - return (False, None) + return (False, None, -1) ego_polygon = Polygon(route_bb) # Compare the two polygons @@ -344,8 +348,8 @@ class BasicAgent(object): target_polygon = Polygon(target_list) if ego_polygon.intersects(target_polygon): - return (True, target_vehicle) + return (True, target_vehicle, compute_distance(target_vehicle.get_location(), ego_location)) - return (False, None) + return (False, None, -1) - return (False, None) + return (False, None, -1) diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 59c9f03cc..2a5071824 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -81,62 +81,6 @@ class BehaviorAgent(BasicAgent): if self._incoming_direction is None: self._incoming_direction = RoadOption.LANEFOLLOW - def _vehicle_obstacle_detected(self, vehicle_list, proximity_th, up_angle_th, low_angle_th=0, lane_offset=0): - """ - Check if a given vehicle is an obstacle in our way. To this end we take - into account the road and lane the target vehicle is on and run a - geometry test to check if the target vehicle is under a certain distance - in front of our ego vehicle. We also check the next waypoint, just to be - sure there's not a sudden road id change. - - WARNING: This method is an approximation that could fail for very large - vehicles, which center is actually on a different lane but their - extension falls within the ego vehicle lane. Also, make sure to remove - the ego vehicle from the list. Lane offset is set to +1 for right lanes - and -1 for left lanes, but this has to be inverted if lane values are - negative. - - :param vehicle_list: list of potential obstacle to check - :param proximity_th: threshold for the agent to be alerted of - a possible collision - :param up_angle_th: upper threshold for angle - :param low_angle_th: lower threshold for angle - :param lane_offset: for right and left lane changes - :return: a tuple given by (bool_flag, vehicle, distance), where: - - bool_flag is True if there is a vehicle ahead blocking us - and False otherwise - - vehicle is the blocker object itself - - distance is the meters separating the two vehicles - """ - ego_transform = self._vehicle.get_transform() - ego_location = ego_transform.location - ego_wpt = self._map.get_waypoint(ego_location) - - # Get the right offset - if ego_wpt.lane_id < 0 and lane_offset != 0: - lane_offset *= -1 - - for target_vehicle in vehicle_list: - - target_transform = target_vehicle.get_transform() - target_location = target_transform.location - # If the object is not in our next or current lane it's not an obstacle - - target_wpt = self._map.get_waypoint(target_location) - if target_wpt.road_id != ego_wpt.road_id or \ - target_wpt.lane_id != ego_wpt.lane_id + lane_offset: - next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=5)[0] - if target_wpt.road_id != next_wpt.road_id or \ - target_wpt.lane_id != next_wpt.lane_id + lane_offset: - continue - - if is_within_distance(target_transform, ego_transform, - proximity_th, [low_angle_th, up_angle_th]): - - return (True, target_vehicle, compute_distance(target_location, ego_location)) - - return (False, None, -1) - def traffic_light_manager(self): """ This method is in charge of behaviors for red lights.