ros使用自动驾驶数据集KITTI【5】IMU数据可视化

将两个marker放在一个MarkerArray中，然后一同发布：
publish_utils.py

#draw a ego-car model and lines of sight
def publish_ego_car(ego_car_pub):
    """Publish left and right 45 degree FOV lines and ego car model mesh"""
    marker_array = MarkerArray()

# draw a ego-car model
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()

    marker.id = 0  #each marker have only one id.
    marker.action = Marker.ADD  #to tell marker the operation is add a new marker
    marker.lifetime = rospy.Duration() #marker's life time.how long it appears in the frame.
    marker.type = Marker.LINE_STRIP # marker's type.Today we use line_strip.

    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 1.0 #apparent degree,transparency
    marker.scale.x = 0.2 #scale of line

    #marker's data
    marker.points = []
    marker.points.append(Point(10, -10, 0))
    marker.points.append(Point(0, 0, 0)) # (0,0,0)is the location of velodyne LiDAR
    marker.points.append(Point(10, 10, 0))

    marker_array.markers.append(marker)     # add marker into marker_array


#draw car model
    mesh_marker = Marker()  # build a class of Marker
    mesh_marker.header.frame_id = FRAME_ID
    mesh_marker.header.stamp = rospy.Time.now()

    mesh_marker.id = -1  # each marker have only one id.
    mesh_marker.action = Marker.ADD  # to tell marker the operation is add a new marker
    mesh_marker.lifetime = rospy.Duration()  # marker's life time.how long it appears in the frame.
    mesh_marker.type = Marker.MESH_RESOURCE  # marker's type.Today we use MESH_RESOURCE.
    # mesh_marker.mesh_resource = "package://kitti_tutorial/Audi R8/Audi R8.dae"
    mesh_marker.mesh_resource = "package://kitti_tutorial/Audi R8/BMW X5 4.dae"

    # set height
    mesh_marker.pose.position.x = 0.0
    mesh_marker.pose.position.y = 0.0
    mesh_marker.pose.position.z = -1.73

    # set rotation
    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 = 0.0
    mesh_marker.color.g = 1.0
    mesh_marker.color.b = 0.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

    marker_array.markers.append(mesh_marker)

    ego_car_pub.publish(marker_array)

kitti.py

if __name__ == '__main__':
    frame = 0
    rospy.init_node('litti_node',anonymous=True)
    cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10) #step 1: build a Publisher
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)  #PointCloud2
    ego_pub = rospy.Publisher('Kitti_ego_car',MarkerArray,queue_size=10)
    # model_pub = rospy.Publisher('Kitti_car_model',Marker,queue_size=10)
    bridge = CvBridge()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        #picture read and publish
        image = read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame)) #step 2: read in data
        publish_camera(cam_pub,bridge,image)        #step 3: publish out.  Done!
        #point cloud read and publish
        point_cloud = read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
        publish_point_cloud(pcl_pub,point_cloud)
        publish_ego_car(ego_pub)
        # publish_car_model(model_pub)

        rospy.loginfo("published")
        rate.sleep()
        frame+=1
        frame%=154

MarkerArray效果：

IMU数据：

IMU:速度，角速度，加速度等数据。参考这个资料README：
https://github.com/yanii/kitti-pcl
https://github.com/yanii/kitti-pcl/blob/master/KITTI_README.TXT

用Pandas读取IMU数据。Pandas可以把excel，csv等表格数据读取进入程序。

data_utils.py

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']

def read_imu(path):
    df = pd.read_csv(path,header = None, sep=' ')
    df.columns = IMU_COLUMN_NAMES
    return df

publish_utils.py

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']

def read_camera(path):
    return cv2.imread(path)

def read_point_cloud(path):
    return np.fromfile(path,dtype = np.float32).reshape(-1,4)

def read_imu(path):
    df = pd.read_csv(path,header = None, sep=' ')
    df.columns = IMU_COLUMN_NAMES
    return df

ROS的IMU数据用法：http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html

kitti.py

imu_pub = rospy.Publisher('kitti_imu',Imu,queue_size=10)  #step 1: build a Publisher
imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))  #step 2: read in data
publish_imu(imu_pub,imu_data)                                     		  #step 3: publish out.  Done!

IMU数据的可视化显示效果：