From b881af3b69ae902aee633d0d3e23f4b21efe033a Mon Sep 17 00:00:00 2001
From: Aitor Miguel Blanco <aitormibl@gmail.com>
Date: Tue, 14 Apr 2020 09:01:22 +1000
Subject: [PATCH 1/2] Changed onCycleUpdate to allow preemption (#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
---
 nav2_recoveries/plugins/wait.cpp | 16 +++++++++++-----
 nav2_recoveries/plugins/wait.hpp |  2 +-
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_recoveries/plugins/wait.cpp
index 7964103b3c..c1d7ba5e08 100644
--- a/nav2_recoveries/plugins/wait.cpp
+++ b/nav2_recoveries/plugins/wait.cpp
@@ -24,7 +24,6 @@ namespace nav2_recoveries
 Wait::Wait()
 : Recovery<WaitAction>()
 {
-  duration_.sec = 0.0;
 }
 
 Wait::~Wait()
@@ -33,15 +32,22 @@ Wait::~Wait()
 
 Status Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
 {
-  duration_ = command->time;
+  run_end_ = std::chrono::steady_clock::now() + rclcpp::Duration(command->time).to_chrono<std::chrono::nanoseconds>();
   return Status::SUCCEEDED;
 }
 
 Status Wait::onCycleUpdate()
 {
-  rclcpp::sleep_for(
-    rclcpp::Duration(duration_).to_chrono<std::chrono::nanoseconds>());
-  return Status::SUCCEEDED;
+  auto current_point = std::chrono::steady_clock::now();
+  auto time_left = std::chrono::duration_cast<std::chrono::nanoseconds>(run_end_ - current_point).count();
+
+  if (time_left > 0) {
+    return Status::RUNNING;
+  }
+  else {
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    return Status::SUCCEEDED;
+  }
 }
 
 }  // namespace nav2_recoveries
diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_recoveries/plugins/wait.hpp
index 4f883220d6..d560c11a6f 100644
--- a/nav2_recoveries/plugins/wait.hpp
+++ b/nav2_recoveries/plugins/wait.hpp
@@ -37,7 +37,7 @@ class Wait : public Recovery<WaitAction>
   Status onCycleUpdate() override;
 
 protected:
-  builtin_interfaces::msg::Duration duration_;
+  std::chrono::time_point<std::chrono::steady_clock> run_end_;
 };
 
 }  // namespace nav2_recoveries

From f6c7160676e52d4a4e44d0f77f265a95dea6e265 Mon Sep 17 00:00:00 2001
From: Aitor Miguel Blanco <aitormibl@gmail.com>
Date: Tue, 14 Apr 2020 16:42:10 +1000
Subject: [PATCH 2/2] Deleted unnecessary wait and corrected style (#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
---
 nav2_recoveries/plugins/wait.cpp | 10 +++++-----
 nav2_recoveries/plugins/wait.hpp |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_recoveries/plugins/wait.cpp
index c1d7ba5e08..10dcc19709 100644
--- a/nav2_recoveries/plugins/wait.cpp
+++ b/nav2_recoveries/plugins/wait.cpp
@@ -32,20 +32,20 @@ Wait::~Wait()
 
 Status Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
 {
-  run_end_ = std::chrono::steady_clock::now() + rclcpp::Duration(command->time).to_chrono<std::chrono::nanoseconds>();
+  wait_end_ = std::chrono::steady_clock::now() +
+    rclcpp::Duration(command->time).to_chrono<std::chrono::nanoseconds>();
   return Status::SUCCEEDED;
 }
 
 Status Wait::onCycleUpdate()
 {
   auto current_point = std::chrono::steady_clock::now();
-  auto time_left = std::chrono::duration_cast<std::chrono::nanoseconds>(run_end_ - current_point).count();
+  auto time_left =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(wait_end_ - current_point).count();
 
   if (time_left > 0) {
     return Status::RUNNING;
-  }
-  else {
-    rclcpp::sleep_for(std::chrono::milliseconds(100));
+  } else {
     return Status::SUCCEEDED;
   }
 }
diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_recoveries/plugins/wait.hpp
index d560c11a6f..af337d3c94 100644
--- a/nav2_recoveries/plugins/wait.hpp
+++ b/nav2_recoveries/plugins/wait.hpp
@@ -37,7 +37,7 @@ class Wait : public Recovery<WaitAction>
   Status onCycleUpdate() override;
 
 protected:
-  std::chrono::time_point<std::chrono::steady_clock> run_end_;
+  std::chrono::time_point<std::chrono::steady_clock> wait_end_;
 };
 
 }  // namespace nav2_recoveries