環境:ubuntu16.04,ros-kinetic,python2,vscode,opencv,rviz
概要:這節課筆記,新增展示的是,讀取kitti資料集的gps資料,釋出到ros裡面,并在終端顯示。
資料準備及預處理可參考部落格,
https://blog.csdn.net/qq_45701501/article/details/116447770
1、源碼及解析
包建立不了解的,可以百度一下或者檢視個人之前的部落格。
這裡為了便于檔案的編寫,我們把
讀取
kitti資料檔案,
釋出函數
定義檔案,
執行
檔案分成三個檔案,分别對應
data_utils.py,publish_utils.py,p6_kitti.py
。
這裡和上一節課筆記對比,最大差別就是在
publish_utils.py
檔案中增加一個釋出gps資料的函數。
對于讀取資料檔案,
data_utils.py
:
#!/usr/bin/env python
# -*- coding:utf8 -*-
import cv2
import numpy as np
import os
import pandas as pd #用于讀取imu資料
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'
]#根據kitti資料集中的名稱進行定義的,個人了解是對照c裡面的宏定義
#讀取圖檔路徑函數
def read_camera(path):
return cv2.imread(path)
#讀取點雲路徑函數
def read_point_cloud(path):
return np.fromfile(path,dtype=np.float32).reshape(-1,4)
#讀取imu資料
def read_imu(path):
df=pd.read_csv(path,header=None,sep=' ')#讀取資料
df.columns=IMU_COLUMN_NAMES#給資料賦予機關
return df
對于釋出函數定義檔案,
publish_utils.py
:
#!/usr/bin/env python
# -*- coding:utf8 -*-
import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray#Marker繪制相機視野訓示線子產品,MarkerArray解決Marker帶來釋出的不同步問題
from sensor_msgs.msg import Image,PointCloud2,Imu,NavSatFix
from geometry_msgs.msg import Point#Point來自ros包定義,是以需要定義;若不清楚,則需要到ros官網上面檢視具體那個包
import sensor_msgs.point_cloud2 as pcl2
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_clond):
header=Header()
header.stamp=rospy.Time.now()
header.frame_id=FRAME_ID
pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_clond[:,:3]))
#釋出相機視野以及車子模型marker函數
def publish_ego_car(ego_car_pub):
#publish left and right 45 degree FOV lines and ego car model mesh
marker_array=MarkerArray()#解決marker釋出不同步問題
marker=Marker()
marker.header.frame_id=FRAME_ID
marker.header.stamp=rospy.Time.now()
marker.id=0#每個marker隻能有一個id,有重複的id,隻會顯示一個
marker.action=Marker.ADD#表示添加marker
marker.lifetime=rospy.Duration()#lifetime表示marker在畫面中顯示的時長;Duration()函數,不給任何參數時,表示一直存在
marker.type=Marker.LINE_STRIP#所釋出marker的類型
#設定訓示線顔色
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#大小,這裡表示線的粗細
#根據雷射點雲的坐标系來定義2号相機的視野範圍
marker.points=[]
marker.points.append(Point(10,-10,0))#Point,屬于ros的資料包裡面的定義,是以需要導入
marker.points.append(Point(0,0,0))
marker.points.append(Point(10,10,0))
marker_array.markers.append(marker)#将訓示線marker放到MarkerArray中
#釋出車子外形函數
mesh_marker=Marker()
mesh_marker.header.frame_id=FRAME_ID
mesh_marker.header.stamp=rospy.Time.now()
mesh_marker.id=-1#id隻能設定整數,不能設定帶有小數的
mesh_marker.lifetime=rospy.Duration()
mesh_marker.type=Marker.MESH_RESOURCE#這裡的MESH_RESOURCE表示導入的是3d模型
mesh_marker.mesh_resource="package://kitti_tutorial/Audi R8/Models/Audi R8.dae"#下載下傳的dae模型存在問題,隻是顯示部分
#設定模型位置
mesh_marker.pose.position.x=0.0
mesh_marker.pose.position.y=0.0
mesh_marker.pose.position.z=-1.73#這裡負數,是因為以雷射雷達坐标系而定義的,1.73是根據官方釋出的位置定義所取的
#設計車子模型的旋轉量
q=tf.transformations.quaternion_from_euler(0,0,np.pi/2)#(np.pi/2,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.6
mesh_marker.scale.y=0.6
mesh_marker.scale.z=0.6
marker_array.markers.append(mesh_marker)#将車子marker放到MarkerArray中
ego_car_pub.publish(marker_array)
#釋出imu資料函數
def publish_imu(imu_pub,imu_data):
imu=Imu()#ros,imu 進行google可以檢視文檔說明
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))#(np.pi/2,0,np.pi)這裡根據下載下傳的車子模型進行調整
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#根據雷達坐标系,确定x方向線性加速度
imu.linear_acceleration.y=imu_data.al#根據雷達坐标系,确定y方向線性加速度
imu.linear_acceleration.z=imu_data.au#根據雷達坐标系,确定z方向線性加速度
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)
#釋出gps資料函數
def publish_gps(gps_pub,imu_data):
gps=NavSatFix()#ros裡面對于gps資料識别包
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.alt#海拔
gps_pub.publish(gps)
對于執行檔案,
p7_kitti.py
:
#!/usr/bin/env python
# -*- coding:utf8 -*-
from data_utils import *
from publish_utils import *
DATA_PATH='/home/ylh/data/kitti/RawData/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)#建立釋出圖檔topic
pcl_pub=rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#建立釋出點雲topic
#ego_pub=rospy.Publisher('kitti_ego_car',Marker,queue_size=10)#建立釋出訓示線marker的topic
ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)#MarkerArray方式釋出
#model_pub=rospy.Publisher('kitti_car_model',Marker,queue_size=10)#建立釋出車子模型的marker的topic
imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)#建立釋出imu資料的topic
gps_pub=rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)#建立釋出gps資料的topic,NavSatFix,ros裡面固定衛星偵測
bridge=CvBridge()
rate=rospy.Rate(10)
while not rospy.is_shutdown():
#讀取圖檔
image=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
#釋出圖檔
publish_camera(cam_pub,bridge,image)
#讀取點雲
point_clond=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
#釋出點雲
publish_point_cloud(pcl_pub,point_clond)
#釋出訓示線marker;由于不需要讀取資料,是以直接釋出即可
#當采用markerarray釋出方式,則車子和訓示線都放在這個topic
#進行釋出即可。故下面的釋出車子模型marker可以删除。這樣子,可以解決不同marker釋出不同步問題
publish_ego_car(ego_pub)
#釋出車子模型marker;由于不需要讀取資料,是以直接釋出即可
#publish_car_model(model_pub)
#讀取imu資料,這裡也包含了gps資料了
imu_data=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
#釋出imu資料
publish_imu(imu_pub,imu_data)
#釋出gps資料
publish_gps(gps_pub,imu_data)
#釋出
rospy.loginfo("published")
rate.sleep()
frame+=1
frame%=154
2、檢視gps消息
終端啟動roscore,rosrun [packeageName] px_kitti.py啟動執行檔案,這些可以參考個人之前部落格,如果繼續這麼寫這裡的話,系統判斷重複了,稽核不通過。
這次釋出的gps資料,并不能在rviz裡面檢視的,但是,可以使用終端,使用指令
rostopic echo [topicName]
,如
[email protected]:~$ rostopic echo /kitti_gps
效果樣式:
header:
seq: 24
stamp:
secs: 1620545823
nsecs: 297415018
frame_id: "map"
status:
status: 0
service: 0
latitude: 49.0109308153
longitude: 8.42329153526
altitude: 112.629547119
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---
header:
seq: 25
stamp:
secs: 1620545823
nsecs: 414661884
frame_id: "map"
status:
status: 0
service: 0
latitude: 49.0109258936
longitude: 8.42329260277
altitude: 112.623275757
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---
3、注意點
1)這節課重點關注在于
publish_utils.py
裡面釋出gps資料函數
2)源碼注釋,有些可以參考之前課程筆記部落格。
#####################
學習課程來源up主,AI葵:
https://www.youtube.com/watch?v=TBdcwwr5Wyk
緻謝AI葵老師
不積矽步,無以至千裡
好記性不如爛筆頭
感覺有點收獲的話,麻煩大大們點贊收藏哈