diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 117c2629d5..075a2ba447 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -77,6 +77,9 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node) add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp) list(APPEND plugin_libs nav2_goal_reached_condition_bt_node) +add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp) +list(APPEND plugin_libs nav2_goal_updated_condition_bt_node) + add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp) list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node) diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp new file mode 100644 index 0000000000..4007bddc9c --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -0,0 +1,76 @@ +#ifndef NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/condition_node.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" + +namespace nav2_behavior_tree +{ + +class GoalUpdatedCondition : public BT::ConditionNode +{ +public: + GoalUpdatedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) + : BT::ConditionNode(condition_name, conf), initialized_(false) + { + } + + GoalUpdatedCondition() = delete; + + BT::NodeStatus tick() override + { + if (!initialized_) { + initialize(); + } + + if (isGoalUpdated()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; + } + + void initialize() + { + goal_ = config().blackboard->get("goal"); + initialized_ = true; + } + + bool + isGoalUpdated() + { + auto current_goal = config().blackboard->get("goal"); + if (goal_ != current_goal) { + goal_ = current_goal; + return true; + } else { + return false; + } + } + + static BT::PortsList providedPorts() + { + return {}; + } + +private: + geometry_msgs::msg::PoseStamped goal_; + bool initialized_; +}; + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdated"); +} + +#endif // NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index e37d52ed1c..2526635ee3 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -59,6 +59,7 @@ bt_navigator: - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 44276b9e30..d5a11f724e 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -59,6 +59,7 @@ bt_navigator: - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 4e6af4da43..8b403ca5a0 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -59,6 +59,7 @@ bt_navigator: - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml index d4fcc6b609..79b973192e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml @@ -18,12 +18,15 @@ - - - - - - + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml index beb246fbe9..f7e06edcc2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml @@ -10,21 +10,30 @@ - - - - + + + + + + + - - - - + + + + + + + + + + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0d9f03dd49..ba9993aaf3 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -42,6 +42,7 @@ BtNavigator::BtNavigator() "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", + "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_recovery_node_bt_node",