* Fixed false red signal detection
* Fixed route interpretation logic EOW work desktop commit
This commit is contained in:
Praveen
Praveen Kumar
parent
1530144f71
commit
106d3e116c
|
|
@ -132,7 +132,7 @@ class Agent(object):
|
|||
magnitude, angle = compute_magnitude_angle(loc,
|
||||
ego_vehicle_location,
|
||||
self._vehicle.get_transform().rotation.yaw)
|
||||
if magnitude < 80.0 and angle < min(25.0, min_angle):
|
||||
if magnitude < 60.0 and angle < min(25.0, min_angle):
|
||||
sel_magnitude = magnitude
|
||||
sel_traffic_light = traffic_light
|
||||
min_angle = angle
|
||||
|
|
|
|||
|
|
@ -63,9 +63,6 @@ class BasicAgent(Agent):
|
|||
x2 = end_waypoint.transform.location.x
|
||||
y2 = end_waypoint.transform.location.y
|
||||
route = grp.plan_route((x1, y1), (x2, y2))
|
||||
print(x1, y1)
|
||||
print(x2, y2)
|
||||
print(route)
|
||||
|
||||
current_waypoint = start_waypoint
|
||||
route.append(RoadOption.VOID)
|
||||
|
|
@ -73,7 +70,7 @@ class BasicAgent(Agent):
|
|||
|
||||
# Generate waypoints to next junction
|
||||
wp_choice = current_waypoint.next(self._hop_resolution)
|
||||
while len(wp_choice) == 1:
|
||||
while len(wp_choice) < 2 and not grp.verify_intersection(wp_choice[0]):
|
||||
current_waypoint = wp_choice[0]
|
||||
solution.append((current_waypoint, RoadOption.LANEFOLLOW))
|
||||
wp_choice = current_waypoint.next(self._hop_resolution)
|
||||
|
|
@ -84,70 +81,68 @@ class BasicAgent(Agent):
|
|||
if action == RoadOption.VOID:
|
||||
break
|
||||
|
||||
# Select appropriate path at the junction
|
||||
if len(wp_choice) > 1 or grp.verify_intersection(wp_choice[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)
|
||||
|
||||
# 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)
|
||||
direction = 0
|
||||
if action == RoadOption.LEFT: direction = 1
|
||||
elif action == RoadOption.RIGHT: direction = -1
|
||||
elif action == RoadOption.STRAIGHT: direction = 0
|
||||
select_criteria = float('inf')
|
||||
|
||||
direction = 0
|
||||
if action == RoadOption.LEFT: direction = 1
|
||||
elif action == RoadOption.RIGHT: direction = -1
|
||||
elif action == RoadOption.STRAIGHT: direction = 0
|
||||
select_criteria = float('inf')
|
||||
# < Seperating overlapping paths >
|
||||
split = False
|
||||
tragectories = []
|
||||
# initializing paths
|
||||
for wp_select in wp_choice:
|
||||
tragectories.append([wp_select])
|
||||
# finding points of seperation
|
||||
while not split and len(tragectories) > 1:
|
||||
# take step in each path
|
||||
for tragectory in tragectories:
|
||||
tragectory.append(tragectory[-1].next(self._path_seperation_hop)[0])
|
||||
# measure seperation
|
||||
for i in range(len(tragectories)-1):
|
||||
if tragectories[i][-1].transform.location.distance(
|
||||
tragectories[i+1][-1].transform.location) > self._path_seperation_threshold:
|
||||
split = True
|
||||
# update waypoints for path choice
|
||||
for i, tragectory in enumerate(tragectories):
|
||||
wp_choice[i] = tragectory[-1]
|
||||
# < Seperating overlapping paths />
|
||||
|
||||
# Seperating overlapping paths
|
||||
split = False
|
||||
tragectories = []
|
||||
# initializing paths
|
||||
for wp_select in wp_choice:
|
||||
tragectories.append([wp_select])
|
||||
# finding points of seperation
|
||||
while not split:
|
||||
# take step in each path
|
||||
for tragectory in tragectories:
|
||||
tragectory.append(tragectory[-1].next(self._path_seperation_hop)[0])
|
||||
# measure seperation
|
||||
for i in range(len(tragectories)-1):
|
||||
if tragectories[i][-1].transform.location.distance(
|
||||
tragectories[i+1][-1].transform.location) > self._path_seperation_threshold:
|
||||
split = True
|
||||
# update waypoints for path choice
|
||||
for i, tragectory in enumerate(tragectories):
|
||||
wp_choice[i] = tragectory[-1]
|
||||
# Choose correct path
|
||||
tragectory_index = 0
|
||||
for i, wp_select in enumerate(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:
|
||||
tragectory_index = i
|
||||
select_criteria = cross
|
||||
current_waypoint = wp_select
|
||||
|
||||
# Choose correct path
|
||||
tragectory_index = 0
|
||||
for i, wp_select in enumerate(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:
|
||||
tragectory_index = i
|
||||
select_criteria = cross
|
||||
current_waypoint = wp_select
|
||||
# Add select tragectory till point of seperation
|
||||
for wp in tragectories[tragectory_index]:
|
||||
solution.append((wp, action))
|
||||
|
||||
# Add select tragectory till point of seperation
|
||||
for wp in tragectories[tragectory_index]:
|
||||
solution.append((wp, action))
|
||||
|
||||
# Generate all waypoints within the junction
|
||||
# along selected path
|
||||
# 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]
|
||||
while current_waypoint.is_intersection:
|
||||
solution.append((current_waypoint, action))
|
||||
current_waypoint = current_waypoint.next(self._hop_resolution)[0]
|
||||
|
||||
assert solution
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ import math
|
|||
import numpy as np
|
||||
import networkx as nx
|
||||
|
||||
import carla
|
||||
from agents.navigation.local_planner import RoadOption
|
||||
|
||||
|
||||
|
|
@ -37,7 +38,8 @@ class GlobalRoutePlanner(object):
|
|||
self._topology = self._dao.get_topology()
|
||||
# Creating graph of the world map and also a maping from
|
||||
# node co-ordinates to node id
|
||||
self._graph, self._id_map = self.build_graph()
|
||||
self._graph, self._id_map = self._build_graph()
|
||||
# self._lane_change_link()
|
||||
|
||||
def plan_route(self, origin, destination):
|
||||
"""
|
||||
|
|
@ -50,7 +52,7 @@ class GlobalRoutePlanner(object):
|
|||
"""
|
||||
|
||||
threshold = math.radians(4.0)
|
||||
route = self.path_search(origin, destination)
|
||||
route = self._path_search(origin, destination)
|
||||
plan = []
|
||||
|
||||
# Compare current edge and next edge to decide on action
|
||||
|
|
@ -96,11 +98,10 @@ class GlobalRoutePlanner(object):
|
|||
"""
|
||||
|
||||
is_intersection = False
|
||||
|
||||
if waypoint.is_intersection :
|
||||
x = waypoint.transform.location.x
|
||||
y = waypoint.transform.location.y
|
||||
segment = self.localise(x, y)
|
||||
segment = self._localise(x, y)
|
||||
entry_node_id = self._id_map[segment['entry']]
|
||||
exit_node_id = self._id_map[segment['exit']]
|
||||
|
||||
|
|
@ -121,7 +122,7 @@ class GlobalRoutePlanner(object):
|
|||
(x2, y2) = self._graph.nodes[n2]['vertex']
|
||||
return ((x1 - x2) ** 2 + (y1 - y2) ** 2) ** 0.5
|
||||
|
||||
def path_search(self, origin, destination):
|
||||
def _path_search(self, origin, destination):
|
||||
"""
|
||||
This function finds the shortest path connecting origin and destination
|
||||
using A* search with distance heuristic.
|
||||
|
|
@ -132,8 +133,8 @@ class GlobalRoutePlanner(object):
|
|||
"""
|
||||
xo, yo = origin
|
||||
xd, yd = destination
|
||||
start = self.localise(xo, yo)
|
||||
end = self.localise(xd, yd)
|
||||
start = self._localise(xo, yo)
|
||||
end = self._localise(xd, yd)
|
||||
|
||||
route = nx.astar_path(
|
||||
self._graph, source=self._id_map[start['entry']],
|
||||
|
|
@ -143,7 +144,7 @@ class GlobalRoutePlanner(object):
|
|||
|
||||
return route
|
||||
|
||||
def localise(self, x, y):
|
||||
def _localise(self, x, y):
|
||||
"""
|
||||
This function finds the road segment closest to (x, y)
|
||||
x, y : co-ordinates of the point to be localized
|
||||
|
|
@ -159,14 +160,14 @@ class GlobalRoutePlanner(object):
|
|||
exitxy = segment['exit']
|
||||
path = segment['path']
|
||||
for xp, yp in [entryxy] + path + [exitxy]:
|
||||
new_distance = self.distance((xp, yp), (x, y))
|
||||
new_distance = self._distance((xp, yp), (x, y))
|
||||
if new_distance < nearest[0]:
|
||||
nearest = (new_distance, segment)
|
||||
|
||||
segment = nearest[1]
|
||||
return segment
|
||||
|
||||
def build_graph(self):
|
||||
def _build_graph(self):
|
||||
"""
|
||||
This function builds a networkx graph representation of topology.
|
||||
The topology is read from self._topology.
|
||||
|
|
@ -204,16 +205,59 @@ class GlobalRoutePlanner(object):
|
|||
graph.add_edge(
|
||||
n1, n2,
|
||||
length=len(path) + 1, path=path,
|
||||
entry_vector=self.unit_vector(
|
||||
entry_vector=self._unit_vector(
|
||||
entryxy, path[0] if len(path) > 0 else exitxy),
|
||||
exit_vector=self.unit_vector(
|
||||
exit_vector=self._unit_vector(
|
||||
path[-1] if len(path) > 0 else entryxy, exitxy),
|
||||
net_vector=self.unit_vector(entryxy, exitxy),
|
||||
intersection=intersection)
|
||||
net_vector=self._unit_vector(entryxy, exitxy),
|
||||
intersection=intersection,
|
||||
type=RoadOption.LANEFOLLOW)
|
||||
|
||||
return graph, id_map
|
||||
|
||||
def distance(self, point1, point2):
|
||||
def _lane_change_link(self):
|
||||
"""
|
||||
This method places zero cost links in the topology graph
|
||||
representing availability of lane changes.
|
||||
"""
|
||||
|
||||
next_waypoint = None
|
||||
next_road_option = None
|
||||
|
||||
for segment in self._topology:
|
||||
lane_change_types = []
|
||||
for point in segment['path']:
|
||||
|
||||
waypoint = self._dao.get_waypoint(*point)
|
||||
if waypoint.lane_change & carla.LaneChange.Right \
|
||||
and RoadOption.CHANGELANERIGHT not in lane_change_types:
|
||||
next_waypoint = waypoint.right_lane()
|
||||
if next_waypoint is not None and next_waypoint.lane_type == 'driving':
|
||||
next_road_option = RoadOption.CHANGELANERIGHT
|
||||
|
||||
elif waypoint.lane_change & carla.LaneChange.Left \
|
||||
and RoadOption.CHANGELANERIGHT not in lane_change_types:
|
||||
next_waypoint = waypoint.left_lane()
|
||||
if next_waypoint is not None and next_waypoint.lane_type == 'driving':
|
||||
next_road_option = RoadOption.CHANGELANELEFT
|
||||
else: pass
|
||||
|
||||
if next_waypoint is not None:
|
||||
next_segment = self._localise(
|
||||
next_waypoint.transform.location.x, next_waypoint.transform.location.x)
|
||||
|
||||
self._graph.add_edge(
|
||||
self._id_map[segment['entry']],
|
||||
self._id_map[next_segment['exit']],
|
||||
type=next_road_option)
|
||||
|
||||
lane_change_types.append(next_road_option)
|
||||
|
||||
next_waypoint = None
|
||||
if len(lane_change_types) == 2:
|
||||
break
|
||||
|
||||
def _distance(self, point1, point2):
|
||||
"""
|
||||
returns the distance between point1 and point2
|
||||
point1 : (x,y) of first point
|
||||
|
|
@ -224,7 +268,7 @@ class GlobalRoutePlanner(object):
|
|||
x2, y2 = point2
|
||||
return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
|
||||
|
||||
def unit_vector(self, point1, point2):
|
||||
def _unit_vector(self, point1, point2):
|
||||
"""
|
||||
This function returns the unit vector from point1 to point2
|
||||
point1 : (x,y) of first point
|
||||
|
|
@ -241,7 +285,7 @@ class GlobalRoutePlanner(object):
|
|||
|
||||
return vector
|
||||
|
||||
def dot(self, vector1, vector2):
|
||||
def _dot(self, vector1, vector2):
|
||||
"""
|
||||
This function returns the dot product of vector1 with vector2
|
||||
vector1 : x, y components of first vector
|
||||
|
|
|
|||
|
|
@ -62,3 +62,9 @@ class GlobalRoutePlannerDAO(object):
|
|||
|
||||
topology.append(seg_dict)
|
||||
return topology
|
||||
|
||||
def get_waypoint(self, x, y):
|
||||
"""
|
||||
The method returns waytpoint at x, y
|
||||
"""
|
||||
return self._wmap.get_waypoint(carla.Location(x=x, y=y))
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@ class RoadOption(Enum):
|
|||
RIGHT = 2
|
||||
STRAIGHT = 3
|
||||
LANEFOLLOW = 4
|
||||
CHANGELANELEFT = 5
|
||||
CHANGELANERIGHT = 6
|
||||
|
||||
|
||||
class LocalPlanner(object):
|
||||
|
|
@ -194,9 +196,8 @@ class LocalPlanner(object):
|
|||
"""
|
||||
|
||||
# not enough waypoints in the horizon? => add more!
|
||||
if len(self.waypoints_queue) < int(self.waypoints_queue.maxlen * 0.5):
|
||||
if not self.global_plan:
|
||||
self._compute_next_waypoints(k=100)
|
||||
if not self._global_plan and len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
|
||||
self._compute_next_waypoints(k=100)
|
||||
|
||||
if len(self.waypoints_queue) == 0:
|
||||
control = carla.VehicleControl()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,101 @@
|
|||
import math
|
||||
import unittest
|
||||
|
||||
import carla
|
||||
|
||||
from global_route_planner import GlobalRoutePlanner
|
||||
from global_route_planner import NavEnum
|
||||
from global_route_planner_dao import GlobalRoutePlannerDAO
|
||||
|
||||
|
||||
class Test_GlobalRoutePlanner(unittest.TestCase):
|
||||
"""
|
||||
Test class for GlobalRoutePlanner class
|
||||
"""
|
||||
|
||||
def setUp(self):
|
||||
# == Utilities test instance without DAO == #
|
||||
self.simple_grp = GlobalRoutePlanner(None)
|
||||
|
||||
# == Integration test instance == #
|
||||
client = carla.Client('localhost', 2000)
|
||||
world = client.get_world()
|
||||
integ_dao = GlobalRoutePlannerDAO(world.get_map())
|
||||
self.integ_grp = GlobalRoutePlanner(integ_dao)
|
||||
self.integ_grp.setup()
|
||||
pass
|
||||
|
||||
def tearDown(self):
|
||||
self.simple_grp = None
|
||||
self.dao_grp = None
|
||||
self.integ_grp = None
|
||||
pass
|
||||
|
||||
def test_plan_route(self):
|
||||
"""
|
||||
Test for GlobalROutePlanner.plan_route()
|
||||
Run this test with carla server running Town03
|
||||
"""
|
||||
plan = self.integ_grp.plan_route((-60, -5), (-77.65, 72.72))
|
||||
self.assertEqual(
|
||||
plan, [NavEnum.START, NavEnum.LEFT, NavEnum.LEFT,
|
||||
NavEnum.GO_STRAIGHT, NavEnum.LEFT, NavEnum.STOP])
|
||||
|
||||
def test_path_search(self):
|
||||
"""
|
||||
Test for GlobalRoutePlanner.path_search()
|
||||
Run this test with carla server running Town03
|
||||
"""
|
||||
self.integ_grp._path_search((191.947, -5.602), (78.730, -50.091))
|
||||
self.assertEqual(
|
||||
self.integ_grp._path_search((196.947, -5.602), (78.730, -50.091)),
|
||||
[256, 157, 158, 117, 118, 59, 55, 230])
|
||||
|
||||
def test_localise(self):
|
||||
"""
|
||||
Test for GlobalRoutePlanner.localise()
|
||||
Run this test with carla server running Town03
|
||||
"""
|
||||
x, y = (200, -250)
|
||||
segment = self.integ_grp._localise(x, y)
|
||||
self.assertEqual(self.integ_grp._id_map[segment['entry']], 5)
|
||||
self.assertEqual(self.integ_grp._id_map[segment['exit']], 225)
|
||||
|
||||
def test_unit_vector(self):
|
||||
"""
|
||||
Test for GlobalROutePlanner.unit_vector()
|
||||
"""
|
||||
vector = self.simple_grp._unit_vector((1, 1), (2, 2))
|
||||
self.assertAlmostEquals(vector[0], 1 / math.sqrt(2))
|
||||
self.assertAlmostEquals(vector[1], 1 / math.sqrt(2))
|
||||
|
||||
def test_dot(self):
|
||||
"""
|
||||
Test for GlobalROutePlanner.test_dot()
|
||||
"""
|
||||
self.assertAlmostEqual(self.simple_grp.dot((1, 0), (0, 1)), 0)
|
||||
self.assertAlmostEqual(self.simple_grp.dot((1, 0), (1, 0)), 1)
|
||||
|
||||
|
||||
def suite():
|
||||
"""
|
||||
Gathering all tests
|
||||
"""
|
||||
|
||||
suite = unittest.TestSuite()
|
||||
suite.addTest(Test_GlobalRoutePlanner('test_unit_vector'))
|
||||
suite.addTest(Test_GlobalRoutePlanner('test_dot'))
|
||||
suite.addTest(Test_GlobalRoutePlanner('test_localise'))
|
||||
suite.addTest(Test_GlobalRoutePlanner('test_path_search'))
|
||||
suite.addTest(Test_GlobalRoutePlanner('test_plan_route'))
|
||||
|
||||
return suite
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
"""
|
||||
Running test suite
|
||||
"""
|
||||
mySuit = suite()
|
||||
runner = unittest.TextTestRunner()
|
||||
runner.run(mySuit)
|
||||
Loading…
Reference in New Issue