From 09244d53b1f2fbb59b4e4e1b3c03ae47539f3331 Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 17 Apr 2025 15:37:26 +0300 Subject: [PATCH 1/4] add new costmap --- .../include/nav2_controller/controller_server.hpp | 2 ++ nav2_controller/src/controller_server.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 865451d3989..1a2902818be 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -228,9 +228,11 @@ class ControllerServer : public nav2_util::LifecycleNode std::shared_ptr costmap_ros_; std::shared_ptr narrow_costmap_ros_; std::shared_ptr sensor_costmap_ros_; + std::shared_ptr global_no_waiting_zone_costmap_ros_; std::unique_ptr costmap_thread_; std::unique_ptr narrow_costmap_thread_; std::unique_ptr sensor_costmap_thread_; + std::unique_ptr global_no_waiting_zone_costmap_thread_; // Publishers and subscribers std::unique_ptr odom_sub_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 375b0941a05..7943e5679f4 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -81,6 +81,11 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) sensor_costmap_ros_ = std::make_shared( "sensor_local_costmap", std::string{get_namespace()}, "sensor_local_costmap", get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); + + // The costmap node is used by BT to validate that a path can be driven on + global_no_waiting_zone_costmap_ros_ = std::make_shared( + "global_no_waiting_zones_costmap", std::string{get_namespace()}, "global_no_waiting_zones_costmap", + get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); } ControllerServer::~ControllerServer() @@ -147,10 +152,12 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->configure(); narrow_costmap_ros_->configure(); sensor_costmap_ros_->configure(); + global_no_waiting_zone_costmap_ros_->configure(); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); narrow_costmap_thread_ = std::make_unique(narrow_costmap_ros_); sensor_costmap_thread_ = std::make_unique(sensor_costmap_ros_); + global_no_waiting_zone_costmap_thread_ = std::make_unique(global_no_waiting_zone_costmap_ros_); for (size_t i = 0; i != progress_checker_ids_.size(); i++) { try { @@ -294,6 +301,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } narrow_costmap_ros_->activate(); sensor_costmap_ros_->activate(); + global_no_waiting_zone_costmap_ros_->activate(); ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -333,6 +341,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) costmap_ros_->deactivate(); narrow_costmap_ros_->deactivate(); sensor_costmap_ros_->deactivate(); + global_no_waiting_zone_costmap_ros_->deactivate(); // Always publish a zero velocity when deactivating the controller server geometry_msgs::msg::TwistStamped velocity; From 97cf2a6fed785b3ec820c5387b1b5cb7229cec12 Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 17 Apr 2025 17:41:29 +0300 Subject: [PATCH 2/4] Revert "add new costmap" This reverts commit 09244d53b1f2fbb59b4e4e1b3c03ae47539f3331. --- .../include/nav2_controller/controller_server.hpp | 2 -- nav2_controller/src/controller_server.cpp | 9 --------- 2 files changed, 11 deletions(-) diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 1a2902818be..865451d3989 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -228,11 +228,9 @@ class ControllerServer : public nav2_util::LifecycleNode std::shared_ptr costmap_ros_; std::shared_ptr narrow_costmap_ros_; std::shared_ptr sensor_costmap_ros_; - std::shared_ptr global_no_waiting_zone_costmap_ros_; std::unique_ptr costmap_thread_; std::unique_ptr narrow_costmap_thread_; std::unique_ptr sensor_costmap_thread_; - std::unique_ptr global_no_waiting_zone_costmap_thread_; // Publishers and subscribers std::unique_ptr odom_sub_; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 7943e5679f4..375b0941a05 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -81,11 +81,6 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) sensor_costmap_ros_ = std::make_shared( "sensor_local_costmap", std::string{get_namespace()}, "sensor_local_costmap", get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); - - // The costmap node is used by BT to validate that a path can be driven on - global_no_waiting_zone_costmap_ros_ = std::make_shared( - "global_no_waiting_zones_costmap", std::string{get_namespace()}, "global_no_waiting_zones_costmap", - get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); } ControllerServer::~ControllerServer() @@ -152,12 +147,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->configure(); narrow_costmap_ros_->configure(); sensor_costmap_ros_->configure(); - global_no_waiting_zone_costmap_ros_->configure(); // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); narrow_costmap_thread_ = std::make_unique(narrow_costmap_ros_); sensor_costmap_thread_ = std::make_unique(sensor_costmap_ros_); - global_no_waiting_zone_costmap_thread_ = std::make_unique(global_no_waiting_zone_costmap_ros_); for (size_t i = 0; i != progress_checker_ids_.size(); i++) { try { @@ -301,7 +294,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } narrow_costmap_ros_->activate(); sensor_costmap_ros_->activate(); - global_no_waiting_zone_costmap_ros_->activate(); ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -341,7 +333,6 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) costmap_ros_->deactivate(); narrow_costmap_ros_->deactivate(); sensor_costmap_ros_->deactivate(); - global_no_waiting_zone_costmap_ros_->deactivate(); // Always publish a zero velocity when deactivating the controller server geometry_msgs::msg::TwistStamped velocity; From 6ec4a93259a17cb38cdf96ed5604e303ad4a2753 Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 17 Apr 2025 17:52:00 +0300 Subject: [PATCH 3/4] add new costmap to planner server --- nav2_planner/include/nav2_planner/planner_server.hpp | 2 ++ nav2_planner/src/planner_server.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 565d4c0dc00..ba24d614a11 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -251,7 +251,9 @@ class PlannerServer : public nav2_util::LifecycleNode // Global Costmap std::shared_ptr costmap_ros_; + std::shared_ptr no_waiting_costmap_ros_; std::unique_ptr costmap_thread_; + std::unique_ptr no_waiting_costmap_thread_; nav2_costmap_2d::Costmap2D * costmap_; std::unique_ptr> collision_checker_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 242a3e42317..77fde3117d3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -67,6 +67,10 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, "global_costmap", get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); + + no_waiting_costmap_ros_ = std::make_shared( + "global_no_waiting_zones_costmap", std::string{get_namespace()}, "global_no_waiting_zones_costmap", + get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); } PlannerServer::~PlannerServer() @@ -77,6 +81,7 @@ PlannerServer::~PlannerServer() */ planners_.clear(); costmap_thread_.reset(); + no_waiting_costmap_thread_.reset(); } nav2_util::CallbackReturn @@ -95,6 +100,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); + no_waiting_costmap_thread_ = std::make_unique(no_waiting_costmap_ros_); RCLCPP_DEBUG( get_logger(), "Costmap size: %d,%d", @@ -231,6 +237,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ costmap_ros_->deactivate(); + no_waiting_costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -256,6 +263,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) tf_.reset(); costmap_ros_->cleanup(); + no_waiting_costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -264,6 +272,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) planners_.clear(); costmap_thread_.reset(); + no_waiting_costmap_thread_.reset(); costmap_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } From c2493b17bbdf169584f66bc14e6d9131da8d02a7 Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Wed, 23 Apr 2025 13:28:17 +0300 Subject: [PATCH 4/4] missed --- nav2_planner/src/planner_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 77fde3117d3..3b00f68d0a2 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -90,6 +90,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Configuring"); costmap_ros_->configure(); + no_waiting_costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); if (!costmap_ros_->getUseRadius()) { @@ -192,6 +193,7 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) plan_publisher_->on_activate(); action_server_pose_->activate(); action_server_poses_->activate(); + no_waiting_costmap_ros_->activate(); const auto costmap_ros_state = costmap_ros_->activate(); if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2_util::CallbackReturn::FAILURE;