From 2f6cdae34768c7f9a34b80ac12175e277da0dc64 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 26 Sep 2024 15:50:13 +0200 Subject: [PATCH] Improvements in RemoveInCollisionGoals and adjacent features (#4676) * improvements Signed-off-by: Tony Najjar * ament_uncrustify Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * fix building Signed-off-by: Tony Najjar * fixes Signed-off-by: Tony Najjar * add stamp Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 12 ++++++--- .../remove_in_collision_goals_action.cpp | 17 +++++++----- nav2_costmap_2d/src/costmap_2d_ros.cpp | 22 ++++++++++----- nav2_costmap_2d/src/layered_costmap.cpp | 8 +++--- .../test/unit/costmap_cost_service_test.cpp | 16 ++++++----- nav2_msgs/srv/GetCosts.srv | 2 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 27 +++++++++---------- 7 files changed, 62 insertions(+), 42 deletions(-) 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 73aab1bd8cd..8992b4d516a 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 @@ -52,17 +52,23 @@ class RemoveInCollisionGoals : public BtServiceNode static BT::PortsList providedPorts() { - return providedBasicPorts({ + return providedBasicPorts( + { BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("cost_threshold", 254.0, + BT::InputPort( + "cost_threshold", 254.0, "Cost threshold for considering a goal in collision"), BT::InputPort("use_footprint", true, "Whether to use footprint cost"), + BT::InputPort( + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), - }); + }); } private: bool use_footprint_; + bool consider_unknown_as_obstacle_; double cost_threshold_; Goals input_goals_; }; 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 2fe8395ad4c..d6f9ee662d0 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 @@ -37,6 +37,7 @@ void RemoveInCollisionGoals::on_tick() getInput("use_footprint", use_footprint_); getInput("cost_threshold", cost_threshold_); getInput("input_goals", input_goals_); + getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); if (input_goals_.empty()) { setOutput("output_goals", input_goals_); @@ -47,11 +48,7 @@ void RemoveInCollisionGoals::on_tick() request_->use_footprint = use_footprint_; for (const auto & goal : input_goals_) { - 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); + request_->poses.push_back(goal); } } @@ -60,10 +57,18 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( { Goals valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { - if (response->costs[i] < cost_threshold_) { + if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || + response->costs[i] < cost_threshold_) + { valid_goal_poses.push_back(input_goals_[i]); } } + // Inform if all goals have been removed + if (valid_goal_poses.empty()) { + RCLCPP_INFO( + node_->get_logger(), + "All goals are in collision and have been removed from the list"); + } setOutput("output_goals", valid_goal_poses); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index dcf6d50b7a0..fa11b60ebfe 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -254,7 +254,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // Service to get the cost at a point get_cost_service_ = create_service( "get_cost_" + getName(), - std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, + std::bind( + &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); // Add cleaning service @@ -835,12 +836,17 @@ void Costmap2DROS::getCostsCallback( Costmap2D * costmap = layered_costmap_->getCostmap(); for (const auto & pose : request->poses) { - bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my); + geometry_msgs::msg::PoseStamped pose_transformed; + transformPoseToGlobalFrame(pose, pose_transformed); + bool in_bounds = costmap->worldToMap( + pose_transformed.pose.position.x, + pose_transformed.pose.position.y, mx, my); if (!in_bounds) { - response->costs.push_back(-1.0); + response->costs.push_back(NO_INFORMATION); continue; } + double yaw = tf2::getYaw(pose_transformed.pose.orientation); if (request->use_footprint) { Footprint footprint = layered_costmap_->getFootprint(); @@ -848,13 +854,17 @@ void Costmap2DROS::getCostsCallback( RCLCPP_DEBUG( get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)", - pose.x, pose.y, pose.theta); + pose_transformed.pose.position.x, pose_transformed.pose.position.y, yaw); response->costs.push_back( - collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint)); + collision_checker.footprintCostAtPose( + pose_transformed.pose.position.x, + pose_transformed.pose.position.y, yaw, footprint)); } else { RCLCPP_DEBUG( - get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y); + get_logger(), "Received request to get cost at point (%f, %f)", + pose_transformed.pose.position.x, + pose_transformed.pose.position.y); // Get the cost at the map coordinates response->costs.push_back(static_cast(costmap->getCost(mx, my))); diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 33e9fc22aa5..2995ae03029 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -72,11 +72,11 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo footprint_(std::make_shared>()) { if (track_unknown) { - primary_costmap_.setDefaultValue(255); - combined_costmap_.setDefaultValue(255); + primary_costmap_.setDefaultValue(NO_INFORMATION); + combined_costmap_.setDefaultValue(NO_INFORMATION); } else { - primary_costmap_.setDefaultValue(0); - combined_costmap_.setDefaultValue(0); + primary_costmap_.setDefaultValue(FREE_SPACE); + combined_costmap_.setDefaultValue(FREE_SPACE); } } 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 803e2a1f8e7..15780196b17 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -50,9 +50,9 @@ class GetCostServiceTest : public ::testing::Test TEST_F(GetCostServiceTest, TestWithoutFootprint) { auto request = std::make_shared(); - geometry_msgs::msg::Pose2D pose; - pose.x = 0.5; - pose.y = 1.0; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0.5; + pose.pose.position.y = 1.0; request->poses.push_back(pose); request->use_footprint = false; @@ -72,10 +72,12 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) TEST_F(GetCostServiceTest, TestWithFootprint) { auto request = std::make_shared(); - geometry_msgs::msg::Pose2D pose; - pose.x = 0.5; - pose.y = 1.0; - pose.theta = 0.5; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0.5; + pose.pose.position.y = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0.5); + pose.pose.orientation = tf2::toMsg(q); request->poses.push_back(pose); request->use_footprint = true; diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 0328d476397..55ebad4f0a2 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -1,6 +1,6 @@ # Get costmap costs at given poses bool use_footprint -geometry_msgs/Pose2D[] poses +geometry_msgs/PoseStamped[] poses --- float32[] costs \ 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 d5ef5658745..03435a913e4 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -94,23 +94,28 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) void CostmapCostTool::callCostService(float x, float y) { + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); // Create request for local costmap auto request = std::make_shared(); - geometry_msgs::msg::Pose2D pose; - pose.x = x; - pose.y = y; + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = context_->getFixedFrame().toStdString(); + pose.header.stamp = node->now(); + pose.pose.position.x = x; + pose.pose.position.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))) { - local_cost_client_->async_send_request(request, + 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, + global_cost_client_->async_send_request( + request, std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1)); } } @@ -120,11 +125,7 @@ void CostmapCostTool::handleLocalCostResponse( { rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); - if (response->costs[0] != -1) { - RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost"); - } + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]); } void CostmapCostTool::handleGlobalCostResponse( @@ -132,11 +133,7 @@ void CostmapCostTool::handleGlobalCostResponse( { rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); - if (response->costs[0] != -1) { - RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost"); - } + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]); } } // namespace nav2_rviz_plugins