From 842de071c15065ddd01f9c0993facf6c7b9f329c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 5 Aug 2024 15:22:39 +0200 Subject: [PATCH 01/21] Remove goals in collision Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 69 +++++++++ .../remove_in_collision_goals_action.cpp | 135 ++++++++++++++++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 17 ++- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 1 + 4 files changed, 216 insertions(+), 6 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp create mode 100644 nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp 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 new file mode 100644 index 0000000000..9a3e3bde84 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "behaviortree_cpp_v3/action_node.h" +#include "nav2_msgs/srv/get_cost.hpp" + +namespace nav2_behavior_tree +{ + +class RemoveInCollisionGoals : public BT::ActionNodeBase +{ +public: + typedef std::vector Goals; + + RemoveInCollisionGoals( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_goals", "Original goals to remove from"), + BT::InputPort("costmap", "Which costmap to use for checking collision. Choices: local, global, both"), + BT::InputPort("cost_threshold", "Cost threshold for considering a goal in collision"), + BT::OutputPort("output_goals", "Goals with in-collision goals removed"), + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; + + rclcpp::Node::SharedPtr node_; + bool initialized_; + std::string which_costmap_; + double cost_threshold_; + rclcpp::Client::SharedPtr get_global_cost_client_; + rclcpp::Client::SharedPtr get_local_cost_client_; + std::chrono::milliseconds server_timeout_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_IN_COLLISION_GOALS_ACTION_HPP_ 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 new file mode 100644 index 0000000000..936da4327c --- /dev/null +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -0,0 +1,135 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace nav2_behavior_tree +{ + +RemoveInCollisionGoals::RemoveInCollisionGoals( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf), + initialized_(false), + which_costmap_("both"), + cost_threshold_(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) +{} + +void RemoveInCollisionGoals::initialize() +{ + node_ = config().blackboard->get("node"); + get_global_cost_client_ = node_->create_client( + "global_costmap/get_cost_global_costmap"); + get_local_cost_client_ = node_->create_client( + "local_costmap/get_cost_local_costmap"); + server_timeout_ = config().blackboard->template get("server_timeout"); +} + +inline BT::NodeStatus RemoveInCollisionGoals::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + if (!initialized_) { + initialize(); + } + + getInput("costmap", which_costmap_); + getInput("cost_threshold", cost_threshold_); + + Goals goal_poses; + getInput("input_goals", goal_poses); + + if (goal_poses.empty()) { + setOutput("output_goals", goal_poses); + return BT::NodeStatus::SUCCESS; + } + + Goals valid_goal_poses; + 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 = true; + + if (which_costmap_ == "global") { + auto future_global = get_global_cost_client_->async_send_request(request); + auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_); + if (ret_global == rclcpp::FutureReturnCode::SUCCESS) { + if (future_global.get()->cost <= cost_threshold_) { + valid_goal_poses.push_back(goal); + } + } else { + RCLCPP_ERROR( + node_->get_logger(), + "RemoveInCollisionGoals BT node failed to call GetCost service of global costmap"); + return BT::NodeStatus::FAILURE; + } + } else if (which_costmap_ == "local") { + auto future_local = get_local_cost_client_->async_send_request(request); + auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_); + if (ret_local == rclcpp::FutureReturnCode::SUCCESS) { + if (future_local.get()->cost <= cost_threshold_) { + valid_goal_poses.push_back(goal); + } + } else { + RCLCPP_ERROR( + node_->get_logger(), + "RemoveInCollisionGoals BT node failed to call GetCost service of local costmap"); + return BT::NodeStatus::FAILURE; + } + } else if (which_costmap_ == "both") { + auto future_global = get_global_cost_client_->async_send_request(request); + auto future_local = get_local_cost_client_->async_send_request(request); + auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_); + auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_); + if (ret_local == rclcpp::FutureReturnCode::SUCCESS && + ret_global == rclcpp::FutureReturnCode::SUCCESS) + { + if (future_local.get()->cost <= cost_threshold_ && + future_global.get()->cost <= cost_threshold_) + { + valid_goal_poses.push_back(goal); + } + } else { + RCLCPP_ERROR( + node_->get_logger(), + "RemoveInCollisionGoals BT node failed to call GetCost service of local or global costmap"); + return BT::NodeStatus::FAILURE; + } + } else { + RCLCPP_ERROR( + node_->get_logger(), "The costmap parameter must be either 'local', 'global', or 'both'"); + return BT::NodeStatus::FAILURE; + } + } + setOutput("output_goals", valid_goal_poses); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("RemoveInCollisionGoals"); +} diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 0fdfb5d2b8..bd1167cf65 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -834,25 +834,30 @@ void Costmap2DROS::getCostCallback( Costmap2D * costmap = layered_costmap_->getCostmap(); + bool in_bounds = costmap->worldToMap(request->x, request->y, mx, my); + + if (!in_bounds) { + response->cost = -1.0; + return; + } + if (request->use_footprint) { Footprint footprint = layered_costmap_->getFootprint(); FootprintCollisionChecker collision_checker(costmap); - RCLCPP_INFO( + RCLCPP_DEBUG( 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( + } + else { + RCLCPP_DEBUG( 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; } } diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index f25a71cd80..0d628e0215 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -89,6 +89,7 @@ void CostmapCostTool::callCostService(float x, float y) auto request = std::make_shared(); request->x = x; request->y = y; + // request->use_footprint = true; // Call local costmap service if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) { From 5411676080d5db7b7b9f6a42f6cd3780304f35c6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 5 Aug 2024 14:12:15 +0000 Subject: [PATCH 02/21] fix for main Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.hpp | 2 +- .../plugins/action/remove_in_collision_goals_action.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 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 9a3e3bde84..2b20668a01 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 @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_msgs/srv/get_cost.hpp" namespace nav2_behavior_tree 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 936da4327c..37a375121e 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 @@ -18,7 +18,6 @@ #include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" -#include "nav2_costmap_2d/cost_values.hpp" #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -31,7 +30,7 @@ RemoveInCollisionGoals::RemoveInCollisionGoals( : BT::ActionNodeBase(name, conf), initialized_(false), which_costmap_("both"), - cost_threshold_(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + cost_threshold_(253) {} void RemoveInCollisionGoals::initialize() From f60d727e62a747438a6973193e644199b5a5fa02 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 5 Aug 2024 14:18:13 +0000 Subject: [PATCH 03/21] Fix linting Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 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 2b20668a01..01cc8119bf 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 @@ -45,8 +45,10 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase { return { BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("costmap", "Which costmap to use for checking collision. Choices: local, global, both"), - BT::InputPort("cost_threshold", "Cost threshold for considering a goal in collision"), + BT::InputPort("costmap", + "Which costmap to use for checking collision. Choices: local, global, both"), + BT::InputPort("cost_threshold", + "Cost threshold for considering a goal in collision"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }; } From 82e95fc54ca9f7198f51c17ae8e003c0a21f1a8d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 5 Aug 2024 14:59:41 +0000 Subject: [PATCH 04/21] linting fixes Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 3 ++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 +-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 37a375121e..6bc56da558 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 @@ -112,7 +112,8 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() } else { RCLCPP_ERROR( node_->get_logger(), - "RemoveInCollisionGoals BT node failed to call GetCost service of local or global costmap"); + "RemoveInCollisionGoals BT node failed to call GetCost " + "service of local or global costmap"); return BT::NodeStatus::FAILURE; } } else { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index bd1167cf65..18d15525a5 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -851,8 +851,7 @@ void Costmap2DROS::getCostCallback( response->cost = collision_checker.footprintCostAtPose( request->x, request->y, request->theta, footprint); - } - else { + } else { RCLCPP_DEBUG( get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y); From e11923a1c74444f363e3bcac840098b40ac4a4d4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 6 Aug 2024 13:18:05 +0200 Subject: [PATCH 05/21] PR fixes Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 18 +++-- .../remove_in_collision_goals_action.cpp | 72 +++++++++---------- .../action/remove_passed_goals_action.cpp | 1 + 3 files changed, 45 insertions(+), 46 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 01cc8119bf..7e2b90fd25 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 @@ -45,10 +45,14 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase { return { BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("costmap", - "Which costmap to use for checking collision. Choices: local, global, both"), - BT::InputPort("cost_threshold", + BT::InputPort("costmap_cost_service", + "Service to get cost from costmap"), + BT::InputPort("costmap_cost_service_2", + "Second service name to get cost from costmap. If empty, only first service is used"), + BT::InputPort("cost_threshold", "Cost threshold for considering a goal in collision"), + BT::InputPort("use_footprint", + "Whether to use footprint cost"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }; } @@ -58,11 +62,13 @@ 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_2_; + std::string costmap_cost_service_; + std::string costmap_cost_service_2_; bool initialized_; - std::string which_costmap_; + bool use_footprint_; double cost_threshold_; - rclcpp::Client::SharedPtr get_global_cost_client_; - rclcpp::Client::SharedPtr get_local_cost_client_; std::chrono::milliseconds server_timeout_; }; 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 6bc56da558..eea32e0bfd 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 @@ -29,18 +29,27 @@ RemoveInCollisionGoals::RemoveInCollisionGoals( const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), initialized_(false), - which_costmap_("both"), + costmap_cost_service_("/global_costmap/get_cost_global_costmap"), + costmap_cost_service_2_(""), + use_footprint_(true), cost_threshold_(253) {} void RemoveInCollisionGoals::initialize() { node_ = config().blackboard->get("node"); - get_global_cost_client_ = node_->create_client( - "global_costmap/get_cost_global_costmap"); - get_local_cost_client_ = node_->create_client( - "local_costmap/get_cost_local_costmap"); server_timeout_ = config().blackboard->template get("server_timeout"); + + getInput("costmap_cost_service", costmap_cost_service_); + getInput("costmap_cost_service_2", costmap_cost_service_2_); + + get_cost_client_ = node_->create_client( + costmap_cost_service_); + + if (!costmap_cost_service_2_.empty()) { + get_cost_client_2_ = node_->create_client( + costmap_cost_service_2_); + } } inline BT::NodeStatus RemoveInCollisionGoals::tick() @@ -51,7 +60,7 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() initialize(); } - getInput("costmap", which_costmap_); + getInput("use_footprint", use_footprint_); getInput("cost_threshold", cost_threshold_); Goals goal_poses; @@ -68,44 +77,31 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() request->x = goal.pose.position.x; request->y = goal.pose.position.y; request->theta = tf2::getYaw(goal.pose.orientation); - request->use_footprint = true; + request->use_footprint = use_footprint_; - if (which_costmap_ == "global") { - auto future_global = get_global_cost_client_->async_send_request(request); - auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_); - if (ret_global == rclcpp::FutureReturnCode::SUCCESS) { - if (future_global.get()->cost <= cost_threshold_) { + if (costmap_cost_service_2_.empty()) { + 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); } } else { RCLCPP_ERROR( node_->get_logger(), - "RemoveInCollisionGoals BT node failed to call GetCost service of global costmap"); + "RemoveInCollisionGoals BT node failed to call GetCost service of costmap"); return BT::NodeStatus::FAILURE; } - } else if (which_costmap_ == "local") { - auto future_local = get_local_cost_client_->async_send_request(request); - auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_); - if (ret_local == rclcpp::FutureReturnCode::SUCCESS) { - if (future_local.get()->cost <= cost_threshold_) { - valid_goal_poses.push_back(goal); - } - } else { - RCLCPP_ERROR( - node_->get_logger(), - "RemoveInCollisionGoals BT node failed to call GetCost service of local costmap"); - return BT::NodeStatus::FAILURE; - } - } else if (which_costmap_ == "both") { - auto future_global = get_global_cost_client_->async_send_request(request); - auto future_local = get_local_cost_client_->async_send_request(request); - auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_); - auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_); - if (ret_local == rclcpp::FutureReturnCode::SUCCESS && - ret_global == rclcpp::FutureReturnCode::SUCCESS) + } else { + auto future = get_cost_client_->async_send_request(request); + auto future_2 = get_cost_client_2_->async_send_request(request); + auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_); + auto ret_2 = rclcpp::spin_until_future_complete(node_, future_2, server_timeout_); + if (ret == rclcpp::FutureReturnCode::SUCCESS && + ret_2 == rclcpp::FutureReturnCode::SUCCESS) { - if (future_local.get()->cost <= cost_threshold_ && - future_global.get()->cost <= cost_threshold_) + if (future.get()->cost <= cost_threshold_ && + future_2.get()->cost <= cost_threshold_) { valid_goal_poses.push_back(goal); } @@ -113,13 +109,9 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() RCLCPP_ERROR( node_->get_logger(), "RemoveInCollisionGoals BT node failed to call GetCost " - "service of local or global costmap"); + "service of one or two costmaps"); return BT::NodeStatus::FAILURE; } - } else { - RCLCPP_ERROR( - node_->get_logger(), "The costmap parameter must be either 'local', 'global', or 'both'"); - return BT::NodeStatus::FAILURE; } } setOutput("output_goals", valid_goal_poses); diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 86f5fffd6b..1b0e449431 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -43,6 +43,7 @@ void RemovePassedGoals::initialize() robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); + initialized_ = true; } inline BT::NodeStatus RemovePassedGoals::tick() From 384bccbeb7e1666381f06d9ec754ba426a986b7f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 6 Aug 2024 13:20:56 +0200 Subject: [PATCH 06/21] remove second costmap Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 4 -- .../remove_in_collision_goals_action.cpp | 46 ++++--------------- 2 files changed, 9 insertions(+), 41 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 7e2b90fd25..f548fae09d 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 @@ -47,8 +47,6 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase BT::InputPort("input_goals", "Original goals to remove from"), BT::InputPort("costmap_cost_service", "Service to get cost from costmap"), - BT::InputPort("costmap_cost_service_2", - "Second service name to get cost from costmap. If empty, only first service is used"), BT::InputPort("cost_threshold", "Cost threshold for considering a goal in collision"), BT::InputPort("use_footprint", @@ -63,9 +61,7 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase rclcpp::Node::SharedPtr node_; rclcpp::Client::SharedPtr get_cost_client_; - rclcpp::Client::SharedPtr get_cost_client_2_; std::string costmap_cost_service_; - std::string costmap_cost_service_2_; bool initialized_; bool use_footprint_; double cost_threshold_; 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 eea32e0bfd..0c65723ec0 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 @@ -30,7 +30,6 @@ RemoveInCollisionGoals::RemoveInCollisionGoals( : BT::ActionNodeBase(name, conf), initialized_(false), costmap_cost_service_("/global_costmap/get_cost_global_costmap"), - costmap_cost_service_2_(""), use_footprint_(true), cost_threshold_(253) {} @@ -41,15 +40,10 @@ void RemoveInCollisionGoals::initialize() server_timeout_ = config().blackboard->template get("server_timeout"); getInput("costmap_cost_service", costmap_cost_service_); - getInput("costmap_cost_service_2", costmap_cost_service_2_); get_cost_client_ = node_->create_client( costmap_cost_service_); - if (!costmap_cost_service_2_.empty()) { - get_cost_client_2_ = node_->create_client( - costmap_cost_service_2_); - } } inline BT::NodeStatus RemoveInCollisionGoals::tick() @@ -79,39 +73,17 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() request->theta = tf2::getYaw(goal.pose.orientation); request->use_footprint = use_footprint_; - if (costmap_cost_service_2_.empty()) { - 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); - } - } else { - RCLCPP_ERROR( - node_->get_logger(), - "RemoveInCollisionGoals BT node failed to call GetCost service of costmap"); - return BT::NodeStatus::FAILURE; + 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); } } else { - auto future = get_cost_client_->async_send_request(request); - auto future_2 = get_cost_client_2_->async_send_request(request); - auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_); - auto ret_2 = rclcpp::spin_until_future_complete(node_, future_2, server_timeout_); - if (ret == rclcpp::FutureReturnCode::SUCCESS && - ret_2 == rclcpp::FutureReturnCode::SUCCESS) - { - if (future.get()->cost <= cost_threshold_ && - future_2.get()->cost <= cost_threshold_) - { - valid_goal_poses.push_back(goal); - } - } else { - RCLCPP_ERROR( - node_->get_logger(), - "RemoveInCollisionGoals BT node failed to call GetCost " - "service of one or two costmaps"); - return BT::NodeStatus::FAILURE; - } + 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); From 45b8a082463ea9c6e8e0dfebd31f607979248ef1 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 6 Aug 2024 15:15:15 +0000 Subject: [PATCH 07/21] Minor fixes Signed-off-by: Tony Najjar --- nav2_behavior_tree/CMakeLists.txt | 3 +++ .../plugins/action/remove_in_collision_goals_action.hpp | 2 +- .../plugins/action/remove_in_collision_goals_action.cpp | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b33948bf90..8f45cd318f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -176,6 +176,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node) add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp) list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) +add_library(nav2_remove_in_collision_goals_action_bt_node SHARED plugins/action/remove_in_collision_goals_action.cpp) +list(APPEND plugin_libs nav2_remove_in_collision_goals_action_bt_node) + add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp) list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node) 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 f548fae09d..7dd3befb15 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 @@ -61,8 +61,8 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase rclcpp::Node::SharedPtr node_; rclcpp::Client::SharedPtr get_cost_client_; - std::string costmap_cost_service_; bool initialized_; + std::string costmap_cost_service_; bool use_footprint_; double cost_threshold_; std::chrono::milliseconds server_timeout_; 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 0c65723ec0..62d88ca27d 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 @@ -92,7 +92,7 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RemoveInCollisionGoals"); From 640b0eb352a58ce84080217b1653b3184351acb6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 08:54:00 +0000 Subject: [PATCH 08/21] 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 7dd3befb15..af303aa296 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 62d88ca27d..fa24ed8a13 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 c95a19902d..33feaa700d 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 18d15525a5..dcf6d50b7a 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 8563d6dd16..803e2a1f8e 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 8cee7cdb7f..a9f93b5ecb 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 577654f55c..0000000000 --- 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 0000000000..0328d47639 --- /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 197fe2b655..3466e8831b 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 0d628e0215..3203c0cd0d 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) { From 8eccb334d007d7125660dac40d2a538856cd50f8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 09:05:47 +0000 Subject: [PATCH 09/21] get future once Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 fa24ed8a13..c278399676 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 @@ -79,8 +79,9 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick() 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_) { + auto response = future.get(); + for (size_t i = 0; i < response->costs.size(); ++i) { + if (response->costs[i] <= cost_threshold_) { valid_goal_poses.push_back(goal_poses[i]); } } From c27c10617294418770f3bf630d5b83fe7745dc4c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 10:37:23 +0000 Subject: [PATCH 10/21] replace with BTServiceNode Signed-off-by: Tony Najjar --- .../remove_in_collision_goals_action.hpp | 32 +++++----- .../remove_in_collision_goals_action.cpp | 62 ++++++------------- 2 files changed, 36 insertions(+), 58 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 af303aa296..c6dea01a82 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 @@ -21,29 +21,38 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp/action_node.h" +#include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" namespace nav2_behavior_tree { -class RemoveInCollisionGoals : public BT::ActionNodeBase +class RemoveInCollisionGoals : public BtServiceNode { public: typedef std::vector Goals; + /** + * @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ RemoveInCollisionGoals( - const std::string & xml_tag_name, + const std::string & service_node_name, const BT::NodeConfiguration & conf); /** - * @brief Function to read parameters and initialize class variables + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution */ - void initialize(); + void on_tick() override; + + BT::NodeStatus on_completion(std::shared_ptr response) + override; static BT::PortsList providedPorts() { - return { + return providedBasicPorts({ BT::InputPort("input_goals", "Original goals to remove from"), BT::InputPort("costmap_cost_service", "Service to get cost from costmap"), @@ -52,20 +61,13 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase BT::InputPort("use_footprint", "Whether to use footprint cost"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), - }; + }); } private: - void halt() override {} - BT::NodeStatus tick() override; - - rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr get_cost_client_; - bool initialized_; - std::string costmap_cost_service_; bool use_footprint_; double cost_threshold_; - std::chrono::milliseconds server_timeout_; + Goals input_goals_; }; } // namespace nav2_behavior_tree 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 c278399676..431f1b2479 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 @@ -25,71 +25,47 @@ namespace nav2_behavior_tree { RemoveInCollisionGoals::RemoveInCollisionGoals( - const std::string & name, + const std::string & service_node_name, const BT::NodeConfiguration & conf) -: BT::ActionNodeBase(name, conf), - initialized_(false), - costmap_cost_service_("/global_costmap/get_cost_global_costmap"), +: BtServiceNode(service_node_name, conf), use_footprint_(true), cost_threshold_(253) {} -void RemoveInCollisionGoals::initialize() -{ - node_ = config().blackboard->get("node"); - server_timeout_ = config().blackboard->template get("server_timeout"); - - getInput("costmap_cost_service", costmap_cost_service_); - get_cost_client_ = node_->create_client( - costmap_cost_service_); -} - -inline BT::NodeStatus RemoveInCollisionGoals::tick() +void RemoveInCollisionGoals::on_tick() { - setStatus(BT::NodeStatus::RUNNING); - - if (!initialized_) { - initialize(); - } - getInput("use_footprint", use_footprint_); getInput("cost_threshold", cost_threshold_); - Goals goal_poses; - getInput("input_goals", goal_poses); + Goals input_goals; + getInput("input_goals", input_goals_); - if (goal_poses.empty()) { - setOutput("output_goals", goal_poses); - return BT::NodeStatus::SUCCESS; + if (input_goals_.empty()) { + setOutput("output_goals", input_goals_); + should_send_request_ = false; } Goals valid_goal_poses; - auto request = std::make_shared(); - request->use_footprint = use_footprint_; + request_->use_footprint = use_footprint_; - for (const auto & goal : goal_poses) { + 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(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) { - auto response = future.get(); - for (size_t i = 0; i < response->costs.size(); ++i) { - if (response->costs[i] <= cost_threshold_) { - valid_goal_poses.push_back(goal_poses[i]); - } +BT::NodeStatus RemoveInCollisionGoals::on_completion( + std::shared_ptr response) +{ + Goals valid_goal_poses; + for (size_t i = 0; i < response->costs.size(); ++i) { + if (response->costs[i] <= cost_threshold_) { + valid_goal_poses.push_back(input_goals_[i]); } - } 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; From 29887dd4abbc2ff3b22b6af93c07900acce98bd2 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 10:37:29 +0000 Subject: [PATCH 11/21] Add test Signed-off-by: Tony Najjar --- .../test/plugins/action/CMakeLists.txt | 2 + .../test_remove_in_collision_goals_action.cpp | 171 ++++++++++++++++++ 2 files changed, 173 insertions(+) create mode 100644 nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 1be5a262ba..664a2eb9a9 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -51,6 +51,8 @@ plugin_add_test(test_truncate_path_local_action test_truncate_path_local_action. plugin_add_test(test_remove_passed_goals_action test_remove_passed_goals_action.cpp nav2_remove_passed_goals_action_bt_node) +plugin_add_test(test_remove_in_collision_goals_action test_remove_in_collision_goals_action.cpp nav2_remove_in_collision_goals_action_bt_node) + plugin_add_test(test_get_pose_from_path_action test_get_pose_from_path_action.cpp nav2_get_pose_from_path_action_bt_node) plugin_add_test(test_planner_selector_node test_planner_selector_node.cpp nav2_planner_selector_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp new file mode 100644 index 0000000000..ada62eb224 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 + +#include "behaviortree_cpp/bt_factory.h" + +#include "utils/test_service.hpp" +#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp" +#include "utils/test_behavior_tree_fixture.hpp" + + +class RemoveInCollisionGoalsService : public TestService +{ +public: + RemoveInCollisionGoalsService() + : TestService("/global_costmap/get_cost_global_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {100, 50, 5, 254}; + } +}; + + +class RemoveInCollisionGoalsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("in_collision_goals_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "RemoveInCollisionGoals", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + static std::shared_ptr server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr; +std::shared_ptr +RemoveInCollisionGoalsTestFixture::server_ = nullptr; +std::shared_ptr RemoveInCollisionGoalsTestFixture::factory_ = nullptr; +std::shared_ptr RemoveInCollisionGoalsTestFixture::tree_ = nullptr; + +TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + std::vector poses; + poses.resize(4); + poses[0].pose.position.x = 0.0; + poses[0].pose.position.y = 0.0; + + poses[1].pose.position.x = 0.5; + poses[1].pose.position.y = 0.0; + + poses[2].pose.position.x = 1.0; + poses[2].pose.position.y = 0.0; + + poses[3].pose.position.x = 2.0; + poses[3].pose.position.y = 0.0; + + config_->blackboard->set("goals", poses); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check that it removed the point in range + std::vector output_poses; + EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); + + EXPECT_EQ(output_poses.size(), 3u); + EXPECT_EQ(output_poses[0], poses[0]); + EXPECT_EQ(output_poses[1], poses[1]); + EXPECT_EQ(output_poses[2], poses[2]); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + RemoveInCollisionGoalsTestFixture::server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} From 94651293b73c256db9a6c64f6217233f7fabd3a6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 10:51:48 +0000 Subject: [PATCH 12/21] fix costmap tool Signed-off-by: Tony Najjar --- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 3203c0cd0d..7f1fa84daf 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -110,8 +110,8 @@ 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); + 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"); } @@ -121,8 +121,8 @@ 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); + 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"); } From 7fb4367c3cd10da836f8b2785d54edc6069bd745 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 11:30:13 +0000 Subject: [PATCH 13/21] fix ament_uncrustify Signed-off-by: Tony Najjar --- .../action/remove_in_collision_goals_action.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 c6dea01a82..57c615027a 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 @@ -53,14 +53,14 @@ class RemoveInCollisionGoals : public BtServiceNode static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("costmap_cost_service", + BT::InputPort("input_goals", "Original goals to remove from"), + BT::InputPort("costmap_cost_service", "Service to get cost from costmap"), - BT::InputPort("cost_threshold", + BT::InputPort("cost_threshold", "Cost threshold for considering a goal in collision"), - BT::InputPort("use_footprint", + BT::InputPort("use_footprint", "Whether to use footprint cost"), - BT::OutputPort("output_goals", "Goals with in-collision goals removed"), + BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }); } From facdb1e3398ff9227c83efb2411404e567cfc91b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 12:40:20 +0000 Subject: [PATCH 14/21] remove duplicate Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 2 -- 1 file changed, 2 deletions(-) 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 431f1b2479..546e2ec80f 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 @@ -45,8 +45,6 @@ void RemoveInCollisionGoals::on_tick() setOutput("output_goals", input_goals_); should_send_request_ = false; } - - Goals valid_goal_poses; request_->use_footprint = use_footprint_; for (const auto & goal : input_goals) { From 135b19be6b64e23c0a2ae596487c76eaae20920a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 12:40:53 +0000 Subject: [PATCH 15/21] add return Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 1 + 1 file changed, 1 insertion(+) 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 546e2ec80f..8b39dacdfe 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 @@ -44,6 +44,7 @@ void RemoveInCollisionGoals::on_tick() if (input_goals_.empty()) { setOutput("output_goals", input_goals_); should_send_request_ = false; + return; } request_->use_footprint = use_footprint_; From f0a46ee4808c0173ea95a995d59228ebc862c084 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 13:05:05 +0000 Subject: [PATCH 16/21] fix typo Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 8b39dacdfe..566e8d9994 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 @@ -38,7 +38,6 @@ void RemoveInCollisionGoals::on_tick() getInput("use_footprint", use_footprint_); getInput("cost_threshold", cost_threshold_); - Goals input_goals; getInput("input_goals", input_goals_); if (input_goals_.empty()) { @@ -48,7 +47,7 @@ void RemoveInCollisionGoals::on_tick() } request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals) { + for (const auto & goal : input_goals_) { geometry_msgs::msg::Pose2D pose; pose.x = goal.pose.position.x; pose.y = goal.pose.position.y; From 926258375077569153ba45e9de2ec3fdc136c350 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 7 Aug 2024 13:16:08 +0000 Subject: [PATCH 17/21] reset request before sending Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 1 + 1 file changed, 1 insertion(+) 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 566e8d9994..7a7f8c226f 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 @@ -45,6 +45,7 @@ void RemoveInCollisionGoals::on_tick() should_send_request_ = false; return; } + request_ = std::make_shared(); request_->use_footprint = use_footprint_; for (const auto & goal : input_goals_) { From 3f0c6c515bfcf1b78698471e3ece3e1564c50c4f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 8 Aug 2024 08:34:29 +0000 Subject: [PATCH 18/21] pr comments fixes Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.hpp | 7 ++----- nav2_behavior_tree/nav2_tree_nodes.xml | 8 ++++++++ .../plugins/action/remove_in_collision_goals_action.cpp | 6 ++---- 3 files changed, 12 insertions(+), 9 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 57c615027a..73aab1bd8c 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 @@ -54,12 +54,9 @@ class RemoveInCollisionGoals : public BtServiceNode { return providedBasicPorts({ BT::InputPort("input_goals", "Original goals to remove from"), - BT::InputPort("costmap_cost_service", - "Service to get cost from costmap"), - BT::InputPort("cost_threshold", + BT::InputPort("cost_threshold", 254.0, "Cost threshold for considering a goal in collision"), - BT::InputPort("use_footprint", - "Whether to use footprint cost"), + BT::InputPort("use_footprint", true, "Whether to use footprint cost"), BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }); } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 925e9c3579..71c20ee4ec 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -100,6 +100,14 @@ Set of goals after removing any passed + + Costmap service name responsible for getting the cost + A vector of goals to check if in collision + Whether to use the footprint cost or the point cost. + The cost threshold above which a waypoint is considered in collision and should be removed. + A vector of goals containing only those that are not in collision. + + Path to be smoothed 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 7a7f8c226f..4ae9929168 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 @@ -27,9 +27,8 @@ namespace nav2_behavior_tree RemoveInCollisionGoals::RemoveInCollisionGoals( const std::string & service_node_name, const BT::NodeConfiguration & conf) -: BtServiceNode(service_node_name, conf), - use_footprint_(true), - cost_threshold_(253) +: BtServiceNode(service_node_name, conf, + "/global_costmap/get_cost_global_costmap") {} @@ -37,7 +36,6 @@ void RemoveInCollisionGoals::on_tick() { getInput("use_footprint", use_footprint_); getInput("cost_threshold", cost_threshold_); - getInput("input_goals", input_goals_); if (input_goals_.empty()) { From a01b09bf3c4e6e46bc7cd1d26d6755d13f034b3c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 8 Aug 2024 09:06:09 +0000 Subject: [PATCH 19/21] fix ament cmake Signed-off-by: Tony Najjar --- nav2_behavior_tree/test/plugins/action/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 664a2eb9a9..e538e63cd0 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -51,7 +51,9 @@ plugin_add_test(test_truncate_path_local_action test_truncate_path_local_action. plugin_add_test(test_remove_passed_goals_action test_remove_passed_goals_action.cpp nav2_remove_passed_goals_action_bt_node) -plugin_add_test(test_remove_in_collision_goals_action test_remove_in_collision_goals_action.cpp nav2_remove_in_collision_goals_action_bt_node) +plugin_add_test(test_remove_in_collision_goals_action + test_remove_in_collision_goals_action.cpp + nav2_remove_in_collision_goals_action_bt_node) plugin_add_test(test_get_pose_from_path_action test_get_pose_from_path_action.cpp nav2_get_pose_from_path_action_bt_node) From 4c035ea92df5377ae25fed357eec5ad5051391e7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 8 Aug 2024 09:10:52 +0000 Subject: [PATCH 20/21] fix test Signed-off-by: Tony Najjar --- .../action/test_remove_in_collision_goals_action.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index ada62eb224..dbb4889298 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -60,6 +60,15 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test config_->blackboard->set( "node", node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) From 7fd9826c6fb41ccd7fa74dd425fd57b5d0e457ee Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 8 Aug 2024 15:34:07 +0000 Subject: [PATCH 21/21] fix Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4ae9929168..2fe8395ad4 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 @@ -60,7 +60,7 @@ 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] < cost_threshold_) { valid_goal_poses.push_back(input_goals_[i]); } }