kitti之ros可视化_学习笔记--第4课：车子模型的添加【代码】

publish.utils.py

#!/usr/bin/env python
# -*- coding:utf8 -*-

import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker#绘制相机视野指示线模块
from sensor_msgs.msg import Image,PointCloud2
from geometry_msgs.msg import Point#Point来自ros包定义，所以需要定义;若不清楚，则需要到ros官网上面查看具体那个包
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import numpy as np
import tf

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_clond):
    header=Header()
    header.stamp=rospy.Time.now()
    header.frame_id=FRAME_ID
    pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_clond[:,:3]))


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

    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 = "package://kt_tutorial/Audi R8/Models/Audi R8.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(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

    model_pub.publish(mesh_marker)

2. data_utils.py

#!/usr/bin/env python
# -*- coding:utf8 -*-

import cv2
import numpy as np
import os

#读取图片路径函数
def read_camera(path):
    return cv2.imread(path)

#读取点云路径函数
def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshape(-1,4)

3. kitti.py

#!/usr/bin/env python
# -*- coding:utf8 -*-

from data_utils import *
from publish_utils import *
import rospy

DATA_PATH='/home/bobo/data/kitti/raw_data/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)

    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))
        publish_camera(cam_pub, bridge, image)
        publish_point_cloud(pcl_pub, point_cloud)
        publish_ego_car(ego_pub)
        publish_car_model(model_car_pub)
        rospy.loginfo("kitti published")
        rate.sleep()
        frame += 1
        frame %= 154