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) route_trace = self.trace_route(start_waypoint, end_waypoint)
self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue) self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue)
for w in route_trace: # for i, w in enumerate(route_trace):
wp = w[0].transform.location + carla.Location(z=0.1) # wp = w[0].transform.location + carla.Location(z=0.1)
if w[1] == RoadOption.LEFT: # Yellow # if w[1] == RoadOption.LEFT: # Yellow
color = carla.Color(255, 255, 0) # color = carla.Color(255, 255, 0)
elif w[1] == RoadOption.RIGHT: # Cyan # elif w[1] == RoadOption.RIGHT: # Cyan
color = carla.Color(0, 255, 255) # color = carla.Color(0, 255, 255)
elif w[1] == RoadOption.CHANGELANELEFT: # Orange # elif w[1] == RoadOption.CHANGELANELEFT: # Orange
color = carla.Color(255, 64, 0) # color = carla.Color(255, 64, 0)
elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan # elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan
color = carla.Color(0, 64, 255) # color = carla.Color(0, 64, 255)
elif w[1] == RoadOption.STRAIGHT: # Gray # elif w[1] == RoadOption.STRAIGHT: # Gray
color = carla.Color(128, 128, 128) # color = carla.Color(128, 128, 128)
else: # LANEFOLLOW # else: # LANEFOLLOW
color = carla.Color(0, 255, 0) # Green # 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_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): 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. LANEFOLLOW is always used.
This has been tested for sampling resolutions up to ~7 meters, and might fail for higher values. 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 :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]) route_with_options.append([route[j], road_option])
entry_index = None 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 lane_change_type = None
for i in range(0, len(route_with_options)): for i in range(0, len(route_with_options)):
@ -112,6 +115,14 @@ class GlobalRoutePlanner(object):
prev_direction = waypoint.transform.get_forward_vector() prev_direction = waypoint.transform.get_forward_vector()
np_prev_direction = np.array([prev_direction.x, prev_direction.y, prev_direction.z]) 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 # Check the dot product between the two consecutive waypoint
route_direction = route_with_options[i+1][0].transform.location - waypoint.transform.location 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]) np_route_direction = np.array([route_direction.x, route_direction.y, route_direction.z])
@ -123,21 +134,11 @@ class GlobalRoutePlanner(object):
dot = 1 dot = 1
if dot < math.cos(math.radians(45)): if dot < math.cos(math.radians(45)):
if lane_change_type: cross = np.cross(np_prev_direction, np_route_direction)
# Last lane change waypoint lane_change_type = RoadOption.CHANGELANERIGHT if cross[2] > 0 else RoadOption.CHANGELANELEFT
new_option = lane_change_type 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
else: else:
if lane_change_type: new_option = option
new_option = lane_change_type
else:
new_option = option
route_with_lane_changes.append([waypoint, new_option]) route_with_lane_changes.append([waypoint, new_option])
np_prev_direction = np_route_direction np_prev_direction = np_route_direction