05 | 添加车辆模型及照相机视野(kitti数据集运用)

kitti_teach.py

#!/usr/bin/env python3
import os 

from data_utils import *
from publish_utils import *

DATA_PATH = '/home/jony/data/kitti/RawData/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_pcl', PointCloud2, queue_size=10)
    ego_pub = rospy.Publisher('kitti_ego_car', MarkerArray, queue_size=10)
    bridge = CvBridge()

    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)
        rospy.loginfo("Published!")
        rate.sleep()
        frame += 1
        frame %= 154

2. publish_utils.py

#!/usr/bin/env python3
import rospy
import sensor_msgs.point_cloud2 as plc2 
import tf
import numpy as np 

from sensor_msgs.msg import Image, PointCloud2 
from cv_bridge import CvBridge 
from std_msgs.msg import Header
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point, Pose

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(plc2.create_cloud_xyz32(header, point_cloud[:, :3]))

def publish_ego_car(ego_pub):
	marker_array = MarkerArray()
	# （1）视野范围
	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

	# 初始化四元数为单位四元数（没有旋转）- 预防rviz中warn
	marker.pose.orientation.w = 1.0  # w是实部，对于单位四元数来说，w=1, x=y=z=0
	marker.pose.orientation.x = 0.0
	marker.pose.orientation.y = 0.0
	marker.pose.orientation.z = 0.0

	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)
	# （2）汽车模型
	marker2 = Marker()
	marker2.header.frame_id = FRAME_ID
	marker2.header.stamp = rospy.Time.now()

	marker2.id = -1
	marker2.lifetime = rospy.Duration()
	marker2.type = Marker.MESH_RESOURCE
	marker2.mesh_resource = "package://kitti_tutorial/bmw_x5/BMW X5 4.dae"

	marker2.pose.position.x = 0.0
	marker2.pose.position.y = 0.0
	marker2.pose.position.z = -1.73

	q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi)
	marker2.pose.orientation.x = q[0]
	marker2.pose.orientation.y = q[1]
	marker2.pose.orientation.z = q[2]
	marker2.pose.orientation.w = q[3]

	marker2.color.r = 1.0
	marker2.color.g = 1.0
	marker2.color.b = 1.0
	marker2.color.a = 1.0

	marker2.scale.x = 0.9
	marker2.scale.y = 0.9
	marker2.scale.z = 0.9

	marker_array.markers.append(marker2)
	
	ego_pub.publish(marker_array)

3. data_utils.py

#!/usr/bin/env python3
import cv2
import numpy as np 

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

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