使用 kitti 数据集实现自动驾驶——发布照片、点云、IMU、GPS、显示 2D 和 3D 侦测框

cd ~/catkin_ws/srccatkin_create_pkg kitti_turtorial rospy 

#创建一个摄像头的发布者cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=10)#读取摄像机数据image = read_camera(os.path.join(DAtA_PATH, 'image_02/data/%010d.png'%frame))#发布数据publish_camera(cam_pub,bridge,image,boxes_2d,types)

#创建一个点云的发布者pcl_pub = rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#读取点云数据point_cloud = read_point_cloud(os.path.join(DAtA_PATH,'velodyne_points/data/%010d.bin'%frame))#发布数据    publish_point_cloud(pcl_pub,point_cloud)

#创建一个汽车的发布者ego_pub = rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)#发布ego_car数据publish_ego_car(ego_pub)
##绘制车子的照相机视野marker.id = 0marker.action = marker.ADDmarker.lifetime = rospy.Duration()marker.type = Marker.LINE_STRIP
marker.color.r = 0.0marker.color.g = 1.0marker.color.b = 0.0marker.color.a = 1.0marker.scale.x = 0.2    marker.points = []marker.points.append(Point(10,-10,0))marker.points.append(Point(0,0,0))marker.points.append(Point(10,10,0))
marker_array.markers.append(marker)

#创建一个IMU发布者imu_pub = rospy.Publisher('kitti_imu',Imu,queue_size=10)
#发布imu数据publish_imu(imu_pub,imu_data)
##IMU发布函数相关设置def publish_imu(imu_pub,imu_data):    imu = Imu()    imu.header.frame_id = FRAME_ID    imu.header.stamp = rospy.Time.now()        #设置旋转量    q = tf.transformations.quaternion_from_euler(float(imu_data.roll),float(imu_data.pitch),float(imu_data.yaw));    imu.orientation.x = q[0]    imu.orientation.y = q[1]    imu.orientation.z = q[2]    imu.orientation.w = q[3]        #设置线性加速度    imu.linear_acceleration.x = imu_data.af    imu.linear_acceleration.y = imu_data.al    imu.linear_acceleration.z = imu_data.au        #设置角加速度    imu.angular_velocity.x = imu_data.wf    imu.angular_velocity.y = imu_data.wl    imu.angular_velocity.z = imu_data.wu        imu_pub.publish(imu)

#创建一个GPS发布者gps_pub = rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)#发布GPS数据publish_gps(gps_pub,imu_data) ##GPS发布函数相关设置def publish_gps(gps_pub,imu_data):    gps = NavSatFix()    gps.header.frame_id = FRAME_ID    gps.header.stamp = rospy.Time.now()        #gps经度、纬度、海拔高度    gps.latitude = imu_data.lat    gps.longitude = imu_data.lon    gps.altitude = imu_data.alt        gps_pub.publish(gps) 

#读取2D检测框数据boxes_2d = np.array(df_tracking_frame[['bbox_left', 'bbox_top', 'bbox_right', 'bbox_bottom']])types = np.array(df_tracking_frame['type'])
## 2D框相关设置for typ,box in zip(types,boxes):    top_left = int(box[0]),int(box[1])    bottom_right = int(box[2]),int(box[3])    cv2.rectangle(image,top_left,bottom_right,DETECTION_COLOR_DICT[typ],2)cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))

 #读取3D检测框数据boxes_3d = np.array(df_tracking_frame[['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']]      corners_3d_velos = []for box_3d in boxes_3d:    corners_3d_cam2 = compute_3d_box_cam2(*box_3d)    corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)    corners_3d_velos += [corners_3d_velo]  ##3D框发布函数相关设置def publish_3dbox(box3d_pub,corners_3d_velos,types):    marker_array = MarkerArray()    for i, corners_3d_velo in enumerate(corners_3d_velos):        # 3d box        marker = Marker()        marker.header.frame_id = FRAME_ID        marker.header.stamp = rospy.Time.now()                marker.id = i        marker.action = Marker.ADD        marker.lifetime = rospy.Duration(LIFETIME)        marker.type = Marker.LINE_LIST                        b, g, r = DETECTION_COLOR_DICT[types[i]]            marker.color.r = r/255.0        marker.color.g = g/255.0        marker.color.b = b/255.0        marker.color.a = 1.0                marker.scale.x = 0.1                marker.points = []        for l in LINES:            p1 = corners_3d_velo[l[0]]            marker.points.append(Point(p1[0], p1[1], p1[2]))            p2 = corners_3d_velo[l[1]]            marker.points.append(Point(p2[0], p2[1], p2[2]))        marker_array.markers.append(marker)            box3d_pub.publish(marker_array)