otimal deep coppy obj

CMakeLists.txt

@@ -299,6 +299,7 @@ if(BUILD_COSTMAP_TESTS)
   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
     add_executable(test_costmap test/coordinates_test.cpp)
     target_link_libraries(test_costmap PRIVATE
+      plugins
       robot_costmap_2d
       GTest::GTest
       GTest::Main

config/costmap_params.yaml

@@ -25,6 +25,8 @@ robot_costmap_2d:
     - [-0.3, 0.3]
 
   transform_tolerance: 0.0
+  performance_metrics_enabled: false
+  performance_metrics_period: 5.0
   update_frequency: 1.0
   width: 0.0
   height: 0.0

include/robot_costmap_2d/costmap_2d.h

@@ -425,6 +425,7 @@ protected:
   double origin_y_;
   unsigned char* costmap_;
   unsigned char default_value_;
+  std::vector<unsigned char> rolling_window_scratch_;
 
   class MarkCell
   {

include/robot_costmap_2d/inflation_layer.h

@@ -42,6 +42,9 @@
 #include <robot_costmap_2d/layered_costmap.h>
 #include <boost/thread.hpp>
 
+#include <cstdint>
+#include <vector>
+
 namespace robot_costmap_2d
 {
 /**
@@ -77,8 +80,7 @@ public:
   virtual ~InflationLayer()
   {
     deleteKernels();
-    if (seen_)
+    delete inflation_access_;
-        delete[] seen_;
   }
 
   virtual void onInitialize();
@@ -184,10 +186,13 @@ private:
 
   unsigned int cell_inflation_radius_;
   unsigned int cached_cell_inflation_radius_;
-  std::map<double, std::vector<CellData> > inflation_cells_;
+  std::vector<std::vector<CellData>> inflation_cells_;
+  std::vector<double> distance_levels_;
+  std::vector<unsigned int> distance_bin_lookup_;
+  unsigned int distance_lookup_size_ = 0;
 
-  bool* seen_;
+  std::vector<std::uint32_t> seen_;
-  int seen_size_;
+  std::uint32_t seen_generation_ = 0;
 
   unsigned char** cached_costs_;
   double** cached_distances_;

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

include/robot_costmap_2d/observation.h

@@ -35,6 +35,9 @@
 #include <robot_geometry_msgs/Point.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/DepthCameraData.h>
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <utility>
 
 namespace robot_costmap_2d
 {
@@ -49,7 +52,8 @@ class DepthCameraObservation
 {
 public:
   DepthCameraObservation()
-    : data_(nullptr),
+    : data_handle_(),
+      data_(nullptr),
       topic_(),
       pixel_step_(0),
       min_range_(0.0),
@@ -64,7 +68,25 @@ public:
       unsigned int pixel_step,
       double min_range,
       double max_range)
-    : data_(new robot_sensor_msgs::DepthCameraData(data)),
+    : data_handle_(boost::make_shared<robot_sensor_msgs::DepthCameraData>(data)),
+      data_(data_handle_.get()),
+      topic_(std::move(topic)),
+      received_time_(received_time),
+      pixel_step_(pixel_step),
+      min_range_(min_range),
+      max_range_(max_range)
+  {
+  }
+
+  DepthCameraObservation(
+      robot_sensor_msgs::DepthCameraData::ConstPtr data,
+      std::string topic,
+      const robot::Time& received_time,
+      unsigned int pixel_step,
+      double min_range,
+      double max_range)
+    : data_handle_(std::move(data)),
+      data_(data_handle_.get()),
       topic_(std::move(topic)),
       received_time_(received_time),
       pixel_step_(pixel_step),
@@ -73,11 +95,9 @@ public:
   {
   }
 
-  // Copy constructor: deep copy
   DepthCameraObservation(const DepthCameraObservation& other)
-    : data_(other.data_
+    : data_handle_(other.data_handle_),
-              ? new robot_sensor_msgs::DepthCameraData(*other.data_)
+      data_(data_handle_.get()),
-              : nullptr),
       topic_(other.topic_),
       received_time_(other.received_time_),
       pixel_step_(other.pixel_step_),
@@ -86,37 +106,9 @@ public:
   {
   }
 
-  // Copy assignment: deep copy
-  DepthCameraObservation& operator=(const DepthCameraObservation& other)
-  {
-    if (this == &other)
-    {
-      return *this;
-    }
-
-    robot_sensor_msgs::DepthCameraData* new_data = nullptr;
-
-    if (other.data_ != nullptr)
-    {
-      new_data =
-          new robot_sensor_msgs::DepthCameraData(*other.data_);
-    }
-
-    delete data_;
-    data_ = new_data;
-
-    topic_ = other.topic_;
-    received_time_ = other.received_time_;
-    pixel_step_ = other.pixel_step_;
-    min_range_ = other.min_range_;
-    max_range_ = other.max_range_;
-
-    return *this;
-  }
-
-  // Move constructor: chuyển quyền sở hữu
   DepthCameraObservation(DepthCameraObservation&& other) noexcept
-    : data_(other.data_),
+    : data_handle_(std::move(other.data_handle_)),
+      data_(data_handle_.get()),
       topic_(std::move(other.topic_)),
       received_time_(other.received_time_),
       pixel_step_(other.pixel_step_),
@@ -129,17 +121,28 @@ public:
     other.max_range_ = 0.0;
   }
 
-  // Move assignment
+  DepthCameraObservation& operator=(const DepthCameraObservation& other)
-  DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept
   {
     if (this == &other)
-    {
+      return *this;
+
+    data_handle_ = other.data_handle_;
+    data_ = data_handle_.get();
+    topic_ = other.topic_;
+    received_time_ = other.received_time_;
+    pixel_step_ = other.pixel_step_;
+    min_range_ = other.min_range_;
+    max_range_ = other.max_range_;
     return *this;
   }
 
-    delete data_;
+  DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept
+  {
+    if (this == &other)
+      return *this;
 
-    data_ = other.data_;
+    data_handle_ = std::move(other.data_handle_);
+    data_ = data_handle_.get();
     topic_ = std::move(other.topic_);
     received_time_ = other.received_time_;
     pixel_step_ = other.pixel_step_;
@@ -154,13 +157,10 @@ public:
     return *this;
   }
 
-  ~DepthCameraObservation()
+  ~DepthCameraObservation() = default;
-  {
-    delete data_;
-    data_ = nullptr;
-  }
 
-  robot_sensor_msgs::DepthCameraData* data_;
+  robot_sensor_msgs::DepthCameraData::ConstPtr data_handle_;
+  const robot_sensor_msgs::DepthCameraData* data_;
   std::string topic_;
   robot::Time received_time_;
   unsigned int pixel_step_;
@@ -180,14 +180,12 @@ public:
    * @brief  Creates an empty observation
    */
   Observation() :
-    cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
+    cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>()),
+    cloud_(cloud_handle_.get()), obstacle_range_(0.0), raytrace_range_(0.0)
   {
   }
 
-  virtual ~Observation()
+  virtual ~Observation() = default;
-  {
-    delete cloud_;
-  }
 
   /**
    * @brief  Creates an observation from an origin point and a point cloud
@@ -198,7 +196,17 @@ public:
    */
   Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
               double obstacle_range, double raytrace_range) :
-      origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
+      origin_(origin), cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>(cloud)),
+      cloud_(cloud_handle_.get()),
+      obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
+  {
+  }
+
+  Observation(robot_geometry_msgs::Point origin,
+              boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud,
+              double obstacle_range, double raytrace_range) :
+      origin_(std::move(origin)), cloud_handle_(std::move(cloud)),
+      cloud_(cloud_handle_.get()),
       obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
   {
   }
@@ -208,22 +216,59 @@ public:
    * @param obs The observation to copy
    */
   Observation(const Observation& obs) :
-      origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
+      origin_(obs.origin_), cloud_handle_(obs.cloud_handle_), cloud_(cloud_handle_.get()),
       obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
   {
   }
 
+  Observation(Observation&& obs) noexcept :
+      origin_(std::move(obs.origin_)), cloud_handle_(std::move(obs.cloud_handle_)),
+      cloud_(cloud_handle_.get()), obstacle_range_(obs.obstacle_range_),
+      raytrace_range_(obs.raytrace_range_)
+  {
+    obs.cloud_ = nullptr;
+  }
+
+  Observation& operator=(const Observation& obs)
+  {
+    if (this == &obs)
+      return *this;
+
+    origin_ = obs.origin_;
+    cloud_handle_ = obs.cloud_handle_;
+    cloud_ = cloud_handle_.get();
+    obstacle_range_ = obs.obstacle_range_;
+    raytrace_range_ = obs.raytrace_range_;
+    return *this;
+  }
+
+  Observation& operator=(Observation&& obs) noexcept
+  {
+    if (this == &obs)
+      return *this;
+
+    origin_ = std::move(obs.origin_);
+    cloud_handle_ = std::move(obs.cloud_handle_);
+    cloud_ = cloud_handle_.get();
+    obstacle_range_ = obs.obstacle_range_;
+    raytrace_range_ = obs.raytrace_range_;
+    obs.cloud_ = nullptr;
+    return *this;
+  }
+
   /**
    * @brief  Creates an observation from a point cloud
    * @param cloud The point cloud of the observation
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    */
   Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
-      cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
+      cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>(cloud)),
+      cloud_(cloud_handle_.get()), obstacle_range_(obstacle_range), raytrace_range_(0.0)
   {
   }
 
   robot_geometry_msgs::Point origin_;
+  boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud_handle_;
   robot_sensor_msgs::PointCloud2* cloud_;
   double obstacle_range_, raytrace_range_;
 };

include/robot_costmap_2d/observation_buffer.h

@@ -109,6 +109,8 @@ public:
    */
   void bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth);
 
+  void bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth);
+
   /**
    * @brief  Pushes copies of all current observations onto the end of the vector passed in
    * @param  observations The vector to be filled

include/robot_costmap_2d/obstacle_layer.h

@@ -134,7 +134,7 @@ protected:
   /**
    * @brief Buffer a depth image and its camera model for frustum clearing.
    */
-  void depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
+  void depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
                           const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
 
   /**

include/robot_costmap_2d/voxel_layer.h

@@ -96,9 +96,12 @@ private:
                             double* min_x, double* min_y, double* max_x, double* max_y);
   bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
                        double& depth_m, bool& is_valid) const;
+  void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
+                           double fx, double fy, double cx, double cy);
   bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
   bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
-                     double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y);
+                     double raytrace_range, unsigned int cell_raytrace_range,
+                     double* min_x, double* min_y, double* max_x, double* max_y);
 
 
   bool publish_voxel_;
@@ -106,6 +109,25 @@ private:
   double z_resolution_, origin_z_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
+  std::vector<unsigned char> rolling_costmap_scratch_;
+  std::vector<unsigned int> rolling_voxel_scratch_;
+
+  struct DepthRay
+  {
+    unsigned int u;
+    unsigned int v;
+    double x;
+    double y;
+    double z;
+  };
+  std::vector<DepthRay> depth_ray_cache_;
+  unsigned int cached_depth_width_ = 0;
+  unsigned int cached_depth_height_ = 0;
+  unsigned int cached_depth_pixel_step_ = 0;
+  double cached_fx_ = 0.0;
+  double cached_fy_ = 0.0;
+  double cached_cx_ = 0.0;
+  double cached_cy_ = 0.0;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
   {

plugins/inflation_layer.cpp

@@ -58,7 +58,6 @@ InflationLayer::InflationLayer()
   , inflate_unknown_(false)
   , cell_inflation_radius_(0)
   , cached_cell_inflation_radius_(0)
-  , seen_(NULL)
   , cached_costs_(NULL)
   , cached_distances_(NULL)
   , last_min_x_(-std::numeric_limits<float>::max())
@@ -76,10 +75,8 @@ void InflationLayer::onInitialize()
 
   boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
   current_ = true;
-  if (seen_)
+  seen_.clear();
-    delete[] seen_;
+  seen_generation_ = 0;
-  seen_ = NULL;
-  seen_size_ = 0;
   need_reinflation_ = false;
   std::string config_file_name = "inflation_layer_params.yaml";
   // std::cout << "InflationLayer: " << config_file_name << std::endl;
@@ -144,10 +141,8 @@ void InflationLayer::matchSize()
   computeCaches();
 
   unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
-  if (seen_)
+  seen_.assign(static_cast<std::size_t>(size_x) * size_y, 0);
-    delete[] seen_;
+  seen_generation_ = 0;
-  seen_size_ = size_x * size_y;
-  seen_ = new bool[seen_size_];
 }
 
 void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -203,26 +198,28 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
   if (cell_inflation_radius_ == 0)
     return;
 
-  // make sure the inflation list is empty at the beginning of the cycle (should always be true)
+  for (std::vector<CellData>& cells : inflation_cells_)
-  if(!inflation_cells_.empty())
+    cells.clear();
-    robot::log_error("The inflation list must be empty at the beginning of inflation\n");
 
   unsigned char* master_array = master_grid.getCharMap();
   unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
 
-  if (seen_ == NULL) {
+  const std::size_t map_size = static_cast<std::size_t>(size_x) * size_y;
-    robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n");
+  if (seen_.size() != map_size)
-    seen_size_ = size_x * size_y;
-    seen_ = new bool[seen_size_];
-  }
-  else if (seen_size_ != size_x * size_y)
   {
-    robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
+    seen_.assign(map_size, 0);
-    delete[] seen_;
+    seen_generation_ = 0;
-    seen_size_ = size_x * size_y;
+  }
-    seen_ = new bool[seen_size_];
+
+  if (seen_generation_ == std::numeric_limits<std::uint32_t>::max())
+  {
+    std::fill(seen_.begin(), seen_.end(), 0);
+    seen_generation_ = 1;
+  }
+  else
+  {
+    ++seen_generation_;
   }
-  memset(seen_, false, size_x * size_y * sizeof(bool));
 
   // We need to include in the inflation cells outside the bounding
   // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
@@ -238,11 +235,13 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
   max_i = std::min(int(size_x), max_i);
   max_j = std::min(int(size_y), max_j);
 
-  // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
+  // Precomputed distance buckets preserve priority ordering without a tree lookup
-  // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
+  // for every enqueued cell.
 
   // Start with lethal obstacles: by definition distance is 0.0
-  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
+  if (inflation_cells_.empty())
+    return;
+  std::vector<CellData>& obs_bin = inflation_cells_.front();
   for (int j = min_j; j < max_j; j++)
   {
     for (int i = min_i; i < max_i; i++)
@@ -258,23 +257,22 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
 
   // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
   // can overtake previously inserted but farther away cells
-  std::map<double, std::vector<CellData> >::iterator bin;
+  for (std::vector<CellData>& bin : inflation_cells_)
-  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
   {
-    for (int i = 0; i < bin->second.size(); ++i)
+    for (std::size_t i = 0; i < bin.size(); ++i)
     {
       // process all cells at distance dist_bin.first
-      const CellData& cell = bin->second[i];
+      const CellData& cell = bin[i];
 
       unsigned int index = cell.index_;
 
       // ignore if already visited
-      if (seen_[index])
+      if (seen_[index] == seen_generation_)
       {
         continue;
       }
 
-      seen_[index] = true;
+      seen_[index] = seen_generation_;
 
       unsigned int mx = cell.x_;
       unsigned int my = cell.y_;
@@ -301,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
     }
   }
 
-  inflation_cells_.clear();
 }
 
 /**
@@ -316,7 +313,7 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                     unsigned int src_x, unsigned int src_y)
 {
-  if (!seen_[index])
+  if (seen_[index] != seen_generation_)
   {
     // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
     double distance = distanceLookup(mx, my, src_x, src_y);
@@ -325,8 +322,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
     if (distance > cell_inflation_radius_)
       return;
 
-    // push the cell data onto the inflation list and mark
+    const unsigned int dx = std::abs(static_cast<int>(mx) - static_cast<int>(src_x));
-    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
+    const unsigned int dy = std::abs(static_cast<int>(my) - static_cast<int>(src_y));
+    const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy];
+    inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y));
   }
 }
 
@@ -354,6 +353,38 @@ void InflationLayer::computeCaches()
     }
 
     cached_cell_inflation_radius_ = cell_inflation_radius_;
+
+    distance_lookup_size_ = cell_inflation_radius_ + 2;
+    distance_levels_.clear();
+    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
+    {
+      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
+      {
+        if (cached_distances_[i][j] <= cell_inflation_radius_)
+          distance_levels_.push_back(cached_distances_[i][j]);
+      }
+    }
+    std::sort(distance_levels_.begin(), distance_levels_.end());
+    distance_levels_.erase(
+        std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end());
+
+    inflation_cells_.clear();
+    inflation_cells_.resize(distance_levels_.size());
+    distance_bin_lookup_.assign(
+        static_cast<std::size_t>(distance_lookup_size_) * distance_lookup_size_, 0);
+    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
+    {
+      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
+      {
+        const double distance = cached_distances_[i][j];
+        if (distance > cell_inflation_radius_)
+          continue;
+        distance_bin_lookup_[i * distance_lookup_size_ + j] =
+            static_cast<unsigned int>(
+                std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) -
+                distance_levels_.begin());
+      }
+    }
   }
 
   for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
@@ -367,6 +398,10 @@ void InflationLayer::computeCaches()
 
 void InflationLayer::deleteKernels()
 {
+  inflation_cells_.clear();
+  distance_levels_.clear();
+  distance_bin_lookup_.clear();
+  distance_lookup_size_ = 0;
   if (cached_distances_ != NULL)
   {
     for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)

plugins/obstacle_layer.cpp

@@ -269,9 +269,10 @@ void ObstacleLayer::handleImpl(const void* data,
                               const std::type_info& type,
                               const std::string& topic)  
 {
-  if(!stop_receiving_data_)
+  if (!enabled_ || stop_receiving_data_)
-  {
+    return;
-    if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) )
+
+  if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr))
   {
       const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr =
           *static_cast<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(data);
@@ -333,7 +334,8 @@ void ObstacleLayer::handleImpl(const void* data,
 
       // std::lock_guard<std::mutex> lock(depth_camera_data_mutex_);
       // pending_depth_camera_data_ = depth_camera_data_ptr;
-      if(depth_observation_buffers_.empty() || callback_depth_infos_.empty()) return;
+      if (depth_observation_buffers_.empty() || callback_depth_infos_.empty())
+        return;
 
       int size_callback_depth = static_cast<int>(callback_depth_infos_.size());
       for(int i = 0; i < size_callback_depth; i++)
@@ -343,17 +345,17 @@ void ObstacleLayer::handleImpl(const void* data,
             topic == callback_depth_infos_[i].topic)
         {
           // robot::log_error_throttle(1.0,"TEST");
-          depthImageCallback(depth_camera_data, buffer);
+          depthImageCallback(depth_camera_data_ptr, buffer);
         }
       }
-      // return;
   }
   else
   {
-    if(observation_buffers_.empty() || callback_infos_.empty()) return;
+    if (observation_buffers_.empty() || callback_infos_.empty())
+      return;
 
     int size_callback = static_cast<int>(callback_infos_.size());
-    for(int i = 0; i < size_callback; i++)
+    for (int i = 0; i < size_callback; i++)
     {
       boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
 
@@ -404,12 +406,6 @@ void ObstacleLayer::handleImpl(const void* data,
       // }
     }
   }
-  }
-  else 
-  {
-    robot::log_info("Stop receiving data!\n");
-    return;
-  }
 }
 
 void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
@@ -510,12 +506,11 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
   buffer->unlock();
 }
 
-void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
+void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
                                        const boost::shared_ptr<ObservationBuffer>& buffer)
 {
   buffer->lock();
-  // robot::log_error_throttle(1.0, "depth data size 1: %d", (int)message.depth.data.size());
+  buffer->bufferDepthCamera(std::move(message));
-  buffer->bufferDepthCamera(message);
   buffer->unlock();
 }
 
@@ -557,6 +552,9 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
     robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
     robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
 
+    std::size_t rejected_height = 0;
+    std::size_t rejected_range = 0;
+    std::size_t rejected_bounds = 0;
     for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
     {
       double px = *iter_x, py = *iter_y, pz = *iter_z;
@@ -564,7 +562,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       // if the obstacle is too high or too far away from the robot we won't add it
       if (pz > max_obstacle_height_)
       {
-        robot::log_error("The point is too high\n");
+        ++rejected_height;
         continue;
       }
 
@@ -575,7 +573,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       // if the point is far enough away... we won't consider it
       if (sq_dist >= sq_obstacle_range)
       {
-        robot::log_error("The point is too far away\n");
+        ++rejected_range;
         continue;
       }
 
@@ -583,7 +581,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       unsigned int mx, my;
       if (!worldToMap(px, py, mx, my))
       {
-        robot::log_error("Computing map coords failed\n");
+        ++rejected_bounds;
         continue;
       }
 
@@ -591,6 +589,14 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       costmap_[index] = LETHAL_OBSTACLE;
       touch(px, py, min_x, min_y, max_x, max_y);
     }
+
+    if (rejected_height + rejected_range + rejected_bounds > 0)
+    {
+      robot::log_info_throttle(
+          5.0,
+          "ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n",
+          rejected_height, rejected_range, rejected_bounds);
+    }
   }
 
   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);

plugins/voxel_layer.cpp

@@ -456,6 +456,43 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned
   return false;
 }
 
+void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
+                                     unsigned int pixel_step, double fx, double fy,
+                                     double cx, double cy)
+{
+  if (cached_depth_width_ == width && cached_depth_height_ == height &&
+      cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
+      cached_cx_ == cx && cached_cy_ == cy)
+  {
+    return;
+  }
+
+  cached_depth_width_ = width;
+  cached_depth_height_ = height;
+  cached_depth_pixel_step_ = pixel_step;
+  cached_fx_ = fx;
+  cached_fy_ = fy;
+  cached_cx_ = cx;
+  cached_cy_ = cy;
+
+  const std::size_t rows = (height + pixel_step - 1) / pixel_step;
+  const std::size_t columns = (width + pixel_step - 1) / pixel_step;
+  depth_ray_cache_.clear();
+  depth_ray_cache_.reserve(rows * columns);
+
+  for (unsigned int v = 0; v < height; v += pixel_step)
+  {
+    for (unsigned int u = 0; u < width; u += pixel_step)
+    {
+      const double x = (static_cast<double>(u) - cx) / fx;
+      const double y = (static_cast<double>(v) - cy) / fy;
+      const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
+      depth_ray_cache_.push_back(
+          DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm});
+    }
+  }
+}
+
 bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
 {
   double a = wx - ox;
@@ -494,7 +531,8 @@ bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& w
 }
 
 bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
-                               double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y)
+                               double raytrace_range, unsigned int cell_raytrace_range,
+                               double* min_x, double* min_y, double* max_x, double* max_y)
 {
   double sensor_x, sensor_y, sensor_z;
   if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
@@ -509,7 +547,7 @@ bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, doubl
 
   robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
                                         unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
-                                        cellDistance(raytrace_range));
+                                        cell_raytrace_range);
   updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
   return true;
 }
@@ -583,15 +621,37 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   const double skip_dist = 2.0 * resolution_;
   const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
   const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
+  updateDepthRayCache(width, height, step, fx, fy, cx, cy);
+
+  double qx = tfm.transform.rotation.x;
+  double qy = tfm.transform.rotation.y;
+  double qz = tfm.transform.rotation.z;
+  double qw = tfm.transform.rotation.w;
+  const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
+  if (quaternion_norm <= 0.0)
+    return false;
+  qx /= quaternion_norm;
+  qy /= quaternion_norm;
+  qz /= quaternion_norm;
+  qw /= quaternion_norm;
+
+  const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz);
+  const double r01 = 2.0 * (qx * qy - qz * qw);
+  const double r02 = 2.0 * (qx * qz + qy * qw);
+  const double r10 = 2.0 * (qx * qy + qz * qw);
+  const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz);
+  const double r12 = 2.0 * (qy * qz - qx * qw);
+  const double r20 = 2.0 * (qx * qz - qy * qw);
+  const double r21 = 2.0 * (qy * qz + qx * qw);
+  const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy);
+  const unsigned int cell_raytrace_range = cellDistance(max_range);
   bool cleared_any = false;
 
-  for (unsigned int v = 0; v < height; v += step)
+  for (const DepthRay& local_ray : depth_ray_cache_)
-  {
-    for (unsigned int u = 0; u < width; u += step)
   {
     double depth_m = 0.0;
     bool valid = false;
-      if (!readDepthMeters(depth, u, v, depth_m, valid))
+    if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
       continue;
 
     double ray_len = max_range;
@@ -601,28 +661,10 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     if (ray_len <= min_range)
       continue;
 
-      double dx = (static_cast<double>(u) - cx) / fx;
-      double dy = (static_cast<double>(v) - cy) / fy;
-      double dz = 1.0;
-      const double norm = std::sqrt(dx * dx + dy * dy + dz * dz);
-      if (norm <= 0.0)
-        continue;
-
-      robot_geometry_msgs::Vector3 local_ray;
-      local_ray.x = dx / norm;
-      local_ray.y = dy / norm;
-      local_ray.z = dz / norm;
-
     robot_geometry_msgs::Vector3 global_ray;
-      tf3::doTransform(local_ray, global_ray, tfm);
+    global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
-      const double global_norm =
+    global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
-          std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z);
+    global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
-      if (global_norm <= 0.0)
-        continue;
-
-      global_ray.x /= global_norm;
-      global_ray.y /= global_norm;
-      global_ray.z /= global_norm;
 
     const double sx = ox + global_ray.x * min_range;
     const double sy = oy + global_ray.y * min_range;
@@ -631,8 +673,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
     const double wy = oy + global_ray.y * ray_len;
     const double wz = oz + global_ray.z * ray_len;
 
-      cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any;
+    cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, cell_raytrace_range,
-    }
+                                min_x, min_y, max_x, max_y) || cleared_any;
   }
 
   return cleared_any;
@@ -645,6 +687,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
   cell_ox = int((new_origin_x - origin_x_) / resolution_);
   cell_oy = int((new_origin_y - origin_y_) / resolution_);
 
+  // Most update cycles do not cross a costmap cell boundary. Avoid copying and
+  // resetting the complete 2D/3D grids when the cell-aligned origin is unchanged.
+  if (cell_ox == 0 && cell_oy == 0)
+    return;
+
   // compute the associated world coordinates for the origin cell
   // beacuase we want to keep things grid-aligned
   double new_grid_ox, new_grid_oy;
@@ -665,15 +712,20 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
   unsigned int cell_size_x = upper_right_x - lower_left_x;
   unsigned int cell_size_y = upper_right_y - lower_left_y;
 
-  // we need a map to store the obstacles in the window temporarily
+  const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
-  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  rolling_costmap_scratch_.resize(overlap_size);
-  unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  rolling_voxel_scratch_.resize(overlap_size);
+  unsigned char* local_map = rolling_costmap_scratch_.data();
+  unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
   unsigned int* voxel_map = robot_voxel_grid_.getData();
 
-  // copy the local window in the costmap to the local map
+  if (overlap_size > 0)
-  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  {
-  copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
+    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
-                cell_size_y);
+                  cell_size_x, cell_size_x, cell_size_y);
+    copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
+                  cell_size_x, cell_size_x, cell_size_y);
+  }
 
   // we'll reset our maps to unknown space if appropriate
   resetMaps();
@@ -687,12 +739,14 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
   int start_y = lower_left_y - cell_oy;
 
   // now we want to copy the overlapping information back into the map, but in its new location
-  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  if (overlap_size > 0)
-  copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  {
+    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
+                  size_x_, cell_size_x, cell_size_y);
+    copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
+                  size_x_, cell_size_x, cell_size_y);
+  }
 
-  // make sure to clean up
-  delete[] local_map;
-  delete[] local_voxel_map;
 }
 
 // Export factory function

src/costmap_2d.cpp

@@ -288,11 +288,17 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
   unsigned int cell_size_x = upper_right_x - lower_left_x;
   unsigned int cell_size_y = upper_right_y - lower_left_y;
 
-  // we need a map to store the obstacles in the window temporarily
+  const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
-  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
 
-  // copy the local window in the costmap to the local map
+  // Reuse the temporary window to avoid allocating on every rolling-window shift.
-  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  rolling_window_scratch_.resize(overlap_size);
+  unsigned char* local_map = rolling_window_scratch_.data();
+
+  if (overlap_size > 0)
+  {
+    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
+                  cell_size_x, cell_size_x, cell_size_y);
+  }
 
   // now we'll set the costmap to be completely unknown if we track unknown space
   resetMaps();
@@ -306,10 +312,12 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
   int start_y = lower_left_y - cell_oy;
 
   // now we want to copy the overlapping information back into the map, but in its new location
-  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  if (overlap_size > 0)
+  {
+    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
+                  size_x_, cell_size_x, cell_size_y);
+  }
 
-  // make sure to clean up
-  delete[] local_map;
 }
 
 bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)

src/costmap_2d_robot.cpp

@@ -155,9 +155,20 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st
     if (priv_nh.hasParam("track_unknown_space"))
       priv_nh.getParam("track_unknown_space", track_unknown_space);
 
+    bool performance_metrics_enabled =
+        loadParam(layer, "performance_metrics_enabled", false);
+    double performance_metrics_period =
+        loadParam(layer, "performance_metrics_period", 5.0);
+    if (priv_nh.hasParam("performance_metrics_enabled"))
+      priv_nh.getParam("performance_metrics_enabled", performance_metrics_enabled);
+    if (priv_nh.hasParam("performance_metrics_period"))
+      priv_nh.getParam("performance_metrics_period", performance_metrics_period);
+
     if (priv_nh.hasParam("library_path"))
       path_plugins = loader.findLibraryPath(name_);
     layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
+    layered_costmap_->setPerformanceMetrics(
+        performance_metrics_enabled, performance_metrics_period);
 
     // find size parameters
     double map_width_meters = loadParam(layer, "width", 0.0);
@@ -692,35 +703,9 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
   // get the global pose of the robot
   try
   {
-    // use current time if possible (makes sure it's not in the future)
+    const tf3::TransformStampedMsg transform =
-    if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
+        tf_.lookupTransform(global_frame_, robot_base_frame_, tf3::Time());
-    {
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
     tf3::doTransform(robot_pose, global_pose, transform);
-      // robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
-      // global_pose.pose.position.x,
-      // global_pose.pose.position.y,
-      // global_pose.pose.position.z,
-      // global_pose.pose.orientation.x,
-      // global_pose.pose.orientation.y,
-      // global_pose.pose.orientation.z,
-      // global_pose.pose.orientation.w);
-      // transform.transform.rotation.x,
-      // transform.transform.rotation.y,
-      // transform.transform.rotation.z,
-      // transform.transform.rotation.w);
-    }
-    // use the latest otherwise
-    else
-    {
-      // tf_.transform(robot_pose, global_pose, global_frame_);
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(
-                    global_frame_,                      // frame đích
-                    robot_base_frame_,                      // frame nguồn
-                    tf3::Time()
-                );
-      tf3::doTransform(robot_pose, global_pose, transform);
-    }
   }
   catch (tf3::LookupException& ex)
   {

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();
+    if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size())
-         ++plugin)
+      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);
-    
+    if (performance_metrics_enabled_)
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
-         ++plugin)
     {
-      if ((*plugin)->isEnabled())
+      performance_reset_nanoseconds_ +=
-        (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
+          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()

src/observation_buffer.cpp

@@ -40,6 +40,8 @@
 #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
 
+#include <cstring>
+
 using namespace std;
 using namespace tf3;
 
@@ -97,6 +99,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
     {
       Observation& obs = *obs_it;
 
+      if (!obs.cloud_handle_.unique())
+      {
+        obs.cloud_handle_ = boost::make_shared<robot_sensor_msgs::PointCloud2>(*obs.cloud_);
+        obs.cloud_ = obs.cloud_handle_.get();
+      }
+
       robot_geometry_msgs::PointStamped origin;
       origin.header.frame_id = global_frame_;
       origin.header.stamp = data_convert::convertTime(transform_time);
@@ -137,9 +145,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
 void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
 {
   robot_geometry_msgs::PointStamped global_origin;
-
+  Observation observation;
-  // create a new observation on the list to be populated
-  observation_list_.push_front(Observation());
 
   // check whether the origin frame has been set explicitly or whether we should get it from the cloud
   string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
@@ -154,81 +160,68 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     local_origin.point.y = 0;
     local_origin.point.z = 0;
     // tf3_buffer_.transform(local_origin, global_origin, global_frame_);
-    tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
+    const tf3::TransformStampedMsg cloud_transform = tf3_buffer_.lookupTransform(
-                        global_frame_,                      // frame đích
+        global_frame_, cloud.header.frame_id, tf3::Time());
-                        local_origin.header.frame_id,                      // frame nguồn
+    if (origin_frame == cloud.header.frame_id)
-                        tf3::Time()
+      tf3::doTransform(local_origin, global_origin, cloud_transform);
-                        // data_convert::convertTime(cloud.header.stamp)
+    else
-                        );
+      tf3::doTransform(
-    tf3::doTransform(local_origin, global_origin, tfm_1);
+          local_origin, global_origin,
+          tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time()));
 
-    /////////////////////////////////////////////////
+    tf3::convert(global_origin.point, observation.origin_);
-    ///////////chú ý hàm này/////////////////////////
+    observation.raytrace_range_ = raytrace_range_;
-    tf3::convert(global_origin.point, observation_list_.front().origin_);
+    observation.obstacle_range_ = obstacle_range_;
-    /////////////////////////////////////////////////
-    /////////////////////////////////////////////////
 
-    // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
+    robot_sensor_msgs::PointCloud2& observation_cloud = *observation.cloud_;
-    observation_list_.front().raytrace_range_ = raytrace_range_;
+    tf3::doTransform(cloud, observation_cloud, cloud_transform);
-    observation_list_.front().obstacle_range_ = obstacle_range_;
+    observation_cloud.header.stamp = cloud.header.stamp;
 
-    robot_sensor_msgs::PointCloud2 global_frame_cloud;
+    const std::size_t cloud_size =
+        static_cast<std::size_t>(observation_cloud.height) * observation_cloud.width;
+    const std::size_t point_step = observation_cloud.point_step;
+    std::size_t point_count = 0;
+    robot_sensor_msgs::PointCloud2Iterator<float> iter_z(observation_cloud, "z");
 
-    // transform the point cloud
+    // Compact accepted points in-place. This avoids allocating and copying a
-    // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
+    // second full-size filtered cloud after the TF transform.
-    tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
+    for (std::size_t read_index = 0; read_index < cloud_size; ++read_index, ++iter_z)
-                        global_frame_,                      // frame đích
-                        cloud.header.frame_id,                      // frame nguồn
-                        tf3::Time()
-                        // data_convert::convertTime(cloud.header.stamp)
-                        );
-    tf3::doTransform(cloud, global_frame_cloud, tfm_2);
-    global_frame_cloud.header.stamp = cloud.header.stamp;
-
-    // now we need to remove observations from the cloud that are below or above our height thresholds
-    robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
-    observation_cloud.height = global_frame_cloud.height;
-    observation_cloud.width = global_frame_cloud.width;
-    observation_cloud.fields = global_frame_cloud.fields;
-    observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
-    observation_cloud.point_step = global_frame_cloud.point_step;
-    observation_cloud.row_step = global_frame_cloud.row_step;
-    observation_cloud.is_dense = global_frame_cloud.is_dense;
-
-    unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
-    robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
-    modifier.resize(cloud_size);
-    unsigned int point_count = 0;
-
-    // copy over the points that are within our height bounds
-    robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
-    std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
-    std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
-    for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
     {
-      if ((*iter_z) <= max_obstacle_height_
+      if ((*iter_z) > max_obstacle_height_ || (*iter_z) < min_obstacle_height_)
-          && (*iter_z) >= min_obstacle_height_)
+        continue;
+
+      if (point_count != read_index)
       {
-        std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
+        std::memmove(observation_cloud.data.data() + point_count * point_step,
-        iter_obs += global_frame_cloud.point_step;
+                     observation_cloud.data.data() + read_index * point_step,
+                     point_step);
+      }
       ++point_count;
     }
-    }
 
-    // resize the cloud for the number of legal points
+    if (point_count != cloud_size)
+    {
+      robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
       modifier.resize(point_count);
-    observation_cloud.header.stamp = cloud.header.stamp;
+    }
-    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
   }
   catch (TransformException& ex)
   {
-    // if an exception occurs, we need to remove the empty observation from the list
-    observation_list_.pop_front();
     robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
               cloud.header.frame_id.c_str(), ex.what());
     return;
   }
 
+  if (observation_keep_time_ == robot::Duration(0.0) && !observation_list_.empty())
+  {
+    observation_list_.front() = std::move(observation);
+    observation_list_.erase(++observation_list_.begin(), observation_list_.end());
+  }
+  else
+  {
+    observation_list_.push_front(std::move(observation));
+  }
+
   // if the update was successful, we want to update the last updated time
   last_updated_ = robot::Time::now();
 
@@ -238,21 +231,28 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
 
 void ObservationBuffer::bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth_camera_data)
 {
-  depth_observation_list_.push_front(DepthCameraObservation());
+  bufferDepthCamera(boost::make_shared<robot_sensor_msgs::DepthCameraData>(depth_camera_data));
-  if (depth_observation_list_.front().data_ == nullptr)
+}
+
+void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth_camera_data)
+{
+  if (!depth_camera_data)
+    return;
+
+  DepthCameraObservation observation(
+      std::move(depth_camera_data), topic_name_, robot::Time::now(),
+      frustum_pixel_step_, frustum_min_range_, frustum_max_range_);
+
+  if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
   {
-    depth_observation_list_.front().data_ =
+    depth_observation_list_.front() = std::move(observation);
-        new robot_sensor_msgs::DepthCameraData(depth_camera_data);
+    depth_observation_list_.erase(++depth_observation_list_.begin(), depth_observation_list_.end());
   }
   else
   {
-    *depth_observation_list_.front().data_ = depth_camera_data;
+    depth_observation_list_.push_front(std::move(observation));
   }
 
-  depth_observation_list_.front().pixel_step_ = frustum_pixel_step_;
-  depth_observation_list_.front().min_range_ = frustum_min_range_;
-  depth_observation_list_.front().max_range_ = frustum_max_range_;
-
   // if the update was successful, we want to update the last updated time
   last_updated_ = robot::Time::now();
 
@@ -280,11 +280,18 @@ void ObservationBuffer::getDepthObservations(vector<DepthCameraObservation>& obs
   purgeStaleDepthObservations();
 
   // now we'll just copy the observations for the caller
-  list<DepthCameraObservation>::iterator obs_it;
+  if (observation_keep_time_ == robot::Duration(0.0))
-  for (obs_it = depth_observation_list_.begin(); obs_it != depth_observation_list_.end(); ++obs_it)
   {
-    observations.push_back(*obs_it);
+    if (!depth_observation_list_.empty())
+    {
+      observations.push_back(std::move(depth_observation_list_.front()));
+      depth_observation_list_.clear();
     }
+    return;
+  }
+
+  observations.insert(
+      observations.end(), depth_observation_list_.begin(), depth_observation_list_.end());
 }
 
 void ObservationBuffer::purgeStaleObservations()
@@ -356,4 +363,3 @@ void ObservationBuffer::resetLastUpdated()
   last_updated_ = robot::Time::now();
 }
 }  // namespace robot_costmap_2d
-

test/coordinates_test.cpp

@@ -36,6 +36,14 @@
 
 #include <gtest/gtest.h>
 #include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/cost_values.h>
+#include <robot_costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/voxel_layer.h>
+
+#include <boost/make_shared.hpp>
+#include <cstdlib>
 
 using namespace robot_costmap_2d;
 
@@ -124,9 +132,100 @@ TEST(CostmapCoordinates, hard_coordinates_test)
   EXPECT_EQ(my, 2);
 }
 
+TEST(CostmapPerformanceRegression, rolling_origin_preserves_overlap)
+{
+  Costmap2D costmap(4, 3, 1.0, 0.0, 0.0, FREE_SPACE);
+  costmap.setCost(1, 1, LETHAL_OBSTACLE);
+  costmap.setCost(3, 2, INSCRIBED_INFLATED_OBSTACLE);
+
+  costmap.updateOrigin(0.25, 0.25);
+  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 0.0);
+  EXPECT_DOUBLE_EQ(costmap.getOriginY(), 0.0);
+  EXPECT_EQ(costmap.getCost(1, 1), LETHAL_OBSTACLE);
+
+  costmap.updateOrigin(1.0, 0.0);
+  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 1.0);
+  EXPECT_EQ(costmap.getCost(0, 1), LETHAL_OBSTACLE);
+  EXPECT_EQ(costmap.getCost(3, 2), FREE_SPACE);
+}
+
+TEST(CostmapPerformanceRegression, voxel_origin_subcell_shift_is_noop)
+{
+  VoxelLayer layer;
+  layer.resizeMap(4, 3, 1.0, 0.0, 0.0);
+  layer.setCost(1, 1, LETHAL_OBSTACLE);
+
+  layer.updateOrigin(0.25, 0.25);
+
+  EXPECT_DOUBLE_EQ(layer.getOriginX(), 0.0);
+  EXPECT_DOUBLE_EQ(layer.getOriginY(), 0.0);
+  EXPECT_EQ(layer.getCost(1, 1), LETHAL_OBSTACLE);
+}
+
+TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
+{
+  robot_geometry_msgs::Point origin;
+  robot_sensor_msgs::PointCloud2 cloud;
+  cloud.height = 1;
+  cloud.width = 1;
+  cloud.point_step = 4;
+  cloud.row_step = 4;
+  cloud.data = {1, 2, 3, 4};
+
+  Observation observation(origin, cloud, 2.5, 3.0);
+  Observation copied = observation;
+
+  EXPECT_EQ(copied.cloud_, observation.cloud_);
+  EXPECT_EQ(copied.cloud_handle_.use_count(), 2);
+  EXPECT_EQ(copied.cloud_->data, cloud.data);
+}
+
+TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
+{
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  ObservationBuffer buffer(
+      "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
+      8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);
+
+  robot_sensor_msgs::DepthCameraData::ConstPtr depth =
+      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
+  buffer.bufferDepthCamera(depth);
+
+  std::vector<DepthCameraObservation> first_snapshot;
+  buffer.getDepthObservations(first_snapshot);
+  ASSERT_EQ(first_snapshot.size(), 1u);
+  EXPECT_EQ(first_snapshot.front().data_, depth.get());
+  EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
+
+  std::vector<DepthCameraObservation> second_snapshot;
+  buffer.getDepthObservations(second_snapshot);
+  EXPECT_TRUE(second_snapshot.empty());
+}
+
+TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)
+{
+  ASSERT_EQ(setenv("PNKX_NAV_CORE_CONFIG_DIR", ROBOT_COSTMAP_2D_DIR, 1), 0);
+
+  LayeredCostmap layered_costmap("map", false, false);
+  layered_costmap.resizeMap(7, 7, 1.0, 0.0, 0.0, true);
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  InflationLayer inflation;
+  inflation.initialize(&layered_costmap, "inflation", &tf_buffer);
+  inflation.setInflationParameters(2.0, 1.0);
+
+  Costmap2D& master = *layered_costmap.getCostmap();
+  master.setCost(3, 3, LETHAL_OBSTACLE);
+  inflation.updateCosts(master, 0, 0, 7, 7);
+
+  EXPECT_EQ(master.getCost(3, 3), LETHAL_OBSTACLE);
+  EXPECT_EQ(master.getCost(2, 3), master.getCost(4, 3));
+  EXPECT_EQ(master.getCost(3, 2), master.getCost(3, 4));
+  EXPECT_GT(master.getCost(4, 3), master.getCost(5, 3));
+  EXPECT_EQ(master.getCost(6, 3), FREE_SPACE);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest( &argc, argv );
   return RUN_ALL_TESTS();
 }
-