Added theta, Updated unit test, Updated rviz tool service call logic

Signed-off-by: Jatin Patil <[email protected]>

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -47,6 +47,7 @@
 #include "geometry_msgs/msg/polygon_stamped.h"
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
 #include "nav2_costmap_2d/footprint.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/clear_costmap_service.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_costmap_2d/layer.hpp"

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -830,29 +830,24 @@ void Costmap2DROS::getCostCallback(
 {
   unsigned int mx, my;
 
-  auto costmap = layered_costmap_->getCostmap();
+  Costmap2D * costmap = layered_costmap_->getCostmap();
 
   if (request->use_footprint) {
-    geometry_msgs::msg::PoseStamped global_pose;
-    // get the pose of footprint
-    if (!getRobotPose(global_pose)) {
-      RCLCPP_WARN(get_logger(), "Failed to get robot pose");
-      response->cost = -1.0;
-    }
+    Footprint footprint = layered_costmap_->getFootprint();
+    FootprintCollisionChecker<Costmap2D *> collision_checker_(costmap);
+
     RCLCPP_INFO(
-      get_logger(), "Received request to get cost at point (%f, %f)", global_pose.pose.position.x,
-      global_pose.pose.position.y);
-    // Get the cost at the map coordinates
-    if (costmap->worldToMap(global_pose.pose.position.x, global_pose.pose.position.y, mx, my)) {
-      auto cost = static_cast<float>(costmap->getCost(mx, my));
-      response->cost = cost;
-    }
+      get_logger(), "Received request to get cost at footprint (%.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(
       get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y);
+
     // Get the cost at the map coordinates
-    auto cost = static_cast<float>(costmap->getCost(mx, my));
-    response->cost = cost;
+    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_costmap_2d/test/unit/costmap_cost_service_test.cpp

@@ -50,16 +50,18 @@ class GetCostServiceTest : public ::testing::Test
 TEST_F(GetCostServiceTest, TestWithoutFootprint)
 {
   auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
-  request->x = 0.0;
-  request->y = 0.0;
+  request->x = 0.5;
+  request->y = 1.0;
   request->use_footprint = false;
 
   auto result_future = client_->async_send_request(request);
   if (rclcpp::spin_until_future_complete(
       costmap_,
       result_future) == rclcpp::FutureReturnCode::SUCCESS)
   {
-    SUCCEED();
+    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";
   } else {
     FAIL() << "Failed to call service";
   }
@@ -68,16 +70,19 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint)
 TEST_F(GetCostServiceTest, TestWithFootprint)
 {
   auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
-  request->x = 0.0;
-  request->y = 0.0;
+  request->x = 1.0;
+  request->y = 1.0;
+  request->theta = 0.5;
   request->use_footprint = true;
 
   auto result_future = client_->async_send_request(request);
   if (rclcpp::spin_until_future_complete(
       costmap_,
       result_future) == rclcpp::FutureReturnCode::SUCCESS)
   {
-    SUCCEED();
+    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";
   } else {
     FAIL() << "Failed to call service";
   }

nav2_msgs/srv/GetCost.srv

@@ -3,5 +3,6 @@
 bool use_footprint
 float32 x
 float32 y
+float32 theta
 ---
 float32 cost

nav2_rviz_plugins/src/costmap_cost_tool.cpp

@@ -73,7 +73,6 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event)
 
       callCostService(point.x, point.y);
 
-
       if (auto_deactivate_property_->getBool()) {
         flags |= Finished;
       }
@@ -92,22 +91,16 @@ void CostmapCostTool::callCostService(float x, float y)
   request->y = y;
 
   // Call local costmap service
-  if (!local_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    RCLCPP_ERROR(node_->get_logger(), "Local costmap service not available");
-    return;
+  if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) {
+    local_cost_client_->async_send_request(request,
+      std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1));
   }
 
-  local_cost_client_->async_send_request(request,
-    std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1));
-
   // Call global costmap service
-  if (!global_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    RCLCPP_ERROR(node_->get_logger(), "Global costmap service not available");
-    return;
+  if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) {
+    global_cost_client_->async_send_request(request,
+      std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1));
   }
-
-  global_cost_client_->async_send_request(request,
-    std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1));
 }
 
 void CostmapCostTool::handleLocalCostResponse(