From 384bccbeb7e1666381f06d9ec754ba426a986b7f Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Tue, 6 Aug 2024 13:20:56 +0200
Subject: [PATCH] remove second costmap

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 .../remove_in_collision_goals_action.hpp      |  4 --
 .../remove_in_collision_goals_action.cpp      | 46 ++++---------------
 2 files changed, 9 insertions(+), 41 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
index 7e2b90fd25..f548fae09d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
@@ -47,8 +47,6 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase
       BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
       BT::InputPort<std::string>("costmap_cost_service",
           "Service to get cost from costmap"),
-      BT::InputPort<std::string>("costmap_cost_service_2",
-          "Second service name to get cost from costmap. If empty, only first service is used"),
       BT::InputPort<double>("cost_threshold",
           "Cost threshold for considering a goal in collision"),
       BT::InputPort<bool>("use_footprint",
@@ -63,9 +61,7 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase
 
   rclcpp::Node::SharedPtr node_;
   rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_;
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_2_;
   std::string costmap_cost_service_;
-  std::string costmap_cost_service_2_;
   bool initialized_;
   bool use_footprint_;
   double cost_threshold_;
diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
index eea32e0bfd..0c65723ec0 100644
--- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
+++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp
@@ -30,7 +30,6 @@ RemoveInCollisionGoals::RemoveInCollisionGoals(
 : BT::ActionNodeBase(name, conf),
   initialized_(false),
   costmap_cost_service_("/global_costmap/get_cost_global_costmap"),
-  costmap_cost_service_2_(""),
   use_footprint_(true),
   cost_threshold_(253)
 {}
@@ -41,15 +40,10 @@ void RemoveInCollisionGoals::initialize()
   server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
 
   getInput("costmap_cost_service", costmap_cost_service_);
-  getInput("costmap_cost_service_2", costmap_cost_service_2_);
 
   get_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
     costmap_cost_service_);
 
-  if (!costmap_cost_service_2_.empty()) {
-    get_cost_client_2_ = node_->create_client<nav2_msgs::srv::GetCost>(
-      costmap_cost_service_2_);
-  }
 }
 
 inline BT::NodeStatus RemoveInCollisionGoals::tick()
@@ -79,39 +73,17 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick()
     request->theta = tf2::getYaw(goal.pose.orientation);
     request->use_footprint = use_footprint_;
 
-    if (costmap_cost_service_2_.empty()) {
-      auto future = get_cost_client_->async_send_request(request);
-      auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
-      if (ret == rclcpp::FutureReturnCode::SUCCESS) {
-        if (future.get()->cost <= cost_threshold_) {
-          valid_goal_poses.push_back(goal);
-        }
-      } else {
-        RCLCPP_ERROR(
-          node_->get_logger(),
-          "RemoveInCollisionGoals BT node failed to call GetCost service of costmap");
-        return BT::NodeStatus::FAILURE;
+    auto future = get_cost_client_->async_send_request(request);
+    auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
+    if (ret == rclcpp::FutureReturnCode::SUCCESS) {
+      if (future.get()->cost <= cost_threshold_) {
+        valid_goal_poses.push_back(goal);
       }
     } else {
-      auto future = get_cost_client_->async_send_request(request);
-      auto future_2 = get_cost_client_2_->async_send_request(request);
-      auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
-      auto ret_2 = rclcpp::spin_until_future_complete(node_, future_2, server_timeout_);
-      if (ret == rclcpp::FutureReturnCode::SUCCESS &&
-        ret_2 == rclcpp::FutureReturnCode::SUCCESS)
-      {
-        if (future.get()->cost <= cost_threshold_ &&
-          future_2.get()->cost <= cost_threshold_)
-        {
-          valid_goal_poses.push_back(goal);
-        }
-      } else {
-        RCLCPP_ERROR(
-          node_->get_logger(),
-          "RemoveInCollisionGoals BT node failed to call GetCost "
-          "service of one or two costmaps");
-        return BT::NodeStatus::FAILURE;
-      }
+      RCLCPP_ERROR(
+        node_->get_logger(),
+        "RemoveInCollisionGoals BT node failed to call GetCost service of costmap");
+      return BT::NodeStatus::FAILURE;
     }
   }
   setOutput("output_goals", valid_goal_poses);