MAVROS二次开发（四）添加消息处理插件

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
)