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

Merge pull request #1198 from carla-simulator/pravinblaze-agent 

Pravinblaze agent
This commit is contained in:
germanros1987 2019-01-30 13:55:47 -08:00 committed by GitHub
parent c22c3d9c07 f7b44d1726
commit 39f8e34bcc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 108 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

@ -1,6 +1,8 @@
## Latest changes
* Fixed global route planner to handle round about turns and made the code consistent with local planner
* Basic agent integrated with global router
* Fixed local planner to avoid premature route pruning at path overlaps
* Upgraded to Unreal Engine 4.21
* Upgraded Boost to 1.69.0
* Added point transformation functionality for LibCarla and PythonAPI

104
PythonAPI/agents/navigation/basic_agent.py
View File

@ -10,10 +10,17 @@
waypoints and avoiding other vehicles.
The agent also responds to traffic lights. """
import math
import numpy as np
import carla
from agents.navigation.agent import *
from agents.navigation.local_planner import LocalPlanner
from agents.navigation.local_planner import compute_connection, RoadOption
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
from agents.tools.misc import vector
class BasicAgent(Agent):
"""
@ -28,49 +35,88 @@ class BasicAgent(Agent):
"""
super(BasicAgent, self).__init__(vehicle)
self._proximity_threshold = 10.0 # meters
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
self._local_planner = LocalPlanner(self._vehicle, opt_dict={'target_speed' : target_speed})
self._hop_resolution = 2.0
# setting up global router
self._current_plan = None
def set_destination(self, location):
start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
end_waypoint = self._map.get_waypoint(carla.Location(location[0],
location[1],
location[2]))
end_waypoint = self._map.get_waypoint(
carla.Location(location[0], location[1], location[2]))
solution = []
# Setting up global router
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.transform.location.x
y2 = end_waypoint.transform.location.y
route = grp.plan_route((x1, y1), (x2, y2))
current_waypoint = start_waypoint
active_list = [ [(current_waypoint, RoadOption.LANEFOLLOW)] ]
route.append(RoadOption.VOID)
for action in route:
solution = []
while not solution:
for _ in range(len(active_list)):
trajectory = active_list.pop()
if len(trajectory) > 1000:
continue
# Generate waypoints to next junction
wp_choice = current_waypoint.next(self._hop_resolution)
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
solution.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(self._hop_resolution)
# Stop at destination
if current_waypoint.transform.location.distance(
end_waypoint.transform.location) < self._hop_resolution: break
if action == RoadOption.VOID: break
# expand this trajectory
current_waypoint, _ = trajectory[-1]
next_waypoints = current_waypoint.next(5.0)
while len(next_waypoints) == 1:
next_option = compute_connection(current_waypoint, next_waypoints[0])
current_distance = next_waypoints[0].transform.location.distance(end_waypoint.transform.location)
if current_distance < 5.0:
solution = trajectory + [(end_waypoint, RoadOption.LANEFOLLOW)]
break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# keep adding nodes
trajectory.append((next_waypoints[0], next_option))
current_waypoint, _ = trajectory[-1]
next_waypoints = current_waypoint.next(5.0)
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(current_transform.rotation.yaw)),
y=math.sin(math.radians(current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
if not solution:
# multiple choices
for waypoint in next_waypoints:
next_option = compute_connection(current_waypoint, waypoint)
active_list.append(trajectory + [(waypoint, next_option)])
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float('inf')
# Choose correct path
for wp_select in wp_choice:
v_select = vector(
current_location, wp_select.transform.location)
cross = float('inf')
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction*np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(self._hop_resolution)[0]
while current_waypoint.is_intersection:
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(self._hop_resolution)[0]
assert solution

17
PythonAPI/agents/navigation/global_route_planner.py
View File

@ -46,13 +46,12 @@ class GlobalRoutePlanner(object):
The following function generates the route plan based on
origin : tuple containing x, y of the route's start position
destination : tuple containing x, y of the route's end position
return : list of turn by turn navigation decisions as
NavEnum elements
Possible values (for now) are START, GO_STRAIGHT, LEFT, RIGHT, STOP
agents.navigation.local_planner.RoadOption elements
Possible values (for now) are STRAIGHT, LEFT, RIGHT, LANEFOLLOW, VOID
"""
threshold = math.radians(5.0)
threshold = math.radians(4.0)
route = self.path_search(origin, destination)
plan = []
@ -99,10 +98,8 @@ class GlobalRoutePlanner(object):
"""
This function finds the shortest path connecting origin and destination
using A* search with distance heuristic.
origin : tuple containing x, y co-ordinates of start position
desitnation : tuple containing x, y co-ordinates of end position
return : path as list of node ids (as int) of the graph self._graph
connecting origin and destination
"""
@ -123,7 +120,6 @@ class GlobalRoutePlanner(object):
"""
This function finds the road segment closest to (x, y)
x, y : co-ordinates of the point to be localized
return : pair of points, tuple of tuples containing co-ordinates
of points that represents the road segment closest to x, y
"""
@ -156,7 +152,6 @@ class GlobalRoutePlanner(object):
net_vector - unit vector of the chord from entry to exit
intersection - boolean indicating if the edge belongs to an
intersection
return : graph -> networkx graph representing the world map,
id_map-> mapping from (x,y) to node id
"""
@ -194,10 +189,8 @@ class GlobalRoutePlanner(object):
def distance(self, point1, point2):
"""
returns the distance between point1 and point2
point1 : (x,y) of first point
point2 : (x,y) of second point
return : distance from point1 to point2
"""
x1, y1 = point1
@ -207,10 +200,8 @@ class GlobalRoutePlanner(object):
def unit_vector(self, point1, point2):
"""
This function returns the unit vector from point1 to point2
point1 : (x,y) of first point
point2 : (x,y) of second point
return : tuple containing x and y components of unit vector
from point1 to point2
"""
@ -226,10 +217,8 @@ class GlobalRoutePlanner(object):
def dot(self, vector1, vector2):
"""
This function returns the dot product of vector1 with vector2
vector1 : x, y components of first vector
vector2 : x, y components of second vector
return : dot porduct scalar between vector1 and vector2
"""
return vector1[0] * vector2[0] + vector1[1] * vector2[1]

22
PythonAPI/agents/navigation/local_planner.py
View File

@ -14,7 +14,6 @@ import random
import carla
from agents.navigation.controller import VehiclePIDController
from agents.navigation.global_route_planner import NavEnum
from agents.tools.misc import distance_vehicle, draw_waypoints
class RoadOption(Enum):
@ -72,9 +71,11 @@ class LocalPlanner(object):
self._vehicle_controller = None
self._global_plan = None
# queue with tuples of (waypoint, RoadOption)
self._waypoints_queue = deque(maxlen=200)
self._waypoints_queue = deque(maxlen=600)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
#
# initializing controller
self.init_controller(opt_dict)
def __del__(self):
@ -202,10 +203,19 @@ class LocalPlanner(object):
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
# target waypoint
self._target_waypoint, self._target_road_option = self._waypoints_queue[0]
self._target_waypoint, self._target_road_option = self._waypoint_buffer[0]
# move using PID controllers
control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint)
@ -213,13 +223,13 @@ class LocalPlanner(object):
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoints_queue):
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(
waypoint, vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoints_queue.popleft()
self._waypoint_buffer.popleft()
if debug:
draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0)

12
PythonAPI/agents/tools/misc.py
View File

@ -88,3 +88,15 @@ def distance_vehicle(waypoint, vehicle_transform):
dy = waypoint.transform.location.y - loc.y
return math.sqrt(dx * dx + dy * dy)
def vector(location_1, location_2):
"""
Returns the unit vector from location_1 to location_2
location_1, location_2 : carla.Location objects
"""
x = location_2.x - location_1.x
y = location_2.y - location_1.y
z = location_2.z - location_1.z
norm = np.linalg.norm([x, y, z])
return [x/norm, y/norm, z/norm]