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()
