From 60043d4067b9dbff73be65291f5df6aaa286cc61 Mon Sep 17 00:00:00 2001 From: Arshad Mehmood Date: Thu, 9 Jun 2022 03:07:30 +0800 Subject: [PATCH 1/2] Fix for costmap nodes lifecycle status (#3005) Lifecycle status for global and local cost nodes not correct. ros2 lifecycle/service commands shows unconfigured for these two. This is due to directly calling on_configure/on_activate/on_cleanup calls in parent node. This PR to replace on_xxxxxx() to configure()/activate()/cleanup() calls of lifecycle base. Signed-off-by: Arshad Mehmood --- nav2_controller/src/nav2_controller.cpp | 16 ++++++++-------- nav2_planner/src/planner_server.cpp | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index dd938c4dbc..e1151590a8 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -67,7 +67,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & state) +ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { auto node = shared_from_this(); @@ -102,7 +102,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); - costmap_ros_->on_configure(state); + costmap_ros_->configure(); try { progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); @@ -171,11 +171,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_activate(const rclcpp_lifecycle::State & state) +ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_ros_->on_activate(state); + costmap_ros_->activate(); ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -187,7 +187,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) +ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); @@ -196,7 +196,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->deactivate(); } - costmap_ros_->on_deactivate(state); + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -205,7 +205,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) +ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); @@ -215,7 +215,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) it->second->cleanup(); } controllers_.clear(); - costmap_ros_->on_cleanup(state); + costmap_ros_->cleanup(); // Release any allocated resources action_server_.reset(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 48eede3686..58ff355cba 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -64,11 +64,11 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & state) +PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - costmap_ros_->on_configure(state); + costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); RCLCPP_DEBUG( @@ -139,13 +139,13 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_activate(const rclcpp_lifecycle::State & state) +PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); plan_publisher_->on_activate(); action_server_->activate(); - costmap_ros_->on_activate(state); + costmap_ros_->activate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -156,13 +156,13 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) +PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); action_server_->deactivate(); plan_publisher_->on_deactivate(); - costmap_ros_->on_deactivate(state); + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -173,14 +173,14 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) +PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); action_server_.reset(); plan_publisher_.reset(); tf_.reset(); - costmap_ros_->on_cleanup(state); + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { From 1922e99c067f3e322059a41c753d0f3cec34e593 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Thu, 18 Aug 2022 14:13:53 -0400 Subject: [PATCH 2/2] Add Client Thread for costmap_client_node (#3108) --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 1 + 2 files changed, 2 insertions(+) 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 124d1666a5..f3767eaf1f 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 @@ -258,6 +258,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode protected: rclcpp::Node::SharedPtr client_node_; + std::unique_ptr client_thread_; // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 89c3d28aca..5e8ec2095c 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -83,6 +83,7 @@ Costmap2DROS::Costmap2DROS( auto options = rclcpp::NodeOptions().arguments( {"--ros-args", "-r", std::string("__node:=") + get_name() + "_client", "--"}); client_node_ = std::make_shared("_", options); + client_thread_ = std::make_unique(client_node_); std::vector clearable_layers{"obstacle_layer"};