class WaypointPlanningOperator(erdos.Operator):
"""Computes waypoints the ego vehicle must follow.
If the operator is running in CARLA challenge mode, then it receives all
the waypoints from the scenario runner agent (on the global trajectory
stream). Otherwise, it computes waypoints using the HD Map.
The planner reduces speed/stops whenever it encounters an obstacle,
and waits for the obstacle to move. It does not implement an obstacle
avoidance policy.
Args:
can_bus_stream (:py:class:`erdos.ReadStream`): Stream on which can bus
info is received.
open_drive_stream (:py:class:`erdos.ReadStream`): Stream on which open
drive string representations are received. The operator can
construct HDMaps out of the open drive strings.
global_trajectory_stream (:py:class:`erdos.ReadStream`): Stream on
which the scenario runner publishes waypoints.
waypoints_stream (:py:class:`erdos.WriteStream`): Stream on which the
operator sends waypoints the ego vehicle must follow.
flags (absl.flags): Object to be used to access absl flags.
goal_location (:py:class:`~pylot.utils.Location`): The goal location of
the ego vehicle.
"""
def __init__(self,
can_bus_stream,
open_drive_stream,
global_trajectory_stream,
obstacles_stream,
traffic_lights_stream,
waypoints_stream,
flags,
goal_location=None):
can_bus_stream.add_callback(self.on_can_bus_update)
open_drive_stream.add_callback(self.on_opendrive_map)
global_trajectory_stream.add_callback(self.on_global_trajectory)
obstacles_stream.add_callback(self.on_obstacles_update)
traffic_lights_stream.add_callback(self.on_traffic_lights_update)
erdos.add_watermark_callback(
[can_bus_stream, obstacles_stream, traffic_lights_stream],
[waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
# Initialize the state of the behaviour planner.
# XXX(ionel): The behaviour planner is not ready yet.
self.__initialize_behaviour_planner()
# We do not know yet the vehicle's location.
self._vehicle_transform = None
self._goal_location = goal_location
self._map = None
# Deque of waypoints the vehicle must follow. The waypoints are either
# received on the global trajectory stream when running using the
# scenario runner, or computed using the Carla global planner when
# running in stand-alone mode. The waypoints are Pylot transforms.
self._waypoints = deque()
self._can_bus_msgs = deque()
self._obstacles_msgs = deque()
self._traffic_light_msgs = deque()
@staticmethod
def connect(can_bus_stream, open_drive_stream, global_trajectory_stream,
obstacles_stream, traffic_lights_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def run(self):
# Run method is invoked after all operators finished initializing,
# including the CARLA operator, which reloads the world. Thus, if
# we get the map here we're sure it is up-to-date.
# We're not running in challenge mode if no track flag is present.
# Thus, we can directly get the map from the simulator.
if not hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout))
self._logger.info('Planner running in stand-alone mode')
# Recompute waypoints every RECOMPUTE_WAYPOINT_EVERY_N_WATERMARKS.
self._recompute_waypoints = True
else:
# Do not recompute waypoints upon each run.
self._recompute_waypoints = False
self._watermark_cnt = 0
def on_opendrive_map(self, msg):
"""Invoked whenever a message is received on the open drive stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
the open drive string.
"""
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._logger.info('Initializing HDMap from open drive stream')
from pylot.map.hd_map import HDMap
self._map = HDMap(carla.Map('map', msg.data))
def on_global_trajectory(self, msg):
"""Invoked whenever a message is received on the trajectory stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
a list of waypoints to the goal location.
"""
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = msg.data[-1][0].location
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destination.
self._goal_location = self._vehicle_transform.location
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_can_bus_update(self, msg):
"""Invoked whenever a message is received on the can bus stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
info about the ego vehicle.
"""
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_obstacles_update(self, msg):
self._logger.debug('@{}: obstacles update'.format(msg.timestamp))
self._obstacles_msgs.append(msg)
def on_traffic_lights_update(self, msg):
self._logger.debug('@{}: traffic lights update'.format(msg.timestamp))
self._traffic_light_msgs.append(msg)
@erdos.profile_method()
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
self._watermark_cnt += 1
# Get hero vehicle info.
can_bus_msg = self._can_bus_msgs.popleft()
self._vehicle_transform = can_bus_msg.data.transform
tl_msg = self._traffic_light_msgs.popleft()
obstacles_msg = self._obstacles_msgs.popleft()
if (self._recompute_waypoints
and self._watermark_cnt % RECOMPUTE_WAYPOINT_EVERY_N_WATERMARKS
== 0):
self._waypoints = self._map.compute_waypoints(
self._vehicle_transform.location, self._goal_location)
self.__remove_completed_waypoints()
if not self._waypoints or len(self._waypoints) == 0:
# If waypoints are empty (e.g., reached destination), set waypoint
# to current vehicle location.
self._waypoints = deque([self._vehicle_transform])
wp_vector, wp_angle = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
self._vehicle_transform, self._waypoints,
DEFAULT_TARGET_WAYPOINT)
speed_factor, _ = pylot.planning.utils.stop_for_agents(
self._vehicle_transform.location, wp_angle, wp_vector,
obstacles_msg.obstacles, tl_msg.obstacles, self._flags,
self._logger, self._map, timestamp)
target_speed = speed_factor * self._flags.target_speed
self._logger.debug('@{}: computed speed factor: {}'.format(
timestamp, speed_factor))
self._logger.debug('@{}: computed target speed: {}'.format(
timestamp, target_speed))
head_waypoints = deque(
itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS))
target_speeds = deque(
[target_speed for _ in range(len(head_waypoints))])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
def __remove_completed_waypoints(self):
"""Removes waypoints that the ego vehicle has already completed.
The method first finds the closest waypoint, removes all waypoints
that are before the closest waypoint, and finally removes the closest
waypoint if the ego vehicle is very close to it (i.e., close to
completion).
"""
min_dist = 10000000
min_index = 0
index = 0
for waypoint in self._waypoints:
# XXX(ionel): We only check the first 10 waypoints.
if index > 10:
break
dist = waypoint.location.distance(self._vehicle_transform.location)
if dist < min_dist:
min_dist = dist
min_index = index
# Remove waypoints that are before the closest waypoint. The ego
# vehicle already completed them.
while min_index > 0:
self._waypoints.popleft()
min_index -= 1
# The closest waypoint is almost complete, remove it.
if min_dist < WAYPOINT_COMPLETION_THRESHOLD:
self._waypoints.popleft()
def __initialize_behaviour_planner(self):
# State the planner is in.
self._state = BehaviorPlannerState.READY
# Cost functions. Output between 0 and 1.
self._cost_functions = [
pylot.planning.cost_functions.cost_speed,
pylot.planning.cost_functions.cost_lane_change,
pylot.planning.cost_functions.cost_inefficiency
]
reach_speed_weight = 10**5
reach_goal_weight = 10**6
efficiency_weight = 10**4
# How important a cost function is.
self._function_weights = [
reach_speed_weight, reach_goal_weight, efficiency_weight
]
def __best_transition(self, vehicle_transform, predictions):
""" Computes most likely state transition from current state."""
# Get possible next state machine states.
possible_next_states = self.__successor_states()
best_next_state = None
min_state_cost = 10000000
for state in possible_next_states:
# Generate trajectory for next state.
vehicle_info, trajectory_for_state = self.__generate_trajectory(
state, vehicle_transform, predictions)
state_cost = 0
# Compute the cost of the trajectory.
for i in range(len(self._cost_functions)):
cost_func = self._cost_functions[i](vehicle_info, predictions,
trajectory_for_state)
state_cost += self._function_weights[i] * cost_func
# Check if it's the best trajectory.
if best_next_state is None or state_cost < min_state_cost:
best_next_state = state
min_state_cost = state_cost
return best_next_state
def __generate_trajectory(self, next_state, vehicle_transform,
predictions):
raise NotImplementedError
def __successor_states(self):
""" Returns possible state transitions from current state."""
if self._state == BehaviorPlannerState.READY:
return [BehaviorPlannerState.READY, BehaviorPlannerState.KEEP_LANE]
elif self._state == BehaviorPlannerState.KEEP_LANE:
# 1) keep_lane -> prepare_lane_change_left
# 2) keep_lane -> prepare_lane_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT:
# 1) prepare_lane_change_left -> keep_lane
# 2) prepare_lane_change_left -> lange_change_left
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_LEFT:
# 1) lange_change_left -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT:
# 1) prepare_lane_change_right -> keep_lane
# 2) prepare_lane_change_right -> lange_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_RIGHT:
# 1) lane_change_right -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
else:
raise ValueError('Unexpected vehicle state {}'.format(self._state))
class BehaviorPlanningOperator(erdos.Operator):
"""Behavior planning operator.
Args:
pose_stream (:py:class:`erdos.ReadStream`): Stream on which pose
info is received.
open_drive_stream (:py:class:`erdos.ReadStream`): Stream on which open
drive string representations are received. The operator can
construct HDMaps out of the open drive strings.
route_stream (:py:class:`erdos.ReadStream`): Stream on which the
scenario runner publishes waypoints.
trajectory_stream (:py:class:`erdos.WriteStream`): Stream on which the
operator sends waypoints the ego vehicle must follow.
flags (absl.flags): Object to be used to access absl flags.
goal_location (:py:class:`~pylot.utils.Location`): The goal location of
the ego vehicle.
"""
def __init__(self,
pose_stream,
open_drive_stream,
route_stream,
trajectory_stream,
flags,
goal_location=None):
pose_stream.add_callback(self.on_pose_update)
open_drive_stream.add_callback(self.on_opendrive_map)
route_stream.add_callback(self.on_route_msg)
erdos.add_watermark_callback(
[pose_stream, open_drive_stream, route_stream],
[trajectory_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
# Do not set the goal location here so that the behavior planner
# issues an initial message.
self._goal_location = None
# Initialize the state of the behaviour planner.
self.__initialize_behaviour_planner()
self._pose_msgs = deque()
self._ego_info = EgoInfo()
if goal_location:
self._route = Waypoints(
deque([
pylot.utils.Transform(goal_location,
pylot.utils.Rotation())
]))
else:
self._route = None
self._map = None
@staticmethod
def connect(pose_stream, open_drive_stream, route_stream):
trajectory_stream = erdos.WriteStream()
return [trajectory_stream]
def on_opendrive_map(self, msg):
"""Invoked whenever a message is received on the open drive stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
the open drive string.
"""
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._logger.info('Initializing HDMap from open drive stream')
from pylot.map.hd_map import HDMap
self._map = HDMap(carla.Map('map', msg.data),
self.config.log_file_name)
def on_route_msg(self, msg):
"""Invoked whenever a message is received on the trajectory stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
a list of waypoints to the goal location.
"""
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.waypoints.waypoints)))
self._route = msg.waypoints
def on_pose_update(self, msg):
"""Invoked whenever a message is received on the pose stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
info about the ego vehicle.
"""
self._logger.debug('@{}: received pose message'.format(msg.timestamp))
self._pose_msgs.append(msg)
def on_watermark(self, timestamp, trajectory_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
pose_msg = self._pose_msgs.popleft()
ego_transform = pose_msg.data.transform
self._ego_info.update(self._state, pose_msg)
old_state = self._state
self._state = self.__best_state_transition(self._ego_info)
self._logger.debug('@{}: agent transitioned from {} to {}'.format(
timestamp, old_state, self._state))
# Remove the waypoint from the route if we're close to it.
if (old_state != self._state
and self._state == BehaviorPlannerState.OVERTAKE):
self._route.remove_waypoint_if_close(ego_transform.location, 10)
else:
if not self._map.is_intersection(ego_transform.location):
self._route.remove_waypoint_if_close(ego_transform.location, 5)
else:
self._route.remove_waypoint_if_close(ego_transform.location,
3.5)
new_goal_location = None
if len(self._route.waypoints) > 1:
new_goal_location = self._route.waypoints[1].location
elif len(self._route.waypoints) == 1:
new_goal_location = self._route.waypoints[0].location
else:
new_goal_location = ego_transform.location
if new_goal_location != self._goal_location:
self._goal_location = new_goal_location
if self._map:
# Use the map to compute more fine-grained waypoints.
waypoints = self._map.compute_waypoints(
ego_transform.location, self._goal_location)
road_options = deque([
pylot.utils.RoadOption.LANE_FOLLOW
for _ in range(len(waypoints))
])
waypoints = Waypoints(waypoints, road_options=road_options)
else:
# Map is not available, send the route.
waypoints = self._route
if not waypoints or waypoints.is_empty():
# If waypoints are empty (e.g., reached destination), set
# waypoints to current vehicle location.
waypoints = Waypoints(
deque([ego_transform]),
road_options=deque([pylot.utils.RoadOption.LANE_FOLLOW]))
trajectory_stream.send(
WaypointsMessage(timestamp, waypoints, self._state))
elif old_state != self._state:
# Send the state update.
trajectory_stream.send(
WaypointsMessage(timestamp, None, self._state))
trajectory_stream.send(erdos.WatermarkMessage(timestamp))
def __initialize_behaviour_planner(self):
# State the planner is in.
self._state = BehaviorPlannerState.FOLLOW_WAYPOINTS
# Cost functions. Output between 0 and 1.
self._cost_functions = [
pylot.planning.cost_functions.cost_overtake,
]
# How important a cost function is.
self._function_weights = [1]
def __successor_states(self):
""" Returns possible state transitions from current state."""
if self._state == BehaviorPlannerState.FOLLOW_WAYPOINTS:
# Can transition to OVERTAKE if the ego vehicle has been stuck
# behind an obstacle for a while.
return [
BehaviorPlannerState.FOLLOW_WAYPOINTS,
BehaviorPlannerState.OVERTAKE
]
elif self._state == BehaviorPlannerState.OVERTAKE:
return [
BehaviorPlannerState.OVERTAKE,
BehaviorPlannerState.FOLLOW_WAYPOINTS
]
elif self._state == BehaviorPlannerState.KEEP_LANE:
# 1) keep_lane -> prepare_lane_change_left
# 2) keep_lane -> prepare_lane_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT:
# 1) prepare_lane_change_left -> keep_lane
# 2) prepare_lane_change_left -> lange_change_left
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_LEFT:
# 1) lange_change_left -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT:
# 1) prepare_lane_change_right -> keep_lane
# 2) prepare_lane_change_right -> lange_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_RIGHT:
# 1) lane_change_right -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
else:
raise ValueError('Unexpected vehicle state {}'.format(self._state))
def __best_state_transition(self, ego_info):
""" Computes most likely state transition from current state."""
# Get possible next state machine states.
possible_next_states = self.__successor_states()
best_next_state = None
min_state_cost = np.infty
for state in possible_next_states:
state_cost = 0
# Compute the cost of the trajectory.
for i in range(len(self._cost_functions)):
cost_func = self._cost_functions[i](self._state, state,
ego_info)
state_cost += self._function_weights[i] * cost_func
# Check if it's the best trajectory.
if state_cost < min_state_cost:
best_next_state = state
min_state_cost = state_cost
return best_next_state
class WaypointPlanningOperator(erdos.Operator):
""" Planning operator for Carla 0.9.x.
The operator either receives all the waypoints from the scenario runner
agent (on the global trajectory stream), or computes waypoints using the
HD Map.
"""
def __init__(self,
can_bus_stream,
open_drive_stream,
global_trajectory_stream,
waypoints_stream,
name,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update, [waypoints_stream])
open_drive_stream.add_callback(self.on_opendrive_map)
global_trajectory_stream.add_callback(self.on_global_trajectory)
self._log_file_name = log_file_name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
# Initialize the state of the behaviour planner.
# XXX(ionel): The behaviour planner is not ready yet.
self.__initialize_behaviour_planner()
# We do not know yet the vehicle's location.
self._vehicle_transform = None
# Deque of waypoints the vehicle must follow. The waypoints are either
# received on the global trajectory stream when running using the
# scenario runner, or computed using the Carla global planner when
# running in stand-alone mode. The waypoints are Pylot transforms.
self._waypoints = collections.deque()
# The operator picks the wp_num_steer-th waypoint to compute the angle
# it has to steer by when taking a turn.
self._wp_num_steer = 9 # use 10th waypoint for steering
# The operator picks the wp_num_speed-th waypoint to compute the angle
# it has to steer by when driving straight.
self._wp_num_speed = 4 # use tth waypoint for speed
# We're not running in challenge mode if no track flag is present.
# Thus, we can directly get the map from the simulator.
if not hasattr(self._flags, 'track'):
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.info('Planner running in stand-alone mode')
assert goal_location, 'Planner has not received a goal location'
# Transform goal location to carla.Location
self._goal_location = goal_location
# Recompute waypoints upon each run.
self._recompute_waypoints = True
else:
# Do not recompute waypoints upon each run.
self._recompute_waypoints = False
# TODO(ionel): HACK! In Carla challenge track 1 and 2 the waypoints
# are coarse grained (30 meters apart). We pick the first waypoint
# to compute the angles. However, we should instead implement
# trajectory planning.
if self._flags.track == 1 or self._flags == 2:
self._wp_num_steer = 1
self._wp_num_speed = 1
@staticmethod
def connect(can_bus_stream, open_drive_stream, global_trajectory_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def on_opendrive_map(self, msg):
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
self._logger.info('Initializing HDMap from open drive stream')
self._map = HDMap(carla.Map('map', msg.data), self._log_file_name)
def on_global_trajectory(self, msg):
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = msg.data[-1][0].location.as_carla_location()
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
goal_loc = self._vehicle_transform.location
self._goal_location = goal_loc.as_carla_location()
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = collections.deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_can_bus_update(self, msg, waypoints_stream):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._vehicle_transform = msg.data.transform
self.__update_waypoints()
next_waypoint_steer = self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_steer)]
next_waypoint_speed = self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_speed)]
# Get vectors and angles to corresponding speed and steer waypoints.
wp_steer_vector, wp_steer_angle = get_waypoint_vector_and_angle(
next_waypoint_steer, self._vehicle_transform)
wp_speed_vector, wp_speed_angle = get_waypoint_vector_and_angle(
next_waypoint_speed, self._vehicle_transform)
head_waypoints = collections.deque(
itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS))
output_msg = WaypointsMessage(msg.timestamp,
waypoints=head_waypoints,
target_speed=self._flags.target_speed,
wp_angle=wp_steer_angle,
wp_vector=wp_steer_vector,
wp_angle_speed=wp_speed_angle)
waypoints_stream.send(output_msg)
def __update_waypoints(self):
""" Updates the waypoints.
Depending on setup, the method either recomputes the waypoints
between the ego vehicle and the goal location, or just garbage collects
waypoints that have already been achieved.
Returns:
(wp_steer, wp_speed): The waypoints to be used to compute steer and
speed angles.
"""
if self._recompute_waypoints:
ego_location = self._vehicle_transform.location.as_carla_location()
self._waypoints = self._map.compute_waypoints(
ego_location, self._goal_location)
self.__remove_completed_waypoints()
if not self._waypoints or len(self._waypoints) == 0:
# If waypoints are empty (e.g., reached destination), set waypoint
# to current vehicle location.
self._waypoints = collections.deque([self._vehicle_transform])
def __remove_completed_waypoints(self):
""" Removes waypoints that the ego vehicle has already completed.
The method first finds the closest waypoint, removes all waypoints
that are before the closest waypoint, and finally removes the
closest waypoint if the ego vehicle is very close to it
(i.e., close to completion)."""
min_dist = 10000000
min_index = 0
index = 0
for waypoint in self._waypoints:
# XXX(ionel): We only check the first 10 waypoints.
if index > 10:
break
dist = waypoint.location.distance(self._vehicle_transform.location)
if dist < min_dist:
min_dist = dist
min_index = index
# Remove waypoints that are before the closest waypoint. The ego
# vehicle already completed them.
while min_index > 0:
self._waypoints.popleft()
min_index -= 1
# The closest waypoint is almost complete, remove it.
if min_dist < WAYPOINT_COMPLETION_THRESHOLD:
self._waypoints.popleft()
def __initialize_behaviour_planner(self):
# State the planner is in.
self._state = BehaviorPlannerState.READY
# Cost functions. Output between 0 and 1.
self._cost_functions = [
pylot.planning.cost_functions.cost_speed,
pylot.planning.cost_functions.cost_lane_change,
pylot.planning.cost_functions.cost_inefficiency
]
reach_speed_weight = 10**5
reach_goal_weight = 10**6
efficiency_weight = 10**4
# How important a cost function is.
self._function_weights = [
reach_speed_weight, reach_goal_weight, efficiency_weight
]
def __best_transition(self, vehicle_transform, predictions):
""" Computes most likely state transition from current state."""
# Get possible next state machine states.
possible_next_states = self.__successor_states()
best_next_state = None
min_state_cost = 10000000
for state in possible_next_states:
# Generate trajectory for next state.
vehicle_info, trajectory_for_state = self.__generate_trajectory(
state, vehicle_transform, predictions)
state_cost = 0
# Compute the cost of the trajectory.
for i in range(len(self._cost_functions)):
cost_func = self._cost_functions[i](vehicle_info, predictions,
trajectory_for_state)
state_cost += self._function_weights[i] * cost_func
# Check if it's the best trajectory.
if best_next_state is None or state_cost < min_state_cost:
best_next_state = state
min_state_cost = state_cost
return best_next_state
def __generate_trajectory(self, next_state, vehicle_transform,
predictions):
raise NotImplementedError
def __successor_states(self):
""" Returns possible state transitions from current state."""
if self._state == BehaviorPlannerState.READY:
return [BehaviorPlannerState.READY, BehaviorPlannerState.KEEP_LANE]
elif self._state == BehaviorPlannerState.KEEP_LANE:
# 1) keep_lane -> prepare_lane_change_left
# 2) keep_lane -> prepare_lane_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT:
# 1) prepare_lane_change_left -> keep_lane
# 2) prepare_lane_change_left -> lange_change_left
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_LEFT:
# 1) lange_change_left -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT:
# 1) prepare_lane_change_right -> keep_lane
# 2) prepare_lane_change_right -> lange_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_RIGHT:
# 1) lane_change_right -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
else:
raise ValueError('Unexpected vehicle state {}'.format(self._state))
class RRTStarPlanningOperator(erdos.Operator):
"""RRTStar Planning operator for Carla 0.9.x.
Args:
flags: Config flags.
goal_location: Goal pylot.utils.Location for planner to route to.
"""
def __init__(self,
can_bus_stream,
prediction_stream,
global_trajectory_stream,
open_drive_stream,
waypoints_stream,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
global_trajectory_stream.add_callback(self.on_global_trajectory)
open_drive_stream.add_callback(self.on_opendrive_map)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
self._vehicle_transform = None
self._map = None
self._waypoints = None
self._prev_waypoints = None
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
@staticmethod
def connect(can_bus_stream, prediction_stream, global_trajectory_stream,
open_drive_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def run(self):
# Run method is invoked after all operators finished initializing,
# including the CARLA operator, which reloads the world. Thus, if
# we get the map here we're sure it is up-to-date.
if not hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout))
self._logger.info('Planner running in stand-alone mode')
def on_can_bus_update(self, msg):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_prediction_update(self, msg):
self._logger.debug('@{}: received prediction message'.format(
msg.timestamp))
self._prediction_msgs.append(msg)
def on_global_trajectory(self, msg):
"""Invoked whenever a message is received on the trajectory stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
a list of waypoints to the goal location.
"""
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = msg.data[-1][0].location
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
self._goal_location = self._vehicle_transform.location
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_opendrive_map(self, msg):
"""Invoked whenever a message is received on the open drive stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
the open drive string.
"""
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._logger.info('Initializing HDMap from open drive stream')
from pylot.map.hd_map import HDMap
self._map = HDMap(carla.Map('map', msg.data))
@erdos.profile_method()
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self._build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
else:
# haven't received waypoints from global trajectory stream
self._logger.debug("@{}: Sending target speed 0, haven't"
"received global trajectory"
.format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# run rrt star
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
success, (path_x, path_y) = \
self._apply_rrt_star(obstacle_list, timestamp)
speeds = [0]
if success:
speeds = [self._flags.target_speed] * len(path_x)
self._logger.debug("@{}: RRT* Path X: {}".format(
timestamp,
path_x.tolist())
)
self._logger.debug("@{}: RRT* Path Y: {}".format(
timestamp,
path_y.tolist())
)
self._logger.debug("@{}: RRT* Speeds: {}".format(
timestamp,
[self._flags.target_speed] * len(path_x))
)
# construct and send waypoint message
waypoint_message = self._construct_waypoints(
timestamp, path_x, path_y, speeds, success
)
waypoints_stream.send(waypoint_message)
def _get_closest_index(self, start):
min_dist = np.infty
mindex = 0
for ind, wp in enumerate(self._waypoints):
dist = np.linalg.norm([start[0] - wp.location.x,
start[1] - wp.location.y])
if dist <= min_dist:
mindex = ind
min_dist = dist
return mindex
def _apply_rrt_star(self, obstacles, timestamp):
start = [
self._vehicle_transform.location.x,
self._vehicle_transform.location.y
]
# find the closest point to current location
mindex = self._get_closest_index(start)
end_ind = min(mindex + DEFAULT_TARGET_WAYPOINT, len(self._waypoints) - 1)
end = [
self._waypoints[end_ind].location.x,
self._waypoints[end_ind].location.y
]
# log initial conditions for debugging
initial_conditions = {
"start": start,
"end": end,
"obstacles": obstacles.tolist(),
"step_size": STEP_SIZE,
"max_iterations": MAX_ITERATIONS,
}
self._logger.debug("@{}: Initial conditions: {}".format(
timestamp, initial_conditions))
return apply_rrt_star(start, end, STEP_SIZE, MAX_ITERATIONS, obstacles)
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the rrt* path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.error("@{}: RRT* failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: RRT* succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
@staticmethod
def _build_obstacle_list(vehicle_transform, prediction_msg):
"""
Construct an obstacle list of proximal objects given vehicle_transform.
"""
obstacle_list = []
# look over all predictions
for prediction in prediction_msg.predictions:
# use all prediction times as potential obstacles
for transform in prediction.trajectory:
global_obstacle = vehicle_transform * transform
obstacle_origin = [
global_obstacle.location.x, global_obstacle.location.y
]
dist_to_ego = np.linalg.norm([
vehicle_transform.location.x - obstacle_origin[0],
vehicle_transform.location.y - obstacle_origin[1]
])
# TODO (@fangedward): Fix this hack
# Prediction also sends a prediction for ego vehicle
# This will always be the closest to the ego vehicle
# Filter out until this is removed from prediction
if dist_to_ego < 2: # this allows max vel to be 20m/s
break
elif dist_to_ego < DEFAULT_DISTANCE_THRESHOLD:
# use 3d bounding boxes if available, otherwise use default
if isinstance(prediction.bounding_box, BoundingBox3D):
start_location = \
prediction.bounding_box.transform.location - \
prediction.bounding_box.extent
end_location = \
prediction.bounding_box.transform.location + \
prediction.bounding_box.extent
start_transform = global_obstacle.transform_locations(
[start_location])
end_transform = global_obstacle.transform_locations(
[end_location])
else:
start_transform = [
Location(
obstacle_origin[0] - DEFAULT_OBSTACLE_SIZE,
obstacle_origin[1] - DEFAULT_OBSTACLE_SIZE,
0
)
]
end_transform = [
Location(
obstacle_origin[0] + DEFAULT_OBSTACLE_SIZE,
obstacle_origin[1] + DEFAULT_OBSTACLE_SIZE,
0
)
]
obstacle_list.append([min(start_transform[0].x,
end_transform[0].x),
min(start_transform[0].y,
end_transform[0].y),
max(start_transform[0].x,
end_transform[0].x),
max(start_transform[0].y,
end_transform[0].y)])
if len(obstacle_list) == 0:
return np.empty((0, 4))
return np.array(obstacle_list)
class FOTPlanningOperator(erdos.Operator):
""" Frenet Optimal Trajectory (FOT) Planning operator for Carla 0.9.x.
This planning operator uses a global route and listens for predictions
to produce a frenet optimal trajectory plan. Details can be found in
`~pylot.planning.frenet_optimal_trajectory.frenet_optimal_trajectory.py`.
Args:
flags(:absl.flags:): Object to be used to access absl flags
goal_location(:pylot.utils.Location:): Goal location for route planning
"""
def __init__(self,
can_bus_stream,
prediction_stream,
global_trajectory_stream,
open_drive_stream,
waypoints_stream,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
global_trajectory_stream.add_callback(self.on_global_trajectory)
open_drive_stream.add_callback(self.on_opendrive_map)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
self._vehicle_transform = None
self._map = None
self._waypoints = None
self._prev_waypoints = None
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
self.s0 = 0
@staticmethod
def connect(can_bus_stream, prediction_stream, global_trajectory_stream,
open_drive_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def run(self):
# Run method is invoked after all operators finished initializing,
# including the CARLA operator, which reloads the world. Thus, if
# we get the map here we're sure it is up-to-date.
if not hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout))
self._logger.info('Planner running in stand-alone mode')
def on_can_bus_update(self, msg):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_prediction_update(self, msg):
self._logger.debug('@{}: received prediction message'.format(
msg.timestamp))
self._prediction_msgs.append(msg)
def on_global_trajectory(self, msg):
"""Invoked whenever a message is received on the trajectory stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
a list of waypoints to the goal location.
"""
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = msg.data[-1][0].location
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
self._goal_location = self._vehicle_transform.location
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_opendrive_map(self, msg):
"""Invoked whenever a message is received on the open drive stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
the open drive string.
"""
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._logger.info('Initializing HDMap from open drive stream')
from pylot.map.hd_map import HDMap
self._map = HDMap(carla.Map('map', msg.data))
@erdos.profile_method()
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self._build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
# haven't received waypoints from global trajectory stream
else:
self._logger.debug("@{}: Sending target speed 0, haven't"
"received global trajectory"
.format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# compute optimal frenet trajectory
path_x, path_y, speeds, params, success, s0 = \
self._compute_optimal_frenet_trajectory(can_bus_msg, obstacle_list)
if success:
self._logger.debug("@{}: Frenet Path X: {}".format(
timestamp,
path_x.tolist())
)
self._logger.debug("@{}: Frenet Path Y: {}".format(
timestamp,
path_y.tolist())
)
self._logger.debug("@{}: Frenet Speeds: {}".format(
timestamp,
speeds.tolist())
)
# construct and send waypoint message
waypoints_message = self._construct_waypoints(
timestamp, path_x, path_y, speeds, success
)
waypoints_stream.send(waypoints_message)
def _compute_optimal_frenet_trajectory(self, can_bus_msg, obstacle_list):
"""
Compute the optimal frenet trajectory, given current environment info.
"""
# convert waypoints to frenet coordinates
wx = []
wy = []
for wp in itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS):
wx.append(wp.location.x)
wy.append(wp.location.y)
wx = np.array(wx)
wy = np.array(wy)
# compute frenet optimal trajectory
s0, c_speed, c_d, c_d_d, c_d_dd = \
self._compute_initial_conditions(can_bus_msg, wx, wy)
self.s0 = s0
target_speed = (c_speed + self._flags.target_speed) / 2
path_x, path_y, speeds, params, success = get_fot_frenet_space(
s0, c_speed, c_d, c_d_d, c_d_dd,
wx, wy, obstacle_list, target_speed
)
# log initial conditions for debugging
initial_conditions = {
"s0": s0,
"c_speed": c_speed,
"c_d": c_d,
"c_d_d": c_d_d,
"c_d_dd": c_d_dd,
"wx": wx.tolist(),
"wy": wy.tolist(),
"obstacle_list": obstacle_list.tolist(),
"x": can_bus_msg.data.transform.location.x,
"y": can_bus_msg.data.transform.location.y,
"vx": can_bus_msg.data.velocity_vector.x,
"vy": can_bus_msg.data.velocity_vector.y,
}
timestamp = can_bus_msg.timestamp
self._logger.debug("@{}: Initial conditions: {}".format(
timestamp, initial_conditions))
return path_x, path_y, speeds, params, success, s0
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the optimal frenet path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.debug("@{}: Frenet Optimal Trajectory failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: Frenet Optimal Trajectory succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def _compute_initial_conditions(self, can_bus_msg, wx, wy):
"""
Convert the initial conditions of vehicle into frenet frame parameters.
"""
x = can_bus_msg.data.transform.location.x
y = can_bus_msg.data.transform.location.y
vx = can_bus_msg.data.velocity_vector.x
vy = can_bus_msg.data.velocity_vector.y
return compute_initial_conditions(self.s0, x, y, vx, vy,
can_bus_msg.data.forward_speed, wx,
wy)
@staticmethod
def _build_obstacle_list(vehicle_transform, prediction_msg):
"""
Construct an obstacle list of proximal objects given vehicle_transform.
"""
obstacle_list = []
# look over all predictions
for prediction in prediction_msg.predictions:
# use all prediction times as potential obstacles
for transform in prediction.trajectory:
global_obstacle = vehicle_transform * transform
obstacle_origin = [
global_obstacle.location.x, global_obstacle.location.y
]
dist_to_ego = np.linalg.norm([
vehicle_transform.location.x - obstacle_origin[0],
vehicle_transform.location.y - obstacle_origin[1]
])
# TODO (@fangedward): Fix this hack
# Prediction also sends a prediction for ego vehicle
# This will always be the closest to the ego vehicle
# Filter out until this is removed from prediction
if dist_to_ego < 2: # this allows max vel to be 20m/s
break
elif dist_to_ego < DEFAULT_DISTANCE_THRESHOLD:
obstacle_list.append(obstacle_origin)
if len(obstacle_list) == 0:
return np.empty((0, 2))
return np.array(obstacle_list)
class RRTStarPlanningOperator(erdos.Operator):
""" RRTStar Planning operator for Carla 0.9.x."""
def __init__(self,
can_bus_stream,
prediction_stream,
waypoints_stream,
name,
flags,
goal_location,
log_file_name=None,
csv_file_name=None):
"""
Initialize the RRT* planner. Setup logger and map attributes.
Args:
name: Name of the operator.
flags: Config flags.
goal_location: Global goal carla.Location for planner to route to.
"""
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._wp_index = 9
self._waypoints = None
self._carla_map = get_map(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
self._hd_map = HDMap(self._carla_map, log_file_name)
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
@staticmethod
def connect(can_bus_stream, prediction_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def on_can_bus_update(self, msg):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_prediction_update(self, msg):
self._logger.debug('@{}: received prediction message'.format(
msg.timestamp))
self._prediction_msgs.append(msg)
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
# get obstacles
obstacle_map = self._build_obstacle_map(vehicle_transform)
# compute goals
target_location = self._compute_target_location(vehicle_transform)
# run rrt*
path, cost = self._run_rrt_star(vehicle_transform, target_location,
obstacle_map)
# convert to waypoints if path found, else use default waypoints
if cost is not None:
path_transforms = []
for point in path:
p_loc = self._carla_map.get_waypoint(
carla.Location(x=point[0], y=point[1], z=0),
project_to_road=True,
).transform.location # project to road to approximate Z
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
waypoints = deque(path_transforms)
waypoints.extend(
itertools.islice(self._waypoints, self._wp_index,
len(self._waypoints))
) # add the remaining global route for future
else:
waypoints = self._waypoints
# construct waypoints message
waypoints = collections.deque(
itertools.islice(waypoints, 0,
DEFAULT_NUM_WAYPOINTS)) # only take 50 meters
next_waypoint = waypoints[self._wp_index]
wp_steer_speed_vector, wp_steer_speed_angle = \
get_waypoint_vector_and_angle(
next_waypoint, vehicle_transform
)
output_msg = WaypointsMessage(timestamp,
waypoints=waypoints,
wp_angle=wp_steer_speed_angle,
wp_vector=wp_steer_speed_vector,
wp_angle_speed=wp_steer_speed_angle)
# send waypoints message
waypoints_stream.send(output_msg)
waypoints_stream.send(erdos.WatermarkMessage(timestamp))
def _build_obstacle_map(self, vehicle_transform):
"""
Construct an obstacle map given vehicle_transform.
Args:
vehicle_transform: pylot.utils.Transform of vehicle from can_bus
stream
Returns:
an obstacle map that maps
{id_time: (obstacle_origin, obstacle_range)}
only obstacles within DEFAULT_DISTANCE_THRESHOLD in front of the
ego vehicle are considered to save computation cost.
"""
obstacle_map = {}
prediction_msg = self._prediction_msgs.popleft()
# look over all predictions
for prediction in prediction_msg.predictions:
time = 0
# use all prediction times as potential obstacles
for transform in prediction.trajectory:
if vehicle_transform.location.is_within_distance_ahead(
transform.location, DEFAULT_DISTANCE_THRESHOLD):
# compute the obstacle origin and range of the obstacle
obstacle_origin = ((transform.location.x -
DEFAULT_OBSTACLE_LENGTH / 2,
transform.location.y -
DEFAULT_OBSTACLE_WIDTH / 2),
(DEFAULT_OBSTACLE_LENGTH,
DEFAULT_OBSTACLE_WIDTH))
obs_id = str("{}_{}".format(prediction.id, time))
obstacle_map[obs_id] = obstacle_origin
time += 1
return obstacle_map
def _compute_target_location(self, vehicle_transform):
"""
Update the global waypoint route and compute the target location for
RRT* search to plan for.
Args:
vehicle_transform: pylot.utils.Transform of vehicle from can_bus
stream
Returns:
target location
"""
ego_location = vehicle_transform.location.as_carla_location()
self._waypoints = self._hd_map.compute_waypoints(
ego_location, self._goal_location)
target_waypoint = self._waypoints[self._wp_index]
target_location = target_waypoint.location
return target_location
@staticmethod
def _run_rrt_star(vehicle_transform, target_location, obstacle_map):
"""
Run the RRT* algorithm given the vehicle_transform, target_location,
and obstacle_map.
Args:
vehicle_transform: pylot.utils.Transform of vehicle from can_bus
stream
target_location: Location target
obstacle_map: an obstacle map that maps
{id_time: (obstacle_origin, obstacle_range)}
Returns:
np.ndarray, float
return the path in form [[x0, y0],...] and final cost
if solution not found, returns the path to the closest point to the
target space and final cost is none
"""
starting_state = (vehicle_transform.location.x,
vehicle_transform.location.y)
target_space = ((target_location.x - DEFAULT_TARGET_LENGTH / 2,
target_location.y - DEFAULT_TARGET_WIDTH / 2),
(DEFAULT_TARGET_LENGTH, DEFAULT_TARGET_WIDTH))
state_space = start_target_to_space(starting_state, target_space,
DEFAULT_TARGET_LENGTH,
DEFAULT_TARGET_WIDTH)
path, cost = apply_rrt_star(state_space=state_space,
starting_state=starting_state,
target_space=target_space,
obstacle_map=obstacle_map)
return path, cost
