def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer, is_simulation, plot_planning_path, plot_planning_refpath, all_data): is_auto_drive = False for msg in reader.read_messages(): if msg.topic == "/apollo/canbus/chassis": chassis = chassis_pb2.Chassis() chassis.ParseFromString(msg.message) if chassis.driving_mode == \ chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE: is_auto_drive = True else: is_auto_drive = False if msg.topic == "/apollo/control": if (not is_auto_drive and not all_data) or \ is_simulation or plot_planning_path or plot_planning_refpath: continue control_cmd = control_cmd_pb2.ControlCommand() control_cmd.ParseFromString(msg.message) control_analyzer.put(control_cmd) lidar_endtoend_analyzer.put_pb('control', control_cmd) if msg.topic == "/apollo/planning": if (not is_auto_drive) and (not all_data): continue adc_trajectory = planning_pb2.ADCTrajectory() adc_trajectory.ParseFromString(msg.message) planning_analyzer.put(adc_trajectory) lidar_endtoend_analyzer.put_pb('planning', adc_trajectory) if plot_planning_path: planning_analyzer.plot_path(plt, adc_trajectory) if plot_planning_refpath: planning_analyzer.plot_refpath(plt, adc_trajectory) if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2" or \ msg.topic == "/apollo/sensor/lidar128/compensator/PointCloud2": if ((not is_auto_drive) and (not all_data)) or is_simulation or \ plot_planning_path or plot_planning_refpath: continue point_cloud = pointcloud_pb2.PointCloud() point_cloud.ParseFromString(msg.message) lidar_endtoend_analyzer.put_lidar(point_cloud) if msg.topic == "/apollo/perception/obstacles": if ((not is_auto_drive) and (not all_data)) or is_simulation or \ plot_planning_path or plot_planning_refpath: continue perception = perception_obstacle_pb2.PerceptionObstacles() perception.ParseFromString(msg.message) lidar_endtoend_analyzer.put_pb('perception', perception) if msg.topic == "/apollo/prediction": if ((not is_auto_drive) and (not all_data)) or is_simulation or \ plot_planning_path or plot_planning_refpath: continue prediction = prediction_obstacle_pb2.PredictionObstacles() prediction.ParseFromString(msg.message) lidar_endtoend_analyzer.put_pb('prediction', prediction)
def mobileye_callback(mobileye_pb): perception_pb = perception_obstacle_pb2.PerceptionObstacles() lane_type_mapper = {} lane_type_mapper[0] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_DASHED lane_type_mapper[1] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_SOLID lane_type_mapper[2] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_UNKNOWN lane_type_mapper[ 3] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_ROAD_EDGE lane_type_mapper[4] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_SOLID lane_type_mapper[5] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_DASHED lane_type_mapper[6] = perception_obstacle_pb2.LaneMarker.LANE_TYPE_UNKNOWN perception_pb.lane_marker.left_lane_marker.lane_type = \ lane_type_mapper.get(mobileye_pb.lka_766.lane_type, perception_obstacle_pb2.LaneMarker.LANE_TYPE_UNKNOWN) perception_pb.lane_marker.left_lane_marker.quality = \ mobileye_pb.lka_766.quality / 4.0 perception_pb.lane_marker.left_lane_marker.model_degree = \ mobileye_pb.lka_766.model_degree perception_pb.lane_marker.left_lane_marker.c0_position = \ mobileye_pb.lka_766.position perception_pb.lane_marker.left_lane_marker.c1_heading_angle = \ mobileye_pb.lka_767.heading_angle perception_pb.lane_marker.left_lane_marker.c2_curvature = \ mobileye_pb.lka_766.curvature perception_pb.lane_marker.left_lane_marker.c3_curvature_derivative = \ mobileye_pb.lka_766.curvature_derivative perception_pb.lane_marker.left_lane_marker.view_range = \ mobileye_pb.lka_767.view_range perception_pb.lane_marker.right_lane_marker.lane_type = \ lane_type_mapper.get(mobileye_pb.lka_766.lane_type, perception_obstacle_pb2.LaneMarker.LANE_TYPE_UNKNOWN) perception_pb.lane_marker.right_lane_marker.quality = \ mobileye_pb.lka_768.quality / 4.0 perception_pb.lane_marker.right_lane_marker.model_degree = \ mobileye_pb.lka_768.model_degree perception_pb.lane_marker.right_lane_marker.c0_position = \ mobileye_pb.lka_768.position perception_pb.lane_marker.right_lane_marker.c1_heading_angle = \ mobileye_pb.lka_769.heading_angle perception_pb.lane_marker.right_lane_marker.c2_curvature = \ mobileye_pb.lka_768.curvature perception_pb.lane_marker.right_lane_marker.c3_curvature_derivative = \ mobileye_pb.lka_768.curvature_derivative perception_pb.lane_marker.right_lane_marker.view_range = \ mobileye_pb.lka_769.view_range perception_pub.publish(perception_pb)
def generate_message(topic, filename): """generate message from file""" message = None if topic == "/apollo/planning": message = planning_pb2.ADCTrajectory() elif topic == "/apollo/localization/pose": message = localization_pb2.LocalizationEstimate() elif topic == "/apollo/perception/obstacles": message = perception_obstacle_pb2.PerceptionObstacles() elif topic == "/apollo/prediction": message = prediction_obstacle_pb2.PredictionObstacles() elif topic == "/apollo/routing_response": message = routing_pb2.RoutingResponse() if not message: print "Unknown topic:", topic sys.exit(0) if not os.path.exists(filename): return None f_handle = file(filename, 'r') text_format.Merge(f_handle.read(), message) f_handle.close() return message
if __name__ == "__main__": parser = argparse.ArgumentParser( description="Recode Analyzer is a tool to analyze record files.", prog="main.py") parser.add_argument( "-f", "--file", action="store", type=str, required=True, help="Specify the message file for sending.") args = parser.parse_args() cyber.init() node = cyber.Node("perception_obstacle_sender") perception_pub = node.create_writer( "/apollo/perception/obstacles", perception_obstacle_pb2.PerceptionObstacles) perception_obstacles = perception_obstacle_pb2.PerceptionObstacles() with open(args.file, 'r') as f: text_format.Merge(f.read(), perception_obstacles) while not cyber.is_shutdown(): now = cyber_time.Time.now().to_sec() perception_obstacles = update(perception_obstacles) perception_pub.write(perception_obstacles) sleep_time = 0.1 - (cyber_time.Time.now().to_sec() - now) if sleep_time > 0: time.sleep(sleep_time) cyber.shutdown()