From 11797d4d22b5c0ce83f85814eddaf9458e0624d9 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sat, 24 Dec 2016 22:02:47 +0300 Subject: [PATCH] uas fix #639: Remove Boost::signals2 from UAS --- mavros/include/mavros/mavros_plugin.h | 14 ++++++++++++++ mavros/include/mavros/mavros_uas.h | 19 +++++++------------ mavros/src/lib/mavros.cpp | 5 ++--- mavros/src/lib/uas_data.cpp | 10 +++++++++- mavros/src/plugins/3dr_radio.cpp | 6 +++--- mavros/src/plugins/imu_pub.cpp | 6 +++--- mavros/src/plugins/param.cpp | 5 ++--- mavros/src/plugins/rc_io.cpp | 5 ++--- mavros/src/plugins/sys_status.cpp | 6 ++---- mavros/src/plugins/waypoint.cpp | 5 ++--- 10 files changed, 46 insertions(+), 35 deletions(-) diff --git a/mavros/include/mavros/mavros_plugin.h b/mavros/include/mavros/mavros_plugin.h index 74772cd17..36ddb4a8e 100644 --- a/mavros/include/mavros/mavros_plugin.h +++ b/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 diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index 855ad276e..084f3d698 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -30,9 +30,6 @@ #include #include -// XXX decide what to do with UAS signals -#include - namespace mavros { /** * @brief helper accessor to FCU link interface @@ -64,6 +61,7 @@ namespace mavros { */ class UAS { public: + using ConnectionCb = std::function; using lock_guard = std::lock_guard; using unique_lock = std::unique_lock; @@ -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 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 connected; + std::vector connection_cb_vec; sensor_msgs::Imu::Ptr imu_data; diff --git a/mavros/src/lib/mavros.cpp b/mavros/src/lib/mavros.cpp index 211c76960..aa55a1489 100644 --- a/mavros/src/lib/mavros.cpp +++ b/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 diff --git a/mavros/src/lib/uas_data.cpp b/mavros/src/lib/uas_data.cpp index 798ad7dce..f6c92e2f1 100644 --- a/mavros/src/lib/uas_data.cpp +++ b/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 -*- */ diff --git a/mavros/src/plugins/3dr_radio.cpp b/mavros/src/plugins/3dr_radio.cpp index b7de38500..1b4fe09c2 100644 --- a/mavros/src/plugins/3dr_radio.cpp +++ b/mavros/src/plugins/3dr_radio.cpp @@ -40,8 +40,7 @@ class TDRRadioPlugin : public plugin::PluginBase { status_pub = nh.advertise("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; } diff --git a/mavros/src/plugins/imu_pub.cpp b/mavros/src/plugins/imu_pub.cpp index c38f60516..23f3559a4 100644 --- a/mavros/src/plugins/imu_pub.cpp +++ b/mavros/src/plugins/imu_pub.cpp @@ -88,8 +88,8 @@ class IMUPubPlugin : public plugin::PluginBase { press_pub = imu_nh.advertise("atm_pressure", 10); imu_raw_pub = imu_nh.advertise("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; diff --git a/mavros/src/plugins/param.cpp b/mavros/src/plugins/param.cpp index 2f889ecd5..1c356087a 100644 --- a/mavros/src/plugins/param.cpp +++ b/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) { diff --git a/mavros/src/plugins/rc_io.cpp b/mavros/src/plugins/rc_io.cpp index 0420c99bb..aea3ea68d 100644 --- a/mavros/src/plugins/rc_io.cpp +++ b/mavros/src/plugins/rc_io.cpp @@ -42,8 +42,7 @@ class RCIOPlugin : public plugin::PluginBase { rc_out_pub = rc_nh.advertise("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(); diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index 2bea24223..ae933e2cc 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/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("state", 10, true); extended_state_pub = nh.advertise("extended_state", 10); batt_pub = nh.advertise("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; diff --git a/mavros/src/plugins/waypoint.cpp b/mavros/src/plugins/waypoint.cpp index f06453ac6..f5a1eda71 100644 --- a/mavros/src/plugins/waypoint.cpp +++ b/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)