Changed onCycleUpdate to allow preemption (ros-navigation#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
gimait · Apr 13, 2020 · b881af3 · b881af3
1 parent e346948
commit b881af3
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 6 deletions.
diff --git 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
@@ -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