create action server on BtNavigator node

nav2_bt_navigator/src/bt_navigator.cpp

@@ -43,7 +43,6 @@ nav2_util::CallbackReturn
 BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Configuring");
-  auto node = shared_from_this();
 
   auto options = rclcpp::NodeOptions().arguments(
     {"--ros-args",
@@ -55,14 +54,17 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   self_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
     client_node_, "NavigateToPose");
 
-  goal_sub_ = rclcpp_node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+  goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
     "/goal_pose",
     rclcpp::SystemDefaultsQoS(),
     std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));
 
-  // Create an action server that we implement with our navigateToPose method
-  action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "NavigateToPose",
-      std::bind(&BtNavigator::navigateToPose, this), false);
+  action_server_ = std::make_unique<ActionServer>(
+    get_node_base_interface(),
+    get_node_clock_interface(),
+    get_node_logging_interface(),
+    get_node_waitables_interface(),
+    "NavigateToPose", std::bind(&BtNavigator::navigateToPose, this), false);
 
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>();

nav2_util/include/nav2_util/simple_action_server.hpp

@@ -36,7 +36,27 @@ public:
     const std::string & action_name,
     ExecuteCallback execute_callback,
     bool autostart = true)
-  : node_(node), action_name_(action_name), execute_callback_(execute_callback)
+  : SimpleActionServer(
+      node->get_node_base_interface(),
+      node->get_node_clock_interface(),
+      node->get_node_logging_interface(),
+      node->get_node_waitables_interface(),
+      action_name, execute_callback, autostart)
+  {}
+
+  explicit SimpleActionServer(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
+    const std::string & action_name,
+    ExecuteCallback execute_callback,
+    bool autostart = true)
+  : node_base_interface_(node_base_interface),
+    node_clock_interface_(node_clock_interface),
+    node_logging_interface_(node_logging_interface),
+    node_waitables_interface_(node_waitables_interface),
+    action_name_(action_name), execute_callback_(execute_callback)
   {
     if (autostart) {
       server_active_ = true;
@@ -103,7 +123,10 @@ public:
       };
 
     action_server_ = rclcpp_action::create_server<ActionT>(
-      node_,
+      node_base_interface_,
+      node_clock_interface_,
+      node_logging_interface_,
+      node_waitables_interface_,
       action_name_,
       handle_goal,
       handle_cancel,
@@ -262,8 +285,11 @@ public:
   }
 
 protected:
-  // The SimpleActionServer isn't itself a node, so needs to know which one to use
-  rclcpp::Node::SharedPtr node_;
+  // The SimpleActionServer isn't itself a node, so it needs interfaces to one
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_;
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface_;
   std::string action_name_;
 
   ExecuteCallback execute_callback_;
@@ -289,17 +315,20 @@ protected:
 
   void debug_msg(const std::string & msg) const
   {
-    RCLCPP_DEBUG(node_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+    RCLCPP_DEBUG(node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
   }
 
   void error_msg(const std::string & msg) const
   {
-    RCLCPP_ERROR(node_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+    RCLCPP_ERROR(node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
   }
 
   void warn_msg(const std::string & msg) const
   {
-    RCLCPP_WARN(node_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+    RCLCPP_WARN(node_logging_interface_->get_logger(),
+      "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
   }
 };