otimal deep coppy obj

include/robot_costmap_2d/layered_costmap.h

@@ -43,6 +43,8 @@
 #include <robot_costmap_2d/costmap_2d.h>
 #include <vector>
 #include <string>
+#include <chrono>
+#include <cstdint>
 
 namespace robot_costmap_2d
 {
@@ -71,6 +73,8 @@ public:
    */
   void updateMap(double robot_x, double robot_y, double robot_yaw);
 
+  void setPerformanceMetrics(bool enabled, double reporting_period_seconds);
+
   inline const std::string& getGlobalFrameID() const noexcept
   {
     return global_frame_;
@@ -155,6 +159,17 @@ public:
   double getInscribedRadius() { return inscribed_radius_; }
 
 private:
+  struct LayerPerformance
+  {
+    std::uint64_t bounds_nanoseconds = 0;
+    std::uint64_t costs_nanoseconds = 0;
+    std::uint64_t bounds_calls = 0;
+    std::uint64_t costs_calls = 0;
+  };
+
+  void resetPerformanceMetrics();
+  void maybeReportPerformance();
+
   Costmap2D costmap_;
   std::string global_frame_;
 
@@ -170,6 +185,15 @@ private:
   bool size_locked_;
   double circumscribed_radius_, inscribed_radius_;
   std::vector<robot_geometry_msgs::Point> footprint_;
+
+  bool performance_metrics_enabled_ = false;
+  double performance_metrics_period_seconds_ = 5.0;
+  std::chrono::steady_clock::time_point performance_window_start_;
+  std::uint64_t performance_cycle_nanoseconds_ = 0;
+  std::uint64_t performance_reset_nanoseconds_ = 0;
+  std::uint64_t performance_cycles_ = 0;
+  std::vector<std::uint64_t> performance_cycle_samples_;
+  std::vector<LayerPerformance> layer_performance_;
 };
 
 }  // namespace robot_costmap_2d