readd steady clock for metrics on planner/smoother servers

ros-navigation · Dec 5, 2023 · 06dcff5 · 06dcff5
1 parent 822cd77
commit 06dcff5
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 6 deletions.
diff --git 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<tf2_ros::Buffer> tf_;
+  rclcpp::Clock steady_clock_;
 
   // Global Costmap
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;

diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -373,7 +373,7 @@ void PlannerServer::computePlanThroughPoses()
 {
   std::lock_guard<std::mutex> 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<std::mutex> 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
@@ -149,6 +149,7 @@ class SmootherServer : public nav2_util::LifecycleNode
   // Transforms
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+  rclcpp::Clock steady_clock_;
 
   // Publishers and subscribers
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;

diff --git 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(