Skip to content

Commit 2fd2054

Browse files
JatinPatil2003JatinPatil2003
authored
Rviz tool to get cost of costmap cell (#4546)
* Added GetCost srv Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Added Service in costmap_2d Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Added Rviz tool Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Fixed Styling Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Fixed Styles and Linting Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Fixed Linting Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Added Bool use_footprint to srv Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Added unit test for costmap costcell cost service Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Fixed unit test script Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Added theta, Updated unit test, Updated rviz tool service call logic Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> * Updated requested changes Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com> --------- Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>
1 parent e03013b commit 2fd2054

File tree

10 files changed

+353
-0
lines changed

10 files changed

+353
-0
lines changed

‎nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,10 +47,12 @@
4747
#include "geometry_msgs/msg/polygon_stamped.h"
4848
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
4949
#include "nav2_costmap_2d/footprint.hpp"
50+
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
5051
#include "nav2_costmap_2d/clear_costmap_service.hpp"
5152
#include "nav2_costmap_2d/layered_costmap.hpp"
5253
#include "nav2_costmap_2d/layer.hpp"
5354
#include "nav2_util/lifecycle_node.hpp"
55+
#include "nav2_msgs/srv/get_cost.hpp"
5456
#include "pluginlib/class_loader.hpp"
5557
#include "tf2/convert.h"
5658
#include "tf2/LinearMath/Transform.h"
@@ -339,6 +341,15 @@ class Costmap2DROS : public nav2_util::LifecycleNode
339341
*/
340342
double getRobotRadius() {return robot_radius_;}
341343

344+
/** @brief Get the cost at a point in costmap
345+
* @param request x and y coordinates in map
346+
* @param response cost of the point
347+
*/
348+
void getCostCallback(
349+
const std::shared_ptr<rmw_request_id_t>,
350+
const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
351+
const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response);
352+
342353
protected:
343354
// Publishers and subscribers
344355
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
@@ -412,6 +423,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
412423
std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
413424
std::vector<geometry_msgs::msg::Point> padded_footprint_;
414425

426+
// Services
427+
rclcpp::Service<nav2_msgs::srv::GetCost>::SharedPtr get_cost_service_;
415428
std::unique_ptr<ClearCostmapService> clear_costmap_service_;
416429

417430
// Dynamic parameters handler

‎nav2_costmap_2d/src/costmap_2d_ros.cpp‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -250,6 +250,12 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
250250
setRobotFootprint(new_footprint);
251251
}
252252

253+
// Service to get the cost at a point
254+
get_cost_service_ = create_service<nav2_msgs::srv::GetCost>(
255+
"get_cost_" + getName(),
256+
std::bind(&Costmap2DROS::getCostCallback, this, std::placeholders::_1, std::placeholders::_2,
257+
std::placeholders::_3));
258+
253259
// Add cleaning service
254260
clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);
255261

@@ -817,4 +823,35 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
817823
return result;
818824
}
819825

826+
void Costmap2DROS::getCostCallback(
827+
const std::shared_ptr<rmw_request_id_t>,
828+
const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
829+
const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response)
830+
{
831+
unsigned int mx, my;
832+
833+
Costmap2D * costmap = layered_costmap_->getCostmap();
834+
835+
if (request->use_footprint) {
836+
Footprint footprint = layered_costmap_->getFootprint();
837+
FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
838+
839+
RCLCPP_INFO(
840+
get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
841+
request->x, request->y, request->theta);
842+
843+
response->cost = collision_checker.footprintCostAtPose(
844+
request->x, request->y, request->theta, footprint);
845+
} else if (costmap->worldToMap(request->x, request->y, mx, my)) {
846+
RCLCPP_INFO(
847+
get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y);
848+
849+
// Get the cost at the map coordinates
850+
response->cost = static_cast<float>(costmap->getCost(mx, my));
851+
} else {
852+
RCLCPP_WARN(get_logger(), "Point (%f, %f) is out of bounds", request->x, request->y);
853+
response->cost = -1.0;
854+
}
855+
}
856+
820857
} // namespace nav2_costmap_2d

‎nav2_costmap_2d/test/unit/CMakeLists.txt‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,11 @@ target_link_libraries(costmap_convesion_test
88
${PROJECT_NAME}::nav2_costmap_2d_core
99
)
1010

11+
ament_add_gtest(costmap_cost_service_test costmap_cost_service_test.cpp)
12+
target_link_libraries(costmap_cost_service_test
13+
${PROJECT_NAME}::nav2_costmap_2d_core
14+
)
15+
1116
ament_add_gtest(declare_parameter_test declare_parameter_test.cpp)
1217
target_link_libraries(declare_parameter_test
1318
${PROJECT_NAME}::nav2_costmap_2d_core

‎nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
// Copyright (c) 2024 Jatin Patil
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <gtest/gtest.h>
16+
17+
#include <memory>
18+
#include <chrono>
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
#include "nav2_msgs/srv/get_cost.hpp"
22+
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
23+
24+
class RclCppFixture
25+
{
26+
public:
27+
RclCppFixture() {rclcpp::init(0, nullptr);}
28+
~RclCppFixture() {rclcpp::shutdown();}
29+
};
30+
RclCppFixture g_rclcppfixture;
31+
32+
using namespace std::chrono_literals;
33+
34+
class GetCostServiceTest : public ::testing::Test
35+
{
36+
protected:
37+
void SetUp() override
38+
{
39+
costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap");
40+
client_ = costmap_->create_client<nav2_msgs::srv::GetCost>(
41+
"/costmap/get_cost_costmap");
42+
costmap_->on_configure(rclcpp_lifecycle::State());
43+
ASSERT_TRUE(client_->wait_for_service(10s));
44+
}
45+
46+
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
47+
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr client_;
48+
};
49+
50+
TEST_F(GetCostServiceTest, TestWithoutFootprint)
51+
{
52+
auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
53+
request->x = 0.5;
54+
request->y = 1.0;
55+
request->use_footprint = false;
56+
57+
auto result_future = client_->async_send_request(request);
58+
if (rclcpp::spin_until_future_complete(
59+
costmap_,
60+
result_future) == rclcpp::FutureReturnCode::SUCCESS)
61+
{
62+
auto response = result_future.get();
63+
EXPECT_GE(response->cost, 0.0) << "Cost is less than 0";
64+
EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255";
65+
} else {
66+
FAIL() << "Failed to call service";
67+
}
68+
}
69+
70+
TEST_F(GetCostServiceTest, TestWithFootprint)
71+
{
72+
auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
73+
request->x = 1.0;
74+
request->y = 1.0;
75+
request->theta = 0.5;
76+
request->use_footprint = true;
77+
78+
auto result_future = client_->async_send_request(request);
79+
if (rclcpp::spin_until_future_complete(
80+
costmap_,
81+
result_future) == rclcpp::FutureReturnCode::SUCCESS)
82+
{
83+
auto response = result_future.get();
84+
EXPECT_GE(response->cost, 0.0) << "Cost is less than 0";
85+
EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255";
86+
} else {
87+
FAIL() << "Failed to call service";
88+
}
89+
}

‎nav2_msgs/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
3030
"msg/Particle.msg"
3131
"msg/ParticleCloud.msg"
3232
"msg/MissedWaypoint.msg"
33+
"srv/GetCost.srv"
3334
"srv/GetCostmap.srv"
3435
"srv/IsPathValid.srv"
3536
"srv/ClearCostmapExceptRegion.srv"

‎nav2_msgs/srv/GetCost.srv‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# Get costmap cost at given point
2+
3+
bool use_footprint
4+
float32 x
5+
float32 y
6+
float32 theta
7+
---
8+
float32 cost

‎nav2_rviz_plugins/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ find_package(visualization_msgs REQUIRED)
3434
find_package(yaml_cpp_vendor REQUIRED)
3535

3636
set(nav2_rviz_plugins_headers_to_moc
37+
include/nav2_rviz_plugins/costmap_cost_tool.hpp
3738
include/nav2_rviz_plugins/docking_panel.hpp
3839
include/nav2_rviz_plugins/goal_pose_updater.hpp
3940
include/nav2_rviz_plugins/goal_common.hpp
@@ -52,6 +53,7 @@ include_directories(
5253
set(library_name ${PROJECT_NAME})
5354

5455
add_library(${library_name} SHARED
56+
src/costmap_cost_tool.cpp
5557
src/docking_panel.cpp
5658
src/goal_tool.cpp
5759
src/nav2_panel.cpp

‎nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
// Copyright (c) 2024 Jatin Patil
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
16+
#define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
17+
18+
#include <nav2_msgs/srv/get_cost.hpp>
19+
#include <rviz_common/tool.hpp>
20+
#include <rviz_default_plugins/tools/point/point_tool.hpp>
21+
#include <rclcpp/rclcpp.hpp>
22+
23+
namespace nav2_rviz_plugins
24+
{
25+
class CostmapCostTool : public rviz_common::Tool
26+
{
27+
Q_OBJECT
28+
29+
public:
30+
CostmapCostTool();
31+
virtual ~CostmapCostTool();
32+
33+
void onInitialize() override;
34+
void activate() override;
35+
void deactivate() override;
36+
37+
int processMouseEvent(rviz_common::ViewportMouseEvent & event) override;
38+
39+
void callCostService(float x, float y);
40+
41+
void handleLocalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);
42+
void handleGlobalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);
43+
44+
private Q_SLOTS:
45+
void updateAutoDeactivate();
46+
47+
private:
48+
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr local_cost_client_;
49+
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr global_cost_client_;
50+
rclcpp::Node::SharedPtr node_;
51+
52+
QCursor std_cursor_;
53+
QCursor hit_cursor_;
54+
rviz_common::properties::BoolProperty * auto_deactivate_property_;
55+
rviz_common::properties::QosProfileProperty * qos_profile_property_;
56+
57+
rclcpp::QoS qos_profile_;
58+
};
59+
60+
} // namespace nav2_rviz_plugins
61+
62+
#endif // NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_

‎nav2_rviz_plugins/plugins_description.xml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,12 @@
2424
<description>The Nav2 rviz panel for dock and undock actions.</description>
2525
</class>
2626

27+
<class name="nav2_rviz_plugins/CostmapCostTool"
28+
type="nav2_rviz_plugins::CostmapCostTool"
29+
base_class_type="rviz_common::Tool">
30+
<description>A Nav2 tool for getting the cost of point in costmap.</description>
31+
</class>
32+
2733
<class name="nav2_rviz_plugins/ParticleCloud"
2834
type="nav2_rviz_plugins::ParticleCloudDisplay"
2935
base_class_type="rviz_common::Display">

0 commit comments

Comments
 (0)