MAVROS二次开发（五）进行测试

MAVROS二次开发

五、进行测试

1、测试环境

• PX4：v1.10.1（含自定义mavlink消息收发）
• ROS：Kinetic
• Ubuntu：16.04LTS
• QGC：STABLE_V3.5.6（含自定义mavllink消息收发及界面显示）

2、PX4运行在Ubuntu的相关配置

（1）/boards/px4/sitl/default.cmake

​ 将PX4中自己编写的mavlink消息发布接收的线程加入sitl编译。

EXAMPLES
		crawl_control #add 

（2）/ROMFS/px4fmu_common/init.d-posix/rcS

​ 配置MAVLINK自定义消息流，发送频率为2Hz（如果在mavlink_messages.cpp中配置了消息流就不用再配置了）

mavlink stream -r 2  -s CRAWL_CONTROL_STATUS -u $udp_gcs_port_local

（3）/ROMFS/px4fmu_common/init.d-posix/rcS

​ 开启PX4中相关mavlink消息发布接收的线程。

crawl_control start

3、运行PX4

cd <Firmware-clone> #px4源码路径
no_sim=1 make px4_sitl_default gazebo

在运行编译过程中，发现源码有报错，检查后发现当前版本源码中/Tools/sitl_gazebo/include/gazebo_opticalflow_plugin.h文件里有个宏定义写错了，布尔类型的宏定义错误的写为大写，改为小写即可，如下图。

4、使用MAVROS连接PX4

• 本地连接

roslaunch mavros px4.launch fcu_url:="udp://:[e-mail]:14557"

• 远程连接

roslaunch mavros px4.launch fcu_url:="udp://:[e-mail]:14557"

连接时可以看到控制台输出中，我们自定义的消息处理插件已经存在并且初始化成功。

5、运行ROS节点

rosrun offboard_example offb_node

6、测试rostopic

（1）检测自定义mavros消息是否存在

rostopic list

（2）接收PX4发送的自定义MAVLINK消息

​ 可以在节点中订阅此topic消息并且使用ROS_INFO函数将其打印至控制台。

7、测试rosservice

（1）检测自定义mavros消息是否存在

rosservice list

（2）向server发送请求信息使两个电机停转

rosservice call mavros/crawl_control/send 0 0 0 0

地面站将接收到的消息进行显示，可以看见电机状态发生了改变。

（3）向server发送请求信息使两个电机运行

rosservice call mavros/crawl_control/send 1 1 0 0

地面站将接收到的消息进行显示，可以看见电机状态发生了改变。