“天不生我李淳罡”
本节将在 3.2节的基础上,进行功能的添加(IMU发布),其他内容不变
工作空间是test3_autodrive_ws
python代码存在test3_autodrive_ws/src/demo1_kitti_pub_photo/scripts/kitti_all.py和publish_utils.py和data_utils.py中
数据文件存放在kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/oxts/data下,可以看到是txt文件
1. 预知识
GPS/IMU实际记录的数据可以在浏览器键入 “kitti raw data”,找到gihub的readme.txt文档查看.这里给出链接,图如下:
上图可以看出IMU/GPS资料包含:经纬度,海拔,旋转角度roll,pitch,yaw,速度,加速度,角速度等资料.今天选取一部分数据进行展示.
2. 发布GPS资料流程
2.1 建立发布者
kitti_all.py中
from sensor_msgs.msg import NavSatFix
...
gps_pub = rospy.Publisher("kitti_gps", NavSatFix, queue_size=10)
- NavSatFix(Navigation satellite Fixed)固定的卫星的资料/是GPS的消息格式
- 先定义gps发布者
- 再读取gps数据,因为GPS和IMU数据都被存放在txt下,所有读取一边imu即可获取,由于上节已经读取了imu的全部数据,这里便不用再读取,而是直接使用
2.2 读取GPS/IMU资料
先看上节课读取的txt的数据内容:
这里的内容不仅包含IMU的,也包含GPS的,所以在IMU里读取的IMU_DATA可以直接用来提取出GPS数据,即这里无需再读
2.3 发布
publish_utils中写发布函数:
from sensor_msgs.msg import Imu
- 引入消息头文件
以下仍然是比较常规的,消息实例化, 消息头填充,数据填充
这里只给出比较关键的数据填充:
def publish_gps(gps_pub, imu_data):
gps = NavSatFix()
#头填充
gps.header.frame_id = FRAME_ID
gps.header.stamp = rospy.Time.now()
#维度, 经度 和海拔
gps.latitude = imu_data.lat
gps.longitude = imu_data.lon
gps.altitude = imu_data.al
gps_pub.publish(gps)
- 其实imu包含很多参数都可以设定,但是这里只以旋转角度,线性加速度和角速度为例说明,其余参数可以参考之前的readme文档说明设定,f:forward, l:left, u:up
kitti_all.py中:
调用
publish_gps(gps_pub, imu_data)
3. 执行
- 打开roscore
- rosrun 运行该文件
- 打开rviz并合理配置,可以看到如下界面:
- 问题是,rviz中没有,gps选项,因为它是不可视化的数据
3.1从任务台可视化GPS数据
GPS作为一组不可视的数据,可以通过如下指令看到消息内容:
rostopic info /kitti_gps
内容如下,数据一直变化
4. 源码
data_utils.py与上节一致
4.1 publish_utils.py
#!/usr/bin/env python3
#coding:utf-8
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from visualization_msgs.msg import Marker, MarkerArray
import sensor_msgs.point_cloud2 as pcl2
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
import numpy as np
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.frame_id = FRAME_ID
header.stamp = rospy.Time.now()
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))
def publish_ego_car(ego_car_pub):
'publish left and right 45 degree FOV and ego car model mesh'
#header部分
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
# marker的id
marker.id = 0
marker.action = Marker.ADD # 加入一个marker
marker.lifetime = rospy.Duration() # 生存时间,()中无参数永远出现
marker.type = Marker.LINE_STRIP #marker 的type,有很多种,这里选择线条
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0 #这条线的颜色
marker.color.a = 1.0 #透明度 1不透明
marker.scale.x = 0.2 #大小,粗细
#设定marker中的资料
marker.points = []
# 两条线,三个点即可
#原点是0,0,0, 看左上角和右上角的数据要看kitti的设定,看坐标
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):
#header部分
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 #marker 的type,有很多种,这里选择mesh
mesh_marker.mesh_resource = "package://demo1_kitti_pub_photo/mesh/car_model/car.DAE"
#平移量设置
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
#以为0,0,0 是velodyne的坐标(车顶),这里坐标是车底,所以是负数
mesh_marker.pose.position.z = -1.73
#旋转量设定
q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi/2)
# 这里的参数和下载模型的预设角度有关,旋转关系,根据显示效果而调整,转成四元数q
#x轴旋转
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
#设置体积: x,y,z方向的膨胀程度
mesh_marker.scale.x = 0.4
mesh_marker.scale.y = 0.4
mesh_marker.scale.z = 0.4
model.publish(mesh_marker) #设定完毕,发布
def publish_two_marker(kitti_two_marker):
#建立markerarray
markerarray = MarkerArray()
# 绿线设定
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
# marker的id
marker.id = 0
marker.action = Marker.ADD # 加入一个marker
marker.lifetime = rospy.Duration() # 生存时间,()中无参数永远出现
marker.type = Marker.LINE_STRIP #marker 的type,有很多种,这里选择线条
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0 #这条线的颜色
marker.color.a = 1.0 #透明度 1不透明
marker.scale.x = 0.2 #大小,粗细
#设定marker中的资料
marker.points = []
# 两条线,三个点即可
#原点是0,0,0, 看左上角和右上角的数据要看kitti的设定,看坐标
marker.points.append(Point(10, -10, 0))
marker.points.append(Point(0, 0, 0))
marker.points.append(Point(10, 10, 0))
#加入第一个
markerarray.markers.append(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 #marker 的type,有很多种,这里选择mesh
mesh_marker.mesh_resource = "package://demo1_kitti_pub_photo/mesh/car_model/car.DAE"
#平移量设置
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
#以为0,0,0 是velodyne的坐标(车顶),这里坐标是车底,所以是负数
mesh_marker.pose.position.z = -1.73
#旋转量设定
q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi/2)
# 这里的参数和下载模型的预设角度有关,旋转关系,根据显示效果而调整,转成四元数q
#x轴旋转
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
#设置体积: x,y,z方向的膨胀程度
mesh_marker.scale.x = 0.4
mesh_marker.scale.y = 0.4
mesh_marker.scale.z = 0.4
# 加入第二个:车子模型
markerarray.markers.append(mesh_marker)
#发布
kitti_two_marker.publish(markerarray)
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))
# 将roll, pitch, yaw转成可被电脑识别的四元数q,并设定出去
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_pub, imu_data):
gps = NavSatFix()
#头填充
gps.header.frame_id = FRAME_ID
gps.header.stamp = rospy.Time.now()
#维度, 经度 和海拔
gps.latitude = imu_data.lat
gps.longitude = imu_data.lon
gps.altitude = imu_data.al
gps_pub.publish(gps)
4.2 kitti_all.py
#!/usr/bin/env python3
#coding:utf-8
from data_utils import *
from publish_utils import *
DATA_PATH = '/home/qinsir/kitti_folder/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_point_cloud", 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)
two_marker_pub = rospy.Publisher("kitti_two_mark", MarkerArray, queue_size=10)
imu_pub = rospy.Publisher("kitti_imu", Imu, queue_size=10) #IMU发布者
gps_pub = rospy.Publisher("kitti_gps", NavSatFix, queue_size=10)
bridge = CvBridge() #opencv支持的图片和ROS可以读取的图片之间转换的桥梁
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#使用OS,路径串接,%010d,这个字串有10个数字(比如0000000001).png
img = 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))
imu_data = read_imu(os.path.join(DATA_PATH, "oxts/data/%010d.txt"%frame))
publish_camera(cam_pub, bridge, img)
publish_point_cloud(pcl_pub, point_cloud)
#publish_ego_car(ego_pub)
#publish_car_model(model_pub)
publish_two_marker(two_marker_pub)
publish_imu(imu_pub, imu_data)
publish_gps(gps_pub, imu_data)
rospy.loginfo('new file publish')
rate.sleep()
frame += 1
frame %= 154
980
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
