Fix issue #58: use weak_ptr to avoid circular shared_ptr ownership

facontidavide · facontidavide · commit 374edcf54393 · 2024-04-30T12:51:46.000+02:00
diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
@@ -188,7 +188,20 @@ class RosActionNode : public BT::ActionNodeBase
 
   rclcpp::Logger logger()
   {
-    return node_->get_logger();
+    if(auto node = node_.lock())
+    {
+      return node->get_logger();
+    }
+    return rclcpp::get_logger("RosActionNode");
+  }
+
+  rclcpp::Time now()
+  {
+    if(auto node = node_.lock())
+    {
+      return node->now();
+    }
+    return rclcpp::Clock(RCL_ROS_TIME).now();
   }
 
   using ClientsRegistry =
@@ -200,7 +213,7 @@ class RosActionNode : public BT::ActionNodeBase
     return action_clients_registry;
   }
 
-  std::shared_ptr<rclcpp::Node> node_;
+  std::weak_ptr<rclcpp::Node> node_;
   std::shared_ptr<ActionClientInstance> client_instance_;
   std::string action_name_;
   bool action_name_may_change_ = false;
@@ -302,13 +315,14 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)
   }
 
   std::unique_lock lk(getMutex());
-  action_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + action_name;
+  auto node = node_.lock();
+  action_client_key_ = std::string(node->get_fully_qualified_name()) + "/" + action_name;
 
   auto& registry = getRegistry();
   auto it = registry.find(action_client_key_);
   if(it == registry.end() || it->second.expired())
   {
-    client_instance_ = std::make_shared<ActionClientInstance>(node_, action_name);
+    client_instance_ = std::make_shared<ActionClientInstance>(node, action_name);
     registry.insert({ action_client_key_, client_instance_ });
   }
   else
@@ -421,7 +435,7 @@ inline NodeStatus RosActionNode<T>::tick()
     }
 
     future_goal_handle_ = action_client->async_send_goal(goal, goal_options);
-    time_goal_sent_ = node_->now();
+    time_goal_sent_ = now();
 
     return NodeStatus::RUNNING;
   }
@@ -442,7 +456,7 @@ inline NodeStatus RosActionNode<T>::tick()
           future_goal_handle_, nodelay);
       if(ret != rclcpp::FutureReturnCode::SUCCESS)
       {
-        if((node_->now() - time_goal_sent_) > timeout)
+        if((now() - time_goal_sent_) > timeout)
         {
           return CheckStatus(onFailure(SEND_GOAL_TIMEOUT));
         }
diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp
@@ -159,7 +159,20 @@ class RosServiceNode : public BT::ActionNodeBase
 
   rclcpp::Logger logger()
   {
-    return node_->get_logger();
+    if(auto node = node_.lock())
+    {
+      return node->get_logger();
+    }
+    return rclcpp::get_logger("RosServiceNode");
+  }
+
+  rclcpp::Time now()
+  {
+    if(auto node = node_.lock())
+    {
+      return node->now();
+    }
+    return rclcpp::Clock(RCL_ROS_TIME).now();
   }
 
   using ClientsRegistry =
@@ -171,7 +184,7 @@ class RosServiceNode : public BT::ActionNodeBase
     return clients_registry;
   }
 
-  std::shared_ptr<rclcpp::Node> node_;
+  std::weak_ptr<rclcpp::Node> node_;
   std::string service_name_;
   bool service_name_may_change_ = false;
   const std::chrono::milliseconds service_timeout_;
@@ -268,13 +281,14 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)
   }
 
   std::unique_lock lk(getMutex());
-  auto client_key = std::string(node_->get_fully_qualified_name()) + "/" + service_name;
+  auto node = node_.lock();
+  auto client_key = std::string(node->get_fully_qualified_name()) + "/" + service_name;
 
   auto& registry = getRegistry();
   auto it = registry.find(client_key);
   if(it == registry.end() || it->second.expired())
   {
-    srv_instance_ = std::make_shared<ServiceClientInstance>(node_, service_name);
+    srv_instance_ = std::make_shared<ServiceClientInstance>(node, service_name);
     registry.insert({ client_key, srv_instance_ });
 
     RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(),
@@ -289,8 +303,8 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)
   bool found = srv_instance_->service_client->wait_for_service(wait_for_service_timeout_);
   if(!found)
   {
-    RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.",
-                 name().c_str(), service_name_.c_str());
+    RCLCPP_ERROR(logger(), "%s: Service with name '%s' is not reachable.", name().c_str(),
+                 service_name_.c_str());
   }
   return found;
 }
@@ -350,7 +364,7 @@ inline NodeStatus RosServiceNode<T>::tick()
     }
 
     future_response_ = srv_instance_->service_client->async_send_request(request).share();
-    time_request_sent_ = node_->now();
+    time_request_sent_ = now();
 
     return NodeStatus::RUNNING;
   }
@@ -371,7 +385,7 @@ inline NodeStatus RosServiceNode<T>::tick()
 
       if(ret != rclcpp::FutureReturnCode::SUCCESS)
       {
-        if((node_->now() - time_request_sent_) > timeout)
+        if((now() - time_request_sent_) > timeout)
         {
           return CheckStatus(onFailure(SERVICE_TIMEOUT));
         }
diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp
@@ -39,7 +39,7 @@ class RosTopicPubNode : public BT::ConditionNode
   /** You are not supposed to instantiate this class directly, the factory will do it.
    * To register this class into the factory, use:
    *
-   *    RegisterRosAction<DerivedClasss>(factory, params)
+   *    RegisterRosAction<DerivedClass>(factory, params)
    *
    * Note that if the external_action_client is not set, the constructor will build its own.
    * */
diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp
@@ -76,7 +76,7 @@ class RosTopicSubNode : public BT::ConditionNode
     return subscribers_registry;
   }
 
-  std::shared_ptr<rclcpp::Node> node_;
+  std::weak_ptr<rclcpp::Node> node_;
   std::shared_ptr<SubscriberInstance> sub_instance_;
   std::shared_ptr<TopicT> last_msg_;
   std::string topic_name_;
@@ -85,7 +85,11 @@ class RosTopicSubNode : public BT::ConditionNode
 
   rclcpp::Logger logger()
   {
-    return node_->get_logger();
+    if(auto node = node_.lock())
+    {
+      return node->get_logger();
+    }
+    return rclcpp::get_logger("RosTopicSubNode");
   }
 
 public:
@@ -244,13 +248,16 @@ inline bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name)
 
   // find SubscriberInstance in the registry
   std::unique_lock lk(registryMutex());
-  subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name;
+
+  auto shared_node = node_.lock();
+  subscriber_key_ =
+      std::string(shared_node->get_fully_qualified_name()) + "/" + topic_name;
 
   auto& registry = getRegistry();
   auto it = registry.find(subscriber_key_);
   if(it == registry.end() || it->second.expired())
   {
-    sub_instance_ = std::make_shared<SubscriberInstance>(node_, topic_name);
+    sub_instance_ = std::make_shared<SubscriberInstance>(shared_node, topic_name);
     registry.insert({ subscriber_key_, sub_instance_ });
 
     RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(),
diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp
@@ -24,7 +24,7 @@ namespace BT
 
 struct RosNodeParams
 {
-  std::shared_ptr<rclcpp::Node> nh;
+  std::weak_ptr<rclcpp::Node> nh;
 
   // This has different meaning based on the context:
   //
diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp
@@ -10,22 +10,21 @@ bool SleepAction::setGoal(RosActionNode::Goal& goal)
 
 NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult& wr)
 {
-  RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(),
+  RCLCPP_INFO(logger(), "%s: onResultReceived. Done = %s", name().c_str(),
               wr.result->done ? "true" : "false");
 
   return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
 }
 
 NodeStatus SleepAction::onFailure(ActionNodeErrorCode error)
 {
-  RCLCPP_ERROR(node_->get_logger(), "%s: onFailure with error: %s", name().c_str(),
-               toStr(error));
+  RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
   return NodeStatus::FAILURE;
 }
 
 void SleepAction::onHalt()
 {
-  RCLCPP_INFO(node_->get_logger(), "%s: onHalt", name().c_str());
+  RCLCPP_INFO(logger(), "%s: onHalt", name().c_str());
 }
 
 // Plugin registration.

Original file line number	Diff line number	Diff line change
`@@ -188,7 +188,20 @@ class RosActionNode : public BT::ActionNodeBase`
`188`	`188`
`189`	`189`	`rclcpp::Logger logger()`
`190`	`190`	`{`
`191`		`- return node_->get_logger();`
	`191`	`+ if(auto node = node_.lock())`
	`192`	`+ {`
	`193`	`+ return node->get_logger();`
	`194`	`+ }`
	`195`	`+ return rclcpp::get_logger("RosActionNode");`
	`196`	`+ }`
	`197`	`+`
	`198`	`+ rclcpp::Time now()`
	`199`	`+ {`
	`200`	`+ if(auto node = node_.lock())`
	`201`	`+ {`
	`202`	`+ return node->now();`
	`203`	`+ }`
	`204`	`+ return rclcpp::Clock(RCL_ROS_TIME).now();`
`192`	`205`	`}`
`193`	`206`
`194`	`207`	`using ClientsRegistry =`
`@@ -200,7 +213,7 @@ class RosActionNode : public BT::ActionNodeBase`
`200`	`213`	`return action_clients_registry;`
`201`	`214`	`}`
`202`	`215`
`203`		`- std::shared_ptr<rclcpp::Node> node_;`
	`216`	`+ std::weak_ptr<rclcpp::Node> node_;`
`204`	`217`	`std::shared_ptr<ActionClientInstance> client_instance_;`
`205`	`218`	`std::string action_name_;`
`206`	`219`	`bool action_name_may_change_ = false;`
`@@ -302,13 +315,14 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)`
`302`	`315`	`}`
`303`	`316`
`304`	`317`	`std::unique_lock lk(getMutex());`
`305`		`- action_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + action_name;`
	`318`	`+ auto node = node_.lock();`
	`319`	`+ action_client_key_ = std::string(node->get_fully_qualified_name()) + "/" + action_name;`
`306`	`320`
`307`	`321`	`auto& registry = getRegistry();`
`308`	`322`	`auto it = registry.find(action_client_key_);`
`309`	`323`	`if(it == registry.end() \|\| it->second.expired())`
`310`	`324`	`{`
`311`		`- client_instance_ = std::make_shared<ActionClientInstance>(node_, action_name);`
	`325`	`+ client_instance_ = std::make_shared<ActionClientInstance>(node, action_name);`
`312`	`326`	`registry.insert({ action_client_key_, client_instance_ });`
`313`	`327`	`}`
`314`	`328`	`else`
`@@ -421,7 +435,7 @@ inline NodeStatus RosActionNode<T>::tick()`
`421`	`435`	`}`
`422`	`436`
`423`	`437`	`future_goal_handle_ = action_client->async_send_goal(goal, goal_options);`
`424`		`- time_goal_sent_ = node_->now();`
	`438`	`+ time_goal_sent_ = now();`
`425`	`439`
`426`	`440`	`return NodeStatus::RUNNING;`
`427`	`441`	`}`
`@@ -442,7 +456,7 @@ inline NodeStatus RosActionNode<T>::tick()`
`442`	`456`	`future_goal_handle_, nodelay);`
`443`	`457`	`if(ret != rclcpp::FutureReturnCode::SUCCESS)`
`444`	`458`	`{`
`445`		`- if((node_->now() - time_goal_sent_) > timeout)`
	`459`	`+ if((now() - time_goal_sent_) > timeout)`
`446`	`460`	`{`
`447`	`461`	`return CheckStatus(onFailure(SEND_GOAL_TIMEOUT));`
`448`	`462`	`}`
Original file line number	Diff line number	Diff line change
`@@ -159,7 +159,20 @@ class RosServiceNode : public BT::ActionNodeBase`
`159`	`159`
`160`	`160`	`rclcpp::Logger logger()`
`161`	`161`	`{`
`162`		`- return node_->get_logger();`
	`162`	`+ if(auto node = node_.lock())`
	`163`	`+ {`
	`164`	`+ return node->get_logger();`
	`165`	`+ }`
	`166`	`+ return rclcpp::get_logger("RosServiceNode");`
	`167`	`+ }`
	`168`	`+`
	`169`	`+ rclcpp::Time now()`
	`170`	`+ {`
	`171`	`+ if(auto node = node_.lock())`
	`172`	`+ {`
	`173`	`+ return node->now();`
	`174`	`+ }`
	`175`	`+ return rclcpp::Clock(RCL_ROS_TIME).now();`
`163`	`176`	`}`
`164`	`177`
`165`	`178`	`using ClientsRegistry =`
`@@ -171,7 +184,7 @@ class RosServiceNode : public BT::ActionNodeBase`
`171`	`184`	`return clients_registry;`
`172`	`185`	`}`
`173`	`186`
`174`		`- std::shared_ptr<rclcpp::Node> node_;`
	`187`	`+ std::weak_ptr<rclcpp::Node> node_;`
`175`	`188`	`std::string service_name_;`
`176`	`189`	`bool service_name_may_change_ = false;`
`177`	`190`	`const std::chrono::milliseconds service_timeout_;`
`@@ -268,13 +281,14 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)`
`268`	`281`	`}`
`269`	`282`
`270`	`283`	`std::unique_lock lk(getMutex());`
`271`		`- auto client_key = std::string(node_->get_fully_qualified_name()) + "/" + service_name;`
	`284`	`+ auto node = node_.lock();`
	`285`	`+ auto client_key = std::string(node->get_fully_qualified_name()) + "/" + service_name;`
`272`	`286`
`273`	`287`	`auto& registry = getRegistry();`
`274`	`288`	`auto it = registry.find(client_key);`
`275`	`289`	`if(it == registry.end() \|\| it->second.expired())`
`276`	`290`	`{`
`277`		`- srv_instance_ = std::make_shared<ServiceClientInstance>(node_, service_name);`
	`291`	`+ srv_instance_ = std::make_shared<ServiceClientInstance>(node, service_name);`
`278`	`292`	`registry.insert({ client_key, srv_instance_ });`
`279`	`293`
`280`	`294`	`RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(),`
`@@ -289,8 +303,8 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)`
`289`	`303`	`bool found = srv_instance_->service_client->wait_for_service(wait_for_service_timeout_);`
`290`	`304`	`if(!found)`
`291`	`305`	`{`
`292`		`- RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.",`
`293`		`- name().c_str(), service_name_.c_str());`
	`306`	`+ RCLCPP_ERROR(logger(), "%s: Service with name '%s' is not reachable.", name().c_str(),`
	`307`	`+ service_name_.c_str());`
`294`	`308`	`}`
`295`	`309`	`return found;`
`296`	`310`	`}`
`@@ -350,7 +364,7 @@ inline NodeStatus RosServiceNode<T>::tick()`
`350`	`364`	`}`
`351`	`365`
`352`	`366`	`future_response_ = srv_instance_->service_client->async_send_request(request).share();`
`353`		`- time_request_sent_ = node_->now();`
	`367`	`+ time_request_sent_ = now();`
`354`	`368`
`355`	`369`	`return NodeStatus::RUNNING;`
`356`	`370`	`}`
`@@ -371,7 +385,7 @@ inline NodeStatus RosServiceNode<T>::tick()`
`371`	`385`
`372`	`386`	`if(ret != rclcpp::FutureReturnCode::SUCCESS)`
`373`	`387`	`{`
`374`		`- if((node_->now() - time_request_sent_) > timeout)`
	`388`	`+ if((now() - time_request_sent_) > timeout)`
`375`	`389`	`{`
`376`	`390`	`return CheckStatus(onFailure(SERVICE_TIMEOUT));`
`377`	`391`	`}`
Original file line number	Diff line number	Diff line change
`@@ -39,7 +39,7 @@ class RosTopicPubNode : public BT::ConditionNode`
`39`	`39`	`/** You are not supposed to instantiate this class directly, the factory will do it.`
`40`	`40`	`* To register this class into the factory, use:`
`41`	`41`	`*`
`42`		`- * RegisterRosAction<DerivedClasss>(factory, params)`
	`42`	`+ * RegisterRosAction<DerivedClass>(factory, params)`
`43`	`43`	`*`
`44`	`44`	`* Note that if the external_action_client is not set, the constructor will build its own.`
`45`	`45`	`* */`
Original file line number	Diff line number	Diff line change
`@@ -24,7 +24,7 @@ namespace BT`
`24`	`24`
`25`	`25`	`struct RosNodeParams`
`26`	`26`	`{`
`27`		`- std::shared_ptr<rclcpp::Node> nh;`
	`27`	`+ std::weak_ptr<rclcpp::Node> nh;`
`28`	`28`
`29`	`29`	`// This has different meaning based on the context:`
`30`	`30`	`//`
Original file line number	Diff line number	Diff line change
`@@ -10,22 +10,21 @@ bool SleepAction::setGoal(RosActionNode::Goal& goal)`
`10`	`10`
`11`	`11`	`NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult& wr)`
`12`	`12`	`{`
`13`		`- RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(),`
	`13`	`+ RCLCPP_INFO(logger(), "%s: onResultReceived. Done = %s", name().c_str(),`
`14`	`14`	`wr.result->done ? "true" : "false");`
`15`	`15`
`16`	`16`	`return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE;`
`17`	`17`	`}`
`18`	`18`
`19`	`19`	`NodeStatus SleepAction::onFailure(ActionNodeErrorCode error)`
`20`	`20`	`{`
`21`		`- RCLCPP_ERROR(node_->get_logger(), "%s: onFailure with error: %s", name().c_str(),`
`22`		`- toStr(error));`
	`21`	`+ RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));`
`23`	`22`	`return NodeStatus::FAILURE;`
`24`	`23`	`}`
`25`	`24`
`26`	`25`	`void SleepAction::onHalt()`
`27`	`26`	`{`
`28`		`- RCLCPP_INFO(node_->get_logger(), "%s: onHalt", name().c_str());`
	`27`	`+ RCLCPP_INFO(logger(), "%s: onHalt", name().c_str());`
`29`	`28`	`}`
`30`	`29`
`31`	`30`	`// Plugin registration.`