From 5fee4985aa7a207d9c364e5b476aa95267d50b6c Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Thu, 2 Jun 2022 20:01:34 +0800 Subject: [PATCH 1/2] remove LifecycleNode's argument use_rclcpp_node Signed-off-by: zhenpeng ge --- nav2_amcl/src/amcl_node.cpp | 2 +- nav2_behaviors/src/behavior_server.cpp | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 2 +- nav2_controller/src/controller_server.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- .../test/integration/inflation_tests.cpp | 2 +- .../test_costmap_topic_collision_checker.cpp | 21 +++++++++--- nav2_costmap_2d/test/testing_helper.hpp | 20 ++++++----- nav2_map_server/src/map_saver/map_saver.cpp | 2 +- nav2_map_server/src/map_server/map_server.cpp | 2 +- nav2_planner/src/planner_server.cpp | 2 +- nav2_smoother/src/nav2_smoother.cpp | 2 +- .../include/nav2_util/lifecycle_node.hpp | 20 +---------- nav2_util/src/lifecycle_node.cpp | 33 ++----------------- nav2_util/test/test_lifecycle_node.cpp | 9 ++--- .../src/waypoint_follower.cpp | 2 +- 16 files changed, 45 insertions(+), 80 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 0d0e665705..a28b4927a3 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -59,7 +59,7 @@ namespace nav2_amcl using nav2_util::geometry_utils::orientationAroundZAxis; AmclNode::AmclNode(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("amcl", "", false, options) +: nav2_util::LifecycleNode("amcl", "", options) { RCLCPP_INFO(get_logger(), "Creating"); diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 7a6a714461..d6f7568c40 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -23,7 +23,7 @@ namespace behavior_server { BehaviorServer::BehaviorServer(const rclcpp::NodeOptions & options) -: LifecycleNode("behavior_server", "", false, options), +: LifecycleNode("behavior_server", "", options), plugin_loader_("nav2_core", "nav2_core::Behavior"), default_ids_{"spin", "backup", "drive_on_heading", "wait"}, default_types_{"nav2_behaviors/Spin", diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index fb8b64db02..0e31d13cd0 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -29,7 +29,7 @@ namespace nav2_bt_navigator { BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("bt_navigator", "", false, options) +: nav2_util::LifecycleNode("bt_navigator", "", options) { RCLCPP_INFO(get_logger(), "Creating"); diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index be6110119d..711cb96e0c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -34,7 +34,7 @@ namespace nav2_controller { ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("controller_server", "", false, options), +: nav2_util::LifecycleNode("controller_server", "", options), progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), default_progress_checker_id_{"progress_checker"}, default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"}, diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 437df3eace..12d6b2ee68 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -65,7 +65,7 @@ Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, const std::string & local_namespace) -: nav2_util::LifecycleNode(name, "", false, +: nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace // TODO(orduno) Pass a sub-node instead of creating a new node for better handling diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index ee6b6db665..ad867128b1 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -183,7 +183,7 @@ void TestNode::initNode(std::vector parameters) options.parameter_overrides(parameters); node_ = std::make_shared( - "inflation_test_node", "", false, options); + "inflation_test_node", "", options); // Declare non-plugin specific costmap parameters node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 147d79c1d0..cfafb6eb4e 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -88,7 +88,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode { public: explicit TestCollisionChecker(std::string name) - : LifecycleNode(name, "", true), + : LifecycleNode(name), global_frame_("map") { // Declare non-plugin specific costmap parameters @@ -106,10 +106,13 @@ class TestCollisionChecker : public nav2_util::LifecycleNode on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); tf_buffer_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface(), + callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); tf_broadcaster_ = std::make_shared(shared_from_this()); @@ -132,15 +135,18 @@ class TestCollisionChecker : public nav2_util::LifecycleNode layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false); // Add Static Layer std::shared_ptr slayer = nullptr; - addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer); + addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_); while (!slayer->isCurrent()) { rclcpp::spin_some(this->get_node_base_interface()); } // Add Inflation Layer std::shared_ptr ilayer = nullptr; - addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer); + addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer, callback_group_); + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); return nav2_util::CallbackReturn::SUCCESS; } @@ -165,6 +171,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode delete layers_; layers_ = nullptr; + executor_thread_.reset(); tf_buffer_.reset(); footprint_sub_.reset(); @@ -278,6 +285,10 @@ class TestCollisionChecker : public nav2_util::LifecycleNode std::shared_ptr tf_listener_; std::shared_ptr tf_broadcaster_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::unique_ptr collision_checker_; diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index a24b166a73..8ab95e6d44 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -71,30 +71,33 @@ unsigned int countValues( void addStaticLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & slayer) + std::shared_ptr & slayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { slayer = std::make_shared(); layers.addPlugin(std::shared_ptr(slayer)); - slayer->initialize(&layers, "static", &tf, node, nullptr); + slayer->initialize(&layers, "static", &tf, node, callback_group); } void addObstacleLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & olayer) + std::shared_ptr & olayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { olayer = std::make_shared(); - olayer->initialize(&layers, "obstacles", &tf, node, nullptr); + olayer->initialize(&layers, "obstacles", &tf, node, callback_group); layers.addPlugin(std::shared_ptr(olayer)); } void addRangeLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & rlayer) + std::shared_ptr & rlayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { rlayer = std::make_shared(); - rlayer->initialize(&layers, "range", &tf, node, nullptr); + rlayer->initialize(&layers, "range", &tf, node, callback_group); layers.addPlugin(std::shared_ptr(rlayer)); } @@ -130,10 +133,11 @@ void addObservation( void addInflationLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & ilayer) + std::shared_ptr & ilayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { ilayer = std::make_shared(); - ilayer->initialize(&layers, "inflation", &tf, node, nullptr); + ilayer->initialize(&layers, "inflation", &tf, node, callback_group); std::shared_ptr ipointer(ilayer); layers.addPlugin(ipointer); } diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 580825f8ed..d79005a567 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -42,7 +42,7 @@ using namespace std::placeholders; namespace nav2_map_server { MapSaver::MapSaver(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("map_saver", "", false, options) +: nav2_util::LifecycleNode("map_saver", "", options) { RCLCPP_INFO(get_logger(), "Creating"); diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 332c67ee5e..5920e095be 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -63,7 +63,7 @@ namespace nav2_map_server { MapServer::MapServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("map_server", "", false, options) +: nav2_util::LifecycleNode("map_server", "", options) { RCLCPP_INFO(get_logger(), "Creating"); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 4a62b1e83e..a81dd8d886 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -40,7 +40,7 @@ namespace nav2_planner { PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("planner_server", "", false, options), +: nav2_util::LifecycleNode("planner_server", "", options), gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), default_ids_{"GridBased"}, default_types_{"nav2_navfn_planner/NavfnPlanner"}, diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 5d9c67c26d..ada1f664b0 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -34,7 +34,7 @@ namespace nav2_smoother { SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) -: LifecycleNode("smoother_server", "", false, options), +: LifecycleNode("smoother_server", "", options), lp_loader_("nav2_core", "nav2_core::Smoother"), default_ids_{"simple_smoother"}, default_types_{"nav2_smoother::SimpleSmoother"} diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index e8915ce75a..d4ba168849 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -30,15 +30,9 @@ namespace nav2_util using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -// The following is a temporary wrapper for rclcpp_lifecycle::LifecycleNode. This class -// adds the optional creation of an rclcpp::Node that can be used by derived classes -// to interface to classes, such as MessageFilter and TransformListener, that don't yet -// support lifecycle nodes. Once we get the fixes into ROS2, this class will be removed. - /** * @class nav2_util::LifecycleNode - * @brief A lifecycle node wrapper to enable common Nav2 needs such as background node threads - * and manipulating parameters + * @brief A lifecycle node wrapper to enable common Nav2 needs such as manipulating parameters */ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode { @@ -47,13 +41,11 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode * @brief A lifecycle node constructor * @param node_name Name for the node * @param namespace Namespace for the node, if any - * @param use_rclcpp_node Whether to create an internal client node * @param options Node options */ LifecycleNode( const std::string & node_name, const std::string & ns = "", - bool use_rclcpp_node = false, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~LifecycleNode(); @@ -191,16 +183,6 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode */ void printLifecycleNodeNotification(); - // Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't - // yet support lifecycle nodes - bool use_rclcpp_node_; - - // The local node - rclcpp::Node::SharedPtr rclcpp_node_; - - // When creating a local node, this class will launch a separate thread created to spin the node - std::unique_ptr rclcpp_thread_; - // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; }; diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index f0061c82a1..9868a38bbb 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -23,29 +23,11 @@ namespace nav2_util { -// The nav2_util::LifecycleNode class is temporary until we get the -// required support for lifecycle nodes in MessageFilter, TransformListener, -// and TransforBroadcaster. We have submitted issues for these and will -// be submitting PRs to add the fixes: -// -// https://github.com/ros2/geometry2/issues/95 -// https://github.com/ros2/geometry2/issues/94 -// https://github.com/ros2/geometry2/issues/70 -// -// Until then, this class can provide a normal ROS node that has a thread -// that processes the node's messages. If a derived class needs to interface -// to one of these classes - MessageFilter, etc. - that don't yet support -// lifecycle nodes, it can simply set the use_rclcpp_node flag in the -// constructor and then provide the rclcpp_node_ to the helper classes, like -// MessageFilter. -// - LifecycleNode::LifecycleNode( const std::string & node_name, - const std::string & ns, bool use_rclcpp_node, + const std::string & ns, const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode(node_name, ns, options), - use_rclcpp_node_(use_rclcpp_node) +: rclcpp_lifecycle::LifecycleNode(node_name, ns, options) { // server side never times out from lifecycle manager this->declare_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true); @@ -53,17 +35,6 @@ LifecycleNode::LifecycleNode( rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); - if (use_rclcpp_node_) { - std::vector new_args = options.arguments(); - new_args.push_back("--ros-args"); - new_args.push_back("-r"); - new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); - new_args.push_back("--"); - rclcpp_node_ = std::make_shared( - "_", ns, rclcpp::NodeOptions(options).arguments(new_args)); - rclcpp_thread_ = std::make_unique(rclcpp_node_); - } - printLifecycleNodeNotification(); } diff --git a/nav2_util/test/test_lifecycle_node.cpp b/nav2_util/test/test_lifecycle_node.cpp index 358e83ddbe..fd204df47f 100644 --- a/nav2_util/test/test_lifecycle_node.cpp +++ b/nav2_util/test/test_lifecycle_node.cpp @@ -33,7 +33,7 @@ RclCppFixture g_rclcppfixture; TEST(LifecycleNode, RclcppNodeExitsCleanly) { // Make sure the node exits cleanly when using an rclcpp_node and associated thread - auto node1 = std::make_shared("test_node", "", true); + auto node1 = std::make_shared("test_node", ""); std::this_thread::sleep_for(std::chrono::seconds(1)); SUCCEED(); } @@ -41,11 +41,8 @@ TEST(LifecycleNode, RclcppNodeExitsCleanly) TEST(LifecycleNode, MultipleRclcppNodesExitCleanly) { // Try a couple nodes w/ rclcpp_node and threads - auto node1 = std::make_shared("test_node1", "", true); - auto node2 = std::make_shared("test_node2", "", true); - - // Another one without rclcpp_node and on the stack - nav2_util::LifecycleNode node3("test_node3", "", false); + auto node1 = std::make_shared("test_node1", ""); + auto node2 = std::make_shared("test_node2", ""); std::this_thread::sleep_for(std::chrono::seconds(1)); SUCCEED(); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index cb7ca2ebb3..6f8d9e1dab 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -28,7 +28,7 @@ using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("waypoint_follower", "", false, options), +: nav2_util::LifecycleNode("waypoint_follower", "", options), waypoint_task_executor_loader_("nav2_waypoint_follower", "nav2_core::WaypointTaskExecutor") { From 21d1df441945a72244b0e70b829f71f9d1032581 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Thu, 2 Jun 2022 21:50:18 +0800 Subject: [PATCH 2/2] update comment Signed-off-by: zhenpeng ge --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index fc152ccb95..a5dfc6182a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -311,7 +311,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; - // Dedicated callback group and executor for tf timer_interface, costmap plugins and filters + // Dedicated callback group and executor for tf timer_interface and message fillter rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::unique_ptr executor_thread_;