MAVROS二次开发
四、添加消息处理插件
mavros插件所在路径:~/catkin_ws/src/mavros/mavros/src/plugins
1、自定义消息处理插件的编写
- 参考代码:~/catkin_ws/src/mavros/mavros/src/plugins/manual_control.cpp(很典型)
- 插件文件名称:crawl_control.cpp
- 文件路径:~/catkin_ws/src/mavros/mavros/src/plugins/crawl_control.cpp
- 文件内容:
/**
* @brief CrawlControls plugin
* @file crawl_controls.cpp
* @author Yanfeng <962353916@qq.com>
*
* @addtogroup plugin
*/
#include <mavros/mavros_plugin.h>
#include <mavros_msgs/CrawlControlStatus.h>
#include <mavros_msgs/CrawlControlSet.h>
namespace mavros {
namespace std_plugins {
/**
* @brief Crawl Control plugin
*/
class CrawlControlPlugin : public plugin::PluginBase {
public:
CrawlControlPlugin() : PluginBase(),
crawl_control_nh("~crawl_control")
{ }
void initialize(UAS &uas_)
{
PluginBase::initialize(uas_);
control_pub = crawl_control_nh.advertise<mavros_msgs::CrawlControlStatus>("status", 10);
send_service = crawl_control_nh.advertiseService("send", &CrawlControlPlugin::send_cb, this);
}
//用来获取mavlink解析到的消息
Subscriptions get_subscriptions() {
return {
make_handler(&CrawlControlPlugin::handle_crawl_control),
};
}
private:
ros::NodeHandle crawl_control_nh;
ros::Publisher control_pub;
ros::ServiceServer send_service;
/* -*- rx handlers -*- */
//mavlink::crawl_control_status::msg::CRAWL_CONTROL_STATUS为自动生成的消息头文件中所定义的,也是依据此来解析收到的mavlink消息。
void handle_crawl_control(const mavlink::mavlink_message_t *msg, mavlink::crawl_control_status::msg::CRAWL_CONTROL_STATUS &crawl_control)
{
auto crawl_control_msg = boost::make_shared<mavros_msgs::CrawlControlStatus>();
// crawl_control_msg->header.stamp = ros::Time::now();
crawl_control_msg->sw_state = crawl_control.sw_state;
crawl_control_msg->clamp_state = crawl_control.clamp_state;
crawl_control_msg->crawl_state = crawl_control.crawl_state;
crawl_control_msg->clamp_speed = crawl_control.clamp_speed;
crawl_control_msg->crawl_speed = crawl_control.crawl_speed;
//将解析到的消息发布至topic
control_pub.publish(crawl_control_msg);
}
/* -*- callbacks -*- */
bool send_cb(mavros_msgs::CrawlControlSet::Request &req, mavros_msgs::CrawlControlSet::Response &responce)
{
mavlink::crawl_control_set::msg::CRAWL_CONTROL_SET msg;
//讲server收到的request赋值给mavlink消息
msg.clamp_state_set = req.clamp_set;
msg.crawl_state_set = req.crawl_set;
msg.clamp_speed_set = req.clamp_speed;
msg.crawl_speed_set = req.crawl_speed;
//响应发送成功
responce.send_success = true;
//调用mavlink消息发送API
UAS_FCU(m_uas)->send_message_ignore_drop(msg);
return true;
}
};
} // namespace std_plugins
} // namespace mavros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::CrawlControlPlugin, mavros::plugin::PluginBase)
2、添加自定义插件到插件列表中
- 路径:~/catkin_ws/src/mavros/mavros/mavros_plugins.xml
- 作用:用于MAVROS自动加载插件
- 添加内容:
<class name="crawl_control" type="mavros::std_plugins::CrawlControlPlugin" base_class_type="mavros::plugin::PluginBase">
<description>Handle the crawl_control messages</description>
</class>
3、添加至CMakeLists.txt
- 路径:~/catkin_ws/src/mavros/mavros/CMakeLists.txt
- 作用:将插件添加至编译
- 添加内容:
add_library(mavros_plugins
src/plugins/crawl_control.cpp #add
)