From 3f0c6c515bfcf1b78698471e3ece3e1564c50c4f Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Thu, 8 Aug 2024 08:34:29 +0000
Subject: [PATCH] pr comments fixes

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 .../plugins/action/remove_in_collision_goals_action.hpp   | 7 ++-----
 nav2_behavior_tree/nav2_tree_nodes.xml                    | 8 ++++++++
 .../plugins/action/remove_in_collision_goals_action.cpp   | 6 ++----
 3 files changed, 12 insertions(+), 9 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 57c615027a..73aab1bd8c 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
@@ -54,12 +54,9 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
   {
     return providedBasicPorts({
         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<double>("cost_threshold",
+        BT::InputPort<double>("cost_threshold", 254.0,
           "Cost threshold for considering a goal in collision"),
-        BT::InputPort<bool>("use_footprint",
-          "Whether to use footprint cost"),
+        BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
         BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
     });
   }
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index 925e9c3579..71c20ee4ec 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -100,6 +100,14 @@
       <output_port name="output_goals">Set of goals after removing any passed</output_port>
     </Action>
 
+    <Action ID="RemoveInCollisionGoals">
+      <input_port name="service_name">Costmap service name responsible for getting the cost</input_port>
+      <input_port name="input_goals">A vector of goals to check if in collision</input_port>
+      <input_port name="use_footprint">Whether to use the footprint cost or the point cost.</input_port>
+      <input_port name="cost_threshold">The cost threshold above which a waypoint is considered in collision and should be removed.</input_port>
+      <output_port name="output_goals">A vector of goals containing only those that are not in collision.</output_port>
+    </Action>
+
     <Action ID="SmoothPath">
       <input_port name="smoother_id" default="SmoothPath"/>
       <input_port name="unsmoothed_path">Path to be smoothed</input_port>
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 7a7f8c226f..4ae9929168 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
@@ -27,9 +27,8 @@ namespace nav2_behavior_tree
 RemoveInCollisionGoals::RemoveInCollisionGoals(
   const std::string & service_node_name,
   const BT::NodeConfiguration & conf)
-: BtServiceNode<nav2_msgs::srv::GetCosts>(service_node_name, conf),
-  use_footprint_(true),
-  cost_threshold_(253)
+: BtServiceNode<nav2_msgs::srv::GetCosts>(service_node_name, conf,
+    "/global_costmap/get_cost_global_costmap")
 {}
 
 
@@ -37,7 +36,6 @@ void RemoveInCollisionGoals::on_tick()
 {
   getInput("use_footprint", use_footprint_);
   getInput("cost_threshold", cost_threshold_);
-
   getInput("input_goals", input_goals_);
 
   if (input_goals_.empty()) {