用ROS同时发布kitti数据集的图像和点云数据rviz显示,同时加载一个小车模型,效果如下:
图像数据如下:
#!/usr/bin/env python
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).reshap(-1,4)
定义了四个发布数据的函数:publish_camera、publish_pcl、publish_ego_car和publish_car_model,分别用于发布图像、点云、maker标记(图中的两条绿色的线)以及车辆模型(图中的小车模型)
#!/usr/bin/env python
from pyexpat import model
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import tf
import numpy as np
import tf_conversions
FRAME_ID='map'
def publish_camera(cam_pub,bridge,image):
cam_pub.publish(bridge.cv2_to_imgmsg(image,'bgr8'))
def publish_pcl(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=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)
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_tutorial/Audi R8/Models/Audi R8.dae"
mesh_marker.mesh_resource="package://kitti_tutorial/Car-Model/Car.dae"
mesh_marker.pose.position.x=0
mesh_marker.pose.position.y=0
mesh_marker.pose.position.z=-1.73
q = tf_conversions.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)
#!/usr/bin/env python
import os
from data_utils import *
from publish_utils import *
DATA_PATH='/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/'
if __name__=='__main__':
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',Marker,queue_size=10)
model_pub=rospy.Publisher('kitti_car_model',Marker,queue_size=10)
bridge=CvBridge()
rate=rospy.Rate(10)
frame=0
while not rospy.is_shutdown():
img=cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
pcl=np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame),dtype=np.float32).reshape(-1,4)
publish_camera(cam_pub,bridge,img)
publish_pcl(pcl_pub,pcl)
publish_ego_car(ego_pub)
publish_car_model(model_pub)
rospy.loginfo('published')
rate.sleep()
frame+=1
frame%=154
google搜索car dae,从里面找免费的小车模型即可,需要找带有.dae后缀的模型,放到功能包下,在publish_utils.py的publish_car_model函数中修改路径: mesh_marker.mesh_resource="package://kitti_tutorial/Car-Model/Car.dae"
,其中路径开头是package://
,kitti_tutorial是我的功能包。
如果运行代码之后发现小车没有加载进来,并且rviz报错:Could not load resource [package://kitti_tutorial/Car_Model/Car.dae]: Unable to open file "package://kitti_tutorial/Car_Model/Car.dae".
,这大概率是运行rviz的终端没有将环境变量添加进来,source一下就可以了。