Merge pull request #95 from logivations/amrnav-6948

Amrnav 6948

Author: AnastasiaPittel

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -251,7 +251,9 @@ class PlannerServer : public nav2_util::LifecycleNode
 
   // Global Costmap
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> no_waiting_costmap_ros_;
   std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
+  std::unique_ptr<nav2_util::NodeThread> no_waiting_costmap_thread_;
   nav2_costmap_2d::Costmap2D * costmap_;
   std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
   collision_checker_;

nav2_planner/src/planner_server.cpp

@@ -67,6 +67,10 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "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<nav2_costmap_2d::Costmap2DROS>(
+    "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<nav2_util::NodeThread>(costmap_ros_);
+  no_waiting_costmap_thread_ = std::make_unique<nav2_util::NodeThread>(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;
 }