kitties.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',MarkerArray,queue_size=10)
#创建一个IMU发布者
imu_pub = rospy.Publisher('kitti_imu',Imu,queue_size=10)
#发布点云数据
bridge = CvBridge()
gps_pub =rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)
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)
imu_data = read_imu(os.path.join(DATA_PATH, 'oxts/data/%010d.txt'%frame))
publish_imu(imu_pub,imu_data)
rospy.loginfo("published")
rate.sleep()
frame +=1
frame %=154
datautils.py
from email import header
import cv2
import numpy as np
import pandas as pd
IMU_COLUMN_NAMES = ["lat", "lon", "alt", "roll", "pitch", "yaw", "vn", "ve", "vf", "vl", "vu", "ax", "ay", "az", "af", "al", "au", "wx", "wy", "wz", "wf", "wl", "wu", "posacc", "velacc", "navstat", "numsats", "posmode", "velmode", "orimode"]
def read_camera(path):
return cv2.imread(path)
def read_point_cloud(path):
return np.fromfile(path,dtype=np.float32).reshape(-1,4)
def read_imu(path):
df = pd.read_csv(path,header=None,sep=' ')
df.columns = IMU_COLUMN_NAMES
return df
publish_utils
#如何发布资料
#!/usr/bin/env python3
#import 模块:导入一个模块;注:相当于导入的是一个文件夹,是个相对路径
import numpy as np
import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray
from sensor_msgs.msg import Image,PointCloud2,Imu,NavSatFix
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):
#maker阵列
marker_array = MarkerArray()
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))
marker_array.markers.append(marker)
#发布汽车模型
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
marker_array.markers.append(mesh_marker)
ego_car_pub.publish(marker_array)
##IMU发布函数相关设置
def publish_imu(imu_pub,imu_data):
imu =Imu()
imu.header.frame_id = FRAME_ID
imu.header.stamp = rospy.Time.now()
#设置旋转量
q=tf.transformations.quaternion_from_euler(float(imu_data.roll),float(imu_data.pitch),float(imu_data.yaw));
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
#设置线性加速度
imu.linear_acceleration.x = imu_data.af
imu.linear_acceleration.y = imu_data.al
imu.linear_acceleration.z = imu_data.au
#设置角加速度
imu.angular_velocity.x = imu_data.wf
imu.angular_velocity.y = imu_data.wl
imu.angular_velocity.z = imu_data.wu
imu_pub.publish(imu)
def publish_gps(gps_data, gps_pub):
gps = NavSatFix()
gps.header.frame_id = "map"
gps.header.stamp = rospy.Time.now()
# 赋值经纬度和海拔
gps.latitude = gps_data.lat
gps.longitude = gps_data.lon
gps.altitude = gps_data.alt
gps_pub.publish(gps)