diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 2413e2eb3f0..761664bc370 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -243,6 +243,7 @@ class PlannerServer : public nav2_util::LifecycleNode // TF buffer std::shared_ptr tf_; + rclcpp::Clock steady_clock_; // Global Costmap std::shared_ptr costmap_ros_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 95ef9b790c5..84edc6e4df3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -373,7 +373,7 @@ void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = this->now(); + auto start_time = steady_clock_.now(); // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); @@ -433,7 +433,7 @@ void PlannerServer::computePlanThroughPoses() result->path = concat_path; publishPlan(result->path); - auto cycle_duration = this->now() - start_time; + auto cycle_duration = steady_clock_.now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { @@ -492,7 +492,7 @@ PlannerServer::computePlan() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = this->now(); + auto start_time = steady_clock_.now(); // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); @@ -529,7 +529,7 @@ PlannerServer::computePlan() // Publish the plan for visualization purposes publishPlan(result->path); - auto cycle_duration = this->now() - start_time; + auto cycle_duration = steady_clock_.now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index fac9ffe3af6..efff389cfaa 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -149,6 +149,7 @@ class SmootherServer : public nav2_util::LifecycleNode // Transforms std::shared_ptr tf_; std::shared_ptr transform_listener_; + rclcpp::Clock steady_clock_; // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 4e7f7242deb..92d567a0573 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -259,7 +259,7 @@ bool SmootherServer::findSmootherId( void SmootherServer::smoothPlan() { - auto start_time = this->now(); + auto start_time = steady_clock_.now(); RCLCPP_INFO(get_logger(), "Received a path to smooth."); @@ -283,7 +283,7 @@ void SmootherServer::smoothPlan() result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); - result->smoothing_duration = this->now() - start_time; + result->smoothing_duration = steady_clock_.now() - start_time; if (!result->was_completed) { RCLCPP_INFO(