機(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ū)域的光流，使其具有很高的光敏度。此外它還配有超聲波傳感器，可測得模塊離地高度。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是超聲波模塊測得的距離地面高度，測距范圍應(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ì)列長度為10px4flow_pub=rospy.Publisher('/px4flow/position',PoseStamped,queue_size=10)#軌跡發(fā)布，話題為/px4flow/path，數(shù)據(jù)類型為Path，隊(duì)列長度為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é)果如圖所示：$rosrunpx4flow