機(jī)器人操作系統(tǒng)（ROS）課件 8 ROS 視覺應(yīng)用實(shí)例

機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）機(jī)器視覺8.1機(jī)器人操作系統(tǒng)（ROS）8.1機(jī)器視覺機(jī)器視覺作為人工智能技術(shù)的一個(gè)重要分支，近些年發(fā)展迅速。其主要原理為通過計(jì)算機(jī)和相機(jī)，對(duì)生物的視覺系統(tǒng)進(jìn)行模擬，將被攝目標(biāo)的信息轉(zhuǎn)換成可以被計(jì)算機(jī)進(jìn)行處理的圖像信息。并根據(jù)其圖像特征，經(jīng)由圖像處理系統(tǒng)，代替人類對(duì)目標(biāo)進(jìn)行的識(shí)別、檢測(cè)、分類、跟蹤等功能。機(jī)器人操作系統(tǒng)（ROS）8.1.1相機(jī)驅(qū)動(dòng)安裝本節(jié)使用的攝像頭品牌為RMONCAM林柏視，如圖8-1所示。$sudoapt-getinstallros-melodic-usb-cam運(yùn)行以下命令下載攝像頭驅(qū)動(dòng)(具體版本視安裝的ros版本決定)，如果系統(tǒng)已經(jīng)安裝了驅(qū)動(dòng)，則會(huì)顯示如右圖所示的結(jié)果：安裝攝像頭驅(qū)動(dòng)結(jié)果圖林柏視相機(jī)機(jī)器人操作系統(tǒng)（ROS）8.1.1相機(jī)驅(qū)動(dòng)安裝下面進(jìn)行相機(jī)測(cè)試，運(yùn)行以下命令啟動(dòng)攝像頭驅(qū)動(dòng)功能包，運(yùn)行結(jié)果如下圖所示：$roslaunchusb_camusb_cam-test.launch此時(shí)出現(xiàn)的圖像一般為電腦自帶的攝像頭，如果電腦不帶攝像頭且沒有接入攝像頭，就不會(huì)出現(xiàn)該界面。一般電腦的攝像頭默認(rèn)為video0，外部接入的USB攝像頭編號(hào)視實(shí)際情況而定（本節(jié)攝像頭設(shè)備號(hào)為video2）。機(jī)器人操作系統(tǒng)（ROS）8.1.1相機(jī)驅(qū)動(dòng)安裝如果電腦沒有自帶攝像頭，外部接入的攝像頭就會(huì)為video0，運(yùn)行以下命令查看攝像頭端口號(hào)，運(yùn)行結(jié)果如下圖所示：$ls/dev/當(dāng)電腦自帶攝像頭且外接有攝像頭時(shí)，usb_cam-test.launch只會(huì)調(diào)用自帶的攝像頭，此時(shí)如果要調(diào)用外接攝像頭，需運(yùn)行以下命令修改usb_cam-test.launch，運(yùn)行結(jié)果如下圖所示。機(jī)器人操作系統(tǒng)（ROS）8.1.1相機(jī)驅(qū)動(dòng)安裝將video_device的value從“/dev/video0”變成“/dev/video2”，這樣就能通過usb_cam-test.launch調(diào)用外接攝像頭，usb_cam-test.launch內(nèi)容如下所示：<launch><nodename="usb_cam"pkg="usb_cam"type="usb_cam_node"output="screen"><paramname="video_device"value="/dev/video0"/><paramname="image_width"value="640"/><paramname="image_height"value="480"/><paramname="pixel_format"value="yuyv"/><paramname="camera_frame_id"value="usb_cam"/><paramname="io_method"value="mmap"/></node></launch>修改完usb_cam-test.launch，運(yùn)行以下命令重新啟動(dòng)攝像頭驅(qū)動(dòng)功能包，此時(shí)啟動(dòng)的攝像頭是外接攝像頭，運(yùn)行結(jié)果如右圖所示：$roslaunchusb_camusb_cam-test.launch機(jī)器人操作系統(tǒng)（ROS）8.1.2通過rviz顯示圖像首先通過如下命令查看攝像頭發(fā)布的話題列表，運(yùn)行結(jié)果如圖8-7所示：$rostopiclist可以看到/usb_cam/image_raw是攝像頭功能包發(fā)布的圖像話題。機(jī)器人操作系統(tǒng)（ROS）8.1.2通過rviz顯示圖像運(yùn)行以下命令打開rviz：$rosrunrvizrviz點(diǎn)擊左下角Add，選擇Image，確定并將其勾選，rviz添加Image界面如圖8-9所示機(jī)器人操作系統(tǒng)（ROS）8.1.2通過rviz顯示圖像然后修改Image的ImageTopic為/usb_cam/image_raw，即可通過rviz調(diào)用攝像頭顯示圖像，顯示結(jié)果如圖所示。機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定我們所處的世界是三維的，而照片是二維的。相機(jī)的內(nèi)參標(biāo)定就是假定相機(jī)符合針孔相機(jī)模型，通過內(nèi)參的標(biāo)定求解模型的參數(shù)，以達(dá)到用簡(jiǎn)單的數(shù)學(xué)模型來表達(dá)復(fù)雜成像過程的目的，并求解出成像的反過程。而且由于鏡頭徑向曲率產(chǎn)生的徑向畸變，以及相機(jī)組裝過程中不能使透鏡嚴(yán)格和成像平面平行而產(chǎn)生的切向畸變，會(huì)在圖像的數(shù)據(jù)處理中產(chǎn)生誤差，因此需要針對(duì)攝像頭的參數(shù)進(jìn)行標(biāo)定。本節(jié)采用的是基于ROS的可對(duì)單目和雙目相機(jī)標(biāo)定的camera_calibration功能包。機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定在進(jìn)行標(biāo)定之前，需要先準(zhǔn)備一個(gè)尺寸已知的標(biāo)定板。本書采用如下圖所示的棋盤格標(biāo)定板，每個(gè)棋盤格的邊長(zhǎng)為0.024m。實(shí)際標(biāo)定過程中檢測(cè)的為標(biāo)定板內(nèi)部的角點(diǎn)，標(biāo)定的size也就是內(nèi)部交叉點(diǎn)的個(gè)數(shù)，而不是方形格的個(gè)數(shù)，因此該棋盤格的size為9*6。除了標(biāo)定板的size，還需要知道標(biāo)定板每個(gè)棋盤格的邊長(zhǎng)square，準(zhǔn)備好了標(biāo)定板之后，就能開始標(biāo)定過程了。棋盤格標(biāo)定板相機(jī)內(nèi)存標(biāo)定-標(biāo)定板準(zhǔn)備機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定運(yùn)行以下命令安裝攝像頭標(biāo)定功能包c(diǎn)amera_calibration，運(yùn)行結(jié)果如圖所示：$sudoapt-getinstallros-melodic-camera-calibration

安裝標(biāo)定功能包相機(jī)內(nèi)存標(biāo)定-camera_calibration功能表配置機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定在標(biāo)定前，通過以下命令啟動(dòng)攝像頭：$roslaunchusb_camusb_cam-test.launch運(yùn)行以下命令啟動(dòng)標(biāo)定程序，對(duì)攝像頭進(jìn)行標(biāo)定，啟動(dòng)標(biāo)定程序結(jié)果下圖所示：$rosruncamera_calibrationcameracalibrator.py--size9x6--square0.024image:=/usb_cam/image_rawcamera:=/usb_cam

啟動(dòng)標(biāo)定程序其中--size后面為使用的標(biāo)定板的內(nèi)部角點(diǎn)的個(gè)數(shù)，--square為每個(gè)棋盤格的邊長(zhǎng)，單位為米，image:=/usb_cam/image_raw表示標(biāo)定時(shí)訂閱的圖像來自于/usb_cam/image_raw的話題，camera:=/usb_cam表示的當(dāng)前攝像頭名，具體的參數(shù)請(qǐng)根據(jù)使用的標(biāo)定板和攝像頭進(jìn)行選擇。相機(jī)內(nèi)存標(biāo)定-啟動(dòng)標(biāo)定節(jié)點(diǎn)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定進(jìn)入標(biāo)定界面后，將標(biāo)定板放在攝像頭下，就可以開始標(biāo)定了，標(biāo)定界面如圖所示。相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定標(biāo)定界面右邊有四個(gè)進(jìn)度條，標(biāo)定沒有成功時(shí)，右下角的三個(gè)按鈕都是不能使用的，通過將標(biāo)定板左右移動(dòng)來提高X進(jìn)度條的進(jìn)度，將標(biāo)定板上下移動(dòng)來提高Y進(jìn)度條的進(jìn)度，標(biāo)定過程如圖所示。相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定將標(biāo)定板前后移動(dòng)來提高Size進(jìn)度條的進(jìn)度，將標(biāo)定板左右上下傾斜來提高Skew的進(jìn)度條，標(biāo)定過程如下圖所示。相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定通過不斷變更位置來提高標(biāo)定進(jìn)度條進(jìn)度指導(dǎo)四個(gè)進(jìn)度條都達(dá)到滿意的效果，標(biāo)定結(jié)束如下圖所示。相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定最后按下右下角的CALIBRATE按鈕對(duì)內(nèi)參系數(shù)和畸變系數(shù)進(jìn)行計(jì)算，計(jì)算過程中不要關(guān)閉標(biāo)定界面，參數(shù)計(jì)算完成后，SAVE和COMMIT按鈕會(huì)亮起，點(diǎn)擊SAVE按鈕對(duì)結(jié)果數(shù)據(jù)進(jìn)行保存，此時(shí)在終端中也會(huì)出現(xiàn)標(biāo)定的結(jié)果，如下圖所示。相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定點(diǎn)擊COMMIT按鈕可以提交數(shù)據(jù)并退出標(biāo)定界面，并在~/.ros/camera_info/文件夾下生成標(biāo)定參數(shù)配置文件head_camera.yaml如圖所示，此時(shí)重新打開相機(jī)，將自動(dòng)加載該配置文件，矯正相機(jī)畸變，相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.3相機(jī)內(nèi)參標(biāo)定根據(jù)終端中輸出的路徑可以找到保存的標(biāo)定參數(shù)，標(biāo)定參數(shù)保存路徑如下圖所示。根據(jù)上述路徑打開壓縮包c(diǎn)alibrationdata.tar.gz，里面放著標(biāo)定過程的圖片以及存放標(biāo)定結(jié)果的ost.txt和ost.yaml文件，這里的ost.yaml文件就是所需要的內(nèi)參標(biāo)定結(jié)果文件，如圖所示。相機(jī)內(nèi)存標(biāo)定-標(biāo)定相機(jī)參數(shù)機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)OpenCV作為開源的計(jì)算機(jī)視覺庫(kù)，由一系列的C函數(shù)和少量的C++類構(gòu)成，實(shí)現(xiàn)了圖像處理和計(jì)算機(jī)視覺方面的很多通用算法，能夠運(yùn)行于Windows、Linux、macOS等操作系統(tǒng)中,由于我們使用的ROS版本為Melodic，在該版本中ROS會(huì)默認(rèn)安裝OpenCV3，可以通過以下命令查看OpenCV是否安裝成功以及其版本：$python>>>importcv2>>>cv2.__version__當(dāng)出現(xiàn)如圖所示結(jié)果時(shí)，就說明OpenCV被正常安裝，對(duì)應(yīng)的版本為OpenCV3.2.0：

若未安裝OpenCV，可以運(yùn)行如下命令安裝：$sudo

apt-get

install

ros-melodic-vision-opencv

libopencv-dev

python-opencv人臉識(shí)別實(shí)驗(yàn)-Opencv配置機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)人臉識(shí)別實(shí)驗(yàn)-初始化函數(shù)部分def__init__(self):rospy.on_shutdown(self.cleanup);#創(chuàng)建cv_bridge，由于在Opencv中，圖像是以Mat矩陣的形式儲(chǔ)存的，這與ROS的圖像消息的格式有所區(qū)別，因此需要通過cv_bridge將其聯(lián)系起來。self.bridge=CvBridge()self.image_pub=rospy.Publisher("cv_bridge_image",Image,queue_size=1)

#獲取haar特征的級(jí)聯(lián)表的XML文件，設(shè)置在啟動(dòng)文件中。cascade_1=rospy.get_param("~cascade_1","")cascade_2=rospy.get_param("~cascade_2","")

#使用級(jí)聯(lián)表初始化haar特征檢測(cè)器。self.cascade_1=cv2.CascadeClassifier(cascade_1)self.cascade_2=cv2.CascadeClassifier(cascade_2)

#設(shè)置級(jí)聯(lián)表的參數(shù)，優(yōu)化人臉識(shí)別，可以在launch文件中重新配置。self.haar_scaleFactor=rospy.get_param("~haar_scaleFactor",1.2)self.haar_minNeighbors=rospy.get_param("~haar_minNeighbors",2)self.haar_minSize=rospy.get_param("~haar_minSize",40)self.haar_maxSize=rospy.get_param("~haar_maxSize",60)self.color=(50,255,50)

#初始化圖像數(shù)據(jù)的訂閱者，使圖像的話題名能在launch文件中重映射。self.image_sub=rospy.Subscriber("input_rgb_image",Image,self.image_callback,queue_size=1)機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)人臉識(shí)別實(shí)驗(yàn)-圖像處理函數(shù)部分defimage_callback(self,data):#使用cv_bridge將ROS的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的圖像格式。try:cv_image=self.bridge.imgmsg_to_cv2(data,"bgr8")frame=np.array(cv_image,dtype=np.uint8)exceptCvBridgeError,e:printe

#將圖像轉(zhuǎn)化成灰度圖。grey_image=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)#平衡直方圖以減少光線影響。grey_image=cv2.equalizeHist(grey_image)#嘗試檢測(cè)人臉faces_result=self.detect_face(grey_image#在opencv的窗口中框出所有人臉區(qū)域。iflen(faces_result)>0:forfaceinfaces_result:x,y,w,h=facecv2.rectangle(cv_image,(x,y),(x+w,y+h),self.color,2

#將識(shí)別后的圖像轉(zhuǎn)換成ROS消息并發(fā)布。self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image,"bgr8"))機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)人臉識(shí)別實(shí)驗(yàn)-人臉識(shí)別函數(shù)部分defdetect_face(self,input_image):#正面人臉的模型匹配。ifself.cascade_1:faces=self.cascade_1.detectMultiScale(input_image,self.haar_scaleFactor,self.haar_minNeighbors,cv2.CASCADE_SCALE_IMAGE,(self.haar_minSize,self.haar_maxSize))#側(cè)面人臉的模型匹配。iflen(faces)==0andself.cascade_2:faces=self.cascade_2.detectMultiScale(input_image,self.haar_scaleFactor,self.haar_minNeighbors,cv2.CASCADE_SCALE_IMAGE,self.haar_minSize,self.haar_maxSize))returnfaces機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)人臉識(shí)別進(jìn)行人臉識(shí)別的代碼在本書所附的代碼庫(kù)中，將其復(fù)制到工作空間。接下來運(yùn)行以下命令啟動(dòng)相機(jī)節(jié)點(diǎn)：$roslaunchusb_camusb_cam-test.launch運(yùn)行以下命令，進(jìn)行人臉識(shí)別，運(yùn)行結(jié)果如圖所示：$roslaunchrobot_visionface_detector.launch機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)人臉識(shí)別進(jìn)行人臉識(shí)別的代碼在本書所附的代碼庫(kù)中，將其復(fù)制到工作空間。接下來運(yùn)行以下命令啟動(dòng)相機(jī)節(jié)點(diǎn)：$roslaunchusb_camusb_cam-test.launch運(yùn)行以下命令，進(jìn)行人臉識(shí)別，運(yùn)行結(jié)果如圖所示：$roslaunchrobot_visionface_detector.launch機(jī)器人操作系統(tǒng)（ROS）8.1.4人臉識(shí)別實(shí)驗(yàn)人臉識(shí)別運(yùn)行以下命令啟動(dòng)rviz：$rosrunrvizrviz在rviz中添加Image，并將Image的ImageTopic修改成cv_bridge_image，就能看到如圖所示的人臉識(shí)別結(jié)果。機(jī)器人操作系統(tǒng)（ROS）8.1.5本節(jié)小結(jié)本節(jié)我們學(xué)習(xí)了相機(jī)的使用方法和標(biāo)定方法，通過相機(jī)標(biāo)定得到相機(jī)內(nèi)參，從而校正相機(jī)畸變，并基于ROS+OpenCV實(shí)現(xiàn)了人臉識(shí)別，讀者也可以通過ROS+OpenCV實(shí)現(xiàn)物體的跟蹤、二維碼的識(shí)別等。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）多傳感器數(shù)據(jù)同步8.2機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用PX4FLOW是一款用于無人機(jī)的光流智能相機(jī)，它的自然分辨率為752x480像素，并在400Hz下計(jì)算4倍剪切區(qū)域的光流，使其具有很高的光敏度。此外它還配有超聲波傳感器，可測(cè)得模塊離地高度。PX4FLOW光流模塊的實(shí)物圖如圖所示。PX4FLOW實(shí)物圖機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用進(jìn)入工作空間的src目錄下，可以通過以下指令將px-ros-pkg功能包下載到該目錄下，也可以拷貝本書源碼到工作空間：$cd~/catkin_ws/src$gitclone/cvg/px-ros-pkg.git在工作空間中進(jìn)行編譯：$cd~/catkin_ws/$catkin_make

在接入PX4FLOW光流模塊前查看一下設(shè)備端口：$ls/dev機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用將PX4FLOW通過USB線連接到電腦USB端口，如圖所示。再次通過該命令查看設(shè)備端口，根據(jù)接入前后端口的變化，確定PX4FLOW的端口名稱，所用設(shè)備的端口名稱為ttyACM0，如圖所示。PX4FLOW連接示意圖查看設(shè)備號(hào)機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用

進(jìn)入px-ros-pkg/drivers/px4flow/launch/目錄下，打開px4flow_parameters.yaml文件，將serial_port:/dev/ttyS0中的ttyS0改為你所使用的PX4FLOW的端口名稱，本節(jié)所用設(shè)備的端口名稱為ttyACM0，故改為serial_port:/dev/ttyACM0。修改完的配置文件如下：serial_port:/dev/ttyACM0baudrate:115200通過以下命令修改/dev/ttyACM0串口權(quán)限為可讀可寫可執(zhí)行：$sudochmod777/dev/ttyACM0機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用運(yùn)行l(wèi)aunch文件啟動(dòng)節(jié)點(diǎn)：$roslaunchpx4flowpx4flow.launch運(yùn)行l(wèi)aunch文件運(yùn)行以下命令查看發(fā)布的話題，可以看到該節(jié)點(diǎn)發(fā)布/px4flow/camera_image和/px4flow/opt_flow兩個(gè)話題：

查看話題機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用運(yùn)行rqt_image_view/px4flow/camera_image，查看PX4FLOW的圖像，調(diào)節(jié)焦距直至清晰（實(shí)際操作中可通過拍攝書中的印刷的字體進(jìn)行對(duì)焦，直至可以看清字跡為止）：$rqt_image_view/px4flow/camera_imagePX4FLOW圖像

機(jī)器人操作系統(tǒng)（ROS）8.2.1PX4FLOW光流模塊的使用通過以下命令查看/px4flow/opt_flow話題消息，運(yùn)行結(jié)果如圖所示。$rostopicecho/px4flow/opt_flowPX4FLOW消息格式

其中，ground_distance是超聲波模塊測(cè)得的距離地面高度，測(cè)距范圍應(yīng)大于0.3m，若小于0.3m則均輸出0.3m左右的數(shù)據(jù)；flow_x和flow_y為x和y方向上移動(dòng)的像素位移；velocity_x和velocity_y為x和y方向上速度；quality表示圖像質(zhì)量，取值范圍為0-255，數(shù)值越大表示圖像質(zhì)量越好（紋理豐富），若為0則表示圖像不可用。此時(shí)將PX4FLOW朝向紋理較好的地面，并水平移動(dòng)它，就可以看到數(shù)據(jù)的變化，如圖所示。移動(dòng)后數(shù)據(jù)變化圖機(jī)器人操作系統(tǒng)（ROS）8.2.2PX4FLOW位置解算并顯示

本節(jié)不僅使用了PX4FLOW光流模塊，還結(jié)合了IMU提供的信息進(jìn)行位置解算，使用的IMU型號(hào)為R6093U，將本書提供的IMU的解算代碼（文件名read_gyro_data.py）復(fù)制到工作空間即可。PX4FLOW位置解算代碼optical_flow_xy_test.py：#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportmathfromgeometry_msgs.msgimportPoseStampedfrompx_comm.msgimportOpticalFlowfromnav_msgs.msgimportPath#位置發(fā)布，話題為/px4flow/position，數(shù)據(jù)類型為PoseStamped，隊(duì)列長(zhǎng)度為10px4flow_pub=rospy.Publisher('/px4flow/position',PoseStamped,queue_size=10)#軌跡發(fā)布，話題為/px4flow/path，數(shù)據(jù)類型為Path，隊(duì)列長(zhǎng)度為1000px4path_pub=rospy.Publisher('/px4flow/path',Path,queue_size=1000)#定義各參數(shù)x=0.0y=0.0delta_x=0.0delta_y=0.0current_time=0.0last_time=0.0dt=0.0yaw=0.0path_msg=Path()path_temp_poses=[]#定義四元數(shù)temp=PoseStamped()temp.pose.orientation.x=0.0temp.pose.orientation.y=0.0temp.pose.orientation.z=0.0temp.pose.orientation.w=1.0機(jī)器人操作系統(tǒng)（ROS）8.2.2PX4FLOW位置解算并顯示#定義回調(diào)函數(shù)defpx4flow_callback(px4flow): globalx globaly globalcurrent_time globallast_time globaldt globalyaw globaldelta_x globaldelta_y #記錄當(dāng)前時(shí)間

current_time=rospy.get_time() vx=px4flow.velocity_x vy=px4flow.velocity_y dt=current_time-last_time #角度單位轉(zhuǎn)換

yaw=yaw*math.pi/180 #距離計(jì)算

delta_x=(vx*math.cos(yaw)-vy*math.sin(yaw))*dt delta_y=(vy*math.cos(yaw)+vx*math.sin(yaw))*dt x+=delta_x y+=delta_y #定義px4flow_msg相關(guān)參數(shù)

px4flow_msg=PoseStamped() px4flow_msg.header.frame_id="px4flow_position" px4flow_msg.header.stamp=rospy.Time.now() px4flow_msg.pose.position.x=x px4flow_msg.pose.position.y=y px4flow_msg.pose.orientation=temp.pose.orientation px4flow_pub.publish(px4flow_msg) #定義path相關(guān)參數(shù)

path_msg.header.frame_id="path" path_msg.header.stamp=rospy.Time.now() path_temp_poses.append(px4flow_msg) path_msg.poses=path_temp_poses px4path_pub.publish(path_msg) rospy.loginfo("px4flowdatapub!") last_time=current_time#定義imu回調(diào)函數(shù)defimu_callback(angle): globalyaw yaw=angle.pose.position.z#定義一個(gè)listenerdeflistener(): globalcurrent_time globallast_time #初始化節(jié)點(diǎn)

rospy.init_node('listener',anonymous=True) current_time=rospy.get_time() last_time=rospy.get_time() #訂閱/px4flow/opt_flow話題

px4flow_sub=rospy.Subscriber('/px4flow/opt_flow',OpticalFlow,px4flow_callback) #訂閱/imu/angle話題

angle_sub=rospy.Subscriber('/imu/angle',PoseStamped,imu_callback) rospy.loginfo("successfullyinitialized!") rospy.spin()if__name__=='__main__’: listener() rospy.loginfo("datasuccessfullysaved!")機(jī)器人操作系統(tǒng)（ROS）8.2.2PX4FLOW位置解算并顯示將PX4FLOW和IMU安裝在實(shí)驗(yàn)小車上，如圖所示：實(shí)驗(yàn)示意圖

運(yùn)行l(wèi)aunch文件啟動(dòng)節(jié)點(diǎn)：$roslaunchpx4flowpx4flow.launch

通過如下命令修改IMU所用端口（IMU所用設(shè)備的端口名稱為ttyUSB0）權(quán)限為可讀可寫可執(zhí)行，每次拔插IMU模塊之后需重新設(shè)置:$sudochmod777/dev/ttyUSB0

運(yùn)行IMU數(shù)據(jù)發(fā)布節(jié)點(diǎn)，若失敗，重新拔插IMU的串口，再次運(yùn)行節(jié)點(diǎn)，運(yùn)行成功結(jié)果如圖所示：$rosrunpx4flowread_gyro_data.py運(yùn)行成功結(jié)果圖機(jī)器人操作系統(tǒng)（ROS）8.2.2PX4FLOW位置解算并顯示運(yùn)行PX4FLOW位置解算節(jié)點(diǎn)optical_flow_xy_test.py，運(yùn)行結(jié)果如圖所示：$rosrunpx4flowoptical_flow_xy_test.py運(yùn)行如下命令，可以看到如圖所示的話題消息，即PX4FLOW在x和y方向上的位置分量：$rostopicecho/px4flow/position運(yùn)行PX4FLOW位置解算節(jié)點(diǎn)結(jié)果圖/px4flow/position位置消息機(jī)器人操作系統(tǒng)（ROS）8.2.2PX4FLOW位置解算并顯示運(yùn)行如下命令打開rviz界面，在Displays中點(diǎn)擊左下角的add按鈕，分別添加Image和Path話題，將GlobalOptions的FixedFrame設(shè)置為path，將Path的Topic設(shè)置為/px4flow/path，將Image的Topic設(shè)置為/px4flow/camera_image，詳細(xì)配置如圖所示。$rosrunrvizrviz運(yùn)行PX4FLOW位置解算節(jié)點(diǎn)optical_flow_xy_test.py，運(yùn)行結(jié)果如圖所示：$rosrunpx4flowoptical_flow_xy_test.pyrviz配置界面機(jī)器人操作系統(tǒng)（ROS）8.2.2PX4FLOW位置解算并顯示移動(dòng)小車即可在右側(cè)界面中顯示出運(yùn)動(dòng)軌跡，如圖所示。如果看不見軌跡，可能是因?yàn)檐壽E的距離相比于網(wǎng)格來說太小，可以通過調(diào)整網(wǎng)格數(shù)量和大小，并滑動(dòng)鼠標(biāo)滾輪進(jìn)行觀察:PX4FLOW運(yùn)動(dòng)軌跡機(jī)器人操作系統(tǒng)（ROS）8.2.3本節(jié)小結(jié)本節(jié)中學(xué)習(xí)了PX4FLOW光流模塊的使用，根據(jù)PX4FLOW輸出的速度信息，并結(jié)合IMU輸出的姿態(tài)信息來解算PX4FLOW的當(dāng)前位置，實(shí)現(xiàn)基于光流法的移動(dòng)機(jī)器人定位。機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）基于Autoware的目標(biāo)檢測(cè)實(shí)驗(yàn)8.1機(jī)器人操作系統(tǒng)（ROS）8.3.1

Autoware簡(jiǎn)介Autoware是世界上第一個(gè)用于自動(dòng)駕駛汽車的“all-in-one”開源軟件，基于ROS-1操作系統(tǒng)，并在Apache2許可下使用，提供了一套豐富的自驅(qū)動(dòng)模塊，由感知、計(jì)算和驅(qū)動(dòng)功能組成，支持功能包括：3D定位、3D映射、目標(biāo)檢測(cè)和跟蹤、交通燈識(shí)別、任務(wù)和運(yùn)動(dòng)規(guī)劃、軌跡生成、車道檢測(cè)和選擇、車輛控制、傳感器融合、攝像機(jī)、LiDAR、雷達(dá)、深度學(xué)習(xí)、基于規(guī)則的系統(tǒng)、連接導(dǎo)航、日志記錄、虛擬現(xiàn)實(shí)等，ROS框架結(jié)構(gòu)如下圖。機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware首先運(yùn)行如下命令安裝Autoware的依賴：$sudoapt-getupdate$sudoapt-getinstall-ypython-catkin-pkgpython-rosdepros-$ROS_DISTRO-catkingksu$sudoapt-getinstall-ypython3-pippython3-colcon-common-extensionspython3-setuptoolspython3-vcstool$pip3

install

-U

setuptools

-i

/simple

--trusted-host

依賴安裝完成后，建立autoware的工作空間：$mkdir-pautoware/src$cdautoware運(yùn)行如下命令下載版本autoware1.12.0的配置文件：$wget-Oautoware.ai.repos"/autowarefoundation/autoware.ai/autoware/raw/1.12.0/autoware.ai.repos?inline=false"機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware運(yùn)行如下命令將autoware安裝到工作空間中，如下圖所示：$vcsimportsrc<autoware.ai.repos正常情況下，終端會(huì)出現(xiàn)一行’··········’，代表安裝過程沒有問題，如果在指令后出現(xiàn)’···E·······’或者多個(gè)’·’變成了’E’，則說明在連接到的時(shí)候出現(xiàn)錯(cuò)誤，此時(shí)可以考慮更換網(wǎng)絡(luò)或者重復(fù)嘗試幾次，直到所有的包都下載安裝成功安裝autoware無報(bào)錯(cuò)情況安裝autoware出現(xiàn)報(bào)錯(cuò)情況機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware接下來通過rosdep安裝依賴項(xiàng)，首先進(jìn)行rosdep的初始化：$sudorosdepinit此時(shí)若出現(xiàn)報(bào)錯(cuò)顯示不能下載，如下圖所示，主要是因?yàn)榫W(wǎng)絡(luò)問題。進(jìn)行rosdep出現(xiàn)報(bào)錯(cuò)情況此時(shí)按照以下操作進(jìn)行修改，采用/進(jìn)行g(shù)ithub的資源代理，以此來解決不能下載資源的問題：機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（1）修改sources_list.py目錄下的download_rosdep_data函數(shù)，然后添加如下內(nèi)容：$cd/usr/lib/python2.7/dist-packages/rosdep2

$sudogeditsources_list.py

具體位置如下:defdownload_rosdep_data(url):""":raises::exc:`DownloadFailure`Ifdatacannotberetrieved(e.g.404,badYAMLformat,serverdown)."""try:

url="/"+url#http/httpsURLsneedcustomrequeststospecifytheuser-agent,sincesomerepositoriesreject#requestsfromthedefaultuser-agent.ifurl.startswith("http://")orurl.startswith("https://"):url_request=request.Request(url,headers={'User-Agent':'rosdep/{version}'.format(version=__version__)})機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（2）修改/usr/lib/python2.7/dist-packages/rosdistro目錄下的__init__.py文件：$cd/usr/lib/python2.7/dist-packages/rosdistro$sudogedit__init__.py

在打開的文件中，修改原有的DEFAULT_INDEX_URL，在原來的URL連接前加入‘/’:#sameversionasin:#-setup.py#-stdeb.cfg__version__='0.8.3'

#indexinformation

DEFAULT_INDEX_URL='//ros/rosdistro/master/index-v4.yaml'

defget_index_url():#environmentvariablehasprecedenceoverconfigurationfilesif'ROSDISTRO_INDEX_URL'inos.environ:returnos.environ['ROSDISTRO_INDEX_URL']...機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（3）修改/usr/lib/python2.7/dist-packages/rosdep2目錄下的gbpdistro_support.py文件：$cd/usr/lib/python2.7/dist-packages/rosdep2$sudogeditgbpdistro_support.py修改文件中的FUERTE_GBPDISTRO_URL：#py3ktry:unicodeexceptNameError:basestring=unicode=str

#locationofanexamplegbpdistrofileforreferenceandtestingFUERTE_GBPDISTRO_URL='//ros/rosdistro/'\'master/releases/fuerte.yaml'

#secondstowaitbeforeabortingdownloadofgbpdistrodataDOWNLOAD_TIMEOUT=15.0機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（4）修改/usr/lib/python2.7/dist-packages/rosdep2目錄下的sources_list.py文件：修改文件中的DEFAULT_SOURCES_LIST_URL：#defaultfiletodownloadwith'init'commandinordertobootstrap#rosdepDEFAULT_SOURCES_LIST_URL='//ros/rosdistro/master/rosdep/sources.list.d/20-default.list'

#secondstowaitbeforeabortingdownloadofrosdepdataDOWNLOAD_TIMEOUT=15.0$cd/usr/lib/python2.7/dist-packages/rosdep2$sudogeditsources_list.py機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（5）修改/usr/lib/python2.7/dist-packages/rosdep2目錄下的rep3.py文件：修改文件中的REP3_TARGETS_URL：from.coreimportDownloadFailurefrom.rosdistrohelperimportPreRep137Warning

#locationoftargetsfileforprocessinggbpdistrofilesREP3_TARGETS_URL='//ros/rosdistro/master/releases/targets.yaml'

#secondstowaitbeforeabortingdownloadofgbpdistrodataDOWNLOAD_TIMEOUT=15.0$cd/usr/lib/python2.7/dist-packages/rosdep2$sudogeditrep3.py機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（6）修改/usr/lib/python2.7/dist-packages/rosdistro/manifest_provider目錄下的github.py文件：修改github.py文件中第68行的url地址：ifnotrepo.has_remote_tag(release_tag):raiseRuntimeError('specifiedtag"%s"isnotagittag'%release_tag)

url='//%s/%s/package.xml'%(path,release_tag)try:logger.debug('Loadpackage.xmlfilefromurl"%s"'%url)return_get_url_contents(url)exceptURLErrorase:logger.debug('-failed(%s),trying"%s"'%(e,url))raiseRuntimeError()$cd/usr/lib/python2.7/dist-packages/rosdistro/manifest_provider$sudogeditgithub.py機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理同時(shí)，修改github.py文件中第119行的url地址：cache=SourceRepositoryCache.from_ref(tree_json['sha'])forpackage_xml_pathinpackage_xml_paths:

url

='//%s/%s/%s';

%

\

(path,

cache.ref(),

package_xml_path

+

'/package.xml'

if

package_xml_path

else

'package.xml') logger.debug('-loadpackage.xmlfrom%s'%url)package_xml=_get_url_contents(url)name=parse_package_string(package_xml).namecache.add(name,package_xml_path,package_xml)returncache機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-GitHub資源代理（7）修改/usr/lib/python2.7/dist-packages/rosdep2目錄下的gbpdistro_support.py文件：在文件的第202行下添加“gbpdistro_url=‘/’+gbpdistro_url”：#wecanconvertagbpdistrofileintorosdepdatabyfollowinga#couplerules#willoutputawarningtargets_data=download_targets_data(targets_url=targets_url)

gbpdistro_url='/'+gbpdistro_urltry:f=urlopen(gbpdistro_url,timeout=DOWNLOAD_TIMEOUT)text=f.read()f.close()gbpdistro_data=yaml.safe_load(text)#willoutputawarningreturngbprepo_to_rosdep_data(gbpdistro_data,targets_data,gbpdistro_url)exceptExceptionase:raiseDownloadFailure('Failedtodownloadtargetplatformdata''forgbpdistro:\n\t'+str(e))$cd/usr/lib/python2.7/dist-packages/rosdep2$sudogeditgbpdistro_support.py機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-rosdep初始化所有文件修改完成后，重新運(yùn)行如下命令進(jìn)行rosdep的初始化，運(yùn)行結(jié)果如下圖所示：$sudorosdepinit機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware安裝配置Autoware-rosdep初始化rosdep初始化成功后，運(yùn)行如下命令進(jìn)行rosdep更新，如下圖所示：$rosdepupdate如果整個(gè)更新過程如上圖一樣沒有出現(xiàn)報(bào)錯(cuò)，則更新成功，如果出現(xiàn)報(bào)錯(cuò)，則需要檢測(cè)一下在前文的修改文件過程中是否出現(xiàn)缺漏或者錯(cuò)誤，如果檢查無誤依然無法更新成功，則可以考慮切換網(wǎng)絡(luò)進(jìn)行更新。機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware然后運(yùn)行如下命令采用rosdep安裝依賴，如下圖所示：$cd~/autoware/$rosdepinstall-y--from-pathssrc--ignore-src--rosdistro$ROS_DISTRO安裝配置Autoware-rosdep初始化機(jī)器人操作系統(tǒng)（ROS）8.3.2安裝配置Autoware若部分依賴無法安裝，如下圖所示：安裝配置Autoware-rosdep初始化則可以采用手動(dòng)安裝的方式，如nmea_msgs無法安裝成功，則