From 640b0eb352a58ce84080217b1653b3184351acb6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 08:54:00 +0000 Subject: [PATCH] getcosts instead of cost Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 4 +- .../remove_in_collision_goals_action.cpp | 40 +++++++++------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 10 ++-- nav2_costmap_2d/src/costmap_2d_ros.cpp | 48 ++++++++++--------- .../test/unit/costmap_cost_service_test.cpp | 32 +++++++------ nav2_msgs/CMakeLists.txt | 2 +- nav2_msgs/srv/GetCost.srv | 8 ---- nav2_msgs/srv/GetCosts.srv | 6 +++ .../nav2_rviz_plugins/costmap_cost_tool.hpp | 10 ++-- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 18 +++---- 10 files changed, 94 insertions(+), 84 deletions(-) delete mode 100644 nav2_msgs/srv/GetCost.srv create mode 100644 nav2_msgs/srv/GetCosts.srv diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 7dd3befb154..af303aa2968 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp/action_node.h" -#include "nav2_msgs/srv/get_cost.hpp" +#include "nav2_msgs/srv/get_costs.hpp" namespace nav2_behavior_tree { @@ -60,7 +60,7 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase BT::NodeStatus tick() override; rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr get_cost_client_; + rclcpp::Client::SharedPtr get_cost_client_; bool initialized_; std::string costmap_cost_service_; bool use_footprint_; diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 62d88ca27d7..fa24ed8a135 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -41,9 +41,8 @@ void RemoveInCollisionGoals::initialize() getInput("costmap_cost_service", costmap_cost_service_); - get_cost_client_ = node_->create_client( + get_cost_client_ = node_->create_client( costmap_cost_service_); - } inline BT::NodeStatus RemoveInCollisionGoals::tick() @@ -66,25 +65,30 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() } Goals valid_goal_poses; + auto request = std::make_shared(); + request->use_footprint = use_footprint_; + for (const auto & goal : goal_poses) { - auto request = std::make_shared(); - request->x = goal.pose.position.x; - request->y = goal.pose.position.y; - request->theta = tf2::getYaw(goal.pose.orientation); - request->use_footprint = use_footprint_; - - auto future = get_cost_client_->async_send_request(request); - auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_); - if (ret == rclcpp::FutureReturnCode::SUCCESS) { - if (future.get()->cost <= cost_threshold_) { - valid_goal_poses.push_back(goal); + geometry_msgs::msg::Pose2D pose; + pose.x = goal.pose.position.x; + pose.y = goal.pose.position.y; + pose.theta = tf2::getYaw(goal.pose.orientation); + request->poses.push_back(pose); + } + + auto future = get_cost_client_->async_send_request(request); + auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_); + if (ret == rclcpp::FutureReturnCode::SUCCESS) { + for (size_t i = 0; i < future.get()->costs.size(); ++i) { + if (future.get()->costs[i] <= cost_threshold_) { + valid_goal_poses.push_back(goal_poses[i]); } - } else { - RCLCPP_ERROR( - node_->get_logger(), - "RemoveInCollisionGoals BT node failed to call GetCost service of costmap"); - return BT::NodeStatus::FAILURE; } + } else { + RCLCPP_ERROR( + node_->get_logger(), + "RemoveInCollisionGoals BT node failed to call GetCost service of costmap"); + return BT::NodeStatus::FAILURE; } setOutput("output_goals", valid_goal_poses); return BT::NodeStatus::SUCCESS; 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 c95a19902d1..33feaa700df 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 @@ -52,7 +52,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 "nav2_msgs/srv/get_costs.hpp" #include "pluginlib/class_loader.hpp" #include "tf2/convert.h" #include "tf2/LinearMath/Transform.h" @@ -345,10 +345,10 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @param request x and y coordinates in map * @param response cost of the point */ - void getCostCallback( + void getCostsCallback( const std::shared_ptr, - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); protected: // Publishers and subscribers @@ -425,7 +425,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector padded_footprint_; // Services - rclcpp::Service::SharedPtr get_cost_service_; + 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 18d15525a56..dcf6d50b7a0 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -252,9 +252,9 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Service to get the cost at a point - get_cost_service_ = create_service( + get_cost_service_ = create_service( "get_cost_" + getName(), - std::bind(&Costmap2DROS::getCostCallback, this, std::placeholders::_1, std::placeholders::_2, + std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service @@ -825,38 +825,40 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } -void Costmap2DROS::getCostCallback( +void Costmap2DROS::getCostsCallback( const std::shared_ptr, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { unsigned int mx, my; Costmap2D * costmap = layered_costmap_->getCostmap(); - bool in_bounds = costmap->worldToMap(request->x, request->y, mx, my); + for (const auto & pose : request->poses) { + bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my); - if (!in_bounds) { - response->cost = -1.0; - return; - } + if (!in_bounds) { + response->costs.push_back(-1.0); + continue; + } - if (request->use_footprint) { - Footprint footprint = layered_costmap_->getFootprint(); - FootprintCollisionChecker collision_checker(costmap); + if (request->use_footprint) { + Footprint footprint = layered_costmap_->getFootprint(); + FootprintCollisionChecker collision_checker(costmap); - RCLCPP_DEBUG( - get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)", - request->x, request->y, request->theta); + RCLCPP_DEBUG( + get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)", + pose.x, pose.y, pose.theta); - response->cost = collision_checker.footprintCostAtPose( - request->x, request->y, request->theta, footprint); - } else { - RCLCPP_DEBUG( - get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y); + response->costs.push_back( + collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint)); + } else { + RCLCPP_DEBUG( + get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y); - // Get the cost at the map coordinates - response->cost = static_cast(costmap->getCost(mx, my)); + // Get the cost at the map coordinates + response->costs.push_back(static_cast(costmap->getCost(mx, my))); + } } } 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 8563d6dd16d..803e2a1f8e7 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav2_msgs/srv/get_cost.hpp" +#include "nav2_msgs/srv/get_costs.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" class RclCppFixture @@ -37,21 +37,23 @@ class GetCostServiceTest : public ::testing::Test void SetUp() override { costmap_ = std::make_shared("costmap"); - client_ = costmap_->create_client( + 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_; + rclcpp::Client::SharedPtr client_; }; TEST_F(GetCostServiceTest, TestWithoutFootprint) { - auto request = std::make_shared(); - request->x = 0.5; - request->y = 1.0; + auto request = std::make_shared(); + geometry_msgs::msg::Pose2D pose; + pose.x = 0.5; + pose.y = 1.0; + request->poses.push_back(pose); request->use_footprint = false; auto result_future = client_->async_send_request(request); @@ -60,8 +62,8 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) 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"; + EXPECT_GE(response->costs[0], 0.0) << "Cost is less than 0"; + EXPECT_LE(response->costs[0], 255.0) << "Cost is greater than 255"; } else { FAIL() << "Failed to call service"; } @@ -69,10 +71,12 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) TEST_F(GetCostServiceTest, TestWithFootprint) { - auto request = std::make_shared(); - request->x = 1.0; - request->y = 1.0; - request->theta = 0.5; + auto request = std::make_shared(); + geometry_msgs::msg::Pose2D pose; + pose.x = 0.5; + pose.y = 1.0; + pose.theta = 0.5; + request->poses.push_back(pose); request->use_footprint = true; auto result_future = client_->async_send_request(request); @@ -81,8 +85,8 @@ TEST_F(GetCostServiceTest, TestWithFootprint) 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"; + EXPECT_GE(response->costs[0], 0.0) << "Cost is less than 0"; + EXPECT_LE(response->costs[0], 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 8cee7cdb7f2..a9f93b5ecbc 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,7 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" - "srv/GetCost.srv" + "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/srv/GetCost.srv b/nav2_msgs/srv/GetCost.srv deleted file mode 100644 index 577654f55cd..00000000000 --- a/nav2_msgs/srv/GetCost.srv +++ /dev/null @@ -1,8 +0,0 @@ -# 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_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv new file mode 100644 index 00000000000..0328d476397 --- /dev/null +++ b/nav2_msgs/srv/GetCosts.srv @@ -0,0 +1,6 @@ +# Get costmap costs at given poses + +bool use_footprint +geometry_msgs/Pose2D[] poses +--- +float32[] costs \ No newline at end of file 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 index 197fe2b6550..3466e8831b1 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp @@ -15,7 +15,7 @@ #ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_ #define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_ -#include +#include #include #include #include @@ -38,15 +38,15 @@ class CostmapCostTool : public rviz_common::Tool void callCostService(float x, float y); - void handleLocalCostResponse(rclcpp::Client::SharedFuture); - void handleGlobalCostResponse(rclcpp::Client::SharedFuture); + 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::Client::SharedPtr local_cost_client_; + rclcpp::Client::SharedPtr global_cost_client_; rclcpp::Node::SharedPtr node_; QCursor std_cursor_; diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 0d628e0215b..3203c0cd0d9 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -45,9 +45,9 @@ void CostmapCostTool::onInitialize() node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); local_cost_client_ = - node_->create_client("/local_costmap/get_cost_local_costmap"); + node_->create_client("/local_costmap/get_cost_local_costmap"); global_cost_client_ = - node_->create_client("/global_costmap/get_cost_global_costmap"); + node_->create_client("/global_costmap/get_cost_global_costmap"); } void CostmapCostTool::activate() {} @@ -86,10 +86,12 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) void CostmapCostTool::callCostService(float x, float y) { // Create request for local costmap - auto request = std::make_shared(); - request->x = x; - request->y = y; - // request->use_footprint = true; + auto request = std::make_shared(); + geometry_msgs::msg::Pose2D pose; + pose.x = x; + pose.y = y; + request->poses.push_back(pose); + request->use_footprint = false; // Call local costmap service if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { @@ -105,7 +107,7 @@ void CostmapCostTool::callCostService(float x, float y) } void CostmapCostTool::handleLocalCostResponse( - rclcpp::Client::SharedFuture future) + rclcpp::Client::SharedFuture future) { auto response = future.get(); if (response->cost != -1) { @@ -116,7 +118,7 @@ void CostmapCostTool::handleLocalCostResponse( } void CostmapCostTool::handleGlobalCostResponse( - rclcpp::Client::SharedFuture future) + rclcpp::Client::SharedFuture future) { auto response = future.get(); if (response->cost != -1) {