def __init__(self, pose_stream: erdos.ReadStream,
prediction_stream: erdos.ReadStream,
static_obstacles_stream: erdos.ReadStream,
lanes_stream: erdos.ReadStream,
route_stream: erdos.ReadStream,
open_drive_stream: erdos.ReadStream,
time_to_decision_stream: erdos.ReadStream,
waypoints_stream: erdos.WriteStream, flags):
pose_stream.add_callback(self.on_pose_update)
prediction_stream.add_callback(self.on_prediction_update)
static_obstacles_stream.add_callback(self.on_static_obstacles_update)
lanes_stream.add_callback(self.on_lanes_update)
route_stream.add_callback(self.on_route)
open_drive_stream.add_callback(self.on_opendrive_map)
time_to_decision_stream.add_callback(self.on_time_to_decision)
erdos.add_watermark_callback([
pose_stream, prediction_stream, static_obstacles_stream,
lanes_stream, time_to_decision_stream, route_stream
], [waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
# We do not know yet the vehicle's location.
self._ego_transform = None
self._map = None
self._world = World(flags, self._logger)
if self._flags.planning_type == 'waypoint':
# Use the FOT planner for overtaking.
from pylot.planning.frenet_optimal_trajectory.fot_planner \
import FOTPlanner
self._planner = FOTPlanner(self._world, self._flags, self._logger)
elif self._flags.planning_type == 'frenet_optimal_trajectory':
from pylot.planning.frenet_optimal_trajectory.fot_planner \
import FOTPlanner
self._planner = FOTPlanner(self._world, self._flags, self._logger)
elif self._flags.planning_type == 'hybrid_astar':
from pylot.planning.hybrid_astar.hybrid_astar_planner \
import HybridAStarPlanner
self._planner = HybridAStarPlanner(self._world, self._flags,
self._logger)
elif self._flags.planning_type == 'rrt_star':
from pylot.planning.rrt_star.rrt_star_planner import RRTStarPlanner
self._planner = RRTStarPlanner(self._world, self._flags,
self._logger)
else:
raise ValueError('Unexpected planning type: {}'.format(
self._flags.planning_type))
self._state = BehaviorPlannerState.FOLLOW_WAYPOINTS
self._pose_msgs = deque()
self._prediction_msgs = deque()
self._static_obstacles_msgs = deque()
self._lanes_msgs = deque()
self._ttd_msgs = deque()
def __init__(self, pose_stream, rgb_camera_stream, tl_camera_stream,
prediction_camera_stream, depth_camera_stream,
point_cloud_stream, segmentation_stream, imu_stream,
obstacles_stream, obstacles_error_stream, traffic_lights_stream,
tracked_obstacles_stream, lane_detection_stream,
prediction_stream, waypoints_stream, control_stream,
display_control_stream, pygame_display, flags):
visualize_streams = []
self._pose_msgs = deque()
pose_stream.add_callback(
partial(self.save, msg_type="Pose", queue=self._pose_msgs))
visualize_streams.append(pose_stream)
self._bgr_msgs = deque()
rgb_camera_stream.add_callback(
partial(self.save, msg_type="RGB", queue=self._bgr_msgs))
visualize_streams.append(rgb_camera_stream)
self._imu_msgs = deque()
imu_stream.add_callback(
partial(self.save, msg_type="IMU", queue=self._imu_msgs))
visualize_streams.append(imu_stream)
"""
self._obstacle_msgs = deque()
obstacles_stream.add_callback(
partial(self.save, msg_type="Obstacle", queue=self._obstacle_msgs))
visualize_streams.append(obstacles_stream)
"""
self._obstacle_error_msgs = deque()
obstacles_error_stream.add_callback(
partial(self.save, msg_type="ObstacleError", queue=self._obstacle_error_msgs))
visualize_streams.append(obstacles_error_stream)
self._tracked_obstacle_msgs = deque()
tracked_obstacles_stream.add_callback(
partial(self.save,
msg_type="TrackedObstacle",
queue=self._tracked_obstacle_msgs))
visualize_streams.append(tracked_obstacles_stream)
self._tl_camera_msgs = deque()
tl_camera_stream.add_callback(
partial(self.save, msg_type="TLCamera",
queue=self._tl_camera_msgs))
visualize_streams.append(tl_camera_stream)
self._traffic_light_msgs = deque()
traffic_lights_stream.add_callback(
partial(self.save,
msg_type="TrafficLight",
queue=self._traffic_light_msgs))
visualize_streams.append(traffic_lights_stream)
self._waypoint_msgs = deque()
waypoints_stream.add_callback(
partial(self.save, msg_type="Waypoint", queue=self._waypoint_msgs))
visualize_streams.append(waypoints_stream)
self._prediction_camera_msgs = deque()
prediction_camera_stream.add_callback(
partial(self.save,
msg_type="PredictionCamera",
queue=self._prediction_camera_msgs))
visualize_streams.append(prediction_camera_stream)
self._prediction_msgs = deque()
prediction_stream.add_callback(
partial(self.save,
msg_type="Prediction",
queue=self._prediction_msgs))
visualize_streams.append(prediction_stream)
self._point_cloud_msgs = deque()
point_cloud_stream.add_callback(
partial(self.save,
msg_type="PointCloud",
queue=self._point_cloud_msgs))
visualize_streams.append(point_cloud_stream)
self._lane_detection_msgs = deque()
lane_detection_stream.add_callback(
partial(self.save,
msg_type="Lanes",
queue=self._lane_detection_msgs))
visualize_streams.append(lane_detection_stream)
self._depth_msgs = deque()
depth_camera_stream.add_callback(
partial(self.save, msg_type="Depth", queue=self._depth_msgs))
visualize_streams.append(depth_camera_stream)
self._segmentation_msgs = deque()
segmentation_stream.add_callback(
partial(self.save,
msg_type="Segmentation",
queue=self._segmentation_msgs))
visualize_streams.append(segmentation_stream)
self._control_msgs = deque()
control_stream.add_callback(
partial(self.save, msg_type="Control", queue=self._control_msgs))
visualize_streams.append(control_stream)
# Register a watermark callback on all the streams to be visualized.
erdos.add_watermark_callback(visualize_streams, [], self.on_watermark)
# Add a callback on a control stream to figure out what to display.
display_control_stream.add_callback(self.change_display)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self.display = pygame_display
# Set the font.
fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
default_font = 'ubuntumono'
mono = default_font if default_font in fonts else fonts[0]
mono = pygame.font.match_font(mono)
self.font = pygame.font.Font(mono, 14)
# Array of keys to figure out which message to display.
self.current_display = 0
self.display_array = []
self.window_titles = []
if flags.visualize_rgb_camera:
self.display_array.append("RGB")
self.window_titles.append("RGB Camera")
if flags.visualize_detected_obstacles:
# include obstacles error here; todo: seperate into flags
self.display_array.append("ObstacleError")
self.window_titles.append("Detected obstacles")
#self.display_array.append("Obstacle")
#self.window_titles.append("Detected obstacles")
if flags.visualize_tracked_obstacles:
self.display_array.append("TrackedObstacle")
self.window_titles.append("Obstacle tracking")
if flags.visualize_detected_traffic_lights:
self.display_array.append("TLCamera")
self.window_titles.append("Detected traffic lights")
if flags.visualize_waypoints:
self.display_array.append("Waypoint")
self.window_titles.append("Planning")
if flags.visualize_prediction:
self.display_array.append("PredictionCamera")
self.window_titles.append("Prediction")
if flags.visualize_lidar:
self.display_array.append("PointCloud")
self.window_titles.append("LiDAR")
if flags.visualize_detected_lanes:
self.display_array.append("Lanes")
self.window_titles.append("Detected lanes")
if flags.visualize_depth_camera:
self.display_array.append("Depth")
self.window_titles.append("Depth Camera")
if flags.visualize_segmentation:
self.display_array.append("Segmentation")
self.window_titles.append("Segmentation")
if flags.visualize_world:
self._planning_world = World(flags, self._logger)
top_down_transform = pylot.utils.get_top_down_transform(
pylot.utils.Transform(pylot.utils.Location(),
pylot.utils.Rotation()),
flags.top_down_camera_altitude)
self._bird_eye_camera_setup = RGBCameraSetup(
'bird_eye_camera', flags.camera_image_width,
flags.camera_image_height, top_down_transform, 90)
self.display_array.append("PlanningWorld")
self.window_titles.append("Planning world")
else:
self._planning_world = None
assert len(self.display_array) == len(self.window_titles), \
"The display and titles differ."
# Save the flags.
self._flags = flags