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
import matplotlib
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:
