赞
踩
自定义MAVROS消息需要首先通过源码安装MAVROS。Ubuntu18.04下MAVROS源码安装教程见https://blog.csdn.net/real86/article/details/131891303?spm=1001.2014.3001.5501
在源码安装的MAVROS工作空间中创建msg消息。具体的,在~/mavros_catkin_ws/src/mavros/mavros_msgs/msg文件夹下新建TargetDetection.msg文件,该消息文件是我们要传输的消息的数据类型,在文件中写入:
- std_msgs/Header header
-
- int8 targetsDetected
- uint8 num_targets
在本例中,targetsDetected为检测到目标框的标志位,num_targets为检测到目标框的数量。
在~/mavros_catkin_ws/src/mavros/mavros_msgs目录下的CmakeLists.txt文件中添加TargetDetection.msg文件,具体如下:
- add_message_files(
- ...
- TargetDetection.msg
- )
利用catkin build在~/mavros_catkin_ws目录下编译,确保在~/mavros_catkin_ws/devel/include/mavros_msgs目录下生成了TargetDetection.h头文件。
在~/mavros_catkin_ws/src/mavlink/message_definition/v1.0目录下,修改common.xml文件,添加自定义的mavros消息,具体如下:
- <message id="227" name="TARGET_DETECTION">
- <description>the detection message from nx to vehicle.</description>
- <field type="int8_t" name="targetsDetected">int8_t</field>
- <field type="uint8_t" name="num_targets">uint8_t</field>
- </message>
在~/mavros_catkin_ws/src/mavlink/message_definitions目录下,使用MAVLINK的GUI代码mavgenerate.py生成MAVLINK库文件,具体如下:
python -m mavgenerate
在弹出的GUI中选择如下:
- XML: ~/mavros_catkin_ws/src/mavlink/message_definitions/v1.0/common.xml
- OUT: ~/mavros_catkin_ws/src/mavlink/message_definitions/v1.0
- Language: C
- Protocol: 1.0
- Validate: 勾选
点击Generate后,会在~/mavros_catkin_ws/src/mavlink/message_definitions/v1.0目录下生成common、minimal、standard文件夹。
为了将自定义的MAVROS消息发送出去,还需要编写插件。具体的,在~/mavros_catkin_ws/src/mavros/mavros_extras/src/plugins目录下创建target_detection.cpp文件,具体如下:
- #include <mavros/mavros_plugin.h>
- #include <pluginlib/class_list_macros.h>
- #include <iostream>
- #include <mavros_msgs/TargetDetection.h>
-
- namespace mavros {
- namespace extra_plugins{
-
- class TargetDetectionPlugin : public plugin::PluginBase {
- public:
- TargetDetectionPlugin() : PluginBase(),
- nh("~target_detection"){ };
-
- void initialize(UAS &uas_)
- {
- PluginBase::initialize(uas_);
- mavros2fcu_sub = nh.subscribe("send_data", 10, &TargetDetectionPlugin::TargetDetection_cb, this);
- };
-
- Subscriptions get_subscriptions()
- {
- return {/* RX disabled */ };
- }
-
- private:
- ros::NodeHandle nh;
- ros::Subscriber mavros2fcu_sub;
-
- void TargetDetection_cb(const mavros_msgs::TargetDetection::ConstPtr &req)
- {
- mavros::UAS *m_uas_ = static_cast<TargetDetectionPlugin *>(this)->m_uas;
-
- mavlink::common::msg::TARGET_DETECTION send_data = {};
-
- send_data.targetsDetected = req->targetsDetected;
-
- send_data.num_targets = req->num_targets;
-
- UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
- }
- };
- } // namespace extra_plugins
- } // namespace mavros
-
- PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::TargetDetectionPlugin, mavros::plugin::PluginBase)
将自定义的插件添加到插件列表中,用于MAVROS自启动插件。具体的,在~/mavros_catkin_ws/src/mavros/mavros_extras/mavros_plugins.xml中添加:
- <class name="target_detection" type="mavros::extra_plugins::TargetDetectionPlugin" base_class_type="mavros::plugin::PluginBase">
- <description>Send target detection.</description>
- </class>
在~/mavros_catkin_ws/src/mavros/mavros_extras/CmakeLists.txt中添加编译信息:
- add_library(mavros_extras
- src/plugins/target_detection.cpp
- )
在~/mavros_catkin_ws目录下进行编译,在终端输入:
catkin build
自定义的插件target_detection.cpp会订阅数据并发布。为了验证自定义MAVROS消息的正确性,需要写一个ROS节点发布供自定义的插件订阅的数据,让自定义的插件订阅到ROS节点发布的数据并发送给飞控。首先创建发布消息的ROS功能包,具体如下:
- mkdir ros_test
- cd ros_test
- mkdir src
- cd src
- catkin_init_workspace
- cd ~/ros_test
- catkin_make
- cd src
- catkin_create_pkg msg_test
在msg_test目录下创建pub_data.cpp,用于发布自定义MAVROS消息。具体如下:
- #include <time.h>
- #include <ros/ros.h>
- #include <mavros_msgs/TargetDetection.h>
-
- using namespace std;
-
- mavros_msgs::TargetDetection targets_msg;
-
- int main(int argc, char **argv){
- ros::init(argc, argv, "target_msg");
- ros::NodeHandle nh;
-
- targets_msg.targetsDetected = 1;
- targets_msg.num_targets = 6;
-
- ros::Publisher test;
- test = nh.advertise<mavros_msgs::TargetDetection>("/mavros/target_detection/send_data", 10);
-
- ros::Rate rate(20.0);
- while(ros::ok()){
- ros::spinOnce();
- rate.sleep();
- test.publish(targets_msg);
- }
- return 0;
- }
pub_data.cpp能够发布TargetDetection类型的消息,以供自定义插件target_detection.cpp订阅。
在msg_test目录下,修改CmakeLists.txt文件。在文件中添加:
- find_package(catkin REQUIRED COMPONENTS
- mavros
- roscpp
- std_msgs
- mavros_msgs
- )
-
- include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- )
-
- add_executable(target_msg pub_data.cpp)
- target_link_libraries(target_msg ${catkin_LIBRARIES})
在msg_test目录下,修改package.xml文件。在文件中添加:
- <build_depend>mavros</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>mavros_msgs</build_depend>
-
- <build_export_depend>mavros</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>std_msgs</build_export_depend>
-
- <exec_depend>mavros</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>mavros_msgs</exec_depend>
最后,在ros_test目录下编译,执行catkin_make指令。
测试流程如下:
1)在机载电脑的终端中启动roscore;
2)在~/ros_test/devel目录下依次执行:
- source setup.bash
- rosrun msg_test target_msg
开始通过ROS节点发布自定义的MAVROS消息;
3)在新终端中执行:
rostopic echo /mavros/target_detection/send_data
若输出如下,代表自定义的MAVROS消息发布成功。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。