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

Another lane change option iteration

This commit is contained in:
Guillermo 2021-07-02 13:30:29 +02:00 committed by bernat
parent 1408f6de53
commit 23a6091109
2 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
PythonAPI/carla/agents/navigation/basic_agent.py
View File

@ -116,21 +116,22 @@ class BasicAgent(object):
route_trace = self.trace_route(start_waypoint, end_waypoint)
self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue)
for w in route_trace:
wp = w[0].transform.location + carla.Location(z=0.1)
if w[1] == RoadOption.LEFT: # Yellow
color = carla.Color(255, 255, 0)
elif w[1] == RoadOption.RIGHT: # Cyan
color = carla.Color(0, 255, 255)
elif w[1] == RoadOption.CHANGELANELEFT: # Orange
color = carla.Color(255, 64, 0)
elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan
color = carla.Color(0, 64, 255)
elif w[1] == RoadOption.STRAIGHT: # Gray
color = carla.Color(128, 128, 128)
else: # LANEFOLLOW
color = carla.Color(0, 255, 0) # Green
self._world.debug.draw_point(wp, size=0.08, color=color, life_time=100)
# for i, w in enumerate(route_trace):
# wp = w[0].transform.location + carla.Location(z=0.1)
# if w[1] == RoadOption.LEFT: # Yellow
# color = carla.Color(255, 255, 0)
# elif w[1] == RoadOption.RIGHT: # Cyan
# color = carla.Color(0, 255, 255)
# elif w[1] == RoadOption.CHANGELANELEFT: # Orange
# color = carla.Color(255, 64, 0)
# elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan
# color = carla.Color(0, 64, 255)
# elif w[1] == RoadOption.STRAIGHT: # Gray
# color = carla.Color(128, 128, 128)
# else: # LANEFOLLOW
# color = carla.Color(0, 255, 0) # Green
# self._world.debug.draw_point(wp, size=0.08, color=color, life_time=100)
# self._world.debug.draw_string(wp, str(i), color=color, life_time=100)
def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True):
"""

31
PythonAPI/carla/agents/navigation/global_route_planner.py
View File

@ -59,6 +59,8 @@ class GlobalRoutePlanner(object):
LANEFOLLOW is always used.
This has been tested for sampling resolutions up to ~7 meters, and might fail for higher values.
As lane changes are marked in pairs, it will fail for even number of consecutive lane changes
(as they have an odd amount of lane change waypoints)
:param route (list of carla.Waypoint): list of waypoints representing the route
"""
@ -98,7 +100,8 @@ class GlobalRoutePlanner(object):
route_with_options.append([route[j], road_option])
entry_index = None
# Part 2: Add lane changes
# Part 2: Add lane changes. Use the route direction and not the waypoint as waypoints might not
# correspond to the correct lane
lane_change_type = None
for i in range(0, len(route_with_options)):
@ -112,6 +115,14 @@ class GlobalRoutePlanner(object):
prev_direction = waypoint.transform.get_forward_vector()
np_prev_direction = np.array([prev_direction.x, prev_direction.y, prev_direction.z])
# Lane changes are marked at both lanes
if lane_change_type:
route_with_lane_changes.append([waypoint, lane_change_type])
prev_direction = waypoint.transform.get_forward_vector()
np_prev_direction = np.array([prev_direction.x, prev_direction.y, prev_direction.z])
lane_change_type = None
continue
# Check the dot product between the two consecutive waypoint
route_direction = route_with_options[i+1][0].transform.location - waypoint.transform.location
np_route_direction = np.array([route_direction.x, route_direction.y, route_direction.z])
@ -123,21 +134,11 @@ class GlobalRoutePlanner(object):
dot = 1
if dot < math.cos(math.radians(45)):
if lane_change_type:
# Last lane change waypoint
new_option = lane_change_type
lane_change_type = None
else:
# First lane change waypoint
cross = np.cross(np_prev_direction, np_route_direction)
lane_change_type = RoadOption.CHANGELANERIGHT if cross[2] > 0 else RoadOption.CHANGELANELEFT
new_option = lane_change_type
cross = np.cross(np_prev_direction, np_route_direction)
lane_change_type = RoadOption.CHANGELANERIGHT if cross[2] > 0 else RoadOption.CHANGELANELEFT
new_option = lane_change_type
else:
if lane_change_type:
new_option = lane_change_type
else:
new_option = option
new_option = option
route_with_lane_changes.append([waypoint, new_option])
np_prev_direction = np_route_direction