otimal deep coppy obj

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_
-              ? new robot_sensor_msgs::DepthCameraData(*other.data_)
-              : nullptr),
+    : data_handle_(other.data_handle_),
+      data_(data_handle_.get()),
       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)
+  {
+    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;
+  }
+
   DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept
   {
     if (this == &other)
-    {
       return *this;
-    }
 
-    delete data_;
-
-    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()
-  {
-    delete data_;
-    data_ = nullptr;
-  }
+  ~DepthCameraObservation() = default;
 
-  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()
-  {
-    delete cloud_;
-  }
+  virtual ~Observation() = default;
 
   /**
    * @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_;
 };