Refactor/observation buffer (#4890)

* Add benchmark for observation buffer

Signed-off-by: Fabian König <[email protected]>

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

Signed-off-by: Fabian König <[email protected]>

* ObservationBuffer stores shared ptrs of observations to avoid copies when queried

Signed-off-by: Fabian König <[email protected]>

* Check for nullptr if *_buffers_ members are accessed

Signed-off-by: Fabian König <[email protected]>

---------

Signed-off-by: Fabian König <[email protected]>

Author: kfabian

nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp

@@ -32,52 +32,27 @@
 #ifndef NAV2_COSTMAP_2D__OBSERVATION_HPP_
 #define NAV2_COSTMAP_2D__OBSERVATION_HPP_
 
+#include <utility>
 #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 +64,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 +83,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/include/nav2_costmap_2d/observation_buffer.hpp

@@ -87,11 +87,6 @@ class ObservationBuffer
     std::string sensor_frame,
     tf2::Duration tf_tolerance);
 
-  /**
-   * @brief  Destructor... cleans up
-   */
-  ~ObservationBuffer();
-
   /**
    * @brief  Transforms a PointCloud to the global frame and buffers it
    * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
@@ -103,7 +98,7 @@ class ObservationBuffer
    * @brief  Pushes copies of all current observations onto the end of the vector passed in
    * @param  observations The vector to be filled
    */
-  void getObservations(std::vector<Observation> & observations);
+  void getObservations(std::vector<Observation::ConstSharedPtr> & observations);
 
   /**
    * @brief  Check if the observation buffer is being update at its expected rate
@@ -146,7 +141,7 @@ class ObservationBuffer
   rclcpp::Time last_updated_;
   std::string global_frame_;
   std::string sensor_frame_;
-  std::list<Observation> observation_list_;
+  std::list<Observation::ConstSharedPtr> observation_list_;
   std::string topic_name_;
   double min_obstacle_height_, max_obstacle_height_;
   std::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -175,7 +175,7 @@ class ObstacleLayer : public CostmapLayer
     const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer);
 
   // for testing purposes
-  void addStaticObservation(nav2_costmap_2d::Observation & obs, bool marking, bool clearing);
+  void addStaticObservation(nav2_costmap_2d::Observation obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
 protected:
@@ -185,15 +185,15 @@ class ObstacleLayer : public CostmapLayer
    * @return True if all the observation buffers are current, false otherwise
    */
   bool getMarkingObservations(
-    std::vector<nav2_costmap_2d::Observation> & marking_observations) const;
+    std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> & marking_observations) const;
 
   /**
    * @brief  Get the observations used to clear space
    * @param clearing_observations A reference to a vector that will be populated with the observations
    * @return True if all the observation buffers are current, false otherwise
    */
   bool getClearingObservations(
-    std::vector<nav2_costmap_2d::Observation> & clearing_observations) const;
+    std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> & clearing_observations) const;
 
   /**
    * @brief  Clear freespace based on one observation
@@ -256,8 +256,8 @@ class ObstacleLayer : public CostmapLayer
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 
   // Used only for testing purposes
-  std::vector<nav2_costmap_2d::Observation> static_clearing_observations_;
-  std::vector<nav2_costmap_2d::Observation> static_marking_observations_;
+  std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> static_clearing_observations_;
+  std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> static_marking_observations_;
 
   bool rolling_window_;
   bool was_reset_;

nav2_costmap_2d/package.xml

@@ -49,6 +49,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>nav2_map_server</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_testing</test_depend>
   <test_depend>nav2_lifecycle_manager</test_depend>

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -206,15 +206,13 @@ void ObstacleLayer::onInitialize()
 
     // create an observation buffer
     observation_buffers_.push_back(
-      std::shared_ptr<ObservationBuffer
-      >(
-        new ObservationBuffer(
-          node, topic, observation_keep_time, expected_update_rate,
+          std::make_shared<ObservationBuffer>(node, topic, observation_keep_time,
+        expected_update_rate,
           min_obstacle_height,
           max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range,
           raytrace_min_range, *tf_,
           global_frame_,
-          sensor_frame, tf2::durationFromSec(transform_tolerance))));
+          sensor_frame, tf2::durationFromSec(transform_tolerance)));
 
     // check if we'll add this buffer to our marking observation buffers
     if (marking) {
@@ -399,7 +397,7 @@ ObstacleLayer::dynamicParametersCallback(
 void
 ObstacleLayer::laserScanCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr message,
-  const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer)
+  const std::shared_ptr<ObservationBuffer> & buffer)
 {
   // project the laser into a point cloud
   sensor_msgs::msg::PointCloud2 cloud;
@@ -433,7 +431,7 @@ ObstacleLayer::laserScanCallback(
 void
 ObstacleLayer::laserScanValidInfCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message,
-  const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer)
+  const std::shared_ptr<ObservationBuffer> & buffer)
 {
   // Filter positive infinities ("Inf"s) to max_range.
   float epsilon = 0.0001;  // a tenth of a millimeter
@@ -499,7 +497,7 @@ ObstacleLayer::updateBounds(
   useExtraBounds(min_x, min_y, max_x, max_y);
 
   bool current = true;
-  std::vector<Observation> observations, clearing_observations;
+  std::vector<Observation::ConstSharedPtr> observations, clearing_observations;
 
   // get the marking observations
   current = current && getMarkingObservations(observations);
@@ -511,17 +509,15 @@ ObstacleLayer::updateBounds(
   current_ = current;
 
   // raytrace freespace
-  for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
-    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+  for (const auto & clearing_observation : clearing_observations) {
+    raytraceFreespace(*clearing_observation, min_x, min_y, max_x, max_y);
   }
 
   // place the new obstacles into a priority queue... each with a priority of zero to begin with
-  for (std::vector<Observation>::const_iterator it = observations.begin();
-    it != observations.end(); ++it)
-  {
-    const Observation & obs = *it;
+  for (const auto & observation : observations) {
+    const Observation & obs = *observation;
 
-    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_;
@@ -632,14 +628,15 @@ ObstacleLayer::updateCosts(
 
 void
 ObstacleLayer::addStaticObservation(
-  nav2_costmap_2d::Observation & obs,
+  nav2_costmap_2d::Observation obs,
   bool marking, bool clearing)
 {
+  const auto observation = Observation::make_shared(std::move(obs));
   if (marking) {
-    static_marking_observations_.push_back(obs);
+    static_marking_observations_.push_back(observation);
   }
   if (clearing) {
-    static_clearing_observations_.push_back(obs);
+    static_clearing_observations_.push_back(observation);
   }
 }
 
@@ -655,15 +652,18 @@ ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
 }
 
 bool
-ObstacleLayer::getMarkingObservations(std::vector<Observation> & marking_observations) const
+ObstacleLayer::getMarkingObservations(
+  std::vector<Observation::ConstSharedPtr> & marking_observations) const
 {
   bool current = true;
   // get the marking observations
-  for (unsigned int i = 0; i < marking_buffers_.size(); ++i) {
-    marking_buffers_[i]->lock();
-    marking_buffers_[i]->getObservations(marking_observations);
-    current = marking_buffers_[i]->isCurrent() && current;
-    marking_buffers_[i]->unlock();
+  for (const auto & marking_buffer : marking_buffers_) {
+    if (marking_buffer) {
+      marking_buffer->lock();
+      marking_buffer->getObservations(marking_observations);
+      current = marking_buffer->isCurrent() && current;
+      marking_buffer->unlock();
+    }
   }
   marking_observations.insert(
     marking_observations.end(),
@@ -672,15 +672,18 @@ ObstacleLayer::getMarkingObservations(std::vector<Observation> & marking_observa
 }
 
 bool
-ObstacleLayer::getClearingObservations(std::vector<Observation> & clearing_observations) const
+ObstacleLayer::getClearingObservations(
+  std::vector<Observation::ConstSharedPtr> & clearing_observations) const
 {
   bool current = true;
   // get the clearing observations
-  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i) {
-    clearing_buffers_[i]->lock();
-    clearing_buffers_[i]->getObservations(clearing_observations);
-    current = clearing_buffers_[i]->isCurrent() && current;
-    clearing_buffers_[i]->unlock();
+  for (const auto & clearing_buffer : clearing_buffers_) {
+    if (clearing_buffer) {
+      clearing_buffer->lock();
+      clearing_buffer->getObservations(clearing_observations);
+      current = clearing_buffer->isCurrent() && current;
+      clearing_buffer->unlock();
+    }
   }
   clearing_observations.insert(
     clearing_observations.end(),
@@ -697,7 +700,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;
@@ -833,9 +836,9 @@ ObstacleLayer::reset()
 void
 ObstacleLayer::resetBuffersLastUpdated()
 {
-  for (unsigned int i = 0; i < observation_buffers_.size(); ++i) {
-    if (observation_buffers_[i]) {
-      observation_buffers_[i]->resetLastUpdated();
+  for (const auto & observation_buffer : observation_buffers_) {
+    if (observation_buffer) {
+      observation_buffer->resetLastUpdated();
     }
   }
 }