（三）使用Marker划出camera视野和本车模型

一、使用Marker画出Camera视野

        官方资料 http://wiki.ros.org/rviz/DisplayTypes/Marker

        使用Line Strip划出相机的两条线，从KITTI采集车辆Camera的布置可以看出 我们使用的Camera-02数据的Camera2在车辆的正中间。其视野场为90度，因此划出两条视野线。

Publish.py

from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point



def publish_ego_car(ego_car_pub):
# maker_arrary = MarkerArrary()

marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()

# 设定marker的ID 
marker.id = 0 
marker.action = Marker.ADD
# 预设无限久
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP

# 设定线的颜色
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))

ego_car_pub.publish(marker)



kitti.py
ego_pub = rospy.Publisher('kitti_ego_car', Marker ,queue_size=10)



publish_ego_car(ego_pub)

二、导入车辆模型

        在一些模型网站上可以下载想要的车辆模型或者自己制作dae文件如：https://free3d.com/3d-models/

        下载好后可以放入src文件夹当中
        使用ros自带的Mesh Resource 进行publish

import numpy as np
import tf



def publish_car_model(model_pub):
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()

# 设定marker的ID 
mesh_marker.id = -1
# 预设无限久
mesh_marker.lifetime = rospy.Duration()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "package://KITTI_tutorials/car_mdoel/PeugeotOnyxConcept.dae"

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

# 使用ros tf 对model进行旋转操作
q=tf.transformations.quaternion_from_euler(0, 0, 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.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)


kitti.py
model_pub = rospy.Publisher('kitti_car_model', Marker ,queue_size=10)

publish_car_model(model_pub)

使用MarkerArray 将上述两个Marker进行合并 可以 合并到一个def函数中。

def publish_ego_car(ego_car_pub):
marker_array = MarkerArray()

# Line_marker
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()

# 设定marker的ID 
marker.id = 0 
marker.action = Marker.ADD
# 预设无限久
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP

# 设定线的颜色
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)

# car_model_marker
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()

# 设定marker的ID 
mesh_marker.id = -1
# 预设无限久
mesh_marker.lifetime = rospy.Duration()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "package://KITTI_tutorials/car_mdoel/ShelbyWD.dae"

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

# 使用ros tf 对model进行旋转操作
q=tf.transformations.quaternion_from_euler(0, 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

marker_array.markers.append(mesh_marker)

ego_car_pub.publish(marker_array)

效果如下图：