Give behavior server access to both costmaps

nav2_behaviors/include/nav2_behaviors/behavior_server.hpp

@@ -45,13 +45,22 @@ class BehaviorServer : public nav2_util::LifecycleNode
   explicit BehaviorServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   ~BehaviorServer();
 
+protected:
   /**
    * @brief Loads behavior plugins from parameter file
    * @return bool if successfully loaded the plugins
    */
   bool loadBehaviorPlugins();
 
-protected:
+  /**
+   * @brief configures behavior plugins
+   */
+  void configureBehaviorPlugins();
+
+  /**
+   * @brief configures behavior plugins
+   */
+  void setupResourcesForBehaviorPlugins();
   /**
    * @brief Configure lifecycle server
    */
@@ -89,9 +98,13 @@ class BehaviorServer : public nav2_util::LifecycleNode
   std::vector<std::string> behavior_types_;
 
   // Utilities
-  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
-  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
-  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> local_costmap_sub_;
+  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> local_footprint_sub_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker_;
+
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> global_costmap_sub_;
+  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> global_footprint_sub_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker_;
 };
 
 }  // namespace behavior_server

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -104,7 +104,9 @@ class TimedBehavior : public nav2_core::Behavior
   void configure(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
-    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) override
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker,
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker)
+  override
   {
     node_ = parent;
     auto node = node_.lock();
@@ -117,6 +119,7 @@ class TimedBehavior : public nav2_core::Behavior
     tf_ = tf;
 
     node->get_parameter("cycle_frequency", cycle_frequency_);
+    node->get_parameter("local_frame", local_frame_);
     node->get_parameter("global_frame", global_frame_);
     node->get_parameter("robot_base_frame", robot_base_frame_);
     node->get_parameter("transform_tolerance", transform_tolerance_);
@@ -125,7 +128,8 @@ class TimedBehavior : public nav2_core::Behavior
       node, behavior_name_,
       std::bind(&TimedBehavior::execute, this));
 
-    collision_checker_ = collision_checker;
+    local_collision_checker_ = local_collision_checker;
+    global_collision_checker_ = global_collision_checker;
 
     vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
 
@@ -164,11 +168,13 @@ class TimedBehavior : public nav2_core::Behavior
   std::string behavior_name_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
   std::shared_ptr<ActionServer> action_server_;
-  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker_;
+  std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
 
   double cycle_frequency_;
   double enabled_;
+  std::string local_frame_;
   std::string global_frame_;
   std::string robot_base_frame_;
   double transform_tolerance_;

nav2_behaviors/plugins/assisted_teleop.cpp

@@ -100,7 +100,7 @@ Status AssistedTeleop::onCycleUpdate()
 
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, *tf_, global_frame_, robot_base_frame_,
+      current_pose, *tf_, local_frame_, robot_base_frame_,
       transform_tolerance_))
   {
     RCLCPP_ERROR_STREAM(
@@ -120,7 +120,7 @@ Status AssistedTeleop::onCycleUpdate()
   {
     projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_);
 
-    if (!collision_checker_->isCollisionFree(projected_pose)) {
+    if (!local_collision_checker_->isCollisionFree(projected_pose)) {
       if (time == simulation_time_step_) {
         RCLCPP_DEBUG_STREAM_THROTTLE(
           logger_,

nav2_behaviors/plugins/assisted_teleop.hpp

@@ -34,6 +34,8 @@ using AssistedTeleopAction = nav2_msgs::action::AssistedTeleop;
  */
 class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
 {
+  using CostmapInfoType = nav2_core::CostmapInfoType;
+
 public:
   AssistedTeleop();
 
@@ -55,6 +57,12 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
    */
   Status onCycleUpdate() override;
 
+  /**
+   * @brief Method to determine the required costmap info
+   * @return costmap resources needed
+   */
+  CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
+
 protected:
   /**
    * @brief Configuration of behavior action

nav2_behaviors/plugins/back_up.cpp

@@ -34,7 +34,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
   end_time_ = steady_clock_.now() + command_time_allowance_;
 
   if (!nav2_util::getCurrentPose(
-      initial_pose_, *tf_, global_frame_, robot_base_frame_,
+      initial_pose_, *tf_, local_frame_, robot_base_frame_,
       transform_tolerance_))
   {
     RCLCPP_ERROR(logger_, "Initial robot pose is not available.");

nav2_behaviors/plugins/drive_on_heading.hpp

@@ -35,6 +35,8 @@ namespace nav2_behaviors
 template<typename ActionT = nav2_msgs::action::DriveOnHeading>
 class DriveOnHeading : public TimedBehavior<ActionT>
 {
+  using CostmapInfoType = nav2_core::CostmapInfoType;
+
 public:
   /**
    * @brief A constructor for nav2_behaviors::DriveOnHeading
@@ -77,7 +79,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     end_time_ = this->steady_clock_.now() + command_time_allowance_;
 
     if (!nav2_util::getCurrentPose(
-        initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_,
+        initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_,
         this->transform_tolerance_))
     {
       RCLCPP_ERROR(this->logger_, "Initial robot pose is not available.");
@@ -104,7 +106,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
 
     geometry_msgs::msg::PoseStamped current_pose;
     if (!nav2_util::getCurrentPose(
-        current_pose, *this->tf_, this->global_frame_, this->robot_base_frame_,
+        current_pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
         this->transform_tolerance_))
     {
       RCLCPP_ERROR(this->logger_, "Current robot pose is not available.");
@@ -144,6 +146,12 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     return Status::RUNNING;
   }
 
+  /**
+   * @brief Method to determine the required costmap info
+   * @return costmap resources needed
+   */
+  CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
+
 protected:
   /**
    * @brief Check if pose is collision free
@@ -175,7 +183,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
         break;
       }
 
-      if (!this->collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+      if (!this->local_collision_checker_->isCollisionFree(pose2d, fetch_data)) {
         return false;
       }
       fetch_data = false;

nav2_behaviors/plugins/spin.cpp

@@ -75,7 +75,7 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 {
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, *tf_, global_frame_, robot_base_frame_,
+      current_pose, *tf_, local_frame_, robot_base_frame_,
       transform_tolerance_))
   {
     RCLCPP_ERROR(logger_, "Current robot pose is not available.");
@@ -109,7 +109,7 @@ Status Spin::onCycleUpdate()
 
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, *tf_, global_frame_, robot_base_frame_,
+      current_pose, *tf_, local_frame_, robot_base_frame_,
       transform_tolerance_))
   {
     RCLCPP_ERROR(logger_, "Current robot pose is not available.");
@@ -178,7 +178,7 @@ bool Spin::isCollisionFree(
       break;
     }
 
-    if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
+    if (!local_collision_checker_->isCollisionFree(pose2d, fetch_data)) {
       return false;
     }
     fetch_data = false;

nav2_behaviors/plugins/spin.hpp

@@ -33,6 +33,8 @@ using SpinAction = nav2_msgs::action::Spin;
  */
 class Spin : public TimedBehavior<SpinAction>
 {
+  using CostmapInfoType = nav2_core::CostmapInfoType;
+
 public:
   /**
    * @brief A constructor for nav2_behaviors::Spin
@@ -58,6 +60,12 @@ class Spin : public TimedBehavior<SpinAction>
    */
   Status onCycleUpdate() override;
 
+  /**
+   * @brief Method to determine the required costmap info
+   * @return costmap resources needed
+   */
+  CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
+
 protected:
   /**
    * @brief Check if pose is collision free

nav2_behaviors/plugins/wait.hpp

@@ -32,6 +32,8 @@ using WaitAction = nav2_msgs::action::Wait;
  */
 class Wait : public TimedBehavior<WaitAction>
 {
+  using CostmapInfoType = nav2_core::CostmapInfoType;
+
 public:
   /**
    * @brief A constructor for nav2_behaviors::Wait
@@ -52,6 +54,12 @@ class Wait : public TimedBehavior<WaitAction>
    */
   Status onCycleUpdate() override;
 
+  /**
+   * @brief Method to determine the required costmap info
+   * @return costmap resources needed
+   */
+  CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
+
 protected:
   std::chrono::time_point<std::chrono::steady_clock> wait_end_;
   WaitAction::Feedback::SharedPtr feedback_;