diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp index 332ac62578..4fd35c4f03 100644 --- 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 CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;} protected: - std::chrono::time_point wait_end_; + rclcpp::Time wait_end_; WaitAction::Feedback::SharedPtr feedback_; }; diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index e47de4a8f8..91e95d6c97 100644 --- 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 command) { - wait_end_ = std::chrono::steady_clock::now() + - rclcpp::Duration(command->time).to_chrono(); + 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(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};