diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp new file mode 100644 index 0000000000000000000000000000000000000000..79b1c65f90a9653acfe0273a146a3f017bc2fee2 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp @@ -0,0 +1,52 @@ +// Copyright (c) 2018 Samsung Research America +// +// 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__WAIT_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ + +#include <string> +#include <memory> +#include <cmath> + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_msgs/action/wait.hpp" + +namespace nav2_behavior_tree +{ + +class WaitAction : public BtActionNode<nav2_msgs::action::Wait> +{ +public: + explicit WaitAction(const std::string & action_name) + : BtActionNode<nav2_msgs::action::Wait>(action_name) + { + } + + void on_init() override + { + int duration; + getParam<int>("wait_duration", duration); + if (duration <= 0) { + RCLCPP_WARN(node_->get_logger(), "Wait duration is negative or zero " + "(%i). Setting to positive.", duration); + duration *= -1; + } + + goal_.time.sec = duration; + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index e34090606ce719c6735917f9087c317e94e6e8d5..5c3a69fa8c0237037ccdde97522ec25d25b5bb76 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -27,6 +27,7 @@ #include "nav2_behavior_tree/rate_controller_node.hpp" #include "nav2_behavior_tree/recovery_node.hpp" #include "nav2_behavior_tree/spin_action.hpp" +#include "nav2_behavior_tree/wait_action.hpp" #include "nav2_behavior_tree/clear_costmap_service.hpp" #include "nav2_behavior_tree/reinitialize_global_localization_service.hpp" #include "rclcpp/rclcpp.hpp" @@ -43,6 +44,7 @@ BehaviorTreeEngine::BehaviorTreeEngine() factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath"); factory_.registerNodeType<nav2_behavior_tree::BackUpAction>("BackUp"); factory_.registerNodeType<nav2_behavior_tree::SpinAction>("Spin"); + factory_.registerNodeType<nav2_behavior_tree::WaitAction>("Wait"); factory_.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap"); factory_.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>( "ReinitializeGlobalLocalization"); diff --git a/nav2_bringup/bringup/launch/bt_navigator.xml b/nav2_bringup/bringup/launch/bt_navigator.xml deleted file mode 100644 index 16eb0564d1d511432968e1caf4c4c24d38528ed7..0000000000000000000000000000000000000000 --- a/nav2_bringup/bringup/launch/bt_navigator.xml +++ /dev/null @@ -1,30 +0,0 @@ -<!-- - This Behavior Tree first computes a path using the global planner (ComputePathToPose). - Then, it runs two sub-branches in parallel. The first sub-branch is a FollowPath - operation (the local planner). In parallel, there is a rate controlled execution of - FollowPath (the global planner). Each time a new path is computed, the path update - is sent to the local planner. The right branch, which is the rate controlled - ComputePathToPose, always returns RUNNING. Because the Parallel node uses a - threshold of 1, whenever the FollowPath returns SUCCESS or FAILURE, the parallel - node will return this result. - - The goal (input to the global planner) and the resulting path (output of the global - planner and input to the local planner) are passed on the blackboard. - - The rate at which the ComputePathToPose operation is invoked can be controlled with - the hz parameter to the RateController node. ---> - -<root main_tree_to_execute="MainTree"> - <BehaviorTree ID="MainTree"> - <Sequence name="root"> - <RateController hz="1.0"> - <Fallback> - <GoalReached/> - <ComputePathToPose goal="${goal}"/> - </Fallback> - </RateController> - <FollowPath path="${path}"/> - </Sequence> - </BehaviorTree> -</root> diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index d8acf8ae94b02b605c614e494db2ed1295349bcd..595cd8187609fc277934fa076010aceb73c5b01d 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -49,7 +49,7 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True - bt_xml_filename: "bt_navigator.xml" + bt_xml_filename: "navigate_w_replanning_and_recovery.xml" bt_navigator_rclcpp_node: ros__parameters: 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 f0bc4e10af3e87c3c034f16efe6ca56e5bfbe956..4fd109b282282933f5fb0a750f7c43883e508bab 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 @@ -24,6 +24,7 @@ <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/> <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/> <Spin/> + <Wait wait_duration="5"/> </SequenceStar> </RecoveryNode> </BehaviorTree> diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index cf1acb6b215b50e842d961bce4bd5826330fb284..357992b3898489c7ce96d9db11ebd0a84d3e2972 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -31,7 +31,8 @@ BtNavigator::BtNavigator() RCLCPP_INFO(get_logger(), "Creating"); // Declare this node's parameters - declare_parameter("bt_xml_filename", rclcpp::ParameterValue(std::string("bt_navigator.xml"))); + declare_parameter("bt_xml_filename", + rclcpp::ParameterValue(std::string("navigate_w_replanning_and_recovery.xml"))); } BtNavigator::~BtNavigator() diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 6e07807da979c7bfc87486f3a07c93848bd19649..56c5d1dc152aa3cd701b72d364f8ab0b67448d57 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/ComputePathToPose.action" "action/FollowPath.action" "action/NavigateToPose.action" + "action/Wait.action" "action/Spin.action" "action/DummyRecovery.action" "action/RandomCrawl.action" diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action new file mode 100644 index 0000000000000000000000000000000000000000..0855b72323a6dcdf2fb6c86f82042b070097ec40 --- /dev/null +++ b/nav2_msgs/action/Wait.action @@ -0,0 +1,7 @@ +#goal definition +builtin_interfaces/Duration time +--- +#result definition +std_msgs/Empty result +--- +#feedback diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index 7d85dcf70c2c974e3374a2e39d1dc8e948020793..b1a741a1888b03aff1ec8c6bc3e77b9bb6d5486a 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_recoveries/CMakeLists.txt @@ -61,6 +61,14 @@ ament_target_dependencies(nav2_backup_recovery ${dependencies} ) +add_library(nav2_wait_recovery SHARED + plugins/wait.cpp +) + +ament_target_dependencies(nav2_wait_recovery + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml) # Library @@ -87,6 +95,7 @@ install(TARGETS ${executable_name} ${library_name} nav2_backup_recovery nav2_spin_recovery + nav2_wait_recovery ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} @@ -109,6 +118,10 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} nav2_backup_recovery nav2_spin_recovery) +ament_export_libraries(${library_name} + nav2_backup_recovery + nav2_spin_recovery + nav2_wait_recovery +) ament_package() diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_recoveries/plugins/wait.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7964103b3cd19672d0abf6136e26e533c7a27c4c --- /dev/null +++ b/nav2_recoveries/plugins/wait.cpp @@ -0,0 +1,50 @@ +// Copyright (c) 2019 Samsung Research America +// +// 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 <cmath> +#include <chrono> +#include <memory> + +#include "wait.hpp" + +namespace nav2_recoveries +{ + +Wait::Wait() +: Recovery<WaitAction>() +{ + duration_.sec = 0.0; +} + +Wait::~Wait() +{ +} + +Status Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command) +{ + duration_ = command->time; + return Status::SUCCEEDED; +} + +Status Wait::onCycleUpdate() +{ + rclcpp::sleep_for( + rclcpp::Duration(duration_).to_chrono<std::chrono::nanoseconds>()); + return Status::SUCCEEDED; +} + +} // namespace nav2_recoveries + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Wait, nav2_core::Recovery) diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_recoveries/plugins/wait.hpp new file mode 100644 index 0000000000000000000000000000000000000000..4f883220d6842f0f231894fbcb6f9824c8319877 --- /dev/null +++ b/nav2_recoveries/plugins/wait.hpp @@ -0,0 +1,45 @@ +// Copyright (c) 2019 Samsung Research America +// +// 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_RECOVERIES__PLUGINS__WAIT_HPP_ +#define NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ + +#include <chrono> +#include <string> +#include <memory> + +#include "nav2_recoveries/recovery.hpp" +#include "nav2_msgs/action/wait.hpp" + +namespace nav2_recoveries +{ +using WaitAction = nav2_msgs::action::Wait; + +class Wait : public Recovery<WaitAction> +{ +public: + Wait(); + ~Wait(); + + Status onRun(const std::shared_ptr<const WaitAction::Goal> command) override; + + Status onCycleUpdate() override; + +protected: + builtin_interfaces::msg::Duration duration_; +}; + +} // namespace nav2_recoveries + +#endif // NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ diff --git a/nav2_recoveries/recovery_plugin.xml b/nav2_recoveries/recovery_plugin.xml index 12a9324145cd2448430614537f1f914034f07abb..b8ccaca0ad5cd217765054b381f7e44342a5056c 100644 --- a/nav2_recoveries/recovery_plugin.xml +++ b/nav2_recoveries/recovery_plugin.xml @@ -10,4 +10,10 @@ <description></description> </class> </library> + + <library path="nav2_wait_recovery"> + <class name="nav2_recoveries/Wait" type="nav2_recoveries::Wait" base_class_type="nav2_core::Recovery"> + <description></description> + </class> + </library> </class_libraries> \ No newline at end of file diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index 95d5d7a35b8ad5622386942aca3a76785e36170f..c86c888316713b51a0ecfee1dc50289358c94500 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -50,9 +50,10 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); std::vector<std::string> plugin_names{std::string("Spin"), - std::string("BackUp")}; + std::string("BackUp"), std::string("Wait")}; std::vector<std::string> plugin_types{std::string("nav2_recoveries/Spin"), - std::string("nav2_recoveries/BackUp")}; + std::string("nav2_recoveries/BackUp"), + std::string("nav2_recoveries/Wait")}; declare_parameter("plugin_names", rclcpp::ParameterValue(plugin_names)); diff --git a/nav2_system_tests/src/updown/nav2_params.yaml b/nav2_system_tests/src/updown/nav2_params.yaml index 2cebab8065f327c111adb09cfef2c49e733bdd0c..46312f8eec84c50528077fadf52ac551f91a9504 100644 --- a/nav2_system_tests/src/updown/nav2_params.yaml +++ b/nav2_system_tests/src/updown/nav2_params.yaml @@ -54,7 +54,7 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True - bt_xml_filename: "bt_navigator.xml" + bt_xml_filename: "navigate_w_replanning_and_recovery.xml" bt_navigator_rclcpp_node: ros__parameters: