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 { return providedBasicPorts({ BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("costmap_cost_service", - "Service to get cost from costmap"), - BT::InputPort("cost_threshold", + BT::InputPort("cost_threshold", 254.0, "Cost threshold for considering a goal in collision"), - BT::InputPort("use_footprint", - "Whether to use footprint cost"), + BT::InputPort("use_footprint", true, "Whether to use footprint cost"), BT::OutputPort("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 @@ Set of goals after removing any passed + + Costmap service name responsible for getting the cost + A vector of goals to check if in collision + Whether to use the footprint cost or the point cost. + The cost threshold above which a waypoint is considered in collision and should be removed. + A vector of goals containing only those that are not in collision. + + Path to be smoothed 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(service_node_name, conf), - use_footprint_(true), - cost_threshold_(253) +: BtServiceNode(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()) {