uas fix #639: Remove Boost::signals2 from UAS

mavros/include/mavros/mavros_plugin.h

@@ -118,6 +118,20 @@ class PluginBase
 			}
 		};
 	}
+
+	/**
+	 * Common callback called on connection change
+	 */
+	virtual void connection_cb(bool connected) {
+		ROS_BREAK();
+	}
+
+	/**
+	 * Shortcut for connection_cb() registration
+	 */
+	inline void enable_connection_cb() {
+		m_uas->add_connection_change_handler(std::bind(&PluginBase::connection_cb, this, std::placeholders::_1));
+	}
 };
 }	// namespace plugin
 }	// namespace mavros

mavros/include/mavros/mavros_uas.h

@@ -30,9 +30,6 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/NavSatFix.h>
 
-// XXX decide what to do with UAS signals
-#include <boost/signals2.hpp>
-
 namespace mavros {
 /**
  * @brief helper accessor to FCU link interface
@@ -64,6 +61,7 @@ namespace mavros {
  */
 class UAS {
 public:
+	using ConnectionCb = std::function<void(bool)>;
 	using lock_guard = std::lock_guard<std::recursive_mutex>;
 	using unique_lock = std::unique_lock<std::recursive_mutex>;
 
@@ -86,15 +84,6 @@ class UAS {
 	 */
 	diagnostic_updater::Updater diag_updater;
 
-	/**
-	 * @brief This signal emit when status was changed
-	 *
-	 * @param bool connection status
-	 *
-	 * XXX will be removed
-	 */
-	boost::signals2::signal<void(bool)> sig_connection_changed;
-
 	/**
 	 * @brief Return connection status
 	 */
@@ -114,6 +103,11 @@ class UAS {
 	 */
 	void update_connection_status(bool conn_);
 
+	/**
+	 * @brief Add connection change handler callback
+	 */
+	void add_connection_change_handler(ConnectionCb cb);
+
 	/**
 	 * @brief Returns vehicle type
 	 */
@@ -328,6 +322,7 @@ class UAS {
 	uint8_t target_component;
 
 	std::atomic<bool> connected;
+	std::vector<ConnectionCb> connection_cb_vec;
 
 	sensor_msgs::Imu::Ptr imu_data;
 

mavros/src/lib/mavros.cpp

@@ -117,9 +117,8 @@ MavRos::MavRos() :
 	mav_uas.set_tgt(tgt_system_id, tgt_component_id);
 	UAS_FCU(&mav_uas) = fcu_link;
 
-	// XXX!!!
-	mav_uas.sig_connection_changed.connect(boost::bind(&MavlinkDiag::set_connection_status, &fcu_link_diag, _1));
-	mav_uas.sig_connection_changed.connect(boost::bind(&MavRos::log_connect_change, this, _1));
+	mav_uas.add_connection_change_handler(std::bind(&MavlinkDiag::set_connection_status, &fcu_link_diag, std::placeholders::_1));
+	mav_uas.add_connection_change_handler(std::bind(&MavRos::log_connect_change, this, std::placeholders::_1));
 
 	// prepare plugin lists
 	// issue #257 2: assume that all plugins blacklisted

mavros/src/lib/uas_data.cpp

@@ -52,10 +52,18 @@ void UAS::update_connection_status(bool conn_)
 {
 	if (conn_ != connected) {
 		connected = conn_;
-		sig_connection_changed(connected);
+
+		// call all change cb's
+		for (auto &cb : connection_cb_vec)
+			cb(conn_);
 	}
 }
 
+void UAS::add_connection_change_handler(UAS::ConnectionCb cb)
+{
+	lock_guard lock(mutex);
+	connection_cb_vec.push_back(cb);
+}
 
 /* -*- autopilot version -*- */
 

mavros/src/plugins/3dr_radio.cpp

@@ -40,8 +40,7 @@ class TDRRadioPlugin : public plugin::PluginBase {
 
 		status_pub = nh.advertise<mavros_msgs::RadioStatus>("radio_status", 10);
 
-		// XXX!!!
-		m_uas->sig_connection_changed.connect(boost::bind(&TDRRadioPlugin::connection_cb, this, _1));
+		enable_connection_cb();
 	}
 
 	Subscriptions get_subscriptions()
@@ -147,7 +146,8 @@ class TDRRadioPlugin : public plugin::PluginBase {
 		stat.addf("Fixed", "%u", last_status->fixed);
 	}
 
-	void connection_cb(bool connected) {
+	void connection_cb(bool connected) override
+	{
 		UAS_DIAG(m_uas).removeByName("3DR Radio");
 		diag_added = false;
 	}

mavros/src/plugins/imu_pub.cpp

@@ -88,8 +88,8 @@ class IMUPubPlugin : public plugin::PluginBase {
 		press_pub = imu_nh.advertise<sensor_msgs::FluidPressure>("atm_pressure", 10);
 		imu_raw_pub = imu_nh.advertise<sensor_msgs::Imu>("data_raw", 10);
 
-		// XXX! reset has_* flags on connection change
-		m_uas->sig_connection_changed.connect(boost::bind(&IMUPubPlugin::connection_cb, this, _1));
+		// reset has_* flags on connection change
+		enable_connection_cb();
 	}
 
 	Subscriptions get_subscriptions() {
@@ -362,7 +362,7 @@ class IMUPubPlugin : public plugin::PluginBase {
 		press_pub.publish(atmp_msg);
 	}
 
-	void connection_cb(bool connected)
+	void connection_cb(bool connected) override
 	{
 		has_hr_imu = false;
 		has_scaled_imu = false;

mavros/src/plugins/param.cpp

@@ -360,8 +360,7 @@ class ParamPlugin : public plugin::PluginBase {
 		shedule_timer.stop();
 		timeout_timer = param_nh.createTimer(PARAM_TIMEOUT_DT, &ParamPlugin::timeout_cb, this, true);
 		timeout_timer.stop();
-		//XXX!
-		m_uas->sig_connection_changed.connect(boost::bind(&ParamPlugin::connection_cb, this, _1));
+		enable_connection_cb();
 	}
 
 	Subscriptions get_subscriptions()
@@ -555,7 +554,7 @@ class ParamPlugin : public plugin::PluginBase {
 
 	/* -*- mid-level functions -*- */
 
-	void connection_cb(bool connected)
+	void connection_cb(bool connected) override
 	{
 		lock_guard lock(mutex);
 		if (connected) {

mavros/src/plugins/rc_io.cpp

@@ -42,8 +42,7 @@ class RCIOPlugin : public plugin::PluginBase {
 		rc_out_pub = rc_nh.advertise<mavros_msgs::RCOut>("out", 10);
 		override_sub = rc_nh.subscribe("override", 10, &RCIOPlugin::override_cb, this);
 
-		// XXX!
-		m_uas->sig_connection_changed.connect(boost::bind(&RCIOPlugin::connection_cb, this, _1));
+		enable_connection_cb();
 	};
 
 	Subscriptions get_subscriptions() {
@@ -195,7 +194,7 @@ class RCIOPlugin : public plugin::PluginBase {
 
 	/* -*- callbacks -*- */
 
-	void connection_cb(bool connected)
+	void connection_cb(bool connected) override
 	{
 		lock_guard lock(mutex);
 		raw_rc_in.clear();

mavros/src/plugins/sys_status.cpp

@@ -461,9 +461,6 @@ class SystemStatusPlugin : public plugin::PluginBase
 				&SystemStatusPlugin::autopilot_version_cb, this);
 		autopilot_version_timer.stop();
 
-		//XXX! subscribe to connection event
-		m_uas->sig_connection_changed.connect(boost::bind(&SystemStatusPlugin::connection_cb, this, _1));
-
 		state_pub = nh.advertise<mavros_msgs::State>("state", 10, true);
 		extended_state_pub = nh.advertise<mavros_msgs::ExtendedState>("extended_state", 10);
 		batt_pub = nh.advertise<BatteryMsg>("battery", 10);
@@ -472,6 +469,7 @@ class SystemStatusPlugin : public plugin::PluginBase
 
 		// init state topic
 		publish_disconnection();
+		enable_connection_cb();
 	}
 
 	Subscriptions get_subscriptions() {
@@ -876,7 +874,7 @@ class SystemStatusPlugin : public plugin::PluginBase
 		}
 	}
 
-	void connection_cb(bool connected)
+	void connection_cb(bool connected) override
 	{
 		has_battery_status = false;
 

mavros/src/plugins/waypoint.cpp

@@ -158,8 +158,7 @@ class WaypointPlugin : public plugin::PluginBase {
 		wp_timer.stop();
 		shedule_timer = wp_nh.createTimer(BOOTUP_TIME_DT, &WaypointPlugin::sheduled_pull_cb, this, true);
 		shedule_timer.stop();
-		//XXX!
-		m_uas->sig_connection_changed.connect(boost::bind(&WaypointPlugin::connection_cb, this, _1));
+		enable_connection_cb();
 	}
 
 	Subscriptions get_subscriptions() {
@@ -492,7 +491,7 @@ class WaypointPlugin : public plugin::PluginBase {
 		}
 	}
 
-	void connection_cb(bool connected)
+	void connection_cb(bool connected) override
 	{
 		lock_guard lock(mutex);
 		if (connected)