ROS 发布kitti数据集的imu信息，并用rviz显示

  用ROS发布kitti数据集的imu信息，并用rviz显示，效果如下（紫色的箭头）：

1.准备数据

2.读取数据：data_utils.py

#!/usr/bin/env python

import cv2
import numpy as np
import pandas as pd
from sensor_msgs.msg import Imu


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

3.发布数据：publish_utils.py

#!/usr/bin/env python

from pyexpat import model
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2,Imu
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from visualization_msgs.msg import Marker,MarkerArray
from geometry_msgs.msg import Point
import tf
import numpy as np
import tf_conversions



FRAME_ID='map'

def publish_camera(cam_pub,bridge,image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image,'bgr8'))

def publish_pcl(pcl_pub,point_cloud):
    header=Header()
    header.stamp=rospy.Time.now()
    header.frame_id=FRAME_ID
    pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_cloud[:,:3]))


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))
    marker_array.markers.append(marker)
#######################################################
    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="package://kitti_tutorial/Car-Model/Car.dae"

    mesh_marker.pose.position.x=0
    mesh_marker.pose.position.y=0
    mesh_marker.pose.position.z=-1.73
    q = tf_conversions.transformations.quaternion_from_euler(0,0,np.pi/2)
    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


    marker_array.markers.append(mesh_marker)


    ego_car_pub.publish(marker_array)

def publish_imu(imu_pub,imu_data):
    imu=Imu()
    imu.header.frame_id=FRAME_ID
    imu.header.stamp=rospy.Time.now()

    q = tf_conversions.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)

4.编写发布节点：KITTI.py

#!/usr/bin/env python



import os

from data_utils import *
from publish_utils import *



DATA_PATH='/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/'

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_pcl',PointCloud2,queue_size=10)
    ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)
    imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)
    bridge=CvBridge()
    rate=rospy.Rate(10)
    frame=0

    while not rospy.is_shutdown():
        img=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        pcl=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
        imu=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
        publish_camera(cam_pub,bridge,img)
        publish_pcl(pcl_pub,pcl)
        publish_ego_car(ego_pub)
        publish_imu(imu_pub,imu)
        rospy.loginfo('published')
        rate.sleep()
        frame+=1
        frame%=154