Allow reaction to cancellation and preemption on wait recovery plugin (…

…#1636)

* Changed onCycleUpdate to allow preemption (#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Deleted unnecessary wait and corrected style (#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

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;
+  wait_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>(wait_end_ - current_point).count();
+
+  if (time_left > 0) {
+    return Status::RUNNING;
+  } else {
+    return Status::SUCCEEDED;
+  }
 }
 
 }  // namespace nav2_recoveries

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> wait_end_;
 };
 
 }  // namespace nav2_recoveries