Rviz tool to get cost of costmap cell

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -51,6 +51,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 "pluginlib/class_loader.hpp"
 #include "tf2/convert.h"
 #include "tf2/LinearMath/Transform.h"
@@ -339,6 +340,15 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   double getRobotRadius() {return robot_radius_;}
 
+  /** @brief Get the cost at a point in costmap
+   * @param request x and y coordinates in map
+   * @param response cost of the point
+  */
+  void getCostCallback(
+    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);
+
 protected:
   // Publishers and subscribers
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
@@ -412,6 +422,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
+  // Services
+  rclcpp::Service<nav2_msgs::srv::GetCost>::SharedPtr get_cost_service_;
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
 
   // Dynamic parameters handler

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -250,6 +250,12 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
     setRobotFootprint(new_footprint);
   }
 
+  // Service to get the cost at a point
+  get_cost_service_ = create_service<nav2_msgs::srv::GetCost>(
+    "get_cost_" + getName(),
+    std::bind(&Costmap2DROS::getCostCallback, this, std::placeholders::_1, std::placeholders::_2,
+      std::placeholders::_3));
+
   // Add cleaning service
   clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);
 
@@ -817,4 +823,40 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
   return result;
 }
 
+void Costmap2DROS::getCostCallback(
+  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)
+{
+  unsigned int mx, my;
+
+  auto 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;
+    }
+    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;
+  } else {
+    RCLCPP_WARN(get_logger(), "Point (%f, %f) is out of bounds", request->x, request->y);
+    response->cost = -1.0;
+  }
+}
+
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -8,6 +8,11 @@ target_link_libraries(costmap_convesion_test
   ${PROJECT_NAME}::nav2_costmap_2d_core
 )
 
+ament_add_gtest(costmap_cost_service_test costmap_cost_service_test.cpp)
+target_link_libraries(costmap_cost_service_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
 ament_add_gtest(declare_parameter_test declare_parameter_test.cpp)
 target_link_libraries(declare_parameter_test
   ${PROJECT_NAME}::nav2_costmap_2d_core

nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp

@@ -0,0 +1,84 @@
+// Copyright (c) 2024 Jatin Patil
+//
+// 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 <chrono>
+
+#include <rclcpp/rclcpp.hpp>
+#include "nav2_msgs/srv/get_cost.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+using namespace std::chrono_literals;
+
+class GetCostServiceTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap");
+    client_ = costmap_->create_client<nav2_msgs::srv::GetCost>(
+      "/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_;
+};
+
+TEST_F(GetCostServiceTest, TestWithoutFootprint)
+{
+  auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
+  request->x = 0.0;
+  request->y = 0.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();
+  } else {
+    FAIL() << "Failed to call service";
+  }
+}
+
+TEST_F(GetCostServiceTest, TestWithFootprint)
+{
+  auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
+  request->x = 0.0;
+  request->y = 0.0;
+  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();
+  } else {
+    FAIL() << "Failed to call service";
+  }
+}

nav2_msgs/CMakeLists.txt

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

nav2_msgs/srv/GetCost.srv

@@ -0,0 +1,7 @@
+# Get costmap cost at given point
+
+bool use_footprint
+float32 x
+float32 y
+---
+float32 cost

nav2_rviz_plugins/CMakeLists.txt

@@ -34,6 +34,7 @@ find_package(visualization_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
 set(nav2_rviz_plugins_headers_to_moc
+  include/nav2_rviz_plugins/costmap_cost_tool.hpp
   include/nav2_rviz_plugins/docking_panel.hpp
   include/nav2_rviz_plugins/goal_pose_updater.hpp
   include/nav2_rviz_plugins/goal_common.hpp
@@ -52,6 +53,7 @@ include_directories(
 set(library_name ${PROJECT_NAME})
 
 add_library(${library_name} SHARED
+  src/costmap_cost_tool.cpp
   src/docking_panel.cpp
   src/goal_tool.cpp
   src/nav2_panel.cpp

nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp

@@ -0,0 +1,62 @@
+// Copyright (c) 2024 Jatin Patil
+//
+// 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_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
+#define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
+
+#include <nav2_msgs/srv/get_cost.hpp>
+#include <rviz_common/tool.hpp>
+#include <rviz_default_plugins/tools/point/point_tool.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace nav2_rviz_plugins
+{
+class CostmapCostTool : public rviz_common::Tool
+{
+  Q_OBJECT
+
+public:
+  CostmapCostTool();
+  virtual ~CostmapCostTool();
+
+  void onInitialize() override;
+  void activate() override;
+  void deactivate() override;
+
+  int processMouseEvent(rviz_common::ViewportMouseEvent & event) override;
+
+  void callCostService(float x, float y);
+
+  void handleLocalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);
+  void handleGlobalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::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::Node::SharedPtr node_;
+
+  QCursor std_cursor_;
+  QCursor hit_cursor_;
+  rviz_common::properties::BoolProperty * auto_deactivate_property_;
+  rviz_common::properties::QosProfileProperty * qos_profile_property_;
+
+  rclcpp::QoS qos_profile_;
+};
+
+}  // namespace nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_

nav2_rviz_plugins/plugins_description.xml

@@ -24,6 +24,12 @@
     <description>The Nav2 rviz panel for dock and undock actions.</description>
   </class>
 
+  <class name="nav2_rviz_plugins/CostmapCostTool"
+         type="nav2_rviz_plugins::CostmapCostTool"
+         base_class_type="rviz_common::Tool">
+    <description>A Nav2 tool for getting the cost of point in costmap.</description>
+  </class>
+
   <class name="nav2_rviz_plugins/ParticleCloud"
          type="nav2_rviz_plugins::ParticleCloudDisplay"
          base_class_type="rviz_common::Display">