Merge pull request #1176 from SteveMacenski/wait_Recovery

Wait recovery

nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp

+// 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_

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");

nav2_bringup/bringup/launch/bt_navigator.xml

-<!--
-  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>

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:

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>

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()

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"

nav2_msgs/action/Wait.action

+#goal definition
+builtin_interfaces/Duration time
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

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()

nav2_recoveries/plugins/wait.cpp

+// 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)

nav2_recoveries/plugins/wait.hpp

+// 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_

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

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));

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: