From a1b268aed8fd2edee13a75f65df2e320fe613070 Mon Sep 17 00:00:00 2001 From: Guillermo Date: Wed, 1 Sep 2021 13:24:59 +0200 Subject: [PATCH] Improved vehicle detection at junction --- .../carla/agents/navigation/basic_agent.py | 75 +++++++++++++++---- .../carla/agents/navigation/local_planner.py | 10 ++- PythonAPI/carla/requirements.txt | 1 + 3 files changed, 68 insertions(+), 18 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 80bec1a86..dd93ccccf 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -11,6 +11,7 @@ It can also make use of the global route planner to follow a specifed route import carla from enum import Enum +from shapely.geometry import Polygon from agents.navigation.local_planner import LocalPlanner from agents.navigation.global_route_planner import GlobalRoutePlanner @@ -281,21 +282,65 @@ class BasicAgent(object): for target_vehicle in vehicle_list: target_transform = target_vehicle.get_transform() target_wpt = self._map.get_waypoint(target_transform.location) - if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id: - 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: - continue - target_forward_vector = target_transform.get_forward_vector() - target_extent = target_vehicle.bounding_box.extent.x - target_rear_transform = target_transform - target_rear_transform.location -= carla.Location( - x=target_extent * target_forward_vector.x, - y=target_extent * target_forward_vector.y, - ) + # 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: + 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: + continue + + target_forward_vector = target_transform.get_forward_vector() + target_extent = target_vehicle.bounding_box.extent.x + target_rear_transform = target_transform + target_rear_transform.location -= carla.Location( + x=target_extent * target_forward_vector.x, + 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) + + # Waypoints aren't reliable, check the proximity of the vehicle to the route + else: + ego_location = ego_transform.location + lateral_extent = self._vehicle.bounding_box.extent.y + + # Get geodesic boudnary box + route_bb = [] + for wp, _ in self._local_planner.get_plan(): + if ego_location.distance(wp.transform.location) > max_distance: + break + + right_vector = wp.transform.get_right_vector() + right_point = wp.transform.location + carla.Location( + lateral_extent * right_vector.x, lateral_extent * right_vector.y) + route_bb.append([right_point.x, right_point.y, right_point.z]) + left_point = wp.transform.location + carla.Location( + - lateral_extent * right_vector.x, - lateral_extent * right_vector.y) + route_bb.append([left_point.x, left_point.y, left_point.z]) + + ego_polygon = Polygon(route_bb) + + # Compare the two polygons + for target_vehicle in vehicle_list: + target_extent = target_vehicle.bounding_box.extent.x + if target_vehicle.id == self._vehicle.id: + continue + if ego_location.distance(target_vehicle.get_location()) > max_distance: + continue + + target_bb = target_vehicle.bounding_box + target_vertices = target_bb.get_world_vertices(target_vehicle.get_transform()) + target_list = [[v.x, v.y, v.z] for v in target_vertices] + target_polygon = Polygon(target_list) + + if ego_polygon.intersects(target_polygon): + return (True, target_vehicle) + + return (False, None) - if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [0, 90]): - return (True, target_vehicle) return (False, None) diff --git a/PythonAPI/carla/agents/navigation/local_planner.py b/PythonAPI/carla/agents/navigation/local_planner.py index 82f8c3f1d..08141cec5 100644 --- a/PythonAPI/carla/agents/navigation/local_planner.py +++ b/PythonAPI/carla/agents/navigation/local_planner.py @@ -181,12 +181,12 @@ class LocalPlanner(object): def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): """ Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs - If 'clean_queue`, erases the previous plan, and if not, it is added to the old one - The 'stop_waypoint_creation' flag avoids creating more random waypoints + The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one + The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints :param current_plan: list of (carla.Waypoint, RoadOption) :param stop_waypoint_creation: bool - :param ceal_queue: bool + :param clean_queue: bool :return: """ if clean_queue: @@ -275,6 +275,10 @@ class LocalPlanner(object): except IndexError as i: return None, RoadOption.VOID + def get_plan(self): + """Returns the current plan of the local planner""" + return self._waypoints_queue + def done(self): """ Returns whether or not the planner has finished diff --git a/PythonAPI/carla/requirements.txt b/PythonAPI/carla/requirements.txt index fcfed5a2d..fa5196e51 100644 --- a/PythonAPI/carla/requirements.txt +++ b/PythonAPI/carla/requirements.txt @@ -2,3 +2,4 @@ networkx numpy; python_version < '3.0' numpy==1.18.4; python_version >= '3.0' distro +Shapely==1.6.4.post2