use ros clock for wait (#3782)

* use ROS clock for wait * fix backport issue --------- Co-authored-by: Guillaume Doisy <[email protected]>
ros-navigation · Aug 29, 2023 · 2109777 · 2109777
1 parent 6ef3d7b
commit 2109777
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 8 deletions.
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp
@@ -63,7 +63,7 @@ class Wait : public TimedBehavior<WaitAction>
   CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
 
 protected:
-  std::chrono::time_point<std::chrono::steady_clock> wait_end_;
+  rclcpp::Time wait_end_;
   WaitAction::Feedback::SharedPtr feedback_;
 };
 

diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp
@@ -30,21 +30,19 @@ Wait::~Wait() = default;
 
 ResultStatus Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
 {
-  wait_end_ = std::chrono::steady_clock::now() +
-    rclcpp::Duration(command->time).to_chrono<std::chrono::nanoseconds>();
+  wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time);
   return ResultStatus{Status::SUCCEEDED};
 }
 
 ResultStatus Wait::onCycleUpdate()
 {
-  auto current_point = std::chrono::steady_clock::now();
-  auto time_left =
-    std::chrono::duration_cast<std::chrono::nanoseconds>(wait_end_ - current_point).count();
+  auto current_point = node_.lock()->now();
+  auto time_left = wait_end_ - current_point;
 
-  feedback_->time_left = rclcpp::Duration(rclcpp::Duration::from_nanoseconds(time_left));
+  feedback_->time_left = time_left;
   action_server_->publish_feedback(feedback_);
 
-  if (time_left > 0) {
+  if (time_left.nanoseconds() > 0) {
     return ResultStatus{Status::RUNNING};
   } else {
     return ResultStatus{Status::SUCCEEDED};