Remove in collision goals BT node

nav2_behavior_tree/CMakeLists.txt

@@ -176,6 +176,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
 add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
 list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
 
+add_library(nav2_remove_in_collision_goals_action_bt_node SHARED plugins/action/remove_in_collision_goals_action.cpp)
+list(APPEND plugin_libs nav2_remove_in_collision_goals_action_bt_node)
+
 add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
 list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -0,0 +1,72 @@
+// Copyright (c) 2024 Angsa Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_
+
+#include <vector>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_service_node.hpp"
+#include "nav2_msgs/srv/get_costs.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
+{
+public:
+  typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
+
+  /**
+   * @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals
+   * @param service_node_name Service name this node creates a client for
+   * @param conf BT node configuration
+   */
+  RemoveInCollisionGoals(
+    const std::string & service_node_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief The main override required by a BT service
+   * @return BT::NodeStatus Status of tick execution
+   */
+  void on_tick() override;
+
+  BT::NodeStatus on_completion(std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
+  override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+        BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
+        BT::InputPort<double>("cost_threshold", 254.0,
+          "Cost threshold for considering a goal in collision"),
+        BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
+        BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
+    });
+  }
+
+private:
+  bool use_footprint_;
+  double cost_threshold_;
+  Goals input_goals_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_

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>

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -0,0 +1,77 @@
+// Copyright (c) 2024 Angsa Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+#include <limits>
+
+#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
+#include "tf2/utils.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+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,
+    "/global_costmap/get_cost_global_costmap")
+{}
+
+
+void RemoveInCollisionGoals::on_tick()
+{
+  getInput("use_footprint", use_footprint_);
+  getInput("cost_threshold", cost_threshold_);
+  getInput("input_goals", input_goals_);
+
+  if (input_goals_.empty()) {
+    setOutput("output_goals", input_goals_);
+    should_send_request_ = false;
+    return;
+  }
+  request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
+  request_->use_footprint = use_footprint_;
+
+  for (const auto & goal : input_goals_) {
+    geometry_msgs::msg::Pose2D pose;
+    pose.x = goal.pose.position.x;
+    pose.y = goal.pose.position.y;
+    pose.theta = tf2::getYaw(goal.pose.orientation);
+    request_->poses.push_back(pose);
+  }
+}
+
+BT::NodeStatus RemoveInCollisionGoals::on_completion(
+  std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
+{
+  Goals valid_goal_poses;
+  for (size_t i = 0; i < response->costs.size(); ++i) {
+    if (response->costs[i] < cost_threshold_) {
+      valid_goal_poses.push_back(input_goals_[i]);
+    }
+  }
+  setOutput("output_goals", valid_goal_poses);
+  return BT::NodeStatus::SUCCESS;
+}
+
+}   // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RemoveInCollisionGoals>("RemoveInCollisionGoals");
+}

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -43,6 +43,7 @@ void RemovePassedGoals::initialize()
 
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node, "robot_base_frame", this);
+  initialized_ = true;
 }
 
 inline BT::NodeStatus RemovePassedGoals::tick()

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -51,6 +51,10 @@ plugin_add_test(test_truncate_path_local_action test_truncate_path_local_action.
 
 plugin_add_test(test_remove_passed_goals_action test_remove_passed_goals_action.cpp nav2_remove_passed_goals_action_bt_node)
 
+plugin_add_test(test_remove_in_collision_goals_action
+  test_remove_in_collision_goals_action.cpp
+  nav2_remove_in_collision_goals_action_bt_node)
+
 plugin_add_test(test_get_pose_from_path_action test_get_pose_from_path_action.cpp nav2_get_pose_from_path_action_bt_node)
 
 plugin_add_test(test_planner_selector_node test_planner_selector_node.cpp nav2_planner_selector_bt_node)

nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp

@@ -0,0 +1,180 @@
+// Copyright (c) 2024 Angsa Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+#include "behaviortree_cpp/bt_factory.h"
+
+#include "utils/test_service.hpp"
+#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp"
+#include "utils/test_behavior_tree_fixture.hpp"
+
+
+class RemoveInCollisionGoalsService : public TestService<nav2_msgs::srv::GetCosts>
+{
+public:
+  RemoveInCollisionGoalsService()
+  : TestService("/global_costmap/get_cost_global_costmap")
+  {}
+
+  virtual void handle_service(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
+  {
+    (void)request_header;
+    (void)request;
+    response->costs = {100, 50, 5, 254};
+  }
+};
+
+
+class RemoveInCollisionGoalsTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("in_collision_goals_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set(
+      "node",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+
+    BT::NodeBuilder builder =
+      [](const std::string & name, const BT::NodeConfiguration & config)
+      {
+        return std::make_unique<nav2_behavior_tree::RemoveInCollisionGoals>(
+          name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::RemoveInCollisionGoals>(
+      "RemoveInCollisionGoals", builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+  static std::shared_ptr<RemoveInCollisionGoalsService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr;
+std::shared_ptr<RemoveInCollisionGoalsService>
+RemoveInCollisionGoalsTestFixture::server_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> RemoveInCollisionGoalsTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> RemoveInCollisionGoalsTestFixture::tree_ = nullptr;
+
+TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap" input_goals="{goals}" output_goals="{goals}" cost_threshold="253"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  poses.resize(4);
+  poses[0].pose.position.x = 0.0;
+  poses[0].pose.position.y = 0.0;
+
+  poses[1].pose.position.x = 0.5;
+  poses[1].pose.position.y = 0.0;
+
+  poses[2].pose.position.x = 1.0;
+  poses[2].pose.position.y = 0.0;
+
+  poses[3].pose.position.x = 2.0;
+  poses[3].pose.position.y = 0.0;
+
+  config_->blackboard->set("goals", poses);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check that it removed the point in range
+  std::vector<geometry_msgs::msg::PoseStamped> output_poses;
+  EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
+
+  EXPECT_EQ(output_poses.size(), 3u);
+  EXPECT_EQ(output_poses[0], poses[0]);
+  EXPECT_EQ(output_poses[1], poses[1]);
+  EXPECT_EQ(output_poses[2], poses[2]);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize service and spin on new thread
+  RemoveInCollisionGoalsTestFixture::server_ =
+    std::make_shared<RemoveInCollisionGoalsService>();
+  std::thread server_thread([]() {
+      rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}