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)
最新发布
本文介绍了两种在表数据中查找记录的方法：一种是从头到尾遍历并更新记录状态；另一种是从后向前查找并删除指定条件的记录。通常情况下采用从前到后的查找方式，但在需要删除数据结构中的记录时，则更多地使用从后向前的方式。
扫一扫
2092
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
