From 64d0d9d5554582c3108d15906627e13a90ac0f45 Mon Sep 17 00:00:00 2001 From: Ionel Gog Date: Mon, 29 Mar 2021 12:37:55 -0700 Subject: [PATCH] Improved flag validation and added more type hints (#178) * More type hints. * Updated the scripts to ensure ENV variables are set. * Improved flag validation. * Ensure end-to-end tests are not executed by pytest. --- pylot.py | 3 +- pylot/component_creator.py | 87 +++++++++++++----- pylot/flags.py | 91 ++++++++++++++++++- pylot/map/hd_map.py | 23 ++--- .../base_perception_eval_operator.py | 34 ++++--- pylot/perception/camera_frame.py | 2 +- pylot/perception/depth_frame.py | 6 +- pylot/planning/behavior_planning_operator.py | 26 +++--- pylot/planning/messages.py | 15 +-- pylot/planning/planning_operator.py | 39 +++++--- pylot/planning/utils.py | 4 +- pylot/planning/waypoints.py | 35 ++++--- pylot/planning/world.py | 10 +- pylot/prediction/flags.py | 2 +- pylot/prediction/messages.py | 10 +- pylot/prediction/obstacle_prediction.py | 4 +- pylot/prediction/prediction_eval_operator.py | 8 +- pylot/prediction/r2p2_predictor_operator.py | 16 ++-- pylot/utils.py | 50 +++++----- requirements.txt | 11 ++- scripts/collect_data.sh | 25 +++-- scripts/collect_sign_data.sh | 14 +-- scripts/replay_logs.sh | 5 + scripts/run_map_experiments.sh | 42 ++++++--- scripts/run_miou_experiments.sh | 45 +++++---- setup.py | 11 ++- {scripts => tests}/check_3d_2d_conversions.py | 0 .../check_canny_lane_detection.py | 0 .../check_lanenet_lane_detection.py | 0 {scripts => tests}/test_determinism.sh | 0 30 files changed, 410 insertions(+), 208 deletions(-) rename {scripts => tests}/check_3d_2d_conversions.py (100%) rename scripts/test_canny_lane_detection.py => tests/check_canny_lane_detection.py (100%) rename scripts/test_lanenet_lane_detection.py => tests/check_lanenet_lane_detection.py (100%) rename {scripts => tests}/test_determinism.sh (100%) diff --git a/pylot.py b/pylot.py index ce5953590..bdbd34699 100644 --- a/pylot.py +++ b/pylot.py @@ -197,8 +197,7 @@ def driver(): control_loop_stream.set(control_stream) pylot.component_creator.add_evaluation(vehicle_id_stream, pose_stream, - imu_stream, pose_stream_for_control, - waypoints_stream_for_control) + imu_stream) time_to_decision_stream = pylot.operator_creator.add_time_to_decision( pose_stream, obstacles_stream) diff --git a/pylot/component_creator.py b/pylot/component_creator.py index 8420a334a..1a5479820 100644 --- a/pylot/component_creator.py +++ b/pylot/component_creator.py @@ -1,3 +1,5 @@ +import logging + from absl import flags import pylot.operator_creator @@ -6,6 +8,8 @@ FLAGS = flags.FLAGS +logger = logging.getLogger(__name__) + def add_obstacle_detection(center_camera_stream, center_camera_setup=None, @@ -65,16 +69,19 @@ def add_obstacle_detection(center_camera_stream, obstacles_stream_wo_depth = None if any('efficientdet' in model for model in FLAGS.obstacle_detection_model_names): + logger.debug('Using EfficientDet obstacle detector...') obstacles_streams = pylot.operator_creator.\ - add_efficientdet_obstacle_detection( - center_camera_stream, time_to_decision_stream) + add_efficientdet_obstacle_detection( + center_camera_stream, time_to_decision_stream) obstacles_stream_wo_depth = obstacles_streams[0] else: + logger.debug('Using obstacle detector...') # TODO: Only returns the first obstacles stream. obstacles_streams = pylot.operator_creator.add_obstacle_detection( center_camera_stream, time_to_decision_stream) obstacles_stream_wo_depth = obstacles_streams[0] if FLAGS.planning_type == 'waypoint': + logger.debug('Adding obstacle location finder...') # Adds an operator that finds the world locations of the obstacles. obstacles_stream = \ pylot.operator_creator.add_obstacle_location_finder( @@ -89,11 +96,13 @@ def add_obstacle_detection(center_camera_stream, and ground_obstacles_stream is not None and ground_speed_limit_signs_stream is not None and ground_stop_signs_stream is not None) + logger.debug('Using perfect obstacle detector...') perfect_obstacles_stream = pylot.operator_creator.add_perfect_detector( depth_camera_stream, center_camera_stream, segmented_camera_stream, pose_stream, ground_obstacles_stream, ground_speed_limit_signs_stream, ground_stop_signs_stream) if FLAGS.evaluate_obstacle_detection: + logger.debug('Adding obstacle detection evaluation...') pylot.operator_creator.add_detection_evaluation( obstacles_stream_wo_depth, perfect_obstacles_stream, @@ -107,9 +116,11 @@ def add_obstacle_detection(center_camera_stream, matching_policy='ceil', name='timely_detection_eval_operator') if FLAGS.perfect_obstacle_detection: + logger.debug('Using perfect obstacle detector...') obstacles_stream = perfect_obstacles_stream if FLAGS.simulator_obstacle_detection: + logger.debug('Using ground obstacles from the simulator...') obstacles_stream = ground_obstacles_stream return obstacles_stream, perfect_obstacles_stream @@ -147,6 +158,7 @@ def add_traffic_light_detection(tl_transform, """ tl_camera_stream = None if FLAGS.traffic_light_detection or FLAGS.perfect_traffic_light_detection: + logger.debug('Adding traffic light camera...') # Only add the TL camera if traffic light detection is enabled. tl_camera_setup = RGBCameraSetup('traffic_light_camera', FLAGS.camera_image_width, @@ -159,6 +171,7 @@ def add_traffic_light_detection(tl_transform, traffic_lights_stream = None if FLAGS.traffic_light_detection: + logger.debug('Using traffic light detection...') traffic_lights_stream = \ pylot.operator_creator.add_traffic_light_detector( tl_camera_stream) @@ -171,6 +184,7 @@ def add_traffic_light_detection(tl_transform, if FLAGS.perfect_traffic_light_detection: assert (pose_stream is not None and ground_traffic_lights_stream is not None) + logger.debug('Using perfect traffic light detection...') # Add segmented and depth cameras with fov 45. These cameras are needed # by the perfect traffic light detector. tl_depth_camera_setup = DepthCameraSetup('traffic_light_depth_camera', @@ -197,6 +211,7 @@ def add_traffic_light_detection(tl_transform, pose_stream) if FLAGS.simulator_traffic_light_detection: + logger.debug('Using ground traffic lights from the simulator...') traffic_lights_stream = ground_traffic_lights_stream return traffic_lights_stream, tl_camera_stream @@ -227,12 +242,15 @@ def add_depth(transform, vehicle_id_stream, center_camera_setup, """ depth_stream = None if FLAGS.depth_estimation: + logger.debug('Adding left and right cameras for depth estimation...') (left_camera_stream, right_camera_stream) = pylot.operator_creator.add_left_right_cameras( transform, vehicle_id_stream) + logger.debug('Using camera depth estimation...') depth_stream = pylot.operator_creator.add_depth_estimation( left_camera_stream, right_camera_stream, center_camera_setup) - elif FLAGS.perfect_depth_estimation: + if FLAGS.perfect_depth_estimation: + logger.debug('Using perfect depth estimation...') depth_stream = depth_camera_stream return depth_stream @@ -263,15 +281,22 @@ def add_lane_detection(center_camera_stream, lane_detection_stream = None if FLAGS.lane_detection: if FLAGS.lane_detection_type == 'canny': + logger.debug('Using Canny Edge lane detector...') lane_detection_stream = \ pylot.operator_creator.add_canny_edge_lane_detection( center_camera_stream) elif FLAGS.lane_detection_type == 'lanenet': + logger.debug('Using Lanenet lane detector...') lane_detection_stream = \ pylot.operator_creator.add_lanenet_detection( center_camera_stream) - elif FLAGS.perfect_lane_detection: - assert pose_stream is not None + else: + raise ValueError('Unexpected lane detection type {}'.format( + FLAGS.lane_detection_type)) + if FLAGS.perfect_lane_detection: + assert pose_stream is not None, \ + 'Cannot added perfect lane detection without a post stream' + logger.debug('Using perfect lane detector...') lane_detection_stream = \ pylot.operator_creator.add_perfect_lane_detector( pose_stream, open_drive_stream, center_camera_stream) @@ -321,22 +346,26 @@ def add_obstacle_tracking(center_camera_stream, obstacles_tracking_stream = None if FLAGS.obstacle_tracking: if FLAGS.tracker_type == 'center_track': + logger.debug('Using CenterTrack obstacle tracker...') obstacles_wo_history_tracking_stream = \ pylot.operator_creator.add_center_track_tracking( center_camera_stream, center_camera_setup) else: + logger.debug('Using obstacle tracker...') obstacles_wo_history_tracking_stream = \ pylot.operator_creator.add_obstacle_tracking( obstacles_stream, center_camera_stream, time_to_decision_stream) + logger.debug('Adding operator to compute obstacle location history...') obstacles_tracking_stream = \ pylot.operator_creator.add_obstacle_location_history( obstacles_wo_history_tracking_stream, depth_stream, pose_stream, center_camera_setup) - elif FLAGS.perfect_obstacle_tracking: + if FLAGS.perfect_obstacle_tracking: assert (pose_stream is not None and ground_obstacles_stream is not None) + logger.debug('Using perfect obstacle tracker...') obstacles_tracking_stream = \ pylot.operator_creator.add_perfect_tracking( vehicle_id_stream, ground_obstacles_stream, pose_stream) @@ -347,6 +376,7 @@ def add_obstacle_tracking(center_camera_stream, # stream is generated by a perfect detector. # Note: the tracker eval operator cannot compute accuracy # if the obstacles do not contain 2D bounding boxes. + logger.debug('Adding obstacle tracking evaluation...') pylot.operator_creator.add_tracking_evaluation( obstacles_wo_history_tracking_stream, ground_obstacles_stream, @@ -384,13 +414,17 @@ def add_segmentation(center_camera_stream, ground_segmented_stream=None): """ segmented_stream = None if FLAGS.segmentation: + logger.debug('Using semantic segmentation...') segmented_stream = pylot.operator_creator.add_segmentation( center_camera_stream) if FLAGS.evaluate_segmentation: - assert ground_segmented_stream is not None + assert ground_segmented_stream is not None, \ + "Cannot evaluate segmentation without ground truth" + logger.debug('Adding semantic segmentation evaluation...') pylot.operator_creator.add_segmentation_evaluation( ground_segmented_stream, segmented_stream) - elif FLAGS.perfect_segmentation: + if FLAGS.perfect_segmentation: + logger.debug('Using perfect semantic segmentation...') assert ground_segmented_stream is not None return ground_segmented_stream return segmented_stream @@ -429,9 +463,11 @@ def add_prediction(obstacles_tracking_stream, notify_reading_stream = None if FLAGS.prediction: if FLAGS.prediction_type == 'linear': + logger.debug('Using linear prediction...') prediction_stream = pylot.operator_creator.add_linear_prediction( obstacles_tracking_stream) elif FLAGS.prediction_type == 'r2p2': + logger.debug('Using R2P2 prediction...') assert point_cloud_stream is not None assert lidar_setup is not None prediction_stream = pylot.operator_creator.add_r2p2_prediction( @@ -441,9 +477,11 @@ def add_prediction(obstacles_tracking_stream, FLAGS.prediction_type)) if FLAGS.evaluate_prediction: assert pose_stream is not None + logger.debug('Adding prediction evaluation...') pylot.operator_creator.add_prediction_evaluation( pose_stream, obstacles_tracking_stream, prediction_stream) if FLAGS.visualize_prediction: + logger.debug('Adding for prediction evaluation...') # Add bird's eye camera. top_down_transform = pylot.utils.get_top_down_transform( camera_transform, FLAGS.top_down_camera_altitude) @@ -454,6 +492,8 @@ def add_prediction(obstacles_tracking_stream, notify_reading_stream) = pylot.operator_creator.add_camera_driver( top_down_seg_camera_setup, vehicle_id_stream, release_sensor_stream) + else: + logger.debug('Not using prediction...') return (prediction_stream, top_down_segmented_camera_stream, notify_reading_stream) @@ -483,10 +523,11 @@ def add_planning(goal_location, pose_stream, prediction_stream, :py:class:`erdos.ReadStream`: Stream on which the waypoints are published. """ + logger.debug('Using behavior planning...') trajectory_stream = pylot.operator_creator.add_behavior_planning( pose_stream, open_drive_stream, global_trajectory_stream, goal_location) - + logger.debug('Using planning...') waypoints_stream = pylot.operator_creator.add_planning( pose_stream, prediction_stream, traffic_lights_stream, lanes_stream, trajectory_stream, open_drive_stream, time_to_decision_stream) @@ -513,57 +554,55 @@ def add_control(pose_stream, published. """ if FLAGS.control == 'pid': + logger.debug('Using PID controller...') control_stream = pylot.operator_creator.add_pid_control( pose_stream, waypoints_stream) elif FLAGS.control == 'mpc': + logger.debug('Using MPC controller...') control_stream = pylot.operator_creator.add_mpc( pose_stream, waypoints_stream) elif FLAGS.control in ['simulator_auto_pilot', 'manual']: # TODO: Hack! We synchronize on a single stream, based on a # guesestimate of which stream is slowest. + logger.debug('Using the manual control/autopilot...') stream_to_sync_on = waypoints_stream if (FLAGS.evaluate_obstacle_detection and not FLAGS.perfect_obstacle_detection): # Ensure that the perfect obstacle detector doesn't remain # behind. + logger.debug('Synchronizing ticking using the perfect detector' + ' stream') stream_to_sync_on = perfect_obstacles_stream + else: + logger.debug('Synchronizing ticking using the waypoints stream') control_stream = pylot.operator_creator.add_synchronizer( ground_vehicle_id_stream, stream_to_sync_on) else: raise ValueError('Unexpected control {}'.format(FLAGS.control)) if FLAGS.evaluate_control: + logger.debug('Adding control evaluation operator...') pylot.operator_creator.add_control_evaluation(pose_stream, waypoints_stream) return control_stream -def add_evaluation(vehicle_id_stream, - pose_stream, - imu_stream, - pose_stream_for_control=None, - waypoints_stream_for_control=None): +def add_evaluation(vehicle_id_stream, pose_stream, imu_stream): if FLAGS.evaluation: - # Add the collision sensor. + logger.debug('Adding collision logging sensor...') collision_stream = pylot.operator_creator.add_collision_sensor( vehicle_id_stream) - # Add the lane invasion sensor. + logger.debug('Adding lane invasion sensor...') lane_invasion_stream = pylot.operator_creator.add_lane_invasion_sensor( vehicle_id_stream) - # Add the traffic light invasion sensor. + logger.debug('Adding traffic light invasion sensor...') traffic_light_invasion_stream = \ pylot.operator_creator.add_traffic_light_invasion_sensor( vehicle_id_stream, pose_stream) - # Add the evaluation logger. + logger.debug('Adding overall evaluation operator...') pylot.operator_creator.add_eval_metric_logging( collision_stream, lane_invasion_stream, traffic_light_invasion_stream, imu_stream, pose_stream) - - # Add control evaluation logging operator. - if (FLAGS.evaluate_control and pose_stream_for_control - and waypoints_stream_for_control): - pylot.operator_creator.add_control_evaluation( - pose_stream_for_control, waypoints_stream_for_control) diff --git a/pylot/flags.py b/pylot/flags.py index d28b01422..6b7f8a5ed 100644 --- a/pylot/flags.py +++ b/pylot/flags.py @@ -264,6 +264,13 @@ def prediction_validator(flags_dict): message='prediction requires --obstacle_tracking or' ' --perfect_obstacle_tracking') +flags.register_multi_flags_validator( + ['visualize_prediction', 'execution_mode'], + lambda flags_dict: ((not flags_dict['visualize_prediction']) or flags_dict[ + 'execution_mode'] == 'simulator'), + message='--visualize_prediction can only be enabled when running atop the' + ' simulator') + def prediction_ego_agent_validator(flags_dict): if flags_dict['prediction_ego_agent']: @@ -276,6 +283,19 @@ def prediction_ego_agent_validator(flags_dict): prediction_ego_agent_validator, message='ego-agent prediction requires --perfect_obstacle_tracking') + +def prediction_eval_validator(flags_dict): + if flags_dict['evaluate_prediction']: + return flags_dict['prediction'] + return True + + +flags.register_multi_flags_validator( + ['prediction', 'evaluate_prediction'], + prediction_eval_validator, + message='--prediction must be enabled when --evaluate_prediction is ' + 'enabled') + flags.register_multi_flags_validator( [ 'evaluate_prediction', 'prediction_num_future_steps', @@ -340,6 +360,7 @@ def obstacle_detection_validator(flags_dict): if flags_dict['simulator_obstacle_detection']: return not (flags_dict['obstacle_detection'] or flags_dict['perfect_obstacle_detection']) + # return False if neither flag is set. return False @@ -377,6 +398,8 @@ def traffic_light_detection_validator(flags_dict): if flags_dict['simulator_traffic_light_detection']: return not (flags_dict['traffic_light_detection'] or flags_dict['perfect_traffic_light_detection']) + # return False if neither flag is set. + return False flags.register_multi_flags_validator( @@ -394,19 +417,21 @@ def obstacle_tracking_validator(flags_dict): if flags_dict['obstacle_tracking']: return (flags_dict['obstacle_detection'] or flags_dict['perfect_obstacle_detection'] - or flags_dict['simulator_obstacle_detection']) + ) and not (flags_dict['perfect_obstacle_tracking']) + if flags_dict['perfect_obstacle_tracking']: + return not flags_dict['obstacle_tracking'] return True flags.register_multi_flags_validator( [ 'obstacle_detection', 'perfect_obstacle_detection', - 'simulator_obstacle_detection', 'obstacle_tracking' + 'obstacle_tracking', 'perfect_obstacle_tracking' ], obstacle_tracking_validator, - message='--obstacle_detection, --perfect_obstacle_detection, or ' - '--simulator_obstacle_detection must be set when --obstacle_tracking is' - ' enabled') + message='--obstacle_detection or --perfect_obstacle_detection must be set ' + 'when --obstacle_tracking is enabled. Only one of --obstacle_tracking, or ' + '--perfect_obstacle_tracking can be enabled') def obstacle_tracking_evaluation_validator(flags_dict): @@ -438,3 +463,59 @@ def fusion_evaluation_validator(flags_dict): ['fusion', 'evaluate_fusion'], fusion_evaluation_validator, message='--fusion must be set when --evaluate_fusion is enabled') + + +def segmentation_validator(flags_dict): + if flags_dict['segmentation']: + return not flags_dict['perfect_segmentation'] + if flags_dict['perfect_segmentation']: + return not flags_dict['segmentation'] + return True + + +flags.register_multi_flags_validator( + ['perfect_segmentation', 'segmentation'], + segmentation_validator, + message='--segmentation and --perfect_segmentation cannot be both set') + + +def segmentation_evaluation_validator(flags_dict): + if flags_dict['evaluate_segmentation']: + return flags_dict['segmentation'] + return True + + +flags.register_multi_flags_validator( + ['evaluate_segmentation', 'segmentation'], + segmentation_evaluation_validator, + message='--segmentation must be set when --evaluate_segmentation is ' + 'enabled') + + +def depth_validator(flags_dict): + if flags_dict['depth_estimation']: + return not flags_dict['perfect_depth_estimation'] + if flags_dict['perfect_depth_estimation']: + return not flags_dict['depth_estimation'] + return True + + +flags.register_multi_flags_validator( + ['depth_estimation', 'perfect_depth_estimation'], + depth_validator, + message='--depth_estimation and --perfect_depth_estimation cannot be' + ' both set') + + +def lane_detection_validator(flags_dict): + if flags_dict['lane_detection']: + return not flags_dict['perfect_lane_detection'] + if flags_dict['perfect_lane_detection']: + return not flags_dict['lane_detection'] + return True + + +flags.register_multi_flags_validator( + ['lane_detection', 'perfect_lane_detection'], + lane_detection_validator, + message='--lane_detection and --perfect_lane_detection cannot be both set') diff --git a/pylot/map/hd_map.py b/pylot/map/hd_map.py index 67e179097..714f1f769 100644 --- a/pylot/map/hd_map.py +++ b/pylot/map/hd_map.py @@ -40,7 +40,7 @@ def __init__(self, simulator_map, log_file=None): )) self._grp.setup() - def get_closest_lane_waypoint(self, location: Location): + def get_closest_lane_waypoint(self, location: Location) -> Transform: """Returns the road closest waypoint to location. Args: @@ -57,7 +57,7 @@ def get_closest_lane_waypoint(self, location: Location): else: return None - def is_intersection(self, location: Location): + def is_intersection(self, location: Location) -> bool: """Checks if a location is in an intersection. Args: @@ -75,14 +75,14 @@ def is_intersection(self, location: Location): else: return self.__is_intersection(waypoint) - def __is_intersection(self, waypoint): + def __is_intersection(self, waypoint) -> bool: if waypoint.is_junction: return True if hasattr(waypoint, 'is_intersection'): return waypoint.is_intersection return False - def is_on_lane(self, location: Location): + def is_on_lane(self, location: Location) -> bool: """Checks if a location is on a lane. Args: @@ -100,7 +100,8 @@ def is_on_lane(self, location: Location): else: return True - def are_on_same_lane(self, location1: Location, location2: Location): + def are_on_same_lane(self, location1: Location, + location2: Location) -> bool: """Checks if two locations are on the same lane. Args: @@ -209,7 +210,7 @@ def distance_to_intersection(self, waypoint = waypoints[0] return None - def is_on_bidirectional_lane(self, location: Location): + def is_on_bidirectional_lane(self, location: Location) -> bool: """Checks if a location is a bidirectional lane. Args: @@ -224,7 +225,7 @@ def is_on_bidirectional_lane(self, location: Location): return not waypoint def must_obey_traffic_light(self, ego_location: Location, - tl_location: Location): + tl_location: Location) -> bool: """Checks if an ego vehicle must obey a traffic light. Args: @@ -247,7 +248,7 @@ def must_obey_traffic_light(self, ego_location: Location, def _must_obey_european_traffic_light(self, ego_transform: Transform, tl_locations, - tl_max_dist_thresh: float): + tl_max_dist_thresh: float) -> bool: ego_waypoint = self._get_waypoint(ego_transform.location) # We're not on a road, or we're already in the intersection. Carry on. if ego_waypoint is None or self.__is_intersection(ego_waypoint): @@ -265,7 +266,7 @@ def _must_obey_european_traffic_light(self, ego_transform: Transform, def _must_obey_american_traffic_light(self, ego_transform: Transform, tl_locations, - tl_max_dist_thresh: float): + tl_max_dist_thresh: float) -> bool: ego_waypoint = self._get_waypoint(ego_transform.location) # We're not on a road, or we're already in the intersection. Carry on. if ego_waypoint is None or self.__is_intersection(ego_waypoint): @@ -288,7 +289,7 @@ def _must_obey_american_traffic_light(self, ego_transform: Transform, def get_lane(self, location: Location, waypoint_precision: float = 0.05, - lane_id: int = 0): + lane_id: int = 0) -> Lane: lane_waypoints = [] # Consider waypoints in opposite direction of camera so we can get # lane data for adjacent lanes in opposing directions. @@ -403,7 +404,7 @@ def compute_waypoints(self, source_loc: Location, for waypoint in route ]) - def _lateral_shift(self, transform, shift): + def _lateral_shift(self, transform, shift) -> Location: transform.rotation.yaw += 90 shifted = transform.location + shift * transform.get_forward_vector() return Location.from_simulator_location(shifted) diff --git a/pylot/perception/base_perception_eval_operator.py b/pylot/perception/base_perception_eval_operator.py index 33cb2aa73..151a7af77 100644 --- a/pylot/perception/base_perception_eval_operator.py +++ b/pylot/perception/base_perception_eval_operator.py @@ -16,9 +16,11 @@ class BasePerceptionEvalOperator(erdos.Operator): received from the simulator. flags (absl.flags): Object to be used to access absl flags. """ - def __init__(self, prediction_stream, ground_truth_stream, - finished_indicator_stream, evaluate_timely, matching_policy, - frame_gap, scoring_module, flags): + def __init__(self, prediction_stream: erdos.ReadStream, + ground_truth_stream: erdos.ReadStream, + finished_indicator_stream: erdos.WriteStream, + evaluate_timely: bool, matching_policy: str, frame_gap: int, + scoring_module, flags): prediction_stream.add_callback(self.on_prediction) ground_truth_stream.add_callback(self.on_ground_truth) erdos.add_watermark_callback([prediction_stream, ground_truth_stream], @@ -58,7 +60,8 @@ def __init__(self, prediction_stream, ground_truth_stream, self.config.name + '-csv', self.config.csv_log_file_name) @staticmethod - def connect(prediction_stream, ground_truth_stream): + def connect(prediction_stream: erdos.ReadStream, + ground_truth_stream: erdos.ReadStream): """Connects the operator to other streams. Args: @@ -71,7 +74,9 @@ def connect(prediction_stream, ground_truth_stream): finished_indicator_stream = erdos.WriteStream() return [finished_indicator_stream] - def on_watermark(self, timestamp, finished_indicator_stream): + @erdos.profile_method() + def on_watermark(self, timestamp: erdos.Timestamp, + finished_indicator_stream: erdos.WriteStream): """Invoked when all input streams have received a watermark. Args: @@ -116,7 +121,7 @@ def on_watermark(self, timestamp, finished_indicator_stream): if on_new_prediction: self._start_time_frontier += 1 - def __drain_accuracy_compute_buffer(self, up_to_time): + def __drain_accuracy_compute_buffer(self, up_to_time: int): for (st, et, end_anchored) in self._accuracy_compute_buffer: if et <= up_to_time: self.compute_accuracy(st, et, end_anchored) @@ -133,7 +138,8 @@ def __drain_accuracy_compute_buffer(self, up_to_time): if gc_threshold is not None: self.__gc_data_earlier_than(gc_threshold) - def compute_accuracy(self, frame_time, ground_time, end_anchored): + def compute_accuracy(self, frame_time: int, ground_time: int, + end_anchored: bool): anchor_type = "end" if end_anchored else "start" anchor_time = ground_time if end_anchored else frame_time predictions = self.get_prediction_at(frame_time) @@ -146,7 +152,7 @@ def compute_accuracy(self, frame_time, ground_time, end_anchored): time_epoch_ms(), anchor_time, self.config.name, anchor_type, self._matching_policy, k, v)) - def __compute_frame_gap(self, game_time): + def __compute_frame_gap(self, game_time: int): """Infer frame gap if not explicitly provided in constructor.""" if not self._frame_gap: if not self._last_notification: @@ -156,7 +162,7 @@ def __compute_frame_gap(self, game_time): self._frame_gap = (game_time - self._last_notification) self._last_notification = game_time - def get_ground_truth_at(self, timestamp): + def get_ground_truth_at(self, timestamp: int): for (ground_time, obstacles) in self._ground_truths: if ground_time == timestamp: return obstacles @@ -165,7 +171,7 @@ def get_ground_truth_at(self, timestamp): self._logger.fatal( 'Could not find ground obstacles for {}'.format(timestamp)) - def get_prediction_at(self, timestamp): + def get_prediction_at(self, timestamp: int): for (start_time, obstacles) in self._predictions: if start_time == timestamp: return obstacles @@ -174,7 +180,7 @@ def get_prediction_at(self, timestamp): self._logger.fatal( 'Could not find tracked obstacles for {}'.format(timestamp)) - def __gc_data_earlier_than(self, game_time): + def __gc_data_earlier_than(self, game_time: int): index = 0 while (index < len(self._predictions) and self._predictions[index][0] < game_time): @@ -188,7 +194,7 @@ def __gc_data_earlier_than(self, game_time): if index > 0: self._ground_truths = self._ground_truths[index:] - def on_prediction(self, msg): + def on_prediction(self, msg: erdos.Message): game_time = msg.timestamp.coordinates[0] self._predictions.append((game_time, msg.obstacles)) if len(self._predictions) > 1: @@ -207,11 +213,11 @@ def on_prediction(self, msg): self._prediction_start_end_times.append( (game_time, ground_truth_time)) - def on_ground_truth(self, msg): + def on_ground_truth(self, msg: erdos.Message): game_time = msg.timestamp.coordinates[0] self._ground_truths.append((game_time, msg.obstacles)) - def __compute_closest_frame_time(self, time): + def __compute_closest_frame_time(self, time: float) -> int: if self._frame_gap is None: self._logger.info( 'Skipping frame {} because frame gap is not set yet'.format( diff --git a/pylot/perception/camera_frame.py b/pylot/perception/camera_frame.py index 30924229d..3e761f7e1 100644 --- a/pylot/perception/camera_frame.py +++ b/pylot/perception/camera_frame.py @@ -125,7 +125,7 @@ def draw_text(self, thickness=1, lineType=cv2.LINE_AA) - def in_frame(self, point: Vector2D): + def in_frame(self, point: Vector2D) -> bool: """Checks if a point is within the frame.""" return (0 <= point.x <= self.camera_setup.width and 0 <= point.y <= self.camera_setup.height) diff --git a/pylot/perception/depth_frame.py b/pylot/perception/depth_frame.py index 6ae6f2e74..bf49987da 100644 --- a/pylot/perception/depth_frame.py +++ b/pylot/perception/depth_frame.py @@ -116,11 +116,11 @@ def get_pixel_locations(self, pixels): for loc in pixel_locations ] - def pixel_has_same_depth(self, x, y, z, threshold): + def pixel_has_same_depth(self, x, y, z: float, threshold: float) -> bool: """Checks if the depth of pixel (y,x) is within threshold of z.""" return abs(self.frame[int(y)][int(x)] * 1000 - z) < threshold - def resize(self, width, height): + def resize(self, width: int, height: int): """Resizes the frame.""" import cv2 self.camera_setup.set_resolution(width, height) @@ -138,7 +138,7 @@ def visualize(self, pygame_display, timestamp=None): pygame.surfarray.blit_array(pygame_display, image_np) pygame.display.flip() - def save(self, timestamp, data_path, file_base): + def save(self, timestamp: int, data_path: str, file_base: str): """Saves the depth frame to a file. Args: diff --git a/pylot/planning/behavior_planning_operator.py b/pylot/planning/behavior_planning_operator.py index 5acbb6aec..b1c561788 100644 --- a/pylot/planning/behavior_planning_operator.py +++ b/pylot/planning/behavior_planning_operator.py @@ -29,12 +29,12 @@ class BehaviorPlanningOperator(erdos.Operator): the ego vehicle. """ def __init__(self, - pose_stream, - open_drive_stream, - route_stream, - trajectory_stream, + pose_stream: erdos.ReadStream, + open_drive_stream: erdos.ReadStream, + route_stream: erdos.ReadStream, + trajectory_stream: erdos.WriteStream, flags, - goal_location=None): + goal_location: pylot.utils.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) @@ -63,14 +63,16 @@ def __init__(self, self._map = None @staticmethod - def connect(pose_stream, open_drive_stream, route_stream): + def connect(pose_stream: erdos.ReadStream, + open_drive_stream: erdos.ReadStream, + route_stream: erdos.ReadStream): trajectory_stream = erdos.WriteStream() return [trajectory_stream] def destroy(self): self._logger.warn('destroying {}'.format(self.config.name)) - def on_opendrive_map(self, msg): + def on_opendrive_map(self, msg: erdos.Message): """Invoked whenever a message is received on the open drive stream. Args: @@ -82,7 +84,7 @@ def on_opendrive_map(self, msg): from pylot.simulation.utils import map_from_opendrive self._map = map_from_opendrive(msg.data, self.config.log_file_name) - def on_route_msg(self, msg): + def on_route_msg(self, msg: erdos.Message): """Invoked whenever a message is received on the trajectory stream. Args: @@ -93,7 +95,7 @@ def on_route_msg(self, msg): msg.timestamp, len(msg.waypoints.waypoints))) self._route = msg.waypoints - def on_pose_update(self, msg): + def on_pose_update(self, msg: erdos.Message): """Invoked whenever a message is received on the pose stream. Args: @@ -103,7 +105,9 @@ def on_pose_update(self, msg): self._logger.debug('@{}: received pose message'.format(msg.timestamp)) self._pose_msgs.append(msg) - def on_watermark(self, timestamp, trajectory_stream): + @erdos.profile_method() + def on_watermark(self, timestamp: erdos.Timestamp, + trajectory_stream: erdos.WriteStream): self._logger.debug('@{}: received watermark'.format(timestamp)) if timestamp.is_top: return @@ -234,7 +238,7 @@ def __best_state_transition(self, ego_info): min_state_cost = state_cost return best_next_state - def __get_goal_location(self, ego_transform): + def __get_goal_location(self, ego_transform: pylot.utils.Transform): if len(self._route.waypoints) > 1: dist = ego_transform.location.distance( self._route.waypoints[0].location) diff --git a/pylot/planning/messages.py b/pylot/planning/messages.py index 020d97041..f73bc50bf 100644 --- a/pylot/planning/messages.py +++ b/pylot/planning/messages.py @@ -11,7 +11,10 @@ class WaypointsMessage(erdos.Message): the message. waypoints (:py:class:`~pylot.planning.Waypoints`): Waypoints. """ - def __init__(self, timestamp, waypoints, agent_state=None): + def __init__(self, + timestamp: erdos.Timestamp, + waypoints, + agent_state=None): super(WaypointsMessage, self).__init__(timestamp, None) self.waypoints = waypoints self.agent_state = agent_state @@ -27,11 +30,11 @@ def __str__(self): class BehaviorMessage(erdos.Message): def __init__(self, - timestamp, - target_lane_id, - target_speed, - target_deadline, - target_leading_vehicle_id=None): + timestamp: erdos.Timestamp, + target_lane_id: int, + target_speed: float, + target_deadline: float, + target_leading_vehicle_id: int = None): super(BehaviorMessage, self).__init__(timestamp, None) self.target_lane_id = target_lane_id self.target_speed = target_speed diff --git a/pylot/planning/planning_operator.py b/pylot/planning/planning_operator.py index 6f28cc942..30f59be5e 100644 --- a/pylot/planning/planning_operator.py +++ b/pylot/planning/planning_operator.py @@ -39,9 +39,14 @@ class PlanningOperator(erdos.Operator): operator sends waypoints the ego vehicle must follow. flags (absl.flags): Object to be used to access absl flags. """ - def __init__(self, pose_stream, prediction_stream, static_obstacles_stream, - lanes_stream, route_stream, open_drive_stream, - time_to_decision_stream, waypoints_stream, flags): + 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) @@ -89,9 +94,12 @@ def __init__(self, pose_stream, prediction_stream, static_obstacles_stream, self._ttd_msgs = deque() @staticmethod - def connect(pose_stream, prediction_stream, static_obstacles_stream, - lanes_steam, route_stream, open_drive_stream, - time_to_decision_stream): + def connect(pose_stream: erdos.ReadStream, + prediction_stream: erdos.ReadStream, + static_obstacles_stream: erdos.ReadStream, + lanes_steam: erdos.ReadStream, route_stream: erdos.ReadStream, + open_drive_stream: erdos.ReadStream, + time_to_decision_stream: erdos.ReadStream): waypoints_stream = erdos.WriteStream() return [waypoints_stream] @@ -110,7 +118,7 @@ def run(self): self.config.log_file_name) self._logger.info('Planner running in stand-alone mode') - def on_pose_update(self, msg): + def on_pose_update(self, msg: erdos.Message): """Invoked whenever a message is received on the pose stream. Args: @@ -122,21 +130,21 @@ def on_pose_update(self, msg): self._ego_transform = msg.data.transform @erdos.profile_method() - def on_prediction_update(self, msg): + def on_prediction_update(self, msg: erdos.Message): self._logger.debug('@{}: received prediction message'.format( msg.timestamp)) self._prediction_msgs.append(msg) - def on_static_obstacles_update(self, msg): + def on_static_obstacles_update(self, msg: erdos.Message): self._logger.debug('@{}: received static obstacles update'.format( msg.timestamp)) self._static_obstacles_msgs.append(msg) - def on_lanes_update(self, msg): + def on_lanes_update(self, msg: erdos.Message): self._logger.debug('@{}: received lanes update'.format(msg.timestamp)) self._lanes_msgs.append(msg) - def on_route(self, msg): + def on_route(self, msg: erdos.Message): """Invoked whenever a message is received on the trajectory stream. Args: @@ -154,7 +162,7 @@ def on_route(self, msg): self._world.update_waypoints(msg.waypoints.waypoints[-1].location, msg.waypoints) - def on_opendrive_map(self, msg): + def on_opendrive_map(self, msg: erdos.Message): """Invoked whenever a message is received on the open drive stream. Args: @@ -167,13 +175,14 @@ def on_opendrive_map(self, msg): self._map = map_from_opendrive(msg.data) @erdos.profile_method() - def on_time_to_decision(self, msg): + def on_time_to_decision(self, msg: erdos.Message): self._logger.debug('@{}: {} received ttd update {}'.format( msg.timestamp, self.config.name, msg)) self._ttd_msgs.append(msg) @erdos.profile_method() - def on_watermark(self, timestamp, waypoints_stream): + def on_watermark(self, timestamp: erdos.Timestamp, + waypoints_stream: erdos.WriteStream): self._logger.debug('@{}: received watermark'.format(timestamp)) if timestamp.is_top: return @@ -221,7 +230,7 @@ def get_predictions(self, prediction_msg, ego_transform): type(prediction_msg))) return predictions - def update_world(self, timestamp): + def update_world(self, timestamp: erdos.Timestamp): pose_msg = self._pose_msgs.popleft() ego_transform = pose_msg.data.transform prediction_msg = self._prediction_msgs.popleft() diff --git a/pylot/planning/utils.py b/pylot/planning/utils.py index edba5c351..e52dac291 100644 --- a/pylot/planning/utils.py +++ b/pylot/planning/utils.py @@ -15,7 +15,7 @@ class BehaviorPlannerState(enum.Enum): def compute_person_speed_factor(ego_location_2d, person_location_2d, wp_vector, - flags, logger): + flags, logger) -> float: speed_factor_p = 1 p_vector = person_location_2d - ego_location_2d p_dist = person_location_2d.l2_distance(ego_location_2d) @@ -39,7 +39,7 @@ def compute_person_speed_factor(ego_location_2d, person_location_2d, wp_vector, def compute_vehicle_speed_factor(ego_location_2d, vehicle_location_2d, - wp_vector, flags, logger): + wp_vector, flags, logger) -> float: speed_factor_v = 1 v_vector = vehicle_location_2d - ego_location_2d v_dist = vehicle_location_2d.l2_distance(ego_location_2d) diff --git a/pylot/planning/waypoints.py b/pylot/planning/waypoints.py index 726a95076..69b6b7237 100644 --- a/pylot/planning/waypoints.py +++ b/pylot/planning/waypoints.py @@ -36,7 +36,7 @@ def read_from_csv_file(cls, csv_file_name, target_speed): target_speeds = deque([target_speed for _ in range(len(waypoints))]) return cls(deque(waypoints), target_speeds) - def apply_speed_factor(self, speed_factor): + def apply_speed_factor(self, speed_factor: float): if self.target_speeds: self.target_speeds = [ speed_factor * ts for ts in self.target_speeds @@ -51,7 +51,7 @@ def as_numpy_array_2D(self): wy.append(wp.location.y) return np.array([wx, wy]) - def closest_waypoint(self, location): + def closest_waypoint(self, location: pylot.utils.Location): """Finds the closest waypoint to the location.""" min_dist = np.infty min_index = 0 @@ -62,21 +62,27 @@ def closest_waypoint(self, location): min_index = index return min_index - def is_empty(self): + def is_empty(self) -> bool: return len(self.waypoints) == 0 - def remove_waypoint_if_close(self, location, distance=5): + def remove_waypoint_if_close(self, + location: pylot.utils.Location, + distance: float = 5) -> bool: """Removes the first waypoint if it is less than distance m away.""" if self.waypoints is None or len(self.waypoints) == 0: - return + return False if location.distance(self.waypoints[0].location) < distance: self.waypoints.popleft() if self.target_speeds: self.target_speeds.popleft() if self.road_options: self.road_options.popleft() + return True + return False - def remove_completed(self, location, ego_transform=None): + def remove_completed(self, + location: pylot.utils.Location, + ego_transform: pylot.utils.Transform = None): """Removes waypoints that the ego vehicle has already completed. The method first finds the closest waypoint to the location, @@ -109,7 +115,8 @@ def recompute_waypoints(self, hd_map, ego_location, goal_location): self.waypoints = hd_map.compute_waypoints(ego_location, goal_location) self.target_speeds = deque([0 for _ in range(len(self.waypoints))]) - def _get_index(self, transform, min_distance): + def _get_index(self, transform: pylot.utils.Transform, + min_distance: float) -> int: min_index = -1 for index, wp in enumerate(self.waypoints): distance = wp.location.distance(transform.location) @@ -122,7 +129,8 @@ def _get_index(self, transform, min_distance): raise ValueError('No more waypoints') return min_index - def get_angle(self, transform, min_distance): + def get_angle(self, transform: pylot.utils.Transform, + min_distance: float) -> float: """Returns the angle between the transform and the first waypoint that is at least min_distance away.""" wp_index = self._get_index(transform, min_distance) @@ -130,20 +138,25 @@ def get_angle(self, transform, min_distance): self.waypoints[wp_index].location) return angle - def get_vector(self, transform, min_distance): + def get_vector(self, transform: pylot.utils.Transform, + min_distance: float): """Returns the vector between the transform and the first waypoint that is at least min_distance away.""" wp_index = self._get_index(transform, min_distance) return self.waypoints[wp_index].location.as_vector_2D() - \ transform.location.as_vector_2D() - def get_target_speed(self, transform, min_distance): + def get_target_speed(self, transform: pylot.utils.Transform, + min_distance: float) -> float: """Gets the target speed at the first waypoint that is at least min_distance away.""" wp_index = self._get_index(transform, min_distance) return self.target_speeds[wp_index] - def slice_waypoints(self, start_index, end_index, target_speed=None): + def slice_waypoints(self, + start_index: int, + end_index: int, + target_speed: float = None): head_wps = deque( itertools.islice(self.waypoints, start_index, end_index)) if target_speed is not None: diff --git a/pylot/planning/world.py b/pylot/planning/world.py index 32aa63231..4c2752acf 100644 --- a/pylot/planning/world.py +++ b/pylot/planning/world.py @@ -92,7 +92,7 @@ def update_waypoints(self, goal_location, waypoints): self._goal_location = goal_location self.waypoints = waypoints - def follow_waypoints(self, target_speed): + def follow_waypoints(self, target_speed: float): self.waypoints.remove_completed(self.ego_transform.location, self.ego_transform) return self.waypoints.slice_waypoints(0, @@ -148,7 +148,7 @@ def draw_on_frame(self, frame): for lane in self._lanes: lane.draw_on_frame(frame) - def stop_person(self, obstacle, wp_vector): + def stop_person(self, obstacle, wp_vector) -> float: """Computes a stopping factor for ego vehicle given a person obstacle. Args: @@ -191,7 +191,7 @@ def stop_person(self, obstacle, wp_vector): min_speed_factor_p = min(min_speed_factor_p, speed_factor_p) return min_speed_factor_p - def stop_vehicle(self, obstacle, wp_vector): + def stop_vehicle(self, obstacle, wp_vector) -> float: """Computes a stopping factor for ego vehicle given a vehicle pos. Args: @@ -241,7 +241,7 @@ def stop_vehicle(self, obstacle, wp_vector): min_speed_factor_v = min(min_speed_factor_v, speed_factor_v) return min_speed_factor_v - def stop_for_agents(self, timestamp): + def stop_for_agents(self, timestamp) -> float: """Calculates the speed factor in [0, 1] (0 is full stop). Reduces the speed factor whenever the ego vehicle's path is blocked @@ -320,7 +320,7 @@ def stop_for_agents(self, timestamp): 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): + def stop_traffic_light(self, tl, wp_vector, wp_angle) -> float: """Computes a stopping factor for ego vehicle given a traffic light. Args: diff --git a/pylot/prediction/flags.py b/pylot/prediction/flags.py index 63fcf17f9..76e1351e5 100644 --- a/pylot/prediction/flags.py +++ b/pylot/prediction/flags.py @@ -8,7 +8,7 @@ 'Number of future steps outputted by the prediction module.') flags.DEFINE_integer( 'prediction_radius', 50, - 'Make predictions for all vehicles within this radius of the ego-vehicle.') + 'Make predictions for agents within radius (in m) of the ego-vehicle.') flags.DEFINE_boolean('prediction_ego_agent', False, 'Whether we make predictions for the ego agent') diff --git a/pylot/prediction/messages.py b/pylot/prediction/messages.py index 78087d1a3..7159d68d6 100644 --- a/pylot/prediction/messages.py +++ b/pylot/prediction/messages.py @@ -1,9 +1,5 @@ -from typing import List - import erdos -from pylot.prediction.obstacle_prediction import ObstaclePrediction - class PredictionMessage(erdos.Message): """Message class to be used to send obstacle predictions. @@ -18,11 +14,13 @@ class PredictionMessage(erdos.Message): predictions (list(:py:class:`~pylot.prediction.obstacle_prediction.ObstaclePrediction`)): Obstacle predictions. """ - def __init__(self, timestamp: erdos.Timestamp, - predictions: List[ObstaclePrediction]): + def __init__(self, timestamp: erdos.Timestamp, predictions): super(PredictionMessage, self).__init__(timestamp, None) self.predictions = predictions + def __repr__(self): + return self.__str__() + def __str__(self): return 'PredictionMessage(timestamp: {}, predictions: {})'.format( self.timestamp, self.predictions) diff --git a/pylot/prediction/obstacle_prediction.py b/pylot/prediction/obstacle_prediction.py index 6abc3addd..1268a6d7d 100644 --- a/pylot/prediction/obstacle_prediction.py +++ b/pylot/prediction/obstacle_prediction.py @@ -1,5 +1,3 @@ -from typing import List - from pylot.perception.tracking.obstacle_trajectory import ObstacleTrajectory from pylot.utils import Transform @@ -18,7 +16,7 @@ class ObstaclePrediction(object): """ def __init__(self, obstacle_trajectory: ObstacleTrajectory, transform: Transform, probability: float, - predicted_trajectory: List[Transform]): + predicted_trajectory): # Trajectory in ego frame of coordinates. self.obstacle_trajectory = obstacle_trajectory # The transform is in world coordinates. diff --git a/pylot/prediction/prediction_eval_operator.py b/pylot/prediction/prediction_eval_operator.py index 410d34992..6002073f7 100644 --- a/pylot/prediction/prediction_eval_operator.py +++ b/pylot/prediction/prediction_eval_operator.py @@ -3,7 +3,7 @@ from collections import deque import erdos -from erdos import Message, ReadStream, Timestamp +from erdos import Message, ReadStream, Timestamp, WriteStream from pylot.utils import Vector2D, time_epoch_ms @@ -24,7 +24,8 @@ class PredictionEvalOperator(erdos.Operator): flags (absl.flags): Object to be used to access absl flags. """ def __init__(self, pose_stream: ReadStream, tracking_stream: ReadStream, - prediction_stream: ReadStream, flags): + prediction_stream: ReadStream, + finished_indicator_stream: WriteStream, flags): pose_stream.add_callback(self._on_pose_update) tracking_stream.add_callback(self._on_tracking_update) prediction_stream.add_callback(self._on_prediction_update) @@ -47,7 +48,8 @@ def __init__(self, pose_stream: ReadStream, tracking_stream: ReadStream, @staticmethod def connect(pose_stream: ReadStream, tracking_stream: ReadStream, prediction_stream: ReadStream): - return [] + finished_indicator_stream = erdos.WriteStream() + return [finished_indicator_stream] def destroy(self): self._logger.warn('destroying {}'.format(self.config.name)) diff --git a/pylot/prediction/r2p2_predictor_operator.py b/pylot/prediction/r2p2_predictor_operator.py index d2a2e8393..28759b063 100644 --- a/pylot/prediction/r2p2_predictor_operator.py +++ b/pylot/prediction/r2p2_predictor_operator.py @@ -35,8 +35,9 @@ class R2P2PredictorOperator(erdos.Operator): of the lidar. This setup is used to get the maximum range of the lidar. """ - def __init__(self, point_cloud_stream, tracking_stream, prediction_stream, - flags, lidar_setup): + def __init__(self, point_cloud_stream: erdos.ReadStream, + tracking_stream: erdos.ReadStream, + prediction_stream: erdos.WriteStream, flags, lidar_setup): print("WARNING: R2P2 predicts only vehicle trajectories") self._logger = erdos.utils.setup_logging(self.config.name, self.config.log_file_name) @@ -60,14 +61,17 @@ def __init__(self, point_cloud_stream, tracking_stream, prediction_stream, self._tracking_msgs = deque() @staticmethod - def connect(point_cloud_stream, tracking_stream): + def connect(point_cloud_stream: erdos.ReadStream, + tracking_stream: erdos.ReadStream): prediction_stream = erdos.WriteStream() return [prediction_stream] def destroy(self): self._logger.warn('destroying {}'.format(self.config.name)) - def on_watermark(self, timestamp, prediction_stream): + @erdos.profile_method() + def on_watermark(self, timestamp: erdos.Timestamp, + prediction_stream: erdos.WriteStream): self._logger.debug('@{}: received watermark'.format(timestamp)) if timestamp.is_top: return @@ -196,12 +200,12 @@ def _postprocess_predictions(self, prediction_array, vehicle_trajectories, obstacle_transform, 1.0, predictions)) return obstacle_predictions_list - def on_point_cloud_update(self, msg): + def on_point_cloud_update(self, msg: erdos.Message): self._logger.debug('@{}: received point cloud message'.format( msg.timestamp)) self._point_cloud_msgs.append(msg) - def on_trajectory_update(self, msg): + def on_trajectory_update(self, msg: erdos.Message): self._logger.debug('@{}: received trajectories message'.format( msg.timestamp)) self._tracking_msgs.append(msg) diff --git a/pylot/utils.py b/pylot/utils.py index 2ac81f470..d3774e130 100644 --- a/pylot/utils.py +++ b/pylot/utils.py @@ -23,7 +23,7 @@ class Rotation(object): yaw: Rotation about Z-axis. roll: Rotation about X-axis. """ - def __init__(self, pitch=0, yaw=0, roll=0): + def __init__(self, pitch: float = 0, yaw: float = 0, roll: float = 0): self.pitch = pitch self.yaw = yaw self.roll = roll @@ -82,7 +82,7 @@ class Quaternion(object): matrix: A 3x3 numpy array that can be used to rotate 3D vectors from body frame to world frame. """ - def __init__(self, w, x, y, z): + def __init__(self, w: float, x: float, y: float, z: float): norm = np.linalg.norm([w, x, y, z]) if norm < 1e-50: self.w, self.x, self.y, self.z = 0, 0, 0, 0 @@ -120,7 +120,7 @@ def _create_matrix(w, x, y, z): return m @classmethod - def from_rotation(cls, rotation): + def from_rotation(cls, rotation: Rotation): """Creates a Quaternion from a rotation including pitch, roll, yaw. Args: @@ -147,7 +147,7 @@ def from_rotation(cls, rotation): return cls(w, x, y, z) @classmethod - def from_angular_velocity(cls, angular_velocity, dt): + def from_angular_velocity(cls, angular_velocity, dt: float): """Creates a Quaternion from an angular velocity vector and the time delta to apply it for. @@ -174,7 +174,7 @@ def from_angular_velocity(cls, angular_velocity, dt): x, y, z = imaginary return cls(w, x, y, z) - def as_rotation(self): + def as_rotation(self) -> Rotation: """Retrieve the Quaternion as a Rotation in degrees. Returns: @@ -243,7 +243,7 @@ class Vector3D(object): y: The value of the second axis. z: The value of the third axis. """ - def __init__(self, x=0, y=0, z=0): + def __init__(self, x: float = 0, y: float = 0, z: float = 0): self.x, self.y, self.z = float(x), float(y), float(z) @classmethod @@ -291,7 +291,7 @@ def l1_distance(self, other): return abs(self.x - other.x) + abs(self.y - other.y) + abs(self.z - other.z) - def l2_distance(self, other): + def l2_distance(self, other) -> float: """Calculates the L2 distance between the point and another point. Args: @@ -335,7 +335,7 @@ def to_camera_view(self, extrinsic_matrix, intrinsic_matrix): float(position_2D[2])) return location_2D - def rotate(self, angle): + def rotate(self, angle: float): """Rotate the vector by a given angle. Args: @@ -372,7 +372,7 @@ def __str__(self): class Vector2D(object): """Represents a 2D vector and provides helper functions.""" - def __init__(self, x, y): + def __init__(self, x: float, y: float): self.x = x self.y = y @@ -380,7 +380,7 @@ def as_numpy_array(self): """Retrieves the 2D vector as a numpy array.""" return np.array([self.x, self.y]) - def get_angle(self, other): + def get_angle(self, other) -> float: """Computes the angle between the vector and another vector in radians.""" angle = math.atan2(self.y, self.x) - math.atan2(other.y, other.x) @@ -390,7 +390,7 @@ def get_angle(self, other): angle += 2 * math.pi return angle - def l1_distance(self, other): + def l1_distance(self, other) -> float: """Calculates the L1 distance between the point and another point. Args: @@ -402,7 +402,7 @@ def l1_distance(self, other): """ return abs(self.x - other.x) + abs(self.y - other.y) - def l2_distance(self, other): + def l2_distance(self, other) -> float: """Calculates the L2 distance between the point and another point. Args: @@ -447,7 +447,7 @@ class Location(Vector3D): y: The value of the y-axis. z: The value of the z-axis. """ - def __init__(self, x=0, y=0, z=0): + def __init__(self, x: float = 0, y: float = 0, z: float = 0): super(Location, self).__init__(x, y, z) @classmethod @@ -467,7 +467,7 @@ def from_simulator_location(cls, location): return cls(location.x, location.y, location.z) @classmethod - def from_gps(cls, latitude, longitude, altitude): + def from_gps(cls, latitude: float, longitude: float, altitude: float): """Creates Location from GPS (latitude, longitude, altitude). This is the inverse of the _location_to_gps method found in @@ -496,7 +496,7 @@ def from_gps(cls, latitude, longitude, altitude): return cls(x, y, altitude) - def distance(self, other): + def distance(self, other) -> float: """Calculates the Euclidean distance between the given point and the other point. @@ -509,7 +509,7 @@ def distance(self, other): """ return (self - other).magnitude() - def as_vector_2D(self): + def as_vector_2D(self) -> Vector2D: """Transforms the Location into a Vector2D. Note: @@ -565,7 +565,10 @@ class Transform(object): coordinate space with respect to the location and rotation of the given object. """ - def __init__(self, location=None, rotation=None, matrix=None): + def __init__(self, + location: Location = None, + rotation: Rotation = None, + matrix=None): if matrix is not None: self.matrix = matrix self.location = Location(matrix[0, 3], matrix[1, 3], matrix[2, 3]) @@ -784,7 +787,8 @@ def get_angle_and_magnitude(self, target_loc): angle = 0 return angle, magnitude - def is_within_distance_ahead(self, dst_loc, max_distance): + def is_within_distance_ahead(self, dst_loc: Location, + max_distance: float) -> bool: """Checks if a location is within a distance. Args: @@ -841,10 +845,10 @@ class Pose(object): in world frame """ def __init__(self, - transform, - forward_speed, - velocity_vector=None, - localization_time=None): + transform: Transform, + forward_speed: float, + velocity_vector: Vector3D = None, + localization_time: float = None): if not isinstance(transform, Transform): raise ValueError( 'transform should be of type pylot.utils.Transform') @@ -1025,7 +1029,7 @@ def get_top_down_transform(transform, top_down_camera_altitude): return Transform(top_down_location, Rotation(-90, 0, 0)) -def time_epoch_ms(): +def time_epoch_ms() -> int: """Get current time in milliseconds.""" return int(time.time() * 1000) diff --git a/requirements.txt b/requirements.txt index fcc68fd9f..1bd685f3d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,9 +3,7 @@ cvxpy erdos>=0.3.1 gdown lapsolver -matplotlib==2.2.4 motmetrics -networkx==2.2 numpy<1.17 open3d-python==0.5.0.0 opencv-python>=4.1.0.25 @@ -21,11 +19,14 @@ tensorflow-gpu==1.15.4 torch==1.4.0 torchvision==0.5.0 ##### Tracking dependencies ##### +Cython filterpy==1.4.1 -scikit-learn==0.22.2 imgaug==0.2.8 +matplotlib==2.2.4 nonechucks==0.3.1 -Cython +nuscenes-devkit progress pyquaternion -nuscenes-devkit +scikit-learn==0.22.2 +##### CARLA dependencies ##### +networkx==2.2 diff --git a/scripts/collect_data.sh b/scripts/collect_data.sh index dbb20c432..1c36e268a 100755 --- a/scripts/collect_data.sh +++ b/scripts/collect_data.sh @@ -1,28 +1,33 @@ #!/bin/bash # $1 path where to save the data. +if [ -z "$PYLOT_HOME" ]; then + echo "Please set \$PYLOT_HOME before sourcing this script" + exit 1 +fi + if [ -z "$CARLA_HOME" ]; then echo "Please set \$CARLA_HOME before running this script" exit 1 fi -# Change the Carla kill command if you want to collect data with 0.8.4 or 0.9.5. -cd ../ - start_player_nums=(1 10 20 30 40) towns=(1 2 3 4 5) +cd $PYLOT_HOME for pn in ${start_player_nums[@]}; do for town in ${towns[@]}; do - ${CARLA_HOME}/CarlaUE4.sh /Game/Carla/Maps/Town0${town} -windowed -ResX=1920 -ResY=1080 -carla-server -benchmark -fps=10 & - mkdir $1/town0${town}_start${pn}/ + echo "[x] Driving in town $town from start position $pn" + echo "[x] Starting the CARLA simulator" + SDL_VIDEODRIVER=offscreen ${CARLA_HOME}/CarlaUE4.sh -opengl /Game/Carla/Maps/Town0${town} -windowed -ResX=1920 -ResY=1080 -carla-server -benchmark -fps=10 & sleep 10 - python data_gatherer.py --flagfile=configs/data_gatherer.conf --carla_spawn_point_index=${pn} --data_path=$1/town0${town}_start${pn}/ --carla_town=${town} & sleep 4800; kill -9 $! + mkdir $1/town0${town}_start${pn}/ + python3 data_gatherer.py --flagfile=configs/data_gatherer.conf --carla_spawn_point_index=${pn} --data_path=$1/town0${town}_start${pn}/ --simulator_town=${town} & # Collect data for an hour. + sleep 4800 # Kill data gathering script and Carla. - killall python data_gatherer.py - sleep 10 - killall -s 9 ${CARLA_HOME}/CarlaUE4/Binaries/Linux/CarlaUE4-Linux-Shipping - sleep 10 + pkill -9 -f -u $USER data_gatherer + pkill -9 -f -u $USER CarlaUE4 + sleep 5 done done diff --git a/scripts/collect_sign_data.sh b/scripts/collect_sign_data.sh index 05c5e6774..8ab96b2bb 100755 --- a/scripts/collect_sign_data.sh +++ b/scripts/collect_sign_data.sh @@ -8,9 +8,8 @@ fi towns=("Town01" "Town02") -# towns=("Town05") - for town in ${towns[@]}; do + echo "[x] Starting the CARLA Simulator.." SDL_VIDEODRIVER=offscreen ${CARLA_HOME}/CarlaUE4.sh -opengl -windowed -ResX=800 -ResY=600 -carla-server -benchmark -fps=20 -quality-level=Epic & sleep 10 # Change the town using the config file. @@ -18,11 +17,14 @@ for town in ${towns[@]}; do # randomizes the identifiers for the traffic lights. if [ $town != "Town03" ]; then ${CARLA_HOME}/PythonAPI/util/config.py --map $town + sleep 5 fi + mkdir -p $1/signs_${town}/ + echo "[x] Starting the sign data gatherer..." + python3 sign_data_gatherer.py --data_path=$1/signs_${town}/ --log_bbox_images + + # Kill the simulator. + pkill -9 -f -u $USER CarlaUE4 sleep 5 - python sign_data_gatherer.py --data_path=$1/signs_${town}/ --log_bbox_images - sleep 10 - killall -s 9 ${CARLA_HOME}/CarlaUE4/Binaries/Linux/CarlaUE4-Linux-Shipping - sleep 10 done diff --git a/scripts/replay_logs.sh b/scripts/replay_logs.sh index a75dcba96..80ec1fe48 100755 --- a/scripts/replay_logs.sh +++ b/scripts/replay_logs.sh @@ -1,6 +1,11 @@ #!/bin/bash # $1 Directory where the log files are +if [ -z "$PYLOT_HOME" ]; then + echo "Please set \$PYLOT_HOME before sourcing this script" + exit 1 +fi + cd ${CARLA_HOME}/PythonAPI/examples function replay_log { diff --git a/scripts/run_map_experiments.sh b/scripts/run_map_experiments.sh index 61ac45e33..12856c97d 100755 --- a/scripts/run_map_experiments.sh +++ b/scripts/run_map_experiments.sh @@ -1,30 +1,44 @@ -CARLA_PATH=../../CARLA_0.9.6 -SCENARIO_RUNNER_PATH=../../scenario_runner +#!/bin/bash + +# Assumes that $CARLA_HOME AND $PYLOT_HOME are set. +if [ -z "$PYLOT_HOME" ]; then + echo "Please set \$PYLOT_HOME before sourcing this script" + exit 1 +fi + +if [ -z "$CARLA_HOME" ]; then + echo "Please set \$CARLA_HOME before running this script" + exit 1 +fi + +if [ -z "$SCENARIO_RUNNER_HOME" ]; then + echo "Please set \$SCENARIO_RUNNER_HOME before running this script" + exit 1 +fi speeds=( 10 15 20 25 30 35 40 ) SAMPLING_RATE=0.005 # The delta between two subsequent frames. +cd $PYLOT_HOME for speed in ${speeds[@]}; do echo "[x] Running the experiment with the speed $speed and the sampling rate of $SAMPLING_RATE" # Start the simulator. - echo "[x] Starting the Carla Simulator 0.9.6" - ./$CARLA_PATH/CarlaUE4.sh & + echo "[x] Starting the CARLA Simulator..." + SDL_VIDEODRIVER=offscreen ${CARLA_HOME}/CarlaUE4.sh -opengl -windowed -ResX=800 -ResY=600 -carla-server -benchmark -fps=20 -quality-level=Epic & sleep 10 # Start the scenario runner. - echo "[x] Starting the scenario runner." - pushd $SCENARIO_RUNNER_PATH > /dev/null - python scenario_runner.py --scenario ERDOSBenchmarks_2 --reloadWorld & - SCENARIO_RUNNER_PID=$! - sleep 15 + echo "[x] Starting the scenario runner..." + cd $SCENARIO_RUNNER_HOME + python3 scenario_runner.py --scenario ERDOSBenchmarks_2 --reloadWorld & + sleep 5 # Start the mIoU script. - popd > /dev/null - python map_scenario_runner.py -s $speed -d $SAMPLING_RATE -o results_map_$speed.csv + cd $PYLOT_HOME + python3 scripts/map_scenario_runner.py -s $speed -d $SAMPLING_RATE -o results_map_$speed.csv # Kill the scenario runner. - kill -9 $SCENARIO_RUNNER_PID - + pkill -9 -f -u $USER scenario_runner # Kill the simulator. - `ps aux | grep CarlaUE4 | awk '{print $2}' | head -n -1 | xargs kill -9` + pkill -9 -f -u $USER CarlaUE4 done diff --git a/scripts/run_miou_experiments.sh b/scripts/run_miou_experiments.sh index 107449807..a468413ec 100755 --- a/scripts/run_miou_experiments.sh +++ b/scripts/run_miou_experiments.sh @@ -1,5 +1,20 @@ -CARLA_PATH=../../CARLA_0.9.6 -SCENARIO_RUNNER_PATH=../../scenario_runner +#!/bin/bash + +# Assumes that $CARLA_HOME AND $PYLOT_HOME are set. +if [ -z "$PYLOT_HOME" ]; then + echo "Please set \$PYLOT_HOME before sourcing this script" + exit 1 +fi + +if [ -z "$CARLA_HOME" ]; then + echo "Please set \$CARLA_HOME before running this script" + exit 1 +fi + +if [ -z "$SCENARIO_RUNNER_HOME" ]; then + echo "Please set \$SCENARIO_RUNNER_HOME before running this script" + exit 1 +fi speeds=( 10 15 20 25 30 35 40 ) SAMPLING_RATE=0.005 # The delta between two subsequent frames. @@ -7,24 +22,22 @@ SAMPLING_RATE=0.005 # The delta between two subsequent frames. for speed in ${speeds[@]}; do echo "[x] Running the experiment with the speed $speed and the sampling rate of $SAMPLING_RATE" # Start the simulator. - echo "[x] Starting the Carla Simulator 0.9.6" - ./$CARLA_PATH/CarlaUE4.sh & + + echo "[x] Starting the CARLA Simulator..." + SDL_VIDEODRIVER=offscreen ${CARLA_HOME}/CarlaUE4.sh -opengl -windowed -ResX=800 -ResY=600 -carla-server -benchmark -fps=20 -quality-level=Epic & sleep 10 - # Start the scenario runner. - echo "[x] Starting the scenario runner." - pushd $SCENARIO_RUNNER_PATH > /dev/null - python scenario_runner.py --scenario ERDOSBenchmarks_2 --reloadWorld & - SCENARIO_RUNNER_PID=$! - sleep 15 - + echo "[x] Starting the scenario runner..." + cd $SCENARIO_RUNNER_HOME + python3 scenario_runner.py --scenario ERDOSBenchmarks_2 --reloadWorld & + sleep 5 + # Start the mIoU script. - popd > /dev/null - python miou_scenario_runner.py -s $speed -d $SAMPLING_RATE -o results_$speed.csv + cd $PYLOT_HOME + python3 miou_scenario_runner.py -s $speed -d $SAMPLING_RATE -o results_$speed.csv # Kill the scenario runner. - kill -9 $SCENARIO_RUNNER_PID - + pkill -9 -f -u $USER scenario_runner # Kill the simulator. - `ps aux | grep CarlaUE4 | awk '{print $2}' | head -n -1 | xargs kill -9` + pkill -9 -f -u $USER CarlaUE4 done diff --git a/setup.py b/setup.py index a601d7675..889fa9942 100644 --- a/setup.py +++ b/setup.py @@ -16,9 +16,7 @@ "erdos>=0.3.1", "gdown", "lapsolver", - "matplotlib==2.2.4", "motmetrics", - "networkx==2.2", "numpy<1.17", # Update to newer numpy version once we switch to tf2 "open3d-python==0.5.0.0", "opencv-python>=4.1.0.25", @@ -34,13 +32,16 @@ "torch==1.4.0", "torchvision==0.5.0", ##### Tracking dependencies ##### + "Cython", "filterpy==1.4.1", - "scikit-learn==0.22.2", "imgaug==0.2.8", + "matplotlib==2.2.4", "nonechucks==0.3.1", - "Cython", + "nuscenes-devkit" "progress", "pyquaternion", - "nuscenes-devkit" + "scikit-learn==0.22.2", + ##### CARLA dependencies ##### + "networkx==2.2", ], ) diff --git a/scripts/check_3d_2d_conversions.py b/tests/check_3d_2d_conversions.py similarity index 100% rename from scripts/check_3d_2d_conversions.py rename to tests/check_3d_2d_conversions.py diff --git a/scripts/test_canny_lane_detection.py b/tests/check_canny_lane_detection.py similarity index 100% rename from scripts/test_canny_lane_detection.py rename to tests/check_canny_lane_detection.py diff --git a/scripts/test_lanenet_lane_detection.py b/tests/check_lanenet_lane_detection.py similarity index 100% rename from scripts/test_lanenet_lane_detection.py rename to tests/check_lanenet_lane_detection.py diff --git a/scripts/test_determinism.sh b/tests/test_determinism.sh similarity index 100% rename from scripts/test_determinism.sh rename to tests/test_determinism.sh