ros使用自动驾驶数据集KITTI【4】画出自己的车以及照相机视野

画出自己的车以及照相机视野

1、画出车的模型(用已有模型)
2、画出相机图片的视野范围

代码结构：
kitti.py
-data_utils.py
-publish_utils.py

data_utils.py

data_utils.py负责从数据集中读取camera数据和point cloud数据。
data_utils.py代码：

#!/usr/bin/python
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)

publish_utils.py

publish_utils.py中写了若干个publish函数，发布主题。这些函数在kitti.py中调用。

publish_utils.py代码：

#!/usr/bin/python
import numpy as np
import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
from sensor_msgs.msg import Image,PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from geometry_msgs.msg import Point
import tf

FRAME_ID = 'map'

def publish_camera(cam_pub,bridge,img):
    cam_pub.publish(bridge.cv2_to_imgmsg(img, "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]))

#draw a ego-car model
def publish_ego_car(ego_car_pub):

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

    ego_car_pub.publish(marker)

#show a car model by means of 'MESH_RESOURCE'
def publish_car_model(model_pub):
    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

    model_pub.publish(mesh_marker)

kitti.py

kitti.py中，是主函数，调用另外两个文件中的读取和发布函数，完成每个主题的发布。每个消息发布出去时，主要经过3个步骤：

• 建立一个Publisher；
• 读入数据；
• 发布主题。

kitti.py代码：

#!/usr/bin/python
import rospy
from data_utils import *
from publish_utils import *

DATA_PATH = '/home/ros/Documents/ros-kitticlass/2011_09_26/2011_09_26_drive_0005_sync'

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',Marker,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

最终效果

加入视野范围效果：

加入模型效果：