BehaviorTree/BehaviorTree.ROS2

rclcpp::Node as a shared pointer may cause circular dependency

Closed this issue · 4 comments

It's common practice to place all logic inside a ROS node, with the main method simply creating and spinning the node. However, registering BT nodes from within an rclcpp::Node child class can lead to a circular dependency:

	RosNodeParams params;
    params.nh = this->shared_from_this();
    params.default_port_value = "btcpp_string";
    factory_->registerNodeType<ReceiveString>("ReceiveString", params);

This is due to passing this->shared_from_this() as a rclcpp node pointer, creating a situation where the main ROS node holds a reference to a BT node and vice versa, leading to a circular dependency. One potential solution is to use weak pointers for BT nodes. It worked for me in a simple example similar to subscriber_test. Not sure if there are any obvious drawbacks of this approach. What are your thoughts on this?

Also, what's your opinion on creating and ticking BT tree inside ROS node e.g. using ROS timer? Currently, I use ROS parameters to load BT nodes via plugins, and moving this outside the node might get complicated.

I don't believe this is an actual problem.

Yes, the ROS node may holds a reference to BT nodes, but in the destructor, those nodes and their reference will be destroyed.

Using a weak pointer means that we need to check in multiple places if the ROS node still exist or not.

If you have a concrete example where this might be a problem, let me know.

This may be an issue if you want to manage BT factory or tree from inside a ROS node.
I've created simple example to illustrate the issue.

#include <std_msgs/msg/bool.hpp>
#include "behaviortree_ros2/bt_topic_sub_node.hpp"

using namespace BT;

class SimpleNode : public RosTopicSubNode<std_msgs::msg::Bool>
{
public:
  SimpleNode(const std::string & name, const NodeConfig & conf, const RosNodeParams & params)
  : RosTopicSubNode<std_msgs::msg::Bool>(name, conf, params)
  {
  }

  static BT::PortsList providedPorts() { return {}; }

  NodeStatus onTick(const std::shared_ptr<std_msgs::msg::Bool> & last_msg) override
  {
    return NodeStatus::SUCCESS;
  }
};

class ROSNode : public rclcpp::Node
{
public:
  ROSNode(
    const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node(node_name, options)
  {
  }

  ~ROSNode() { std::cout << "Destructor called" << std::endl; }

  void Init()
  {
    RosNodeParams params;
    params.nh = this->shared_from_this();
    factory.registerNodeType<SimpleNode>("SimpleNode", params);
  }

private:
  BT::BehaviorTreeFactory factory;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto nh = std::make_shared<ROSNode>("subscriber_test");

  nh->Init();

  return 0;
}

So, the expected behavior of this node is to run and simply print 'Destructor called' at the end of execution. However, the destructor is actually never called. This is because SimpleNode holds a reference to rclcpp::Node, and ROSNode holds a reference to factory, which in turn holds SimpleNode, creating a circular dependency. If you remove params.nh = this->shared_from_this(); (line 36), the destructor is called normally and prints 'Destructor called' when the node is executed.

I see two possible solutions to this problem: one involves using weak pointers, and the other is to manage the factory and trees outside of a ROS node when using BT ROS nodes.

Ok, I see the issue now. Thanks for taking the time to provide more informations.

I will propose a change

Fixed. Thanks for the help