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
def update(self,
timestamp,
pose,
obstacle_predictions,
static_obstacles,
hd_map=None,
lanes=None):
self.timestamp = timestamp
self.pose = pose
self.ego_transform = pose.transform
self.ego_velocity_vector = pose.velocity_vector
self.ego_trajectory.append(self.ego_transform)
self.obstacle_predictions = obstacle_predictions
self._ego_obstacle_predictions = copy.deepcopy(obstacle_predictions)
# Tranform predictions to world frame of reference.
for obstacle_prediction in self.obstacle_predictions:
obstacle_prediction.to_world_coordinates(self.ego_transform)
# Road signs are in world coordinates. Only maintain the road signs
# that are within the threshold.
self.static_obstacles = []
for obstacle in static_obstacles:
if (obstacle.transform.location.distance(
self.ego_transform.location) <=
self._flags.static_obstacle_distance_threshold):
self.static_obstacles.append(obstacle)
if hd_map is None and lanes is None:
self._logger.error(
'@{}: planning world does not have lanes or HD map'.format(
timestamp))
self._map = hd_map
self._lanes = lanes
# The waypoints are not received on the global trajectory stream.
# We need to compute them using the map.
if not self.waypoints:
if self._map is not None and self._goal_location is not None:
self.waypoints = Waypoints(deque(), deque())
self.waypoints.recompute_waypoints(self._map,
self.ego_transform.location,
self._goal_location)
if pose.forward_speed < 0.7:
# We can't just check if forward_speed is zero because localization
# noise can cause the forward_speed to be non zero even when the
# ego is stopped.
self._num_ticks_stopped += 1
if self._num_ticks_stopped > 10:
self._distance_since_last_full_stop = 0
self._last_stop_ego_location = self.ego_transform.location
else:
self._num_ticks_stopped = 0
if self._last_stop_ego_location is not None:
self._distance_since_last_full_stop = \
self.ego_transform.location.distance(
self._last_stop_ego_location)
else:
self._distance_since_last_full_stop = 0
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 on_pose_watermark(self, timestamp, waypoint_stream, pose_stream):
""" Invoked upon receipt of the watermark on the pose stream.
This callback matches the waypoints to the given timestamp and releases
both the waypoints and the pose message to the control operator.
Args:
timestamp (:py:class:`erdos.Timestamp`): The timestamp of the
watermark.
waypoint_stream (:py:class:`erdos.WriteStream`): The stream to send
the waypoints out on.
pose_stream (:py:class:`erdos.WriteStream`): The stream to send
the pose out on.
"""
self._logger.info("@{}: received pose watermark.".format(timestamp))
# Retrieve the game time.
game_time = timestamp.coordinates[0]
# Retrieve the pose message for the given timestamp.
pose_msg, pose_ingress_time = self._pose_map[game_time]
# Match the waypoints to the given timestamp.
waypoint_index, waypoints = -1, None
for i, (sim_time, _waypoints) in enumerate(self._waypoints):
if sim_time <= game_time:
waypoint_index, waypoints = i, _waypoints
else:
break
self._logger.debug("@{} waypoint index is {}".format(
timestamp, waypoint_index))
if waypoints is None:
# If we haven't received a single waypoint, send an empty message.
self._waypoints_write_stream.send(
WaypointsMessage(timestamp, Waypoints(deque([]), deque([]))))
else:
# Send the trimmed waypoints on the write stream.
waypoints.remove_completed(pose_msg.data.transform.location,
pose_msg.data.transform)
self._waypoints_write_stream.send(
WaypointsMessage(timestamp, waypoints))
# Send the pose and the watermark messages.
watermark = erdos.WatermarkMessage(timestamp)
pose_stream.send(pose_msg)
pose_stream.send(watermark)
waypoint_stream.send(watermark)
# Clean up the pose from the dict.
self._pose_map.pop(game_time, None)
def send_waypoints_msg(self, timestamp):
# Send once the global waypoints.
if self._waypoints is None:
# Gets global waypoints from the agent.
waypoints = deque([])
road_options = deque([])
for (transform, road_option) in self._global_plan_world_coord:
waypoints.append(
pylot.utils.Transform.from_carla_transform(transform))
road_options.append(pylot.utils.RoadOption(road_option.value))
self._waypoints = Waypoints(waypoints, road_options=road_options)
self._global_trajectory_stream.send(
WaypointsMessage(timestamp, self._waypoints))
self._global_trajectory_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def send_global_trajectory_msg(self, global_trajectory_stream, timestamp):
"""Sends the route the agent must follow."""
# Send once the global waypoints.
if not global_trajectory_stream.is_closed():
# Gets global waypoints from the agent.
waypoints = deque([])
road_options = deque([])
for (transform, road_option) in self._global_plan_world_coord:
waypoints.append(
pylot.utils.Transform.from_simulator_transform(transform))
road_options.append(pylot.utils.RoadOption(road_option.value))
waypoints = Waypoints(waypoints, road_options=road_options)
global_trajectory_stream.send(
WaypointsMessage(timestamp, waypoints))
global_trajectory_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def build_output_waypoints(self, path_x, path_y, speeds):
wps = deque()
target_speeds = deque()
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)
wps.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
return Waypoints(wps, target_speeds)
def build_output_waypoints(self, path_x, path_y, speeds):
"""Builds a Waypoints object from 2D locations and speeds."""
wps = deque()
target_speeds = deque()
for point in zip(path_x, path_y, speeds):
if self._map is not None:
# Use the HD Map to transform a 2D location into a
# 3D location.
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)
# Use the computed x and y (not the ones returned by the HDMap)
# to ensure that the vehicles follows the computed plan.
wps.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
return Waypoints(wps, target_speeds)
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 World(object):
"""A representation of the world that is used by the planners."""
def __init__(self, flags, logger):
self._flags = flags
self._logger = logger
self.static_obstacles = None
self.obstacle_predictions = []
self._ego_obstacle_predictions = []
self.pose = None
self.ego_trajectory = deque(maxlen=self._flags.tracking_num_steps)
self.ego_transform = None
self.ego_velocity_vector = None
self._lanes = None
self._map = None
self._goal_location = None
self.waypoints = None
self.timestamp = None
self._last_stop_ego_location = None
self._distance_since_last_full_stop = 0
self._num_ticks_stopped = 0
def update(self,
timestamp,
pose,
obstacle_predictions,
static_obstacles,
hd_map=None,
lanes=None):
self.timestamp = timestamp
self.pose = pose
self.ego_transform = pose.transform
self.ego_trajectory.append(self.ego_transform)
self.obstacle_predictions = obstacle_predictions
self._ego_obstacle_predictions = copy.deepcopy(obstacle_predictions)
# Tranform predictions to world frame of reference.
for obstacle_prediction in self.obstacle_predictions:
obstacle_prediction.to_world_coordinates(self.ego_transform)
# Road signs are in world coordinates.
self.static_obstacles = []
for obstacle in static_obstacles:
if (obstacle.transform.location.distance(
self.ego_transform.location) <=
self._flags.static_obstacle_distance_threshold):
self.static_obstacles.append(obstacle)
self._map = hd_map
self._lanes = lanes
self.ego_velocity_vector = pose.velocity_vector
# The waypoints are not received on the global trajectory stream.
# We need to compute them using the map.
if not self.waypoints:
if self._map is not None and self._goal_location is not None:
self.waypoints = Waypoints(deque(), deque())
self.waypoints.recompute_waypoints(self._map,
self.ego_transform.location,
self._goal_location)
if pose.forward_speed < 0.7:
# We can't just check if forward_speed is zero because localization
# noise can cause the forward_speed to be non zero even when the
# ego is stopped.
self._num_ticks_stopped += 1
if self._num_ticks_stopped > 10:
self._distance_since_last_full_stop = 0
self._last_stop_ego_location = self.ego_transform.location
else:
self._num_ticks_stopped = 0
if self._last_stop_ego_location is not None:
self._distance_since_last_full_stop = \
self.ego_transform.location.distance(
self._last_stop_ego_location)
else:
self._distance_since_last_full_stop = 0
def update_waypoints(self, goal_location, waypoints):
self._goal_location = goal_location
self.waypoints = waypoints
def follow_waypoints(self, target_speed):
self.waypoints.remove_completed(self.ego_transform.location,
self.ego_transform)
return self.waypoints.slice_waypoints(0,
self._flags.num_waypoints_ahead,
target_speed)
def get_obstacle_list(self):
obstacle_list = []
for prediction in self.obstacle_predictions:
# Use all prediction times as potential obstacles.
previous_origin = None
for transform in prediction.predicted_trajectory:
# Ignore predictions that are too close.
if (previous_origin is None
or previous_origin.location.l2_distance(
transform.location) >
self._flags.obstacle_filtering_distance):
previous_origin = transform
# Ensure the prediction is nearby.
if (self.ego_transform.location.l2_distance(
transform.location) <=
self._flags.dynamic_obstacle_distance_threshold):
obstacle = prediction.obstacle_trajectory.obstacle
obstacle_corners = \
obstacle.get_bounding_box_corners(
transform, self._flags.obstacle_radius)
obstacle_list.append(obstacle_corners)
if len(obstacle_list) == 0:
return np.empty((0, 4))
return np.array(obstacle_list)
def draw_on_frame(self, frame):
for obstacle_prediction in self._ego_obstacle_predictions:
obstacle_prediction.draw_trajectory_on_frame(frame)
for obstacle in self.static_obstacles:
if obstacle.is_traffic_light():
world_transform = obstacle.transform
# Transform traffic light to ego frame of reference.
obstacle.transform = (self.ego_transform.inverse_transform() *
obstacle.transform)
obstacle.draw_on_bird_eye_frame(frame)
obstacle.transform = world_transform
if self.waypoints:
self.waypoints.draw_on_frame(
frame, self.ego_transform.inverse_transform())
# TODO: Draw lane markings. We do not draw them currently
# because we need to transform them from world frame of reference
# to ego vehicle frame of reference, which is slow to compute.
# if self._map:
# lane = self._map.get_lane(self.ego_transform.location)
# lane.draw_on_frame(frame, self.ego_transform.inverse_transform())
if self._lanes:
for lane in self._lanes:
lane.draw_on_frame(frame)
def stop_person(self, obstacle, wp_vector):
"""Computes a stopping factor for ego vehicle given a person obstacle.
Args:
obstacle (:py:class:`~pylot.prediction.obstacle_prediction.ObstaclePrediction`): # noqa: E501
Prediction for a person.
wp_vector (:py:class:`~pylot.utils.Vector2D`): vector from the ego
vehicle to the target waypoint.
Returns:
:obj:`float`: A stopping factor between 0 and 1 (i.e., no braking).
"""
if self._map is not None:
if not self._map.are_on_same_lane(self.ego_transform.location,
obstacle.transform.location):
# Person is not on the same lane.
if not any(
map(
lambda transform: self._map.are_on_same_lane(
self.ego_transform.location, transform.location
), obstacle.predicted_trajectory)):
# The person is not going to be on the same lane.
self._logger.debug(
'Ignoring ({},{}); not going to be on the same lane'.
format(obstacle.label, obstacle.id))
return 1
else:
self._logger.warning(
'No HDMap. All people are considered for stopping.')
ego_location_2d = self.ego_transform.location.as_vector_2D()
min_speed_factor_p = compute_person_speed_factor(
ego_location_2d, obstacle.transform.location.as_vector_2D(),
wp_vector, self._flags, self._logger)
transforms = itertools.islice(
obstacle.predicted_trajectory, 0,
min(NUM_FUTURE_TRANSFORMS, len(obstacle.predicted_trajectory)))
for person_transform in transforms:
speed_factor_p = compute_person_speed_factor(
ego_location_2d, person_transform.location.as_vector_2D(),
wp_vector, self._flags, self._logger)
min_speed_factor_p = min(min_speed_factor_p, speed_factor_p)
return min_speed_factor_p
def stop_vehicle(self, obstacle, wp_vector):
"""Computes a stopping factor for ego vehicle given a vehicle pos.
Args:
obstacle (:py:class:`~pylot.prediction.obstacle_prediction.ObstaclePrediction`): # noqa: E501
Prediction for a vehicle.
wp_vector (:py:class:`~pylot.utils.Vector2D`): vector from the ego
vehicle to the target waypoint.
Returns:
:obj:`float`: A stopping factor between 0 and 1 (i.e., no braking).
"""
if self.ego_transform.location.x == obstacle.transform.location.x and \
self.ego_transform.location.y == obstacle.transform.location.y and \
self.ego_transform.location.z == obstacle.transform.location.z:
# Don't stop for ourselves.
return 1
if self._map is not None:
if not self._map.are_on_same_lane(self.ego_transform.location,
obstacle.transform.location):
# Vehicle is not on the same lane as the ego.
if not any(
map(
lambda transform: self._map.are_on_same_lane(
self.ego_transform.location, transform.location
), obstacle.predicted_trajectory)):
# The vehicle is not going to be on the same lane as ego.
self._logger.debug(
'Ignoring ({},{}); not going to be on the same lane'.
format(obstacle.label, obstacle.id))
return 1
else:
self._logger.warning(
'No HDMap. All vehicles are considered for stopping.')
ego_location_2d = self.ego_transform.location.as_vector_2D()
min_speed_factor_v = compute_vehicle_speed_factor(
ego_location_2d, obstacle.transform.location.as_vector_2D(),
wp_vector, self._flags, self._logger)
transforms = itertools.islice(
obstacle.predicted_trajectory, 0,
min(NUM_FUTURE_TRANSFORMS, len(obstacle.predicted_trajectory)))
for vehicle_transform in transforms:
speed_factor_v = compute_vehicle_speed_factor(
ego_location_2d, vehicle_transform.location.as_vector_2D(),
wp_vector, self._flags, self._logger)
min_speed_factor_v = min(min_speed_factor_v, speed_factor_v)
return min_speed_factor_v
def stop_for_agents(self, timestamp):
"""Calculates the speed factor in [0, 1] (0 is full stop).
Reduces the speed factor whenever the ego vehicle's path is blocked
by an obstacle, or the ego vehicle must stop at a traffic light.
"""
speed_factor_tl = 1
speed_factor_p = 1
speed_factor_v = 1
speed_factor_stop = 1
try:
self.waypoints.remove_completed(self.ego_transform.location)
wp_vector = self.waypoints.get_vector(
self.ego_transform,
self._flags.min_pid_steer_waypoint_distance)
wp_angle = self.waypoints.get_angle(
self.ego_transform,
self._flags.min_pid_steer_waypoint_distance)
except ValueError:
# No more waypoints to follow.
self._logger.debug(
'@{}: no more waypoints to follow, target speed 0')
return (0, 0, 0, 0, 0)
for obstacle in self.obstacle_predictions:
if obstacle.is_person() and self._flags.stop_for_people:
new_speed_factor_p = self.stop_person(obstacle, wp_vector)
if new_speed_factor_p < speed_factor_p:
speed_factor_p = new_speed_factor_p
self._logger.debug(
'@{}: person {} reduced speed factor to {}'.format(
timestamp, obstacle.id, speed_factor_p))
elif obstacle.is_vehicle() and self._flags.stop_for_vehicles:
new_speed_factor_v = self.stop_vehicle(obstacle, wp_vector)
if new_speed_factor_v < speed_factor_v:
speed_factor_v = new_speed_factor_v
self._logger.debug(
'@{}: vehicle {} reduced speed factor to {}'.format(
timestamp, obstacle.id, speed_factor_v))
else:
self._logger.debug('@{}: filtering obstacle {}'.format(
timestamp, obstacle.label))
semaphorized_junction = False
for obstacle in self.static_obstacles:
if (obstacle.is_traffic_light()
and self._flags.stop_for_traffic_lights):
valid_tl, new_speed_factor_tl = self.stop_traffic_light(
obstacle, wp_vector, wp_angle)
semaphorized_junction = semaphorized_junction or valid_tl
if new_speed_factor_tl < speed_factor_tl:
speed_factor_tl = new_speed_factor_tl
self._logger.debug(
'@{}: traffic light {} reduced speed factor to {}'.
format(timestamp, obstacle.id, speed_factor_tl))
if self._flags.stop_at_uncontrolled_junctions:
if (self._map is not None and not semaphorized_junction
and not self._map.is_intersection(
self.ego_transform.location)):
dist_to_junction = self._map.distance_to_intersection(
self.ego_transform.location, max_distance_to_check=13)
self._logger.debug('@{}: dist to junc {}, last stop {}'.format(
timestamp, dist_to_junction,
self._distance_since_last_full_stop))
if (dist_to_junction is not None
and self._distance_since_last_full_stop > 13):
speed_factor_stop = 0
speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v,
speed_factor_stop)
self._logger.debug(
'@{}: speed factors: person {}, vehicle {}, traffic light {},'
' stop {}'.format(timestamp, speed_factor_p, speed_factor_v,
speed_factor_tl, speed_factor_stop))
return (speed_factor, speed_factor_p, speed_factor_v, speed_factor_tl,
speed_factor_stop)
def stop_traffic_light(self, tl, wp_vector, wp_angle):
"""Computes a stopping factor for ego vehicle given a traffic light.
Args:
tl (:py:class:`~pylot.perception.detection.traffic_light.TrafficLight`): # noqa: E501
the traffic light.
wp_vector (:py:class:`~pylot.utils.Vector2D`): vector from the ego
vehicle to the target waypoint.
Returns:
:obj:`float`: A stopping factor between 0 and 1 (i.e., no braking).
"""
if self._map is not None:
# The traffic light is not relevant to the ego vehicle.
if not self._map.must_obey_traffic_light(
self.ego_transform.location, tl.transform.location):
self._logger.debug(
'Ignoring traffic light {} that must not be obeyed'.format(
tl))
return False, 1
else:
self._logger.warning(
'No HDMap. All traffic lights are considered for stopping.')
# The ego vehicle is too close to the traffic light.
if (self.ego_transform.location.distance(tl.transform.location) <
self._flags.traffic_light_min_distance):
self._logger.debug(
'Ignoring traffic light {}; vehicle is too close'.format(tl))
return True, 1
# The ego vehicle can carry on driving.
if (tl.state == TrafficLightColor.GREEN
or tl.state == TrafficLightColor.OFF):
return True, 1
height_delta = tl.transform.location.z - self.ego_transform.location.z
if height_delta > 4:
self._logger.debug('Traffic light is American style')
# The traffic ligh is across the road. Increase the max distance.
traffic_light_max_distance = \
self._flags.traffic_light_max_distance * 2.5
traffic_light_max_angle = self._flags.traffic_light_max_angle / 3
american_tl = True
else:
self._logger.debug('Traffic light is European style')
traffic_light_max_distance = self._flags.traffic_light_max_distance
traffic_light_max_angle = self._flags.traffic_light_max_angle
american_tl = False
speed_factor_tl = 1
ego_location_2d = self.ego_transform.location.as_vector_2D()
tl_location_2d = tl.transform.location.as_vector_2D()
tl_vector = tl_location_2d - ego_location_2d
tl_dist = tl_location_2d.l2_distance(ego_location_2d)
tl_angle = tl_vector.get_angle(wp_vector)
self._logger.debug(
'Traffic light vector {}; dist {}; angle {}; wp_angle {}'.format(
tl_vector, tl_dist, tl_angle, wp_angle))
if (-0.2 <= tl_angle < traffic_light_max_angle
and tl_dist < traffic_light_max_distance):
# The traffic light is at most x radians to the right of the
# vehicle path, and is not too far away.
speed_factor_tl = min(
speed_factor_tl, tl_dist /
(self._flags.coast_factor * traffic_light_max_distance))
if (-0.2 <= tl_angle <
traffic_light_max_angle / self._flags.coast_factor and
tl_dist < traffic_light_max_distance * self._flags.coast_factor
and math.fabs(wp_angle) < 0.2):
# The ego is pretty far away, so the angle to the traffic light has
# to be smaller, and the vehicle must be driving straight.
speed_factor_tl = min(
speed_factor_tl, tl_dist /
(self._flags.coast_factor * traffic_light_max_distance))
if (-0.2 <= tl_angle <
traffic_light_max_angle * self._flags.coast_factor
and math.fabs(wp_angle) < 0.2):
if american_tl:
if (-0.1 <= tl_angle < traffic_light_max_angle
and tl_dist < 60):
dist_to_intersection = self._map.distance_to_intersection(
self.ego_transform.location, max_distance_to_check=20)
if (dist_to_intersection is not None
and dist_to_intersection < 12):
if (tl.bounding_box is None
or tl.bounding_box.get_width() *
tl.bounding_box.get_height() > 400):
speed_factor_tl = 0
if (dist_to_intersection is not None and tl_dist < 27
and 12 <= dist_to_intersection <= 20):
speed_factor_tl = 0
else:
if (tl_dist <
traffic_light_max_distance / self._flags.coast_factor):
# The traffic light is nearby and the vehicle is driving
# straight; the angle to the traffic light can be higher.
speed_factor_tl = 0
if speed_factor_tl < 1:
dist_to_intersection = self._map.distance_to_intersection(
self.ego_transform.location, max_distance_to_check=20)
if dist_to_intersection is None:
# Our lidar-based depth estimation does not work when
# we're on a hill.
# XXX(ionel): Hack to avoid getting stuck when we're far
# from intersections (see scenario 28 in the challenge training
# routes).
self._logger.warning(
'Ignored traffic light speed factor because junction '
'is not nearby')
return True, 1
else:
return True, speed_factor_tl
else:
# The traffic light doesn't affect the vehicle.
return False, speed_factor_tl
def main(argv):
(obstacles_stream, traffic_lights_stream, obstacles_tracking_stream,
open_drive_stream, global_trajectory_stream, control_display_stream,
streams_to_send_top_on) = create_data_flow()
# Run the data-flow.
node_handle = erdos.run_async()
signal.signal(signal.SIGINT, shutdown)
# Send waypoints.
waypoints = Waypoints.read_from_csv_file(FLAGS.waypoints_csv_file,
FLAGS.target_speed)
global_trajectory_stream.send(
WaypointsMessage(
erdos.Timestamp(coordinates=[0]), waypoints,
pylot.planning.utils.BehaviorPlannerState.FOLLOW_WAYPOINTS))
# Send top watermark on all streams that require it.
top_msg = erdos.WatermarkMessage(erdos.Timestamp(is_top=True))
open_drive_stream.send(top_msg)
global_trajectory_stream.send(top_msg)
for stream in streams_to_send_top_on:
stream.send(top_msg)
time_to_sleep = 1.0 / FLAGS.sensor_frequency
count = 0
try:
while True:
timestamp = erdos.Timestamp(coordinates=[count])
if not FLAGS.obstacle_detection:
obstacles_stream.send(ObstaclesMessage(timestamp, []))
obstacles_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.traffic_light_detection:
traffic_lights_stream.send(TrafficLightsMessage(timestamp, []))
traffic_lights_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.obstacle_tracking:
obstacles_tracking_stream.send(
ObstacleTrajectoriesMessage(timestamp, []))
obstacles_tracking_stream.send(
erdos.WatermarkMessage(timestamp))
count += 1
if pylot.flags.must_visualize():
import pygame
from pygame.locals import K_n
events = pygame.event.get()
for event in events:
if event.type == pygame.KEYUP:
if event.key == K_n:
control_display_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
event.key))
elif event.type == pygame.QUIT:
raise KeyboardInterrupt
elif event.type == pygame.KEYDOWN:
if (event.key == pygame.K_c
and pygame.key.get_mods() & pygame.KMOD_CTRL):
raise KeyboardInterrupt
# NOTE: We should offset sleep time by the time it takes to send
# the messages.
time.sleep(time_to_sleep)
except KeyboardInterrupt:
node_handle.shutdown()
if pylot.flags.must_visualize():
import pygame
pygame.quit()