Added Bool use_footprint to srv

Signed-off-by: Jatin Patil <[email protected]>
ros-navigation · Jul 23, 2024 · fc6efda · fc6efda
1 parent ccc055f
commit fc6efda
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 6 deletions.
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -828,13 +828,28 @@ void Costmap2DROS::getCostCallback(
   const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
   const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response)
 {
-  RCLCPP_INFO(
-    get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y);
+  unsigned int mx, my;
 
   auto costmap = layered_costmap_->getCostmap();
 
-  unsigned int mx, my;
-  if (costmap->worldToMap(request->x, request->y, mx, my)) {
+  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;
+    }
+    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;
+    }
+  } 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;

diff --git a/nav2_msgs/srv/GetCost.srv b/nav2_msgs/srv/GetCost.srv
@@ -1,5 +1,6 @@
 # Get costmap cost at given point
 
+bool use_footprint
 float32 x
 float32 y
 ---

diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp
@@ -115,7 +115,7 @@ void CostmapCostTool::handleLocalCostResponse(
 {
   auto response = future.get();
   if (response->cost != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %f", response->cost);
+    RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %.1f", response->cost);
   } else {
     RCLCPP_ERROR(node_->get_logger(), "Failed to get local costmap cost");
   }
@@ -126,7 +126,7 @@ void CostmapCostTool::handleGlobalCostResponse(
 {
   auto response = future.get();
   if (response->cost != -1) {
-    RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %f", response->cost);
+    RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %.1f", response->cost);
   } else {
     RCLCPP_ERROR(node_->get_logger(), "Failed to get global costmap cost");
   }