wait recovery behavior

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

@@ -0,0 +1,51 @@
+// 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_ = 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_INFO(node_->get_logger(), "Waiting %i seconds.", duration_);
+  auto sleep_dur = std::chrono::seconds(duration_);
+  rclcpp::sleep_for(sleep_dur);
+  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

@@ -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:
+  int 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>

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