getcosts instead of cost

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

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

@@ -22,7 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "behaviortree_cpp/action_node.h"
-#include "nav2_msgs/srv/get_cost.hpp"
+#include "nav2_msgs/srv/get_costs.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -60,7 +60,7 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase
   BT::NodeStatus tick() override;
 
   rclcpp::Node::SharedPtr node_;
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_;
+  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr get_cost_client_;
   bool initialized_;
   std::string costmap_cost_service_;
   bool use_footprint_;

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -41,9 +41,8 @@ void RemoveInCollisionGoals::initialize()
 
   getInput("costmap_cost_service", costmap_cost_service_);
 
-  get_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
+  get_cost_client_ = node_->create_client<nav2_msgs::srv::GetCosts>(
     costmap_cost_service_);
-
 }
 
 inline BT::NodeStatus RemoveInCollisionGoals::tick()
@@ -66,25 +65,30 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick()
   }
 
   Goals valid_goal_poses;
+  auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
+  request->use_footprint = use_footprint_;
+
   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 = use_footprint_;
-
-    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);
+    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);
+  }
+
+  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) {
+    for (size_t i = 0; i < future.get()->costs.size(); ++i) {
+      if (future.get()->costs[i] <= cost_threshold_) {
+        valid_goal_poses.push_back(goal_poses[i]);
       }
-    } else {
-      RCLCPP_ERROR(
-        node_->get_logger(),
-        "RemoveInCollisionGoals BT node failed to call GetCost service of costmap");
-      return BT::NodeStatus::FAILURE;
     }
+  } else {
+    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);
   return BT::NodeStatus::SUCCESS;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -52,7 +52,7 @@
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "nav2_msgs/srv/get_cost.hpp"
+#include "nav2_msgs/srv/get_costs.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "tf2/convert.h"
 #include "tf2/LinearMath/Transform.h"
@@ -345,10 +345,10 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    * @param request x and y coordinates in map
    * @param response cost of the point
   */
-  void getCostCallback(
+  void getCostsCallback(
     const std::shared_ptr<rmw_request_id_t>,
-    const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
-    const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response);
+    const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response);
 
 protected:
   // Publishers and subscribers
@@ -425,7 +425,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
   // Services
-  rclcpp::Service<nav2_msgs::srv::GetCost>::SharedPtr get_cost_service_;
+  rclcpp::Service<nav2_msgs::srv::GetCosts>::SharedPtr get_cost_service_;
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
 
   // Dynamic parameters handler

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -252,9 +252,9 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   }
 
   // Service to get the cost at a point
-  get_cost_service_ = create_service<nav2_msgs::srv::GetCost>(
+  get_cost_service_ = create_service<nav2_msgs::srv::GetCosts>(
     "get_cost_" + getName(),
-    std::bind(&Costmap2DROS::getCostCallback, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
   // Add cleaning service
@@ -825,38 +825,40 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
   return result;
 }
 
-void Costmap2DROS::getCostCallback(
+void Costmap2DROS::getCostsCallback(
   const std::shared_ptr<rmw_request_id_t>,
-  const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
-  const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response)
+  const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
+  const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
 {
   unsigned int mx, my;
 
   Costmap2D * costmap = layered_costmap_->getCostmap();
 
-  bool in_bounds = costmap->worldToMap(request->x, request->y, mx, my);
+  for (const auto & pose : request->poses) {
+    bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my);
 
-  if (!in_bounds) {
-    response->cost = -1.0;
-    return;
-  }
+    if (!in_bounds) {
+      response->costs.push_back(-1.0);
+      continue;
+    }
 
-  if (request->use_footprint) {
-    Footprint footprint = layered_costmap_->getFootprint();
-    FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
+    if (request->use_footprint) {
+      Footprint footprint = layered_costmap_->getFootprint();
+      FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
 
-    RCLCPP_DEBUG(
-      get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
-      request->x, request->y, request->theta);
+      RCLCPP_DEBUG(
+        get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
+        pose.x, pose.y, pose.theta);
 
-    response->cost = collision_checker.footprintCostAtPose(
-      request->x, request->y, request->theta, footprint);
-  } else {
-    RCLCPP_DEBUG(
-      get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y);
+      response->costs.push_back(
+        collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint));
+    } else {
+      RCLCPP_DEBUG(
+        get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y);
 
-    // Get the cost at the map coordinates
-    response->cost = static_cast<float>(costmap->getCost(mx, my));
+      // Get the cost at the map coordinates
+      response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));
+    }
   }
 }
 

nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp

@@ -18,7 +18,7 @@
 #include <chrono>
 
 #include <rclcpp/rclcpp.hpp>
-#include "nav2_msgs/srv/get_cost.hpp"
+#include "nav2_msgs/srv/get_costs.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
 class RclCppFixture
@@ -37,21 +37,23 @@ class GetCostServiceTest : public ::testing::Test
   void SetUp() override
   {
     costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap");
-    client_ = costmap_->create_client<nav2_msgs::srv::GetCost>(
+    client_ = costmap_->create_client<nav2_msgs::srv::GetCosts>(
       "/costmap/get_cost_costmap");
     costmap_->on_configure(rclcpp_lifecycle::State());
     ASSERT_TRUE(client_->wait_for_service(10s));
   }
 
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr client_;
+  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr client_;
 };
 
 TEST_F(GetCostServiceTest, TestWithoutFootprint)
 {
-  auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
-  request->x = 0.5;
-  request->y = 1.0;
+  auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
+  geometry_msgs::msg::Pose2D pose;
+  pose.x = 0.5;
+  pose.y = 1.0;
+  request->poses.push_back(pose);
   request->use_footprint = false;
 
   auto result_future = client_->async_send_request(request);
@@ -60,19 +62,21 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint)
       result_future) == rclcpp::FutureReturnCode::SUCCESS)
   {
     auto response = result_future.get();
-    EXPECT_GE(response->cost, 0.0) << "Cost is less than 0";
-    EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255";
+    EXPECT_GE(response->costs[0], 0.0) << "Cost is less than 0";
+    EXPECT_LE(response->costs[0], 255.0) << "Cost is greater than 255";
   } else {
     FAIL() << "Failed to call service";
   }
 }
 
 TEST_F(GetCostServiceTest, TestWithFootprint)
 {
-  auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
-  request->x = 1.0;
-  request->y = 1.0;
-  request->theta = 0.5;
+  auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
+  geometry_msgs::msg::Pose2D pose;
+  pose.x = 0.5;
+  pose.y = 1.0;
+  pose.theta = 0.5;
+  request->poses.push_back(pose);
   request->use_footprint = true;
 
   auto result_future = client_->async_send_request(request);
@@ -81,8 +85,8 @@ TEST_F(GetCostServiceTest, TestWithFootprint)
       result_future) == rclcpp::FutureReturnCode::SUCCESS)
   {
     auto response = result_future.get();
-    EXPECT_GE(response->cost, 0.0) << "Cost is less than 0";
-    EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255";
+    EXPECT_GE(response->costs[0], 0.0) << "Cost is less than 0";
+    EXPECT_LE(response->costs[0], 255.0) << "Cost is greater than 255";
   } else {
     FAIL() << "Failed to call service";
   }

nav2_msgs/CMakeLists.txt

@@ -30,7 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Particle.msg"
   "msg/ParticleCloud.msg"
   "msg/MissedWaypoint.msg"
-  "srv/GetCost.srv"
+  "srv/GetCosts.srv"
   "srv/GetCostmap.srv"
   "srv/IsPathValid.srv"
   "srv/ClearCostmapExceptRegion.srv"

nav2_msgs/srv/GetCosts.srv

@@ -0,0 +1,6 @@
+# Get costmap costs at given poses
+
+bool use_footprint
+geometry_msgs/Pose2D[] poses
+---
+float32[] costs

nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp

@@ -15,7 +15,7 @@
 #ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
 #define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
 
-#include <nav2_msgs/srv/get_cost.hpp>
+#include <nav2_msgs/srv/get_costs.hpp>
 #include <rviz_common/tool.hpp>
 #include <rviz_default_plugins/tools/point/point_tool.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -38,15 +38,15 @@ class CostmapCostTool : public rviz_common::Tool
 
   void callCostService(float x, float y);
 
-  void handleLocalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);
-  void handleGlobalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);
+  void handleLocalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture);
+  void handleGlobalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture);
 
 private Q_SLOTS:
   void updateAutoDeactivate();
 
 private:
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr local_cost_client_;
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr global_cost_client_;
+  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr local_cost_client_;
+  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedPtr global_cost_client_;
   rclcpp::Node::SharedPtr node_;
 
   QCursor std_cursor_;

nav2_rviz_plugins/src/costmap_cost_tool.cpp

@@ -45,9 +45,9 @@ void CostmapCostTool::onInitialize()
 
   node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
   local_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCost>("/local_costmap/get_cost_local_costmap");
+    node_->create_client<nav2_msgs::srv::GetCosts>("/local_costmap/get_cost_local_costmap");
   global_cost_client_ =
-    node_->create_client<nav2_msgs::srv::GetCost>("/global_costmap/get_cost_global_costmap");
+    node_->create_client<nav2_msgs::srv::GetCosts>("/global_costmap/get_cost_global_costmap");
 }
 
 void CostmapCostTool::activate() {}
@@ -86,10 +86,12 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event)
 void CostmapCostTool::callCostService(float x, float y)
 {
   // Create request for local costmap
-  auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
-  request->x = x;
-  request->y = y;
-  // request->use_footprint = true;
+  auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
+  geometry_msgs::msg::Pose2D pose;
+  pose.x = x;
+  pose.y = y;
+  request->poses.push_back(pose);
+  request->use_footprint = false;
 
   // Call local costmap service
   if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) {
@@ -105,7 +107,7 @@ void CostmapCostTool::callCostService(float x, float y)
 }
 
 void CostmapCostTool::handleLocalCostResponse(
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture future)
+  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture future)
 {
   auto response = future.get();
   if (response->cost != -1) {
@@ -116,7 +118,7 @@ void CostmapCostTool::handleLocalCostResponse(
 }
 
 void CostmapCostTool::handleGlobalCostResponse(
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture future)
+  rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture future)
 {
   auto response = future.get();
   if (response->cost != -1) {