add multi camera depth

include/robot_costmap_2d/observation.h

@@ -34,10 +34,140 @@
 
 #include <robot_geometry_msgs/Point.h>
 #include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/DepthCameraData.h>
 
 namespace robot_costmap_2d
 {
 
+/**
+ * @brief A depth frame and its per-source frustum-clearing configuration.
+ *
+ * The message is shared so returning buffered observations does not copy the
+ * full depth image on every costmap update.
+ */
+class DepthCameraObservation
+{
+public:
+  DepthCameraObservation()
+    : data_(nullptr),
+      topic_(),
+      pixel_step_(0),
+      min_range_(0.0),
+      max_range_(0.0)
+  {
+  }
+
+  DepthCameraObservation(
+      const robot_sensor_msgs::DepthCameraData& data,
+      std::string topic,
+      const robot::Time& received_time,
+      unsigned int pixel_step,
+      double min_range,
+      double max_range)
+    : data_(new robot_sensor_msgs::DepthCameraData(data)),
+      topic_(std::move(topic)),
+      received_time_(received_time),
+      pixel_step_(pixel_step),
+      min_range_(min_range),
+      max_range_(max_range)
+  {
+  }
+
+  // Copy constructor: deep copy
+  DepthCameraObservation(const DepthCameraObservation& other)
+    : data_(other.data_
+              ? new robot_sensor_msgs::DepthCameraData(*other.data_)
+              : nullptr),
+      topic_(other.topic_),
+      received_time_(other.received_time_),
+      pixel_step_(other.pixel_step_),
+      min_range_(other.min_range_),
+      max_range_(other.max_range_)
+  {
+  }
+
+  // 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_),
+      topic_(std::move(other.topic_)),
+      received_time_(other.received_time_),
+      pixel_step_(other.pixel_step_),
+      min_range_(other.min_range_),
+      max_range_(other.max_range_)
+  {
+    other.data_ = nullptr;
+    other.pixel_step_ = 0;
+    other.min_range_ = 0.0;
+    other.max_range_ = 0.0;
+  }
+
+  // Move assignment
+  DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept
+  {
+    if (this == &other)
+    {
+      return *this;
+    }
+
+    delete data_;
+
+    data_ = other.data_;
+    topic_ = std::move(other.topic_);
+    received_time_ = other.received_time_;
+    pixel_step_ = other.pixel_step_;
+    min_range_ = other.min_range_;
+    max_range_ = other.max_range_;
+
+    other.data_ = nullptr;
+    other.pixel_step_ = 0;
+    other.min_range_ = 0.0;
+    other.max_range_ = 0.0;
+
+    return *this;
+  }
+
+  ~DepthCameraObservation()
+  {
+    delete data_;
+    data_ = nullptr;
+  }
+
+  robot_sensor_msgs::DepthCameraData* data_;
+  std::string topic_;
+  robot::Time received_time_;
+  unsigned int pixel_step_;
+  double min_range_;
+  double max_range_;
+};
+
 /**
  * @brief Stores an observation in terms of a point cloud and the origin of the source
  * @note Tried to make members and constructor arguments const but the compiler would not accept the default