diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 3dadcab8ac..4553f9883f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/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" diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index d2d00afea0..202da9858e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/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 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(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(costmap->getCost(mx, my)); - response->cost = cost; + response->cost = static_cast(costmap->getCost(mx, my)); } else { RCLCPP_WARN(get_logger(), "Point (%f, %f) is out of bounds", request->x, request->y); response->cost = -1.0; diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp index d49c4bb235..8563d6dd16 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -50,8 +50,8 @@ class GetCostServiceTest : public ::testing::Test TEST_F(GetCostServiceTest, TestWithoutFootprint) { auto request = std::make_shared(); - 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); @@ -59,7 +59,9 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) 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,8 +70,9 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) TEST_F(GetCostServiceTest, TestWithFootprint) { auto request = std::make_shared(); - 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); @@ -77,7 +80,9 @@ TEST_F(GetCostServiceTest, TestWithFootprint) 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"; } diff --git a/nav2_msgs/srv/GetCost.srv b/nav2_msgs/srv/GetCost.srv index 20e983827e..577654f55c 100644 --- a/nav2_msgs/srv/GetCost.srv +++ b/nav2_msgs/srv/GetCost.srv @@ -3,5 +3,6 @@ bool use_footprint float32 x float32 y +float32 theta --- float32 cost \ No newline at end of file diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 7320d5c6a4..f25a71cd80 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/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(