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..3b00f68d0a2 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 @@ -85,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()) { @@ -95,6 +101,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", @@ -186,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; @@ -231,6 +239,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 +265,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 +274,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; }