Remove goals in collision

Signed-off-by: Tony Najjar <[email protected]>

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

@@ -0,0 +1,69 @@
+// 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 "behaviortree_cpp_v3/action_node.h"
+#include "nav2_msgs/srv/get_cost.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class RemoveInCollisionGoals : public BT::ActionNodeBase
+{
+public:
+  typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
+
+  RemoveInCollisionGoals(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
+      BT::InputPort<std::string>("costmap", "Which costmap to use for checking collision. Choices: local, global, both"),
+      BT::InputPort<std::string>("cost_threshold", "Cost threshold for considering a goal in collision"),
+      BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
+    };
+  }
+
+private:
+  void halt() override {}
+  BT::NodeStatus tick() override;
+
+  rclcpp::Node::SharedPtr node_;
+  bool initialized_;
+  std::string which_costmap_;
+  double cost_threshold_;
+  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_global_cost_client_;
+  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_local_cost_client_;
+  std::chrono::milliseconds server_timeout_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -0,0 +1,135 @@
+// 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 "nav2_costmap_2d/cost_values.hpp"
+#include "tf2/utils.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RemoveInCollisionGoals::RemoveInCollisionGoals(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(name, conf),
+  initialized_(false),
+  which_costmap_("both"),
+  cost_threshold_(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+{}
+
+void RemoveInCollisionGoals::initialize()
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  get_global_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
+    "global_costmap/get_cost_global_costmap");
+  get_local_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
+    "local_costmap/get_cost_local_costmap");
+  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+}
+
+inline BT::NodeStatus RemoveInCollisionGoals::tick()
+{
+  setStatus(BT::NodeStatus::RUNNING);
+
+  if (!initialized_) {
+    initialize();
+  }
+
+  getInput("costmap", which_costmap_);
+  getInput("cost_threshold", cost_threshold_);
+
+  Goals goal_poses;
+  getInput("input_goals", goal_poses);
+
+  if (goal_poses.empty()) {
+    setOutput("output_goals", goal_poses);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  Goals valid_goal_poses;
+  for (const auto & goal : goal_poses) {
+    auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
+    request->x = goal.pose.position.x;
+    request->y = goal.pose.position.y;
+    request->theta = tf2::getYaw(goal.pose.orientation);
+    request->use_footprint = true;
+
+    if (which_costmap_ == "global") {
+      auto future_global = get_global_cost_client_->async_send_request(request);
+      auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_);
+      if (ret_global == rclcpp::FutureReturnCode::SUCCESS) {
+        if (future_global.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 global costmap");
+        return BT::NodeStatus::FAILURE;
+      }
+    } else if (which_costmap_ == "local") {
+      auto future_local = get_local_cost_client_->async_send_request(request);
+      auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_);
+      if (ret_local == rclcpp::FutureReturnCode::SUCCESS) {
+        if (future_local.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 local costmap");
+        return BT::NodeStatus::FAILURE;
+      }
+    } else if (which_costmap_ == "both") {
+      auto future_global = get_global_cost_client_->async_send_request(request);
+      auto future_local = get_local_cost_client_->async_send_request(request);
+      auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_);
+      auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_);
+      if (ret_local == rclcpp::FutureReturnCode::SUCCESS &&
+        ret_global == rclcpp::FutureReturnCode::SUCCESS)
+      {
+        if (future_local.get()->cost <= cost_threshold_ &&
+          future_global.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 local or global costmap");
+        return BT::NodeStatus::FAILURE;
+      }
+    } else {
+      RCLCPP_ERROR(
+        node_->get_logger(), "The costmap parameter must be either 'local', 'global', or 'both'");
+      return BT::NodeStatus::FAILURE;
+    }
+  }
+  setOutput("output_goals", valid_goal_poses);
+  return BT::NodeStatus::SUCCESS;
+}
+
+}   // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RemoveInCollisionGoals>("RemoveInCollisionGoals");
+}

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -834,25 +834,30 @@ void Costmap2DROS::getCostCallback(
 
   Costmap2D * costmap = layered_costmap_->getCostmap();
 
+  bool in_bounds = costmap->worldToMap(request->x, request->y, mx, my);
+
+  if (!in_bounds) {
+    response->cost = -1.0;
+    return;
+  }
+
   if (request->use_footprint) {
     Footprint footprint = layered_costmap_->getFootprint();
     FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
 
-    RCLCPP_INFO(
+    RCLCPP_DEBUG(
       get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
       request->x, request->y, request->theta);
 
     response->cost = collision_checker.footprintCostAtPose(
       request->x, request->y, request->theta, footprint);
-  } else if (costmap->worldToMap(request->x, request->y, mx, my)) {
-    RCLCPP_INFO(
+  }
+  else {
+    RCLCPP_DEBUG(
       get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y);
 
     // Get the cost at the map coordinates
     response->cost = static_cast<float>(costmap->getCost(mx, my));
-  } else {
-    RCLCPP_WARN(get_logger(), "Point (%f, %f) is out of bounds", request->x, request->y);
-    response->cost = -1.0;
   }
 }
 

nav2_rviz_plugins/src/costmap_cost_tool.cpp

@@ -89,6 +89,7 @@ void CostmapCostTool::callCostService(float x, float y)
   auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
   request->x = x;
   request->y = y;
+  // request->use_footprint = true;
 
   // Call local costmap service
   if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) {