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 7f95dec1bb..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,10 +47,12 @@ #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" #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 +341,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, + const std::shared_ptr request, + const std::shared_ptr response); + protected: // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr @@ -412,6 +423,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector unpadded_footprint_; std::vector padded_footprint_; + // Services + rclcpp::Service::SharedPtr get_cost_service_; std::unique_ptr clear_costmap_service_; // Dynamic parameters handler diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5cba6b70cb..50e69f6f6c 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/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( + "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(shared_from_this(), *this); @@ -817,4 +823,35 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } +void Costmap2DROS::getCostCallback( + const std::shared_ptr, + const std::shared_ptr request, + const std::shared_ptr response) +{ + unsigned int mx, my; + + Costmap2D * costmap = layered_costmap_->getCostmap(); + + if (request->use_footprint) { + Footprint footprint = layered_costmap_->getFootprint(); + FootprintCollisionChecker collision_checker(costmap); + + RCLCPP_INFO( + get_logger(), "Received request to get cost at footprint pose (%.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 + 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; + } +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index d3c5edd813..0d332a4463 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/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 diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp new file mode 100644 index 0000000000..8563d6dd16 --- /dev/null +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -0,0 +1,89 @@ +// 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 + +#include +#include + +#include +#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("costmap"); + client_ = costmap_->create_client( + "/costmap/get_cost_costmap"); + costmap_->on_configure(rclcpp_lifecycle::State()); + ASSERT_TRUE(client_->wait_for_service(10s)); + } + + std::shared_ptr costmap_; + rclcpp::Client::SharedPtr client_; +}; + +TEST_F(GetCostServiceTest, TestWithoutFootprint) +{ + auto request = std::make_shared(); + 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) + { + 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"; + } +} + +TEST_F(GetCostServiceTest, TestWithFootprint) +{ + auto request = std::make_shared(); + 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) + { + 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/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index d0ff352775..8cee7cdb7f 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/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" diff --git a/nav2_msgs/srv/GetCost.srv b/nav2_msgs/srv/GetCost.srv new file mode 100644 index 0000000000..577654f55c --- /dev/null +++ b/nav2_msgs/srv/GetCost.srv @@ -0,0 +1,8 @@ +# Get costmap cost at given point + +bool use_footprint +float32 x +float32 y +float32 theta +--- +float32 cost \ No newline at end of file diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index ef8158d49f..c5f5c788aa 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/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 diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp new file mode 100644 index 0000000000..197fe2b655 --- /dev/null +++ b/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 +#include +#include +#include + +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::SharedFuture); + void handleGlobalCostResponse(rclcpp::Client::SharedFuture); + +private Q_SLOTS: + void updateAutoDeactivate(); + +private: + rclcpp::Client::SharedPtr local_cost_client_; + rclcpp::Client::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_ diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index b392342638..a28b1d9992 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -24,6 +24,12 @@ The Nav2 rviz panel for dock and undock actions. + + A Nav2 tool for getting the cost of point in costmap. + + diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp new file mode 100644 index 0000000000..f25a71cd80 --- /dev/null +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -0,0 +1,130 @@ +// 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 "nav2_rviz_plugins/costmap_cost_tool.hpp" +#include +#include "rviz_common/display_context.hpp" +#include "rviz_common/load_resource.hpp" +#include "rviz_common/interaction/view_picker_iface.hpp" +#include "rviz_common/msg_conversions.hpp" +#include "rviz_common/properties/bool_property.hpp" + +namespace nav2_rviz_plugins +{ +CostmapCostTool::CostmapCostTool() +: qos_profile_(5) +{ + shortcut_key_ = 'm'; + + auto_deactivate_property_ = new rviz_common::properties::BoolProperty( + "Single click", true, + "Switch away from this tool after one click.", + getPropertyContainer(), SLOT(updateAutoDeactivate()), this); +} + +CostmapCostTool::~CostmapCostTool() {} + +void CostmapCostTool::onInitialize() +{ + hit_cursor_ = cursor_; + std_cursor_ = rviz_common::getDefaultCursor(); + + setName("Costmap Cost"); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/PointStamped.png")); + + node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + local_cost_client_ = + node_->create_client("/local_costmap/get_cost_local_costmap"); + global_cost_client_ = + node_->create_client("/global_costmap/get_cost_global_costmap"); +} + +void CostmapCostTool::activate() {} +void CostmapCostTool::deactivate() {} + +int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + int flags = 0; + + Ogre::Vector3 position; + bool success = context_->getViewPicker()->get3DPoint(event.panel, event.x, event.y, position); + setCursor(success ? hit_cursor_ : std_cursor_); + + if (success) { + std::ostringstream s; + s << "Left-Click: Select this point."; + s.precision(3); + s << " [" << position.x << "," << position.y << "," << position.z << "]"; + setStatus(s.str().c_str()); + + if (event.leftUp()) { + auto point = rviz_common::pointOgreToMsg(position); + + callCostService(point.x, point.y); + + if (auto_deactivate_property_->getBool()) { + flags |= Finished; + } + } + } else { + setStatus("Move over an object to select the target point."); + } + return flags; +} + +void CostmapCostTool::callCostService(float x, float y) +{ + // Create request for local costmap + auto request = std::make_shared(); + request->x = x; + request->y = y; + + // Call local costmap service + 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)); + } + + // Call global costmap service + 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)); + } +} + +void CostmapCostTool::handleLocalCostResponse( + rclcpp::Client::SharedFuture future) +{ + auto response = future.get(); + if (response->cost != -1) { + RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %.1f", response->cost); + } else { + RCLCPP_ERROR(node_->get_logger(), "Failed to get local costmap cost"); + } +} + +void CostmapCostTool::handleGlobalCostResponse( + rclcpp::Client::SharedFuture future) +{ + auto response = future.get(); + if (response->cost != -1) { + RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %.1f", response->cost); + } else { + RCLCPP_ERROR(node_->get_logger(), "Failed to get global costmap cost"); + } +} +} // namespace nav2_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool)