机器人操作系统（ROS）课件8.2光流模块的测速及定位实验

机器人操作系统（ROS）机器人操作系统（ROS）多传感器数据同步8.2机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用PX4FLOW是一款用于无人机的光流智能相机，它的自然分辨率为752x480像素，并在400Hz下计算4倍剪切区域的光流，使其具有很高的光敏度。此外它还配有超声波传感器，可测得模块离地高度。PX4FLOW光流模块的实物图如图所示。PX4FLOW实物图机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用进入工作空间的src目录下，可以通过以下指令将px-ros-pkg功能包下载到该目录下，也可以拷贝本书源码到工作空间：$cd~/catkin_ws/src$gitclone/cvg/px-ros-pkg.git在工作空间中进行编译：$cd~/catkin_ws/$catkin_make

在接入PX4FLOW光流模块前查看一下设备端口：$ls/dev机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用将PX4FLOW通过USB线连接到电脑USB端口，如图所示。再次通过该命令查看设备端口，根据接入前后端口的变化，确定PX4FLOW的端口名称，所用设备的端口名称为ttyACM0，如图所示。PX4FLOW连接示意图查看设备号机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用

进入px-ros-pkg/drivers/px4flow/launch/目录下，打开px4flow_parameters.yaml文件，将serial_port:/dev/ttyS0中的ttyS0改为你所使用的PX4FLOW的端口名称，本节所用设备的端口名称为ttyACM0，故改为serial_port:/dev/ttyACM0。修改完的配置文件如下：serial_port:/dev/ttyACM0baudrate:115200通过以下命令修改/dev/ttyACM0串口权限为可读可写可执行：$sudochmod777/dev/ttyACM0机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用运行launch文件启动节点：$roslaunchpx4flowpx4flow.launch运行launch文件运行以下命令查看发布的话题，可以看到该节点发布/px4flow/camera_image和/px4flow/opt_flow两个话题：

查看话题机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用运行rqt_image_view/px4flow/camera_image，查看PX4FLOW的图像，调节焦距直至清晰（实际操作中可通过拍摄书中的印刷的字体进行对焦，直至可以看清字迹为止）：$rqt_image_view/px4flow/camera_imagePX4FLOW图像

机器人操作系统（ROS）8.2.1PX4FLOW光流模块的使用通过以下命令查看/px4flow/opt_flow话题消息，运行结果如图所示。$rostopicecho/px4flow/opt_flowPX4FLOW消息格式

其中，ground_distance是超声波模块测得的距离地面高度，测距范围应大于0.3m，若小于0.3m则均输出0.3m左右的数据；flow_x和flow_y为x和y方向上移动的像素位移；velocity_x和velocity_y为x和y方向上速度；quality表示图像质量，取值范围为0-255，数值越大表示图像质量越好（纹理丰富），若为0则表示图像不可用。此时将PX4FLOW朝向纹理较好的地面，并水平移动它，就可以看到数据的变化，如图所示。移动后数据变化图机器人操作系统（ROS）8.2.2PX4FLOW位置解算并显示

本节不仅使用了PX4FLOW光流模块，还结合了IMU提供的信息进行位置解算，使用的IMU型号为R6093U，将本书提供的IMU的解算代码（文件名read_gyro_data.py）复制到工作空间即可。PX4FLOW位置解算代码optical_flow_xy_test.py：#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportmathfromgeometry_msgs.msgimportPoseStampedfrompx_comm.msgimportOpticalFlowfromnav_msgs.msgimportPath#位置发布，话题为/px4flow/position，数据类型为PoseStamped，队列长度为10px4flow_pub=rospy.Publisher('/px4flow/position',PoseStamped,queue_size=10)#轨迹发布，话题为/px4flow/path，数据类型为Path，队列长度为1000px4path_pub=rospy.Publisher('/px4flow/path',Path,queue_size=1000)#定义各参数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=[]#定义四元数temp=PoseStamped()temp.pose.orientation.x=0.0temp.pose.orientation.y=0.0temp.pose.orientation.z=0.0temp.pose.orientation.w=1.0机器人操作系统（ROS）8.2.2PX4FLOW位置解算并显示#定义回调函数defpx4flow_callback(px4flow): globalx globaly globalcurrent_time globallast_time globaldt globalyaw globaldelta_x globaldelta_y #记录当前时间

current_time=rospy.get_time() vx=px4flow.velocity_x vy=px4flow.velocity_y dt=current_time-last_time #角度单位转换

yaw=yaw*math.pi/180 #距离计算

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相关参数

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相关参数

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回调函数defimu_callback(angle): globalyaw yaw=angle.pose.position.z#定义一个listenerdeflistener(): globalcurrent_time globallast_time #初始化节点

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!")机器人操作系统（ROS）8.2.2PX4FLOW位置解算并显示将PX4FLOW和IMU安装在实验小车上，如图所示：实验示意图

运行launch文件启动节点：$roslaunchpx4flowpx4flow.launch

通过如下命令修改IMU所用端口（IMU所用设备的端口名称为ttyUSB0）权限为可读可写可执行，每次拔插IMU模块之后需重新设置:$sudochmod777/dev/ttyUSB0

运行IMU数据发布节点，若失败，重新拔插IMU的串口，再次运行节点，运行成功结果如图所示：$rosrunpx4flowread_gyro_data.py运行成功结果图机器人操作系统（ROS）8.2.2PX4FLOW位置解算并显示运行PX4FLOW位置解算节点optical_flow_xy_test.py，运行结果如图所示：$rosrunpx4flowoptical_flow_xy_test.py运行如下命令，可以看到如图所示的话题消息，即PX4FLOW在x和y方向上的位置分量：$rostopicecho/px4flow/position运行PX4FLOW位置解算节点结果图/px4flow/position位置消息机器人操作系统（ROS）8.2.2PX4FLOW位置解算并显示运行如下命令打开rviz界面，在Displays中点击左下角的add按钮，分别添加Image和Path话题，将GlobalOptions的FixedFrame设置为path，将Path的Topic设置为/px4flow/path，将Image的Topic设置为/px4flow/camera_image，详细配置如图所示。$rosrunrvizrviz运行PX4FLOW位置解算节点optical_flow_xy_test.py，运行结果如图所示：$rosrunpx4flow