diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 30955681deb..68a05e0079e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -209,6 +209,9 @@ list(APPEND plugin_libs nav2_progress_checker_selector_bt_node) add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) +add_library(nav2_is_goal_nearby_condition_bt_node SHARED plugins/condition/is_goal_nearby_condition.cpp) +list(APPEND plugin_libs nav2_is_goal_nearby_condition_bt_node) + foreach(bt_plugin ${plugin_libs}) target_include_directories(${bt_plugin} PRIVATE diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp new file mode 100644 index 00000000000..b434e75f1d3 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2024 Jakub Chudziński +// +// 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__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ + +#include +#include "nav_msgs/msg/path.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" + + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when the IsGoalNearby + * service returns true and FAILURE otherwise + */ +class IsGoalNearbyCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsGoalNearbyCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsGoalNearbyCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsGoalNearbyCondition() = delete; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("path", "Planned Path"), + BT::InputPort( + "proximity_threshold", 3.0, + "Proximity length (m) of the remaining path considered as a nearby"), + }; + } + +private: + + /** + * @brief Checks if the robot is in the goal proximity + * @param goal_path current planned path to the goal + * @param prox_thr proximity length (m) of the remaining path considered as a nearby + * @return whether the robot is in the goal proximity + */ + bool isRobotInGoalProximity( + const nav_msgs::msg::Path& goal_path, + const double& prox_thr); +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ \ No newline at end of file diff --git a/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp b/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp new file mode 100644 index 00000000000..5b2855ebefd --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.cpp @@ -0,0 +1,42 @@ +#include "nav2_behavior_tree/plugins/condition/is_goal_nearby_condition.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_behavior_tree +{ + +IsGoalNearbyCondition::IsGoalNearbyCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf) +{ +} + +bool IsGoalNearbyCondition::isRobotInGoalProximity( + const nav_msgs::msg::Path& goal_path, + const double& prox_thr) +{ + return nav2_util::geometry_utils::calculate_path_length(goal_path, 0) < prox_thr; +} + + +BT::NodeStatus IsGoalNearbyCondition::tick() +{ + nav_msgs::msg::Path path; + double prox_thr; + getInput("path", path); + getInput("proximity_threshold", prox_thr); + if (!path.poses.empty()){ + if(isRobotInGoalProximity(path,prox_thr)){ + return BT::NodeStatus::SUCCESS; + } + } + return BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsGoalNearby"); +} diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml new file mode 100644 index 00000000000..42030c00b0d --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_path_if_not_in_goal_proximity.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file