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

Checkpoint ! Major code overhauling ahead.

This commit is contained in:
Praveen Kumar 2019-03-05 12:04:10 +05:30
parent acd5ce0127
commit 31b80f86ac
3 changed files with 70 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,8 +39,8 @@ class BasicAgent(Agent):
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
args_lateral_dict = {
'K_P': 0.5,
'K_D': 0.0,
'K_P': 0.8,
'K_D': 0.01,
'K_I': 0,
'dt': 1.0/20.0}
self._local_planner = LocalPlanner(
@ -67,19 +67,30 @@ class BasicAgent(Agent):
carla.Location(location[0], location[1], location[2]))
self._current_waypoint = self._start_waypoint
print "Setting up GRP and retrieving route plan !"
route = self._get_route(self._start_waypoint, self._end_waypoint)
print "Gathered route plan : ", route
for action in route:
print "Begining route assimilation !"
for i, action in enumerate(route):
print "Doing action : ", action
if action == RoadOption.CHANGELANELEFT or action == RoadOption.CHANGELANERIGHT:
self._update_lane_change(action)
self._update_lane_change(action, 20)
else:
wp_choice = self._generate_waypoint_to_junction()
if route[i+1] == RoadOption.VOID:
1+1
pass
next_lane_change = False
if i < len(route)-1 and route[i+1] != RoadOption.LANEFOLLOW and route[i+1] != RoadOption.VOID:
next_lane_change = True
wp_choice = self._generate_waypoint_to_junction(next_lane_change)
if action == RoadOption.VOID:
break
break
if len(wp_choice) > 1:
wp_choice, trajectories = self._seperate_trajectories(wp_choice)
current_transform = self._current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + carla.Location(
@ -177,33 +188,39 @@ class BasicAgent(Agent):
return wp_choice, tragectories
def _generate_waypoint_to_junction(self):
def _generate_waypoint_to_junction(self, next_lane_change):
"""
This method generates waypoint to a junction
"""
wp_choice = self._current_waypoint.next(self._hop_resolution)
found_junction = len(wp_choice) < 2 and not self._grp.verify_intersection(wp_choice[0])
while found_junction:
not_junction = True
while not_junction:
if next_lane_change:
1+1
pass
self._current_waypoint = wp_choice[0]
self._solution.append((self._current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = self._current_waypoint.next(self._hop_resolution)
# Stop at destination
if self._current_waypoint.transform.location.distance(
self._end_waypoint.transform.location) < self._hop_resolution: break
found_junction = len(wp_choice) < 2 and not self._grp.verify_intersection(wp_choice[0])
self._end_waypoint.transform.location) < 2 * self._hop_resolution: break
not_junction = len(wp_choice) < 2 and \
not self._grp.verify_intersection(wp_choice[0]) and \
not next_lane_change
return wp_choice
def _update_lane_change(self, action):
def _update_lane_change(self, action, change_over_distance=10):
"""
This method updates self._solution with waypoints for lane change
"""
if action == RoadOption.CHANGELANELEFT:
self._current_waypoint = self._current_waypoint.get_left_lane().next(3)[0]
self._current_waypoint = self._current_waypoint.get_left_lane().next(change_over_distance)[0]
else:
self._current_waypoint = self._current_waypoint.get_right_lane().next(3)[0]
self._current_waypoint = self._current_waypoint.get_right_lane().next(change_over_distance)[0]
self._solution.append((self._current_waypoint, action))
def _get_route(self, start_waypoint, end_waypoint):

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

@ -110,30 +110,41 @@ class GlobalRoutePlanner(object):
This method places zero cost links in the topology graph
representing availability of lane changes.
"""
# TODO : Remove loop count variables
segmentcount = 0
for segment in self._topology:
segmentcount += 1
left_found, right_found = False, False
pathcounter = 0
for waypoint in segment['path']:
pathcounter += 1
# print segmentcount, pathcounter
if segmentcount == 110 and pathcounter == 12:
1+1
pass
next_waypoint, next_road_option = None, None
if bool(waypoint.lane_change & carla.LaneChange.Right) and not right_found:
next_waypoint = waypoint.get_right_lane()
next_road_option = RoadOption.CHANGELANERIGHT
next_segment = self._localise(next_waypoint.transform.location)
self._graph.add_edge(
self._id_map[segment['entryxyz']], next_segment[0],
length=0, type=next_road_option, change_waypoint = waypoint)
right_found = True
if next_waypoint is not None and next_waypoint.lane_type == 'driving':
next_road_option = RoadOption.CHANGELANERIGHT
next_segment = self._localise(next_waypoint.transform.location)
self._graph.add_edge(
self._id_map[segment['entryxyz']], next_segment[0],
length=0, type=next_road_option, change_waypoint = waypoint)
right_found = True
if bool(waypoint.lane_change & carla.LaneChange.Left) and not left_found:
next_waypoint = waypoint.get_left_lane()
next_road_option = RoadOption.CHANGELANELEFT
next_segment = self._localise(next_waypoint.transform.location)
self._graph.add_edge(
self._id_map[segment['entryxyz']], next_segment[0],
length=0, type=next_road_option, change_waypoint = waypoint)
left_found = True
if next_waypoint is not None and next_waypoint.lane_type == 'driving':
next_road_option = RoadOption.CHANGELANELEFT
next_segment = self._localise(next_waypoint.transform.location)
self._graph.add_edge(
self._id_map[segment['entryxyz']], next_segment[0],
length=0, type=next_road_option, change_waypoint = waypoint)
left_found = True
if left_found and right_found: break
@ -186,7 +197,10 @@ class GlobalRoutePlanner(object):
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 next_edge['type'] != RoadOption.LANEFOLLOW:
if next_edge['type'] != RoadOption.LANEFOLLOW and next_edge['type'] != RoadOption.VOID:
if i == 0 and current_edge['type'] != RoadOption:
plan.append(next_edge['type'])
plan.append(next_edge['type'])
elif current_edge['type'] == RoadOption.LANEFOLLOW:
cv, nv = current_edge['exit_vector'], next_edge['net_vector']
@ -217,6 +231,14 @@ class GlobalRoutePlanner(object):
elif next_cross > max(cross_list):
action = RoadOption.RIGHT
plan.append(action)
elif i < len(route) - 3 and i > 0: # Detecting lane to lane intersection
previous_edge = self._graph.edges[route[i-1], route[i]]
next_to_next_edge = self._graph.edges[route[i + 2], route[i + 3]]
if next_to_next_edge['type'] != RoadOption.LANEFOLLOW and \
next_to_next_edge['type'] != RoadOption.VOID and \
previous_edge['type'] != RoadOption.LANEFOLLOW and \
previous_edge['type'] != RoadOption.VOID:
plan.append(RoadOption.STRAIGHT)
return plan

3
PythonAPI/agents/navigation/global_route_planner_dao.py
View File

@ -61,4 +61,5 @@ class GlobalRoutePlannerDAO(object):
"""
The method returns waytpoint at given location
"""
return self._wmap.get_waypoint(location)
waypoint = self._wmap.get_waypoint(location)
return waypoint