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

1.画出两条45°的线，代表摄像头的可视范围

kitty.py

```cpp
#!/usr/bin/env python3
from data_utils import *
from publish_utils import *
import os
DATA_PATH = "/home/sk/Desktop/data/kitti/2011_09_26/2011_09_26_drive_0005_sync/"    

if __name__ == "__main__":
    frame =0
    rospy.init_node("kitti_node",anonymous=True)
    #建立publisher
    cam_pub  = rospy.Publisher("kitti_cam", Image, queue_size=10)
    pcl_pub  = rospy.Publisher("kitti_point_cloud", PointCloud2, queue_size=10)
    ego_pub = rospy.Publisher('kitti_ego_car',Marker,queue_size=10)
    #发布点云数据
    bridge = CvBridge()
    rate = rospy.Rate(10)
while not rospy.is_shutdown():
         #read the camera data
        image = read_camera(os.path.join(DATA_PATH, "image_02/data/%010d.png"%frame))
         #publish the camera data
        publish_camera(cam_pub,bridge,image)
        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)
        rospy.loginfo("published")
        rate.sleep()
        frame +=1
        frame %=154

publish.utils.py

```python
#如何发布资料
#!/usr/bin/env python3
#import 模块：导入一个模块；注：相当于导入的是一个文件夹，是个相对路径
from numpy import append
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

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):
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()
    # 每个显示的marker都需要不一样的id，否则会覆盖
    marker.id = 0
    marker.type = Marker.LINE_STRIP# 直线
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration()#永久显示
    
    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 # 线粗细

    # 这边点的数据主要看官方提供的位置图
    # 会在每两个连续点之间画一条线0-1，1-2。。。
    marker.points = []
    marker.points.append(Point(10,-10,0)) 
    marker.points.append(Point(0,0,0))
    marker.points.append(Point(10,10,0))

    ego_car_pub.publish(marker)

data.utils.py
#如何读取资料

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)

2.画出一台车子

• 2.1下载车辆模型

  根据Marker官网解释说需要使用.stl、.mesh或者.dae模型。在google上输入car dae关键词。Car dae Free3D
  并不是所有的模型都能用，以下是经过测试的可用模型
  https://free3d.com/3d-model/low-poly-car-40967.html

代码------------------------文件名在注释的第一行

#publish_utils.py
#如何发布资料
#!/usr/bin/env python3
#import 模块：导入一个模块；注：相当于导入的是一个文件夹，是个相对路径
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,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):
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()
    # 每个显示的marker都需要不一样的id，否则会覆盖
    marker.id = 0
    marker.type = Marker.LINE_STRIP# 直线
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration()#永久显示
    
    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 # 线粗细

    # 这边点的数据主要看官方提供的位置图
    # 会在每两个连续点之间画一条线0-1，1-2。。。
    marker.points = []
    marker.points.append(Point(10,-10,0)) 
    marker.points.append(Point(0,0,0))
    marker.points.append(Point(10,10,0))   

    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://kitti_turial/Audi_R8/Models/Car.dae"

    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,np.pi,-np.pi/2.0)
    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.scale.x = 1
    mesh_marker.scale.y = 1
    mesh_marker.scale.z = 1

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

    model_pub.publish(mesh_marker)

#data_utils.py
#如何读取资料
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)

#kitti.py
#!/usr/bin/env python3
from data_utils import *
from publish_utils import *
import os
DATA_PATH = "/home/sk/Desktop/data/kitti/2011_09_26/2011_09_26_drive_0005_sync/"    

if __name__ == "__main__":
    frame =0
    rospy.init_node("kitti_node",anonymous=True)
    #建立publisher
    cam_pub  = rospy.Publisher("kitti_cam", Image, queue_size=10)
    pcl_pub  = rospy.Publisher("kitti_point_cloud", PointCloud2, queue_size=10)
    ego_pub = rospy.Publisher('kitti_ego_car',Marker,queue_size=10)
    model_pub = rospy.Publisher('litti_car_model',Marker,queue_size=10)
    #发布点云数据
    bridge = CvBridge()
    rate = rospy.Rate(10)
while not rospy.is_shutdown():
         #read the camera data
        image = read_camera(os.path.join(DATA_PATH, "image_02/data/%010d.png"%frame))
         #publish the camera data
        publish_camera(cam_pub,bridge,image)
        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

最终实现的效果：