import math import os import struct from argparse import ArgumentParser import av import numpy as np import open3d as o3d import rosbag import yaml from sensor_msgs import point_cloud2 import subprocess from protoc.octopudata_localizationinfo_pb2 import LocalizationInfoFrame, LocalizationInfo from protoc.octopudata_trackedobject_pb2 import TrackedObjectFrame, Object, TrackedObject from protoc.octopusdata_controlcommand_pb2 import CommandFrame, ControlCommand from protoc.octopusdata_gnss_pb2 import GnssPoint, GnssPoints from protoc.octopusdata_plantrajectory_pb2 import Trajectory, TrajectoryPoint, PlanTrajectory from protoc.octopusdata_predictionobstacles_pb2 import PerceptionObstacle, \ Obstacle, PredictionTrajectory, PathPoint, PredictionObstacles from protoc.octopusdata_routingpath_pb2 import RoutingPath, Path, Point, RoutingFrames from protoc.octopusdata_vehicleinfo_pb2 import VehicleFrame, VehicleInfo av.logging.set_level(av.logging.PANIC) codec_ctx = av.codec.Codec('hevc','r') h265_code = codec_ctx.create() class Pose: def __init__(self, q0, q1, q2, q3, x, y, z): self.q0 = q0 self.q1 = q1 self.q2 = q2 self.q3 = q3 self.x = x self.y = y self.z = z def get_ts(secs, nsecs): return int(secs * 1000 + nsecs / 1000000) def get_modulo(x, y, z): return math.sqrt(x * x + y * y + z * z) def yaw_from_quaternions(w, x, y, z): a = 2 * (w * z + x * y) b = 1 - 2 * (y * y + z * z) return math.atan2(a, b) def pose_has_nan(p): return math.isnan(p.x) or math.isnan(p.y) or math.isnan(p.z) or \ math.isnan(p.q0) or math.isnan(p.q1) or math.isnan(p.q2) or \ math.isnan(p.q3) def get_t(q0, q1, q2, q3, x, y, z): aa = 1 - 2 * (q2 * q2 + q3 * q3) ab = 2 * (q1 * q2 - q0 * q3) ac = 2 * (q1 * q3 + q0 * q2) ba = 2 * (q1 * q2 + q0 * q3) bb = 1 - 2 * (q1 * q1 + q3 * q3) bc = 2 * (q2 * q3 - q0 * q1) ca = 2 * (q1 * q3 - q0 * q2) cb = 2 * (q2 * q3 + q0 * q1) cc = 1 - 2 * (q1 * q1 + q2 * q2) t = np.mat([[aa, ab, ac, x], [ba, bb, bc, y], [ca, cb, cc, z], [0, 0, 0, 1]]) return t def get_label(perception_typ, subtype): if perception_typ == 3: return 'Pedestrian' elif perception_typ == 4: return 'Bi_Tricycle' elif perception_typ == 5: if subtype == 5: return 'Truck' elif subtype == 6: return 'Bus' else: return 'Car' else: return 'unknow' def main(args): for file in os.listdir(args.input): if file.endswith('.bag'): bag_path = os.path.join(args.input, file) bag = rosbag.Bag(bag_path, "r") output_dir = os.getenv('output_dir') if not os.path.exists(os.path.join(output_dir, 'innoPtClound_A4')): os.makedirs(os.path.join(output_dir, 'innoPtClound_A4')) if not os.path.exists(os.path.join(output_dir, 'innoPtClound_B2')): os.makedirs(os.path.join(output_dir, 'innoPtClound_B2')) if not os.path.exists(os.path.join(output_dir, 'radar_track_array_0')): os.makedirs(os.path.join(output_dir, 'radar_track_array_0')) if not os.path.exists(os.path.join(output_dir, 'radar_track_array_3')): os.makedirs(os.path.join(output_dir, 'radar_track_array_3')) if not os.path.exists(os.path.join(output_dir, 'mdc_camera_instance_74')): os.makedirs(os.path.join(output_dir, 'mdc_camera_instance_74')) if not os.path.exists(os.path.join(output_dir, 'mdc_camera_instance_73')): os.makedirs(os.path.join(output_dir, 'mdc_camera_instance_73')) if not os.path.exists(os.path.join(output_dir, 'mdc_camera_instance_72')): os.makedirs(os.path.join(output_dir, 'mdc_camera_instance_72')) if not os.path.exists(os.path.join(output_dir, 'mdc_camera_instance_71')): os.makedirs(os.path.join(output_dir, 'mdc_camera_instance_71')) routes = [] controls = [] plans = [] preds = [] gnss = [] vehs = [] locs = [] objs = [] ego_pose = None has_camera_71 = False has_camera_72 = False has_camera_73 = False has_camera_74 = False has_lidar_A4 = False has_lidar_B2 = False has_radar_0 = False has_radar_3 = False lidar_num = 0 image_num = 0 radar_num = 0 for topic, msg, t in bag.read_messages(): time_stamp = int(t.to_sec() * 1000) # 以 rosbag 时间戳（t）为基准，转换为 13 位时间戳 if topic == '/innoPtClound_A4': ### 图达通 时间辍应该是13位数字，图达通雷达8位，华为96线6位 # file_path = os.path.join(output_dir, 'innoPtClound_A4', '{}.pcd'.format(int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000))) file_path = os.path.join(output_dir, 'innoPtClound_A4', '{}.pcd'.format(time_stamp)) # print(file_path) # 提取点云数据，包括 x, y, z points = list(point_cloud2.read_points(msg, field_names=["x", "y", "z", "intensity"], skip_nans=True)) if points: # 转换为 numpy 数组，添加 intensity, ring, timestamp 字段 np_points = np.array(points) # (N, 3), 包含 x, y, z # 转换为 Open3D 格式点云 pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(np_points[:, :3]) # x, y, z pcd.colors = o3d.utility.Vector3dVector(np.tile(np_points[:, 3:4] / np_points[:, 3:4].max(), (1, 3))) # 用 intensity 作为灰度颜色 o3d.io.write_point_cloud(file_path, pcd) lidar_num += 1 has_lidar_A4 = True elif topic == '/innoPtClound_B2': ### 图达通 # file_path = os.path.join(output_dir, 'innoPtClound_B2', '{}.pcd'.format(int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000))) file_path = os.path.join(output_dir, 'innoPtClound_B2', '{}.pcd'.format(time_stamp)) # print(file_path) # 提取点云数据，包括 x, y, z points = list(point_cloud2.read_points(msg, field_names=["x", "y", "z", "intensity"], skip_nans=True)) if points: # 转换为 numpy 数组，添加 intensity, ring, timestamp 字段 np_points = np.array(points) # (N, 3), 包含 x, y, z # 转换为 Open3D 格式点云 pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(np_points[:, :3]) # x, y, z pcd.colors = o3d.utility.Vector3dVector(np.tile(np_points[:, 3:4] / np_points[:, 3:4].max(), (1, 3))) # 用 intensity 作为灰度颜色 o3d.io.write_point_cloud(file_path, pcd) lidar_num += 1 has_lidar_B2 = True elif topic == 'mdc_camera_instance_74': ### 相机 时间辍应该是13位数字 # time_stamp = int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000) file_path = os.path.join(output_dir, 'mdc_camera_instance_74', '{}.jpg'.format(time_stamp)) packet = av.packet.Packet(msg.data) try: out = h265_code.decode(packet) for frame in out: if frame.format.name != 'rgb24': frame = frame.reformat(format='rgb24') img = frame.to_image() img.save(file_path) image_num += 1 has_camera_74 = True except Exception as e: print("{} frame can not trans to jpg".format(time_stamp), e) elif topic == 'mdc_camera_instance_73': ### 相机 时间辍应该是13位数字 # time_stamp = int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000) file_path = os.path.join(output_dir, 'mdc_camera_instance_73', '{}.jpg'.format(time_stamp)) packet = av.packet.Packet(msg.data) try: out = h265_code.decode(packet) for frame in out: if frame.format.name != 'rgb24': frame = frame.reformat(format='rgb24') img = frame.to_image() img.save(file_path) image_num += 1 has_camera_73 = True except Exception as e: print("{} frame can not trans to jpg".format(time_stamp), e) elif topic == 'mdc_camera_instance_72': ### 相机 # time_stamp = int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000) file_path = os.path.join(output_dir, 'mdc_camera_instance_72', '{}.jpg'.format(time_stamp)) packet = av.packet.Packet(msg.data) try: out = h265_code.decode(packet) for frame in out: if frame.format.name != 'rgb24': frame = frame.reformat(format='rgb24') img = frame.to_image() img.save(file_path) image_num += 1 has_camera_72 = True except Exception as e: print("{} frame can not trans to jpg".format(time_stamp), e) elif topic == 'mdc_camera_instance_71': ### 相机 # time_stamp = int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000) file_path = os.path.join(output_dir, 'mdc_camera_instance_71', '{}.jpg'.format(time_stamp)) packet = av.packet.Packet(msg.data) try: out = h265_code.decode(packet) for frame in out: if frame.format.name != 'rgb24': frame = frame.reformat(format='rgb24') img = frame.to_image() img.save(file_path) image_num += 1 has_camera_71 = True except Exception as e: print("{} frame can not trans to jpg".format(time_stamp), e) elif topic == '/radar_track_array_0': ### 大陆408 时间辍应该是13位数字 # file_path = os.path.join(output_dir, 'radar_track_array_0', '{}.pcd'.format(int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000))) file_path = os.path.join(output_dir, 'radar_track_array_0', '{}.pcd'.format(time_stamp)) points = [] for track in msg.trackList: x, y, z = track.x, track.y, track.z vx, vy, ax, ay = track.vx, track.vy, track.ax, track.ay rcs, snr, yawRate = track.rcs, track.snr, track.yawRate obj_id, trackType, lifetime = track.id, track.trackType, track.lifetime # 过滤无效点 if np.isnan(x) or np.isnan(y) or np.isnan(z): continue points.append([x, y, z, vx, vy, ax, ay, rcs, snr, yawRate, obj_id, trackType, lifetime]) if not points: print("没有有效点云数据") continue points = np.array(points) # **写入 PCD 文件** with open(file_path, 'w') as f: # **写入 PCD 头部** f.write("# .PCD v0.7 - Point Cloud Data file format\n") f.write("VERSION 0.7\n") f.write(f"FIELDS x y z vx vy ax ay rcs snr yawRate id trackType lifetime\n") f.write("SIZE 4 4 4 4 4 4 4 4 4 4 4 4 4\n") f.write("TYPE F F F F F F F F F F I I I\n") # F = float, I = int f.write("COUNT 1 1 1 1 1 1 1 1 1 1 1 1 1\n") f.write(f"WIDTH {points.shape[0]}\n") f.write("HEIGHT 1\n") f.write("VIEWPOINT 0 0 0 1 0 0 0\n") f.write(f"POINTS {points.shape[0]}\n") f.write("DATA ascii\n") # **写入点云数据** np.savetxt(f, points, fmt="%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %d %d %d") radar_num += 1 has_radar_0 = True elif topic == '/radar_track_array_3': ### 大陆408 # file_path = os.path.join(output_dir, 'radar_track_array_3', '{}.pcd'.format(int(msg.header.stamp.secs * 1000 + # msg.header.stamp.nsecs / 1000000))) file_path = os.path.join(output_dir, 'radar_track_array_3', '{}.pcd'.format(time_stamp)) points = [] for track in msg.trackList: x, y, z = track.x, track.y, track.z vx, vy, ax, ay = track.vx, track.vy, track.ax, track.ay rcs, snr, yawRate = track.rcs, track.snr, track.yawRate obj_id, trackType, lifetime = track.id, track.trackType, track.lifetime # 过滤无效点 if np.isnan(x) or np.isnan(y) or np.isnan(z): continue points.append([x, y, z, vx, vy, ax, ay, rcs, snr, yawRate, obj_id, trackType, lifetime]) if not points: print("没有有效点云数据") continue points = np.array(points) # **写入 PCD 文件** with open(file_path, 'w') as f: # **写入 PCD 头部** f.write("# .PCD v0.7 - Point Cloud Data file format\n") f.write("VERSION 0.7\n") f.write(f"FIELDS x y z vx vy ax ay rcs snr yawRate id trackType lifetime\n") f.write("SIZE 4 4 4 4 4 4 4 4 4 4 4 4 4\n") f.write("TYPE F F F F F F F F F F I I I\n") # F = float, I = int f.write("COUNT 1 1 1 1 1 1 1 1 1 1 1 1 1\n") f.write(f"WIDTH {points.shape[0]}\n") f.write("HEIGHT 1\n") f.write("VIEWPOINT 0 0 0 1 0 0 0\n") f.write(f"POINTS {points.shape[0]}\n") f.write("DATA ascii\n") # **写入点云数据** np.savetxt(f, points, fmt="%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %d %d %d") radar_num += 1 has_radar_3 = True elif topic == '/routing/routing_response_viz': rv = RoutingPath() rv.timestamp = int(t.secs * 1000 + t.nsecs / 1000000) rv.stamp_secs = t.secs rv.stamp_nsecs = t.nsecs mark_list = list() for mark in msg.markers: path_pb = Path() path_pb.id = mark.id point_list = [] for point in mark.points: point_pb = Point() point_pb.x = point.x point_pb.y = point.y point_pb.z = point.z point_list.append(point_pb) path_pb.path_point.extend(point_list) mark_list.append(path_pb) rv.routing_path_info.extend(mark_list) routes.append(rv) elif topic == '/holo/ControlCommand': cf = CommandFrame() cf.timestamp = int(t.secs * 1000 + t.nsecs / 1000000) cf.stamp_secs = t.secs cf.stamp_nsecs = t.nsecs cf.acceleration = msg.acceleration cf.front_wheel_angle = msg.front_wheel_angle cf.gear = msg.gear controls.append(cf) elif topic == '/planning/trajectory': tj = Trajectory() tj.timestamp = int(t.secs * 1000 + t.nsecs / 1000000) tj.stamp_secs = t.secs tj.stamp_nsecs = t.nsecs p_list = [] for point in msg.trajectory_points: p = TrajectoryPoint() p.x = point.path_point.point.x p.y = point.path_point.point.y p.z = point.path_point.point.z p.theta = point.path_point.theta p.kappa = point.path_point.kappa p.v = point.v p.a = point.a p.relative_time = point.relative_time p_list.append(p) tj.trajectory_points.extend(p_list) plans.append(tj) elif topic == '/prediction/prediction_obstacles': tr_pb = PerceptionObstacle() tr_pb.timestamp = int(msg.header.stamp.secs * 1000 + msg.header.stamp.nsecs / 1000000) tr_pb.stamp_secs = msg.header.stamp.secs tr_pb.stamp_nsecs = msg.header.stamp.nsecs obj_list = list() for obj in msg.prediction_obstacle: ob_pb = Obstacle() ob_pb.obstacle_timestamp = int(obj.timestamp * 1000) ob_pb.id = obj.perception_obstacle.id ob_pb.x = obj.perception_obstacle.position.x ob_pb.y = obj.perception_obstacle.position.y ob_pb.z = obj.perception_obstacle.position.z traj_pbs = [] for traj in obj.trajectory: traj_pb = PredictionTrajectory() points_pbs = [] for trajectory_point in traj.trajectory_points: point_pb = PathPoint() point_pb.x = trajectory_point.path_point.point.x point_pb.y = trajectory_point.path_point.point.y point_pb.z = trajectory_point.path_point.point.z point_pb.theta = trajectory_point.path_point.theta point_pb.kappa = trajectory_point.path_point.kappa point_pb.lane_id = trajectory_point.path_point.lane_id point_pb.v = trajectory_point.v point_pb.a = trajectory_point.a point_pb.relative_time = trajectory_point.relative_time points_pbs.append(point_pb) traj_pb.path_point.extend(points_pbs) traj_pbs.append(traj_pb) ob_pb.prediction_trajectory.extend(traj_pbs) obj_list.append(ob_pb) tr_pb.obstacle_info.extend(obj_list) preds.append(tr_pb) elif topic == '/inspvax': pb_loc_gnss = GnssPoint() pb_loc_gnss.stamp_secs = msg.header.stamp.secs # 1 pb_loc_gnss.stamp_nsecs = msg.header.stamp.nsecs # 2 pb_loc_gnss.timestamp = get_ts(msg.header.stamp.secs, msg.header.stamp.nsecs) pb_loc_gnss.latitude = msg.latitude # 3 pb_loc_gnss.longitude = msg.longitude # 4 pb_loc_gnss.elevation = msg.altitude gnss.append(pb_loc_gnss) elif topic == '/holo/VehicleInfoMagotan': veh_pb = VehicleFrame() veh_pb.stamp_secs = msg.timestamp.secs # 1 veh_pb.stamp_nsecs = msg.timestamp.nsecs # 2 veh_pb.timestamp = get_ts(veh_pb.stamp_secs, veh_pb.stamp_nsecs) veh_pb.gear_value = msg.gear # 4 veh_pb.vehicle_speed = msg.vehicle_speed * 3.6 # 5 veh_pb.steering_angle = msg.steering_angle # 6 veh_pb.longitude_acc = msg.longitude_acc veh_pb.lateral_acc = msg.lateral_acc veh_pb.turn_left_light = msg.turn_left_light veh_pb.turn_right_light = msg.turn_right_light veh_pb.brake = msg.brake_torque veh_pb.autonomy_status = 0 vehs.append(veh_pb) elif topic == '/localization/localization_info': lo_pb = LocalizationInfoFrame() lo_pb.timestamp = get_ts(msg.header.stamp.secs, msg.header.stamp.nsecs) lo_pb.stamp_secs = msg.header.stamp.secs lo_pb.stamp_nsecs = msg.header.stamp.nsecs lo_pb.pose_position_x = msg.pose.position.x lo_pb.pose_position_y = msg.pose.position.y lo_pb.pose_position_z = msg.pose.position.z lo_pb.pose_orientation_x = msg.pose.orientation.x lo_pb.pose_orientation_y = msg.pose.orientation.y lo_pb.pose_orientation_z = msg.pose.orientation.z lo_pb.pose_orientation_w = msg.pose.orientation.w lo_pb.pose_orientation_yaw = \ yaw_from_quaternions(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z) lo_pb.velocity_linear = get_modulo(msg.pose.linear_velocity.x, msg.pose.linear_velocity.y, msg.pose.linear_velocity.z) lo_pb.velocity_angular = get_modulo(msg.pose.angular_velocity.x, msg.pose.angular_velocity.y, msg.pose.angular_velocity.z) lo_pb.acceleration_linear = get_modulo(msg.pose.linear_acceleration_vrf.x, msg.pose.linear_acceleration_vrf.y, msg.pose.linear_acceleration_vrf.z) lo_pb.acceleration_angular = get_modulo(msg.pose.angular_velocity_vrf.x, msg.pose.angular_velocity_vrf.y, msg.pose.angular_velocity_vrf.z) locs.append(lo_pb) ego_pose = Pose(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) elif topic == '/perception/perception_obstacles': if ego_pose is None or pose_has_nan(ego_pose): continue tr_pb = TrackedObjectFrame() tr_pb.timestamp = get_ts(msg.header.stamp.secs, msg.header.stamp.nsecs) tr_pb.stamp_secs = msg.header.stamp.secs tr_pb.stamp_nsecs = msg.header.stamp.nsecs obj_list = list() for object in msg.perception_obstacle: ob_pb = Object() ob_pb.id = object.id ob_pb.label = get_label(object.type, object.sub_type) ob_pb.pose_position_x = object.position.x ob_pb.pose_position_y = object.position.y ob_pb.pose_position_z = object.position.z ob_pb.pose_orientation_x = 0 ob_pb.pose_orientation_y = 0 ob_pb.pose_orientation_z = math.sin(object.theta / 2) ob_pb.pose_orientation_w = math.cos(object.theta / 2) ob_pb.pose_orientation_yaw = object.theta ob_pb.dimensions_x = object.length ob_pb.dimensions_y = object.width ob_pb.dimensions_z = object.height ob_pb.speed_vector_linear_x = object.velocity.x ob_pb.speed_vector_linear_y = object.velocity.y ob_pb.speed_vector_linear_z = object.velocity.z world_obj = np.transpose(np.array([[object.position.x, object.position.y, object.position.z, 1]])) world_ego_t = get_t(ego_pose.q0, ego_pose.q1, ego_pose.q2, ego_pose.q3, ego_pose.x, ego_pose.y, ego_pose.z) try: world_ego_invt = np.linalg.pinv(world_ego_t) except Exception as err: print('pinv failed:', world_ego_t) raise err vehicle_obj = world_ego_invt * world_obj ob_pb.relative_position_x = vehicle_obj[0] ob_pb.relative_position_y = vehicle_obj[1] ob_pb.relative_position_z = vehicle_obj[2] obj_list.append(ob_pb) tr_pb.objects.extend(obj_list) objs.append(tr_pb) print(f"lidar_num : {lidar_num}") print(f"image_num : {image_num}") print(f"radar_num : {radar_num}") folders = [] if len(routes) > 0: os.makedirs(os.path.join(output_dir, 'routing_routing_response_viz')) folders.append({'folder': 'routing_routing_response_viz', 'sensor_type': 'routing_path'}) route_out = RoutingFrames() route_out.routing_frame.extend(routes) with open(os.path.join(output_dir, 'routing_routing_response_viz', 'route.pb'), "wb") as c: c.write(route_out.SerializeToString()) if len(controls) > 0: os.makedirs(os.path.join(output_dir, 'holo_ControlCommand')) folders.append({'folder': 'holo_ControlCommand', 'sensor_type': 'control'}) ctl_cmd_pb_out = ControlCommand() ctl_cmd_pb_out.command_frame.extend(controls) with open(os.path.join(output_dir, 'holo_ControlCommand', 'control.pb'), "wb") as c: c.write(ctl_cmd_pb_out.SerializeToString()) if len(plans) > 0: os.makedirs(os.path.join(output_dir, 'planning_trajectory')) folders.append({'folder': 'planning_trajectory', 'sensor_type': 'planning_trajectory'}) plan_traj_pb_out = PlanTrajectory() plan_traj_pb_out.trajectory_info.extend(plans) with open(os.path.join(output_dir, 'planning_trajectory', 'planning.pb'), "wb") as p: p.write(plan_traj_pb_out.SerializeToString()) if len(preds) > 0: os.makedirs(os.path.join(output_dir, 'prediction_prediction_obstacles')) folders.append({'folder': 'prediction_prediction_obstacles', 'sensor_type': 'predicted_objects'}) pred_obstacles_pb_out = PredictionObstacles() pred_obstacles_pb_out.perception_obstacle.extend(preds) with open(os.path.join(output_dir, 'prediction_prediction_obstacles', 'predicted.pb'), "wb") as p: p.write(pred_obstacles_pb_out.SerializeToString()) if len(gnss) > 0: os.makedirs(os.path.join(output_dir, 'inspvax')) folders.append({'folder': 'inspvax', 'sensor_type': 'gnss'}) gn_pb_out = GnssPoints() gn_pb_out.gnss_points.extend(gnss) with open(os.path.join(output_dir, 'inspvax', 'gnss.pb'), "wb") as g: g.write(gn_pb_out.SerializeToString()) if len(vehs) > 0: os.makedirs(os.path.join(output_dir, 'holo_VehicleInfoMagotan')) folders.append({'folder': 'holo_VehicleInfoMagotan', 'sensor_type': 'vehicle'}) veh_pb_out = VehicleInfo() veh_pb_out.vehicle_info.extend(vehs) with open(os.path.join(output_dir, 'holo_VehicleInfoMagotan', 'vehicle.pb'), "wb") as v: v.write(veh_pb_out.SerializeToString()) if len(locs) > 0: os.makedirs(os.path.join(output_dir, 'localization_localization_info')) folders.append({'folder': 'localization_localization_info', 'sensor_type': 'ego_tf'}) lo_pb_out = LocalizationInfo() lo_pb_out.localization_info.extend(locs) with open(os.path.join(output_dir, 'localization_localization_info', 'ego_tf.pb'), "wb") as lo: lo.write(lo_pb_out.SerializeToString()) if len(objs) > 0: os.makedirs(os.path.join(output_dir, 'perception_perception_obstacles')) folders.append({'folder': 'perception_perception_obstacles', 'sensor_type': 'object_array_vision'}) tr_pb_out = TrackedObject() tr_pb_out.tracked_object.extend(objs) with open(os.path.join(output_dir, 'perception_perception_obstacles', 'object_array_vision.pb'), "wb") as tr: tr.write(tr_pb_out.SerializeToString()) if has_camera_74: folders.append({'folder': 'mdc_camera_instance_74', 'sensor_type': 'camera'}) if has_camera_73: folders.append({'folder': 'mdc_camera_instance_73', 'sensor_type': 'camera'}) if has_camera_72: folders.append({'folder': 'mdc_camera_instance_72', 'sensor_type': 'camera'}) if has_camera_71: folders.append({'folder': 'mdc_camera_instance_71', 'sensor_type': 'camera'}) if has_lidar_A4: if args.calibration_id: folders.append({'folder': 'innoPtClound_A4', 'sensor_type': 'lidar', 'calibration_item_id': args.calibration_id}) else: folders.append({'folder': 'innoPtClound_A4', 'sensor_type': 'lidar'}) if has_lidar_B2: if args.calibration_id: folders.append({'folder': 'innoPtClound_B2', 'sensor_type': 'lidar', 'calibration_item_id': args.calibration_id}) else: folders.append({'folder': 'innoPtClound_B2', 'sensor_type': 'lidar'}) if has_radar_0: folders.append({'folder': 'radar_track_array_0', 'sensor_type': 'radar'}) if has_radar_3: folders.append({'folder': 'radar_track_array_3', 'sensor_type': 'radar'}) collect_yaml = {'folders': folders} with open(os.path.join(output_dir, "opendata_to_platform.yaml"), 'w', encoding='utf-8') as collect_file: yaml.safe_dump(collect_yaml, collect_file) with open(os.path.join(os.getenv('output_dir'), '_SUCCESS'), 'w') as f: f.write("") os.system('chmod -R a+r ${output_dir}/*') if __name__ == '__main__': parser = ArgumentParser() parser.add_argument('-i', '--input', help="input bag path", default=os.getenv('rosbag_path')) parser.add_argument('-o', '--output', default=os.getenv('output_dir'), help="result output directory, default to ./bags/") parser.add_argument('-ci', '--calibration_id', type=int) params = parser.parse_args() main(params)