Apply rule of zero for Observation and do not use ptr for cloud

Signed-off-by: Fabian König <fabiankoenig@gmail.com>

Author: kfabian

nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp

@@ -34,50 +34,24 @@
 
 #include <geometry_msgs/msg/point.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <rclcpp/macros.hpp>
 
 namespace nav2_costmap_2d
 {
 
 /**
  * @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
- * assignment operator for vector insertion!
  */
 class Observation
 {
 public:
-  /**
-   * @brief  Creates an empty observation
-   */
-  Observation()
-  : cloud_(new sensor_msgs::msg::PointCloud2()), obstacle_max_range_(0.0), obstacle_min_range_(0.0),
-    raytrace_max_range_(0.0),
-    raytrace_min_range_(0.0)
-  {
-  }
-  /**
-   * @brief A destructor
-   */
-  virtual ~Observation()
-  {
-    delete cloud_;
-  }
+  RCLCPP_SMART_PTR_DEFINITIONS(Observation)
 
   /**
-   * @brief  Copy assignment operator
-   * @param obs The observation to copy
+   * @brief  Creates an empty observation
    */
-  Observation & operator=(const Observation & obs)
-  {
-    origin_ = obs.origin_;
-    cloud_ = new sensor_msgs::msg::PointCloud2(*(obs.cloud_));
-    obstacle_max_range_ = obs.obstacle_max_range_;
-    obstacle_min_range_ = obs.obstacle_min_range_;
-    raytrace_max_range_ = obs.raytrace_max_range_;
-    raytrace_min_range_ = obs.raytrace_min_range_;
+  Observation() = default;
 
-    return *this;
-  }
 
   /**
    * @brief  Creates an observation from an origin point and a point cloud
@@ -89,28 +63,16 @@ class Observation
    * @param raytrace_min_range The range from which an observation should be able to clear via raytracing
    */
   Observation(
-    geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud,
+    geometry_msgs::msg::Point origin, sensor_msgs::msg::PointCloud2 cloud,
     double obstacle_max_range, double obstacle_min_range, double raytrace_max_range,
     double raytrace_min_range)
-  : origin_(origin), cloud_(new sensor_msgs::msg::PointCloud2(cloud)),
+  : origin_(std::move(origin)), cloud_(std::move(cloud)),
     obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range),
     raytrace_max_range_(raytrace_max_range), raytrace_min_range_(
       raytrace_min_range)
   {
   }
 
-  /**
-   * @brief  Copy constructor
-   * @param obs The observation to copy
-   */
-  Observation(const Observation & obs)
-  : origin_(obs.origin_), cloud_(new sensor_msgs::msg::PointCloud2(*(obs.cloud_))),
-    obstacle_max_range_(obs.obstacle_max_range_), obstacle_min_range_(obs.obstacle_min_range_),
-    raytrace_max_range_(obs.raytrace_max_range_),
-    raytrace_min_range_(obs.raytrace_min_range_)
-  {
-  }
-
   /**
    * @brief  Creates an observation from a point cloud
    * @param cloud The point cloud of the observation
@@ -120,15 +82,17 @@ class Observation
   Observation(
     const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_max_range,
     double obstacle_min_range)
-  : cloud_(new sensor_msgs::msg::PointCloud2(cloud)), obstacle_max_range_(obstacle_max_range),
-    obstacle_min_range_(obstacle_min_range),
-    raytrace_max_range_(0.0), raytrace_min_range_(0.0)
+  : cloud_(std::move(cloud)), obstacle_max_range_(obstacle_max_range),
+    obstacle_min_range_(obstacle_min_range)
   {
   }
 
-  geometry_msgs::msg::Point origin_;
-  sensor_msgs::msg::PointCloud2 * cloud_;
-  double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;
+  geometry_msgs::msg::Point origin_{};
+  sensor_msgs::msg::PointCloud2 cloud_{};
+  double obstacle_max_range_{0.};
+  double obstacle_min_range_{0.};
+  double raytrace_max_range_{0.};
+  double raytrace_min_range_{0.};
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -510,7 +510,7 @@ ObstacleLayer::updateBounds(
   {
     const Observation & obs = *it;
 
-    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+    const sensor_msgs::msg::PointCloud2 & cloud = obs.cloud_;
 
     double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
     double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
@@ -686,7 +686,7 @@ ObstacleLayer::raytraceFreespace(
 {
   double ox = clearing_observation.origin_.x;
   double oy = clearing_observation.origin_.y;
-  const sensor_msgs::msg::PointCloud2 & cloud = *(clearing_observation.cloud_);
+  const sensor_msgs::msg::PointCloud2 & cloud = clearing_observation.cloud_;
 
   // get the map coordinates of the origin of the sensor
   unsigned int x0, y0;

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -183,7 +183,7 @@ void VoxelLayer::updateBounds(
   {
     const Observation & obs = *it;
 
-    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+    const sensor_msgs::msg::PointCloud2 & cloud = obs.cloud_;
 
     double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
     double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
@@ -268,7 +268,7 @@ void VoxelLayer::raytraceFreespace(
 {
   auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
-  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
+  if (clearing_observation.cloud_.height == 0 || clearing_observation.cloud_.width == 0) {
     return;
   }
 
@@ -302,8 +302,8 @@ void VoxelLayer::raytraceFreespace(
   }
 
   clearing_endpoints_->data.clear();
-  clearing_endpoints_->width = clearing_observation.cloud_->width;
-  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->width = clearing_observation.cloud_.width;
+  clearing_endpoints_->height = clearing_observation.cloud_.height;
   clearing_endpoints_->is_dense = true;
   clearing_endpoints_->is_bigendian = false;
 
@@ -322,9 +322,9 @@ void VoxelLayer::raytraceFreespace(
   double map_end_y = origin_y_ + getSizeInMetersY();
   double map_end_z = origin_z_ + getSizeInMetersZ();
 
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(clearing_observation.cloud_, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(clearing_observation.cloud_, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(clearing_observation.cloud_, "z");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
     double wpx = *iter_x;
@@ -422,7 +422,7 @@ void VoxelLayer::raytraceFreespace(
 
   if (publish_clearing_points) {
     clearing_endpoints_->header.frame_id = global_frame_;
-    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
+    clearing_endpoints_->header.stamp = clearing_observation.cloud_.header.stamp;
 
     clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
   }

nav2_costmap_2d/src/observation_buffer.cpp

@@ -118,7 +118,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
 
     // now we need to remove observations from the cloud that are below
     // or above our height thresholds
-    sensor_msgs::msg::PointCloud2 & observation_cloud = *(observation_list_.front().cloud_);
+    sensor_msgs::msg::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;

nav2_costmap_2d/test/testing_helper.hpp

@@ -127,7 +127,8 @@ void addObservation(
   p.y = oy;
   p.z = oz;
 
-  nav2_costmap_2d::Observation obs(p, cloud, obstacle_max_range, obstacle_min_range,
+  nav2_costmap_2d::Observation obs(std::move(p), std::move(cloud), obstacle_max_range,
+    obstacle_min_range,
     raytrace_max_range, raytrace_min_range);
   olayer->addStaticObservation(obs, marking, clearing);
 }