From 02c38696178949f7221f653dc75d4f2de82da41a Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sat, 11 Jan 2025 10:13:38 +1100 Subject: [PATCH 1/7] memory safe subscription callback Inherit from std::enable_shared_from_this and wrap subscription callback in lambda capturing a weak_ptr using weak_from_this() to check that Bond object is still in scope before processing the bondStatusCB member function. --- bondcpp/include/bondcpp/bond.hpp | 5 +++- bondcpp/src/bond.cpp | 42 +++++++++++++++++++++++--------- 2 files changed, 35 insertions(+), 12 deletions(-) diff --git a/bondcpp/include/bondcpp/bond.hpp b/bondcpp/include/bondcpp/bond.hpp index 16aee58..38c507c 100644 --- a/bondcpp/include/bondcpp/bond.hpp +++ b/bondcpp/include/bondcpp/bond.hpp @@ -55,7 +55,7 @@ namespace bond * another process and be notified when it dies. In turn, it will be * notified when you die. */ -class Bond +class Bond : public std::enable_shared_from_this { public: using EventCallback = std::function; @@ -239,7 +239,9 @@ class Bond rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_; rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; rclcpp::TimerBase::SharedPtr connect_timer_; rclcpp::TimerBase::SharedPtr disconnect_timer_; @@ -274,6 +276,7 @@ class Bond rclcpp::Duration heartbeat_period_; rclcpp::Duration dead_publish_period_; + rclcpp::CallbackGroup::SharedPtr sub_callback_group_; rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; }; diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 78fd541..fc77355 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -81,7 +81,9 @@ Bond::Bond( EventCallback on_formed) : node_base_(node_base), node_logging_(node_logging), + node_params_(node_params), node_timers_(node_timers), + node_topics_(node_topics), bondsm_(std::make_unique(this)), sm_(*bondsm_), topic_(topic), @@ -100,29 +102,23 @@ Bond::Bond( dead_publish_period_( rclcpp::Duration::from_seconds(bond::msg::Constants::DEAD_PUBLISH_PERIOD)) { - if (!node_params->has_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM)) { - node_params->declare_parameter( + if (!node_params_->has_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM)) { + node_params_->declare_parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, rclcpp::ParameterValue(false)); } disable_heartbeat_timeout_ = - node_params->get_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM).as_bool(); + node_params_->get_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM).as_bool(); setupConnections(); pub_ = rclcpp::create_publisher( - node_params, - node_topics, + node_params_, + node_topics_, topic_, rclcpp::QoS(rclcpp::KeepLast(5))); - sub_ = rclcpp::create_subscription( - node_params, - node_topics, - topic_, - rclcpp::QoS(100), - std::bind(&Bond::bondStatusCB, this, std::placeholders::_1)); } Bond::Bond( @@ -364,6 +360,30 @@ void Bond::deadpublishingTimerCancel() void Bond::start() { + // Need to move subcriber setup here(out of constructor) + // to allow usage of weak_from_this() + + if (!started_) { + // TBD: Should recreation of subscription be prevented? + + std::weak_ptr weakThis = weak_from_this(); + + sub_ = rclcpp::create_subscription( + node_params_, + node_topics_, + topic_, + rclcpp::QoS(100), + [weakThis](const bond::msg::Status & msg) { + if (auto strongThis = weakThis.lock()) { + strongThis->bondStatusCB(msg); + } else { + // Object has gone out of scope, handle accordingly + } + }); + } else { + RCLCPP_WARN(node_logging_->get_logger(), "start() already started skipping subscription recreation"); + } + connect_timer_reset_flag_ = true; connectTimerReset(); publishingTimerReset(); From 10ae2fe10df981ce80c5790fcb56090de31b505f Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sat, 11 Jan 2025 19:04:43 +1100 Subject: [PATCH 2/7] Create and use createSafeSubscriptionMemFuncCallback Create and use helper function to check that class is still valid before calling member function. --- .../include/bondcpp/createSafeCallback.hpp | 25 +++++++++++++++++++ bondcpp/src/bond.cpp | 24 ++++++++---------- 2 files changed, 35 insertions(+), 14 deletions(-) create mode 100644 bondcpp/include/bondcpp/createSafeCallback.hpp diff --git a/bondcpp/include/bondcpp/createSafeCallback.hpp b/bondcpp/include/bondcpp/createSafeCallback.hpp new file mode 100644 index 0000000..02f971f --- /dev/null +++ b/bondcpp/include/bondcpp/createSafeCallback.hpp @@ -0,0 +1,25 @@ + +#ifndef BONDCPP__CREATE_SAFE_CALLBACK_HPP_ +#define BONDCPP__CREATE_SAFE_CALLBACK_HPP_ + +#include +#include + +// Expects obj to inhert from std::enable_shared_from_this +template +auto createSafeSubscriptionMemFuncCallback( + std::shared_ptr obj, + void (T::*memberFunc)(const MessageT &) +) { + static_assert(std::is_base_of_v, T>, + "Type expected to inherit from std::enable_shared_from_this"); + + std::weak_ptr weak_obj = obj; + return [weak_obj, memberFunc](const MessageT & msg) { + if (auto shared_obj = weak_obj.lock()) { + ((*shared_obj).*memberFunc)(msg); + } + }; +} + +#endif // BONDCPP__CREATE_SAFE_CALLBACK_HPP_ diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index fc77355..54aa956 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -29,6 +29,7 @@ // Author: Stuart Glaser #include +#include #ifdef _WIN32 #include @@ -360,28 +361,23 @@ void Bond::deadpublishingTimerCancel() void Bond::start() { - // Need to move subcriber setup here(out of constructor) - // to allow usage of weak_from_this() + // create_subscription must be done outside of constructor + // to allow usage of shared_from_this() + // TBD: Should recreation of subscription be prevented? if (!started_) { - // TBD: Should recreation of subscription be prevented? - - std::weak_ptr weakThis = weak_from_this(); - sub_ = rclcpp::create_subscription( node_params_, node_topics_, topic_, rclcpp::QoS(100), - [weakThis](const bond::msg::Status & msg) { - if (auto strongThis = weakThis.lock()) { - strongThis->bondStatusCB(msg); - } else { - // Object has gone out of scope, handle accordingly - } - }); + createSafeSubscriptionMemFuncCallback( + shared_from_this(), + &Bond::bondStatusCB + )); } else { - RCLCPP_WARN(node_logging_->get_logger(), "start() already started skipping subscription recreation"); + RCLCPP_WARN(node_logging_->get_logger(), + "start() already started skipping subscription recreation"); } connect_timer_reset_flag_ = true; From c34050730ba2a4eb4150d0b8fc650aad9a2a0ebf Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 Jan 2025 13:57:26 +1100 Subject: [PATCH 3/7] lint --- .../include/bondcpp/createSafeCallback.hpp | 48 +++++++++++++++---- bondcpp/src/bond.cpp | 5 +- 2 files changed, 40 insertions(+), 13 deletions(-) diff --git a/bondcpp/include/bondcpp/createSafeCallback.hpp b/bondcpp/include/bondcpp/createSafeCallback.hpp index 02f971f..f29cf7a 100644 --- a/bondcpp/include/bondcpp/createSafeCallback.hpp +++ b/bondcpp/include/bondcpp/createSafeCallback.hpp @@ -1,6 +1,34 @@ +// Copyright (c) 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. -#ifndef BONDCPP__CREATE_SAFE_CALLBACK_HPP_ -#define BONDCPP__CREATE_SAFE_CALLBACK_HPP_ +/** \author Mike Wake */ +#ifndef BONDCPP__CREATESAFECALLBACK_HPP_ +#define BONDCPP__CREATESAFECALLBACK_HPP_ #include #include @@ -8,18 +36,18 @@ // Expects obj to inhert from std::enable_shared_from_this template auto createSafeSubscriptionMemFuncCallback( - std::shared_ptr obj, - void (T::*memberFunc)(const MessageT &) -) { + std::shared_ptr obj, + void (T::*memberFunc)(const MessageT &)) +{ static_assert(std::is_base_of_v, T>, "Type expected to inherit from std::enable_shared_from_this"); std::weak_ptr weak_obj = obj; return [weak_obj, memberFunc](const MessageT & msg) { - if (auto shared_obj = weak_obj.lock()) { - ((*shared_obj).*memberFunc)(msg); - } - }; + if (auto shared_obj = weak_obj.lock()) { + ((*shared_obj).*memberFunc)(msg); + } + }; } -#endif // BONDCPP__CREATE_SAFE_CALLBACK_HPP_ +#endif // BONDCPP__CREATESAFECALLBACK_HPP_ diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 54aa956..6143bf5 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -119,7 +119,6 @@ Bond::Bond( node_topics_, topic_, rclcpp::QoS(rclcpp::KeepLast(5))); - } Bond::Bond( @@ -373,8 +372,8 @@ void Bond::start() rclcpp::QoS(100), createSafeSubscriptionMemFuncCallback( shared_from_this(), - &Bond::bondStatusCB - )); + &Bond::bondStatusCB) + ); } else { RCLCPP_WARN(node_logging_->get_logger(), "start() already started skipping subscription recreation"); From 63c744b3066103f133f7c45197abb9f97a1c353d Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 Jan 2025 13:59:59 +1100 Subject: [PATCH 4/7] Fix tests, to use Bond it now MUST be created as a shared_ptr This is due to the use of createSafeSubscriptionMemFuncCallback when creating the subscription registering the bondStatusCB member function --- test_bond/test/test_callbacks_cpp.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/test_bond/test/test_callbacks_cpp.cpp b/test_bond/test/test_callbacks_cpp.cpp index 5ccb4da..e587a23 100644 --- a/test_bond/test/test_callbacks_cpp.cpp +++ b/test_bond/test/test_callbacks_cpp.cpp @@ -83,15 +83,15 @@ TEST_F(TestCallbacksCpp, dieInLifeCallback) { auto nh1 = rclcpp::Node::make_shared("test_callbacks_cpp"); std::string id1 = genId(); - bond::Bond a(TOPIC, id1, nh1); - bond::Bond b(TOPIC, id1, nh1); + auto a = std::make_shared(TOPIC, id1, nh1); + auto b = std::make_shared(TOPIC, id1, nh1); - a.setFormedCallback( + a->setFormedCallback( [&a]() { - a.breakBond(); + a->breakBond(); }); - a.start(); - b.start(); + a->start(); + b->start(); std::atomic isRunning {true}; auto runThread = std::thread( @@ -101,8 +101,8 @@ TEST_F(TestCallbacksCpp, dieInLifeCallback) } }); - EXPECT_TRUE(a.waitUntilFormed(rclcpp::Duration(5.0s))); - EXPECT_TRUE(b.waitUntilBroken(rclcpp::Duration(3.0s))); + EXPECT_TRUE(a->waitUntilFormed(rclcpp::Duration(5.0s))); + EXPECT_TRUE(b->waitUntilBroken(rclcpp::Duration(3.0s))); isRunning = false; runThread.join(); @@ -112,9 +112,9 @@ TEST_F(TestCallbacksCpp, remoteNeverConnects) { auto nh2 = rclcpp::Node::make_shared("test_callbacks_cpp_2"); std::string id2 = genId(); - bond::Bond a1(TOPIC, id2, nh2); + auto a1 = std::make_shared(TOPIC, id2, nh2); - a1.start(); + a1->start(); std::atomic isRunning {true}; auto runThread = std::thread( From cf571169f1e0e0fb710c9b8ff6803a5e17a048ff Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Tue, 14 Jan 2025 14:11:29 +1100 Subject: [PATCH 5/7] Missed use shared_ptr test --- test_bond/test/test_callbacks_cpp.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test_bond/test/test_callbacks_cpp.cpp b/test_bond/test/test_callbacks_cpp.cpp index e587a23..a700fa5 100644 --- a/test_bond/test/test_callbacks_cpp.cpp +++ b/test_bond/test/test_callbacks_cpp.cpp @@ -124,8 +124,8 @@ TEST_F(TestCallbacksCpp, remoteNeverConnects) } }); - EXPECT_FALSE(a1.waitUntilFormed(rclcpp::Duration(4.0s))); - EXPECT_TRUE(a1.waitUntilBroken(rclcpp::Duration(10.0s))); + EXPECT_FALSE(a1->waitUntilFormed(rclcpp::Duration(4.0s))); + EXPECT_TRUE(a1->waitUntilBroken(rclcpp::Duration(10.0s))); isRunning = false; runThread.join(); From d43b55568109babd0daa8cbdb6a6366ec82982d3 Mon Sep 17 00:00:00 2001 From: root Date: Thu, 23 Jan 2025 23:32:02 +0000 Subject: [PATCH 6/7] publish Status using unique_ptr, if subscribers listening --- bondcpp/src/bond.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index 6143bf5..b1df8ab 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -565,16 +565,17 @@ void Bond::doPublishing() void Bond::publishStatus(bool active) { - bond::msg::Status msg; + auto msg = std::make_unique(); rclcpp::Clock steady_clock(RCL_STEADY_TIME); rclcpp::Time now = steady_clock.now(); - msg.header.stamp = now; - msg.id = id_; - msg.instance_id = instance_id_; - msg.active = active; - msg.heartbeat_timeout = static_cast(heartbeat_timeout_.seconds()); - msg.heartbeat_period = static_cast(heartbeat_period_.seconds()); - pub_->publish(msg); + msg->header.stamp = now; + msg->id = id_; + msg->instance_id = instance_id_; + msg->active = active; + msg->heartbeat_timeout = static_cast(heartbeat_timeout_.seconds()); + msg->heartbeat_period = static_cast(heartbeat_period_.seconds()); + if(pub_->get_subscription_count() < 1){ return; } + pub_->publish(std::move(msg)); } void Bond::flushPendingCallbacks() From 564e5ec6c9b3e4331716230ca043a5acba3099ac Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Sat, 25 Jan 2025 22:35:16 +1100 Subject: [PATCH 7/7] lint --- bondcpp/include/bondcpp/createSafeCallback.hpp | 5 +++-- bondcpp/src/bond.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/bondcpp/include/bondcpp/createSafeCallback.hpp b/bondcpp/include/bondcpp/createSafeCallback.hpp index f29cf7a..4163d45 100644 --- a/bondcpp/include/bondcpp/createSafeCallback.hpp +++ b/bondcpp/include/bondcpp/createSafeCallback.hpp @@ -37,9 +37,10 @@ template auto createSafeSubscriptionMemFuncCallback( std::shared_ptr obj, - void (T::*memberFunc)(const MessageT &)) + void (T::* memberFunc)(const MessageT &)) { - static_assert(std::is_base_of_v, T>, + static_assert( + std::is_base_of_v, T>, "Type expected to inherit from std::enable_shared_from_this"); std::weak_ptr weak_obj = obj; diff --git a/bondcpp/src/bond.cpp b/bondcpp/src/bond.cpp index b1df8ab..000ddf5 100644 --- a/bondcpp/src/bond.cpp +++ b/bondcpp/src/bond.cpp @@ -375,7 +375,8 @@ void Bond::start() &Bond::bondStatusCB) ); } else { - RCLCPP_WARN(node_logging_->get_logger(), + RCLCPP_WARN( + node_logging_->get_logger(), "start() already started skipping subscription recreation"); } @@ -574,7 +575,7 @@ void Bond::publishStatus(bool active) msg->active = active; msg->heartbeat_timeout = static_cast(heartbeat_timeout_.seconds()); msg->heartbeat_period = static_cast(heartbeat_period_.seconds()); - if(pub_->get_subscription_count() < 1){ return; } + if (pub_->get_subscription_count() < 1) {return;} pub_->publish(std::move(msg)); }