diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index ff03494f3fe87b3ea3b147f23c5956c54011c56b..4c90990327a6d37bd52448e490f783decadfe697 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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>(); diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 47904c6a2227c8f3707b1dd9198da400971d0e13..0a9c0bbb71bcfd27240c28079ce5fda5cc4a2b71 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/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()); } };