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 calculate_route(self, goal):
"""
Function used to calculate a route from the current location to 'goal'
"""
rospy.loginfo("Calculating route to x={}, y={}, z={}".format(
goal.location.x, goal.location.y, goal.location.z))
dao = GlobalRoutePlannerDAO(self.world.get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
route = grp.trace_route(
self.ego_vehicle.get_location(),
carla.Location(goal.location.x, goal.location.y, goal.location.z))
return route
def get_route(self):
map = self.world.get_map()
dao = GlobalRoutePlannerDAO(map, 2)
grp = GlobalRoutePlanner(dao)
grp.setup()
route = dict(grp.trace_route(self.spawn_point.location, self.target.location))
self.route_waypoints = []
self.route_commands = []
self.route_waypoints_unformatted = []
for waypoint in route.keys():
self.route_waypoints.append((waypoint.transform.location.x,waypoint.transform.location.y,waypoint.transform.location.z))
self.route_commands.append(route.get(waypoint))
self.route_waypoints_unformatted.append(waypoint)
self.route_kdtree = KDTree(np.array(self.route_waypoints))
def route_positions_generate(self,start_pos,end_pos):
dao = GlobalRoutePlannerDAO(self.world.get_map(), sampling_resolution=3)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
route = self._grp.trace_route(start_pos, end_pos)
positions = []
for i in range((len(route))):
# xi = route[i][0].transform.location.x
# yi = route[i][0].transform.location.y
position = route[i][0].transform
positions.append(position)
return positions
def _trace_route(self, start_waypoint, end_waypoint):
"""
Private function used to setup a global router and returns the optimal route
from start_waypoint to end_waypoint
"""
# Setting up global router
if self._grp is None:
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(),
self._hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
# Obtain route plan
route = self._grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
return route
def calculate_route(self, goal):
dao = GlobalRoutePlannerDAO(self.mp)
grp = GlobalRoutePlanner(dao)
grp.setup()
if goal_location == True: # System argument 1 for random goal location
route = grp.trace_route(self.veh_pos.transform.location,\
carla.Location(self.goal.transform.location.x,\
self.goal.transform.location.y, self.goal.transform.location.z)) # Determines route based on your goal location and the vehicle initial position
else:
route = grp.trace_route(self.veh_pos.transform.location,\
carla.Location(self.goal.location.x,\
self.goal.location.y, self.goal.location.z))
return route
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)
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
import networkx as nx
import sys
sys.path.append('/home/praveen/workspace/carla/PythonAPI/carla/agents/navigation/')
from local_planner import RoadOption
from global_route_planner import GlobalRoutePlanner
from global_route_planner_dao import GlobalRoutePlannerDAO
client = carla.Client('localhost', 2000)
world = client.get_world()
world_map = world.get_map()
dao = GlobalRoutePlannerDAO(world_map)
grp = GlobalRoutePlanner(dao)
grp.setup()
for edge in grp._graph.edges():
n1, n2 = edge
l1 = grp._graph.nodes[n1]['vertex']
l2 = grp._graph.nodes[n2]['vertex']
x1, y1, _ = l1
x2, y2, _ = l2
x1, x2 = -x1, -x2
edge_obj = grp._graph.edges[n1, n2]
if grp._graph.edges[n1,n2]['type'].value == RoadOption.LANEFOLLOW.value:
if edge_obj['intersection']:
plt.plot([x1, x2], [y1, y2], color='red')
else:
plt.plot([x1, x2], [y1, y2], color='gray')
import time
import os
import sys
sys.path.append('/home/praveen/workspace/carla/PythonAPI/agents/navigation/')
import carla
from global_route_planner import GlobalRoutePlanner
from global_route_planner_dao import GlobalRoutePlannerDAO
client = carla.Client('localhost', 2000)
world = client.get_world()
world_map = world.get_map()
dao = GlobalRoutePlannerDAO(world_map)
grp = GlobalRoutePlanner(dao)
grp.setup()
ego_vehicle = None
possible_vehicles = world.get_actors().filter('vehicle.*')
for vehicle in possible_vehicles:
if vehicle.attributes['role_name'] == "hero":
ego_vehicle = vehicle
clear = lambda: os.system('clear')
# destination = -149, 90 # For Town03
# destination = 88.5, 30 # For Town01
# destination = 200, 9.6
# destination = -7.5, 142 # Town02
# destination = 44.61, -192.88
import carla
sys.path.append(
os.path.abspath(
'/home/praveen/workspace/carla/PythonAPI/agents/navigation/'))
from basic_agent import BasicAgent
from global_route_planner import GlobalRoutePlanner
from global_route_planner_dao import GlobalRoutePlannerDAO
client = carla.Client('localhost', 2000)
world = client.get_world()
world_map = world.get_map()
dao = GlobalRoutePlannerDAO(world_map)
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
# route = grp.plan_route((43.70042037963867, # -7.828187465667725), (44.610050201416016, -192.88201904296875))
route = grp.plan_route((43.70014190673828, -7.828191757202148),
(44.610050201416016, -192.88201904296875))
print(route)
blueprint = world.get_blueprint_library().find('vehicle.lincoln.mkz2017')
blueprint.set_attribute('role_name', 'hero')
spawn_point = carla.Transform(carla.Location(x=43.7, y=-7.8),
carla.Rotation(yaw=180))
vehicle = world.try_spawn_actor(blueprint, spawn_point)