p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fixed collision detection for behavior agent

This commit is contained in:
Guillermo 2021-10-29 11:52:04 +02:00 committed by bernat
parent 5f07128176
commit 030b36eed7
3 changed files with 17 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -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`

28
PythonAPI/carla/agents/navigation/basic_agent.py
View File

@ -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)

56
PythonAPI/carla/agents/navigation/behavior_agent.py
View File

@ -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.