otimal deep coppy obj

src/layered_costmap.cpp

@@ -69,6 +69,68 @@ namespace robot_costmap_2d
       costmap_.setDefaultValue(NO_INFORMATION);
     else
       costmap_.setDefaultValue(FREE_SPACE);
+    performance_window_start_ = std::chrono::steady_clock::now();
+  }
+
+  void LayeredCostmap::setPerformanceMetrics(bool enabled, double reporting_period_seconds)
+  {
+    performance_metrics_enabled_ = enabled;
+    performance_metrics_period_seconds_ = reporting_period_seconds > 0.0 ? reporting_period_seconds : 5.0;
+    resetPerformanceMetrics();
+  }
+
+  void LayeredCostmap::resetPerformanceMetrics()
+  {
+    performance_window_start_ = std::chrono::steady_clock::now();
+    performance_cycle_nanoseconds_ = 0;
+    performance_reset_nanoseconds_ = 0;
+    performance_cycles_ = 0;
+    performance_cycle_samples_.clear();
+    performance_cycle_samples_.reserve(128);
+    layer_performance_.assign(plugins_.size(), LayerPerformance());
+  }
+
+  void LayeredCostmap::maybeReportPerformance()
+  {
+    if (!performance_metrics_enabled_ || performance_cycles_ == 0)
+      return;
+
+    const auto now = std::chrono::steady_clock::now();
+    const double elapsed = std::chrono::duration<double>(now - performance_window_start_).count();
+    if (elapsed < performance_metrics_period_seconds_)
+      return;
+
+    const double average_cycle_ms =
+        static_cast<double>(performance_cycle_nanoseconds_) / performance_cycles_ / 1.0e6;
+    const double average_reset_ms =
+        static_cast<double>(performance_reset_nanoseconds_) / performance_cycles_ / 1.0e6;
+    std::sort(performance_cycle_samples_.begin(), performance_cycle_samples_.end());
+    const auto percentile_ms = [this](double percentile) {
+      if (performance_cycle_samples_.empty())
+        return 0.0;
+      const std::size_t index = static_cast<std::size_t>(
+          percentile * static_cast<double>(performance_cycle_samples_.size() - 1));
+      return static_cast<double>(performance_cycle_samples_[index]) / 1.0e6;
+    };
+    robot::log_info(
+        "Costmap performance: cycles=%llu avg_cycle_ms=%.3f p95_cycle_ms=%.3f "
+        "p99_cycle_ms=%.3f avg_reset_ms=%.3f\n",
+        static_cast<unsigned long long>(performance_cycles_), average_cycle_ms,
+        percentile_ms(0.95), percentile_ms(0.99), average_reset_ms);
+
+    for (std::size_t i = 0; i < plugins_.size() && i < layer_performance_.size(); ++i)
+    {
+      const LayerPerformance& stats = layer_performance_[i];
+      const double average_bounds_ms = stats.bounds_calls == 0 ? 0.0 :
+          static_cast<double>(stats.bounds_nanoseconds) / stats.bounds_calls / 1.0e6;
+      const double average_costs_ms = stats.costs_calls == 0 ? 0.0 :
+          static_cast<double>(stats.costs_nanoseconds) / stats.costs_calls / 1.0e6;
+      robot::log_info(
+          "Costmap layer [%s]: avg_bounds_ms=%.3f avg_costs_ms=%.3f\n",
+          plugins_[i]->getName().c_str(), average_bounds_ms, average_costs_ms);
+    }
+
+    resetPerformanceMetrics();
   }
 
   LayeredCostmap::~LayeredCostmap()
@@ -94,6 +156,8 @@ namespace robot_costmap_2d
 
   void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
   {
+    const auto cycle_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+        std::chrono::steady_clock::time_point();
     // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
     // implement thread unsafe updateBounds() functions.
     boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
@@ -111,23 +175,35 @@ namespace robot_costmap_2d
 
     minx_ = miny_ = 1e30;
     maxx_ = maxy_ = -1e30;
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
-         ++plugin)
+    if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size())
+      layer_performance_.assign(plugins_.size(), LayerPerformance());
+
+    for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
     {
-      if (!(*plugin)->isEnabled())
+      const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
+      if (!plugin->isEnabled())
         continue;
       double prev_minx = minx_;
       double prev_miny = miny_;
       double prev_maxx = maxx_;
       double prev_maxy = maxy_;
-      (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+      const auto bounds_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+          std::chrono::steady_clock::time_point();
+      plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+      if (performance_metrics_enabled_)
+      {
+        layer_performance_[plugin_index].bounds_nanoseconds +=
+            std::chrono::duration_cast<std::chrono::nanoseconds>(
+                std::chrono::steady_clock::now() - bounds_start).count();
+        ++layer_performance_[plugin_index].bounds_calls;
+      }
       if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
       {
         robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                                "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
                           prev_minx, prev_miny, prev_maxx, prev_maxy,
                           minx_, miny_, maxx_, maxy_,
-                          (*plugin)->getName().c_str());
+                          plugin->getName().c_str());
       }
     }
 
@@ -143,13 +219,32 @@ namespace robot_costmap_2d
     if (xn < x0 || yn < y0)
       return;
 
+    const auto reset_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+        std::chrono::steady_clock::time_point();
     costmap_.resetMap(x0, y0, xn, yn);
-    
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
-         ++plugin)
+    if (performance_metrics_enabled_)
     {
-      if ((*plugin)->isEnabled())
-        (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
+      performance_reset_nanoseconds_ +=
+          std::chrono::duration_cast<std::chrono::nanoseconds>(
+              std::chrono::steady_clock::now() - reset_start).count();
+    }
+    
+    for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
+    {
+      const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
+      if (!plugin->isEnabled())
+        continue;
+
+      const auto costs_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+          std::chrono::steady_clock::time_point();
+      plugin->updateCosts(costmap_, x0, y0, xn, yn);
+      if (performance_metrics_enabled_)
+      {
+        layer_performance_[plugin_index].costs_nanoseconds +=
+            std::chrono::duration_cast<std::chrono::nanoseconds>(
+                std::chrono::steady_clock::now() - costs_start).count();
+        ++layer_performance_[plugin_index].costs_calls;
+      }
     }
 
     bx0_ = x0;
@@ -158,6 +253,17 @@ namespace robot_costmap_2d
     byn_ = yn;
 
     initialized_ = true;
+
+    if (performance_metrics_enabled_)
+    {
+      const std::uint64_t cycle_nanoseconds = static_cast<std::uint64_t>(
+          std::chrono::duration_cast<std::chrono::nanoseconds>(
+              std::chrono::steady_clock::now() - cycle_start).count());
+      performance_cycle_nanoseconds_ += cycle_nanoseconds;
+      performance_cycle_samples_.push_back(cycle_nanoseconds);
+      ++performance_cycles_;
+      maybeReportPerformance();
+    }
   }
 
   bool LayeredCostmap::isCurrent()