ubuntu1804发布kitti数据集的gps资料，imu资料（包含发布图片，点云过程）

这是一套完整的学习过程，从发布图片，点云到发布gps，imu，并实现他们的可视化具体的学习链接如下，是对学习的记录，方便自己回顾，同时也希望帮到需要帮助的人。系列1：Ubuntu1804里进行KITTI数据集可视化操作_FYY2LHH的博客-CSDN博客系列2：自己编写程序publish出kitti数据集，可视化kitti数据集_FYY2LHH的博客-CSDN博客系列3：ubuntu1804自己编

FYY2LHH

601人浏览 · 2022-12-05 10:51:20

FYY2LHH · 2022-12-05 10:51:20 发布

这是一套完整的学习过程，从发布图片，点云到发布gps，imu，并实现他们的可视化

具体的学习链接如下，是对学习的记录，方便自己回顾，同时也希望帮到需要帮助的人。

系列1：Ubuntu1804里进行KITTI数据集可视化操作_FYY2LHH的博客-CSDN博客

系列2：自己编写程序publish出kitti数据集，可视化kitti数据集_FYY2LHH的博客-CSDN博客

系列3：ubuntu1804自己编写程序发布kitti数据集的点云数据_FYY2LHH的博客-CSDN博客

系列4：在rviz中利用KITTI数据集画出自己的车子以及照相机的视野_FYY2LHH的博客-CSDN博客

本章节记录内容为发布imu并实现可视化，发布gps，并查看话题发布类型

1、仍然是三个py文件，第一个publish

#!/usr/bin/env python
import rospy 
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from geometry_msgs.msg import Point
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import tf
import numpy as np
FRAME_ID = 'map'

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

def publish_point_cloud(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):
    # publish left and right 45 degree FOV lines and ego car model mesh
    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
    # line
    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 # line width

    marker.points = []

    # check the kitti axis model 
    marker.points.append(Point(5,-5,0)) # left up
    marker.points.append(Point(0,0,0)) # center
    marker.points.append(Point(5, 5,0)) # right up

    marker_array.markers.append(marker111)

    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 = "/root/catkin_ws/src/kitti_tutorial/AudiR8.dae"  #LOAD ERROR, DON'T KNOW WHY

    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")

2、data_utils.py

#!/usr/bin/env python
import cv2
import numpy as np
import pandas as pd 


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、kitti.py

#!/usr/bin/env python
from data_utils import *
from publish_utils import *
import os

DATA_PATH = '/home/ros/dianyun/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync/'

if  __name__ == "__main__":
    frame = 0
    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)
    bridge = CvBridge()
    ego_pub = rospy.Publisher('kitti_ego_car',Marker, queue_size=10)
    # model_car_pub = rospy.Publisher('kitti_model_car',Marker, queue_size=10)

    imu_pub = rospy.Publisher('kitti_imu',Imu, queue_size=10)
    gps_pub = rospy.Publisher('kitti_gps',NavSatFix, queue_size=10)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        image = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
	point_cloud = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))
        imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
	publish_camera(cam_pub, bridge, image)
	publish_point_cloud(pcl_pub, point_cloud)
	publish_ego_car(ego_pub)
	# publish_car_model(model_car_pub)
	publish_imu(imu_pub, imu_data )
	publish_gps(gps_pub, imu_data ) #gps rviz cannot visulize, only use rostopic echo
        rospy.loginfo("kitti published")
        rate.sleep()
        frame += 1
        frame %= 154

三个文件的路径还是原来的的路径

4、在rviz中添加imu的话题