Update design for providing transform to bt nodes

nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp

@@ -22,8 +22,7 @@
 #include "behaviortree_cpp/condition_node.h"
 #include "nav2_util/robot_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree
 {
@@ -59,12 +58,7 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
     node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
-    tf_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-      node_->get_node_base_interface(),
-      node_->get_node_timers_interface());
-    tf_->setCreateTimerInterface(timer_interface);
-    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+    tf_ = blackboard()->template get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
     initialized_ = true;
   }
@@ -74,7 +68,6 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     geometry_msgs::msg::PoseStamped current_pose;
 
-    rclcpp::spin_some(node_);
     if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return false;
@@ -99,7 +92,6 @@ class GoalReachedCondition : public BT::ConditionNode
 private:
   rclcpp::Node::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   geometry_msgs::msg::PoseStamped::SharedPtr goal_;
 
   bool initialized_;

nav2_bt_navigator/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(nav2_msgs REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
@@ -45,6 +46,7 @@ set(dependencies
   behaviortree_cpp
   std_srvs
   nav2_util
+  tf2_ros
 )
 
 ament_target_dependencies(${executable_name}

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -26,6 +26,8 @@
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/create_timer_ros.h"
 
 namespace nav2_bt_navigator
 {
@@ -83,6 +85,10 @@ class BtNavigator : public nav2_util::LifecycleNode
 
   // A regular, non-spinning ROS node that we can use for calls to the action client
   rclcpp::Node::SharedPtr client_node_;
+
+  // Spinning transform that can be used by the BT nodes
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/package.xml

@@ -7,6 +7,8 @@
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>
 
+  <depend>tf2_ros</depend>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
   <build_depend>rclcpp</build_depend>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -54,6 +54,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   self_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
     client_node_, "NavigateToPose");
 
+  tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      get_node_base_interface(), get_node_timers_interface());
+    tf_->setCreateTimerInterface(timer_interface);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, this, false);
+
   goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
     "/goal_pose",
     rclcpp::SystemDefaultsQoS(),
@@ -80,6 +86,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   blackboard_->set<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_);  // NOLINT
   blackboard_->set<nav_msgs::msg::Path::SharedPtr>("path", path_);  // NOLINT
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
+  blackboard_->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10));  // NOLINT
   blackboard_->set<bool>("path_updated", false);  // NOLINT
   blackboard_->set<bool>("initial_pose_received", false);  // NOLINT
@@ -143,6 +150,8 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   goal_sub_.reset();
   client_node_.reset();
   self_client_.reset();
+  tf_.reset();
+  tf_listener_.reset();
   action_server_.reset();
   path_.reset();
   xml_string_.clear();