Merge branch 'master' into NoRenderingMode
This commit is contained in:
Néstor Subirón
GitHub
commit
40928db8e7
|
|
@ -1,7 +1,8 @@
|
|||
## Latest changes
|
||||
|
||||
* Updated the Python API to enable the retrieval of a vehicle's speed limit and impacting traffic light
|
||||
* Added functionality to the Python API to the allow control over traffic lights
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -127,7 +127,10 @@ namespace detail {
|
|||
}
|
||||
|
||||
bool Simulator::DestroyActor(Actor &actor) {
|
||||
auto success = _client.DestroyActor(actor.Serialize());
|
||||
bool success = true;
|
||||
if (actor.GetTypeId() != "sensor.other.lane_detector") { /// @todo
|
||||
success = _client.DestroyActor(actor.Serialize());
|
||||
}
|
||||
if (success) {
|
||||
// Remove it's persistent state so it cannot access the client anymore.
|
||||
actor.GetEpisode().Clear();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -8,21 +8,11 @@ This module provides GlobalRoutePlanner implementation.
|
|||
import math
|
||||
from enum import Enum
|
||||
|
||||
import numpy as np
|
||||
import networkx as nx
|
||||
|
||||
import carla
|
||||
|
||||
|
||||
class NavEnum(Enum):
|
||||
"""
|
||||
Enumeration class containing possible navigation decisions
|
||||
"""
|
||||
START = "START"
|
||||
GO_STRAIGHT = "GO_STRAIGHT"
|
||||
LEFT = "LEFT"
|
||||
RIGHT = "RIGHT"
|
||||
FOLLOW_LANE = "FOLLOW_LANE"
|
||||
STOP = "STOP"
|
||||
pass
|
||||
from local_planner import RoadOption
|
||||
|
||||
|
||||
class GlobalRoutePlanner(object):
|
||||
|
|
@ -56,45 +46,43 @@ 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,
|
||||
FOLLOW_LANE, STOP
|
||||
agents.navigation.local_planner.RoadOption elements
|
||||
Possible values (for now) are STRAIGHT, LEFT, RIGHT, LANEFOLLOW, VOID
|
||||
"""
|
||||
|
||||
threshold = 0.0523599 # 5 degrees
|
||||
threshold = math.radians(4.0)
|
||||
route = self.path_search(origin, destination)
|
||||
plan = []
|
||||
plan.append(NavEnum.START)
|
||||
|
||||
# Compare current edge and next edge to decide on action
|
||||
for i in range(len(route) - 2):
|
||||
current_edge = self._graph.edges[route[i], route[i + 1]]
|
||||
next_edge = self._graph.edges[route[i + 1], route[i + 2]]
|
||||
if not current_edge['intersection']:
|
||||
cv = current_edge['exit_vector']
|
||||
nv = None
|
||||
if next_edge['intersection']:
|
||||
nv = next_edge['net_vector']
|
||||
ca = round(math.atan2(*cv[::-1]) * 180 / math.pi)
|
||||
na = round(math.atan2(*nv[::-1]) * 180 / math.pi)
|
||||
angle_list = [ca, na]
|
||||
for i in range(len(angle_list)):
|
||||
if angle_list[i] > 0:
|
||||
angle_list[i] = 180 - angle_list[i]
|
||||
elif angle_list[i] < 0:
|
||||
angle_list[i] = -1 * (180 + angle_list[i])
|
||||
ca, na = angle_list
|
||||
if abs(na - ca) < threshold:
|
||||
action = NavEnum.GO_STRAIGHT
|
||||
elif na - ca > 0:
|
||||
action = NavEnum.LEFT
|
||||
else:
|
||||
action = NavEnum.RIGHT
|
||||
else:
|
||||
action = NavEnum.FOLLOW_LANE
|
||||
cv = current_edge['exit_vector']
|
||||
nv = next_edge['net_vector']
|
||||
cv, nv = cv + (0,), nv + (0,) # Making vectors 3D
|
||||
num_edges = 0
|
||||
cross_list = []
|
||||
# Accumulating cross products of all other paths
|
||||
for neighbor in self._graph.neighbors(route[i+1]):
|
||||
num_edges+=1
|
||||
if neighbor != route[i + 2]:
|
||||
select_edge = self._graph.edges[route[i+1], neighbor]
|
||||
sv = select_edge['net_vector']
|
||||
cross_list.append(np.cross(cv, sv)[2])
|
||||
# Calculating turn decision
|
||||
if next_edge['intersection'] and num_edges > 1:
|
||||
next_cross = np.cross(cv, nv)[2]
|
||||
deviation = math.acos(np.dot(cv, nv) /\
|
||||
(np.linalg.norm(cv)*np.linalg.norm(nv)))
|
||||
if deviation < threshold:
|
||||
action = RoadOption.STRAIGHT
|
||||
elif next_cross < min(cross_list):
|
||||
action = RoadOption.LEFT
|
||||
elif next_cross > max(cross_list):
|
||||
action = RoadOption.RIGHT
|
||||
plan.append(action)
|
||||
plan.append(NavEnum.STOP)
|
||||
return plan
|
||||
|
||||
def _distance_heuristic(self, n1, n2):
|
||||
|
|
@ -110,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
|
||||
"""
|
||||
|
|
@ -134,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
|
||||
"""
|
||||
|
|
@ -160,13 +145,13 @@ class GlobalRoutePlanner(object):
|
|||
The topology is read from self._topology.
|
||||
graph node properties:
|
||||
vertex - (x,y) of node's position in world map
|
||||
num_edges - Number of exit edges from the node
|
||||
graph edge properties:
|
||||
entry_vector - unit vector along tangent at entry point
|
||||
exit_vector - unit vector along tangent at exit point
|
||||
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
|
||||
"""
|
||||
|
|
@ -204,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
|
||||
|
|
@ -217,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
|
||||
"""
|
||||
|
|
@ -236,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]
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -336,7 +336,7 @@ class HUD(object):
|
|||
|
||||
def on_world_tick(self, timestamp):
|
||||
self._server_clock.tick()
|
||||
self.server_fps = self._server_clock.get_fps()
|
||||
self.server_fps = np.nan_to_num(self._server_clock.get_fps())
|
||||
self.frame_number = timestamp.frame_count
|
||||
self.simulation_time = timestamp.elapsed_seconds
|
||||
|
||||
|
|
@ -573,7 +573,7 @@ class LaneInvasionSensor(object):
|
|||
# -- GnssSensor --------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
|
||||
|
||||
class GnssSensor(object):
|
||||
def __init__(self, parent_actor):
|
||||
self.sensor = None
|
||||
|
|
@ -587,7 +587,7 @@ class GnssSensor(object):
|
|||
# reference.
|
||||
weak_self = weakref.ref(self)
|
||||
self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event))
|
||||
|
||||
|
||||
@staticmethod
|
||||
def _on_gnss_event(weak_self, event):
|
||||
self = weak_self()
|
||||
|
|
|
|||
|
|
@ -90,12 +90,12 @@ if ${REMOVE_INTERMEDIATE} ; then
|
|||
|
||||
rm -Rf ${UE4_INTERMEDIATE_FOLDERS}
|
||||
|
||||
rm -f Makefile
|
||||
|
||||
pushd "${CARLAUE4_PLUGIN_ROOT_FOLDER}" >/dev/null
|
||||
|
||||
rm -Rf ${UE4_INTERMEDIATE_FOLDERS}
|
||||
|
||||
rm -f Makefile
|
||||
|
||||
popd >/dev/null
|
||||
|
||||
fi
|
||||
|
|
|
|||
Loading…
Reference in New Issue