关于ros中发布kitti数据的一些代码（以及遇到的bug处理）

B站视频链接：https://www.bilibili.com/video/BV1qV41167d2?p=14

#!/usr/bin/env python2
# -*- coding: UTF-8 -*-
'''
cv.创建node发布图像数据,topic为 kitti_cam
'''
# 其中环境中安装的opencv基于的是python3，所以需要将ros依赖的python2.7路径先进行移除，以保证opencv的引入
import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')

import os
import cv2
import numpy as np

# 在引入opencv后，可以重新添加路径，引入ros环境，保证功能有效，同时注意第一行的声明，默认使用的python2以保证ros的运行
sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import tf
import pandas as pd


Data_path = "/home/sheng/Desktop/kitti/2011_09_26/2011_09_26_drive_0001_sync/"
FRAME_ID = 'map'

TRACKING_COLUMN_NAMES = ['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha',
                         'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 'width',
                         'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']

IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf',
                    'vl', 'vu', 'ax', 'ay', 'az', 'af','al', 'au', 'wx', 'wy', 'wz',
                    'wf', 'wl', 'wu', 'posacc', 'velacc', 'navstat', 'numsats', 'posmode','velmode', 'orimode']

# 用于读取IMU数据
def read_imu(path):
    df = pd.read_csv(path, header=None, sep=' ')
    df.columns = IMU_COLUMN_NAMES
    return df

def read_tracking(path):
    df = pd.read_csv(path, header=None, sep=' ')
    df.columns = TRACKING_COLUMN_NAMES
    df = df[df['track_id']>=0] # remove DontCare objects
    df.loc[df.type.isin(['Bus', 'Truck', 'Van', 'Tram']), 'type'] = 'Car' # Set all vehicle type to Car
    df = df[df.type.isin(['Car', 'Pedestrian', 'Cyclist'])]
    return df

# 用于绘制表示水平视野角范围的两条线
def publish_ego_car(ego_car_pub):
    # marker_array = MarkerArray()
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()

    marker.id = 0
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration()
    marker.type = Marker.LINE_STRIP

    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 1.0
    marker.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))

    ego_car_pub.publish(marker)


# 用于绘制本车模型
def publish_car_model(model_pub):

    mesh_marker = Marker()
    mesh_marker.header.frame_id = FRAME_ID
    mesh_marker.header.stamp = rospy.Time.now()

    mesh_marker.id = -1
    mesh_marker.lifetime = rospy.Duration()
    mesh_marker.type = Marker.MESH_RESOURCE
    mesh_marker.mesh_resource = "/home/sheng/Desktop/ros-kitti-project-master/ros-kitti-project-master/Audi R8/Models/AudiR8.dae"

    mesh_marker.pose.position.x = 0.0
    mesh_marker.pose.position.y = 0.0
    mesh_marker.pose.position.z = -1.73

    q = tf.transformations.quaternion_from_euler(np.pi / 2, 0, np.pi)
    mesh_marker.pose.orientation.x = q[0]
    mesh_marker.pose.orientation.y = q[1]
    mesh_marker.pose.orientation.z = q[2]
    mesh_marker.pose.orientation.w = q[3]

    mesh_marker.color.r = 1.0
    mesh_marker.color.g = 1.0
    mesh_marker.color.b = 1.0
    mesh_marker.color.a = 1.0

    mesh_marker.scale.x = 0.9
    mesh_marker.scale.y = 0.9
    mesh_marker.scale.z = 0.9

    model_pub.publish(mesh_marker)

def publish_imu(imu_pub, imu_data, log=False):
    """
    Publish IMU data
    http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html
    """
    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))  # prevent the data from being overwritten
    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)
    if log:
        rospy.loginfo("imu msg published")
#
def publish_gps(gps_pub, gps_data, log=False):
    """
    Publish GPS data
    """
    gps = NavSatFix()
    gps.header.frame_id = FRAME_ID
    gps.header.stamp = rospy.Time.now()
    gps.latitude = gps_data.lat
    gps.longitude = gps_data.lon
    gps.altitude = gps_data.alt

    gps_pub.publish(gps)
    if log:
        rospy.loginfo("gps msg published")

if __name__ == "__main__":
    # 创建发布节点
    rospy.init_node('kitti_node', anonymous = True)
    cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
    ego_pub = rospy.Publisher('kitti_ego_car', Marker, queue_size=10)
    model_pub = rospy.Publisher('kitti_car_model', Marker, queue_size=10)
    imu_pub = rospy.Publisher('kitti_imu', Imu, queue_size=10)
    gps_pub = rospy.Publisher('kitti_gps', NavSatFix, queue_size=10)

    bridge = CvBridge()
    rate = rospy.Rate(10)
    frame = 0
    while not rospy.is_shutdown():
        # 数据读取
        img = cv2.imread(os.path.join(Data_path, "image_02/data/%010d.png" % frame))
        point_cloud = np.fromfile(os.path.join(Data_path,'velodyne_points/data/%010d.bin' %frame), dtype=np.float32).reshape(-1,4)
        imu_data = read_imu(os.path.join(Data_path, "oxts/data/%010d.txt" % frame))


        frame = (frame + 1) % 108
        print(type(img))
        # 图像节点发布
        cam_pub.publish(bridge.cv2_to_imgmsg(img , 'bgr8'))

        # 点云节点发布
        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = 'map'
        pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))

        # 自车的视野角度显示
        publish_ego_car(ego_pub)

        # 自车的模型显示
        # publish_car_model(model_pub)

        # imu发布
        publish_imu(imu_pub, imu_data)

        # Gps发布
        publish_gps(gps_pub, imu_data)


        rospy.loginfo("camera image & point published")
        rate.sleep()