ObservationBuffer stores shared ptrs of observations to avoid copies when queried

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

Author: kfabian

nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp

@@ -32,6 +32,7 @@
 #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>

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

@@ -174,7 +174,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:
@@ -184,15 +184,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
@@ -255,8 +255,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/plugins/costmap_filters/costmap_filter.cpp

@@ -217,7 +217,7 @@ unsigned char CostmapFilter::getMaskCost(
 {
   const unsigned int index = my * filter_mask->info.width + mx;
 
-  const char data = filter_mask->data[index];
+  const auto data = filter_mask->data[index];
   if (data == nav2_util::OCC_GRID_UNKNOWN) {
     return NO_INFORMATION;
   } else {

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -202,15 +202,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) {
@@ -388,7 +386,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;
@@ -422,7 +420,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
@@ -488,7 +486,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);
@@ -500,15 +498,13 @@ 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_;
 
@@ -621,14 +617,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);
   }
 }
 
@@ -644,15 +641,16 @@ 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_) {
+    marking_buffer->lock();
+    marking_buffer->getObservations(marking_observations);
+    current = marking_buffer->isCurrent() && current;
+    marking_buffer->unlock();
   }
   marking_observations.insert(
     marking_observations.end(),
@@ -661,15 +659,16 @@ 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_) {
+    clearing_buffer->lock();
+    clearing_buffer->getObservations(clearing_observations);
+    current = clearing_buffer->isCurrent() && current;
+    clearing_buffer->unlock();
   }
   clearing_observations.insert(
     clearing_observations.end(),
@@ -821,10 +820,8 @@ 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_) {
+    observation_buffer->resetLastUpdated();
   }
 }
 

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -161,7 +161,7 @@ void VoxelLayer::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 = getMarkingObservations(observations) && current;
@@ -173,15 +173,13 @@ void VoxelLayer::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_;
 

nav2_costmap_2d/src/observation_buffer.cpp

@@ -76,22 +76,18 @@ ObservationBuffer::ObservationBuffer(
   last_updated_ = node->now();
 }
 
-ObservationBuffer::~ObservationBuffer()
-{
-}
-
 void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
 {
   geometry_msgs::msg::PointStamped global_origin;
 
-  // 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
   std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
 
   try {
+    // create a new observation to  add to the list after being populated
+    auto observation = Observation::make_shared();
+
     // given these observations come from sensors...
     // we'll need to store the origin pt of the sensor
     geometry_msgs::msg::PointStamped local_origin;
@@ -101,14 +97,14 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
     local_origin.point.y = 0;
     local_origin.point.z = 0;
     tf2_buffer_.transform(local_origin, global_origin, global_frame_, tf_tolerance_);
-    tf2::convert(global_origin.point, observation_list_.front().origin_);
+    tf2::convert(global_origin.point, observation->origin_);
 
     // make sure to pass on the raytrace/obstacle range
     // of the observation buffer to the observations
-    observation_list_.front().raytrace_max_range_ = raytrace_max_range_;
-    observation_list_.front().raytrace_min_range_ = raytrace_min_range_;
-    observation_list_.front().obstacle_max_range_ = obstacle_max_range_;
-    observation_list_.front().obstacle_min_range_ = obstacle_min_range_;
+    observation->raytrace_max_range_ = raytrace_max_range_;
+    observation->raytrace_min_range_ = raytrace_min_range_;
+    observation->obstacle_max_range_ = obstacle_max_range_;
+    observation->obstacle_min_range_ = obstacle_min_range_;
 
     sensor_msgs::msg::PointCloud2 global_frame_cloud;
 
@@ -118,7 +114,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->cloud_;
     observation_cloud.height = global_frame_cloud.height;
     observation_cloud.width = global_frame_cloud.width;
     observation_cloud.fields = global_frame_cloud.fields;
@@ -153,9 +149,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
     modifier.resize(point_count);
     observation_cloud.header.stamp = cloud.header.stamp;
     observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
+
+    observation_list_.push_front(std::move(observation));
   } catch (tf2::TransformException & ex) {
     // if an exception occurs, we need to remove the empty observation from the list
-    observation_list_.pop_front();
     RCLCPP_ERROR(
       logger_,
       "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
@@ -171,40 +168,27 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
   purgeStaleObservations();
 }
 
-// returns a copy of the observations
-void ObservationBuffer::getObservations(std::vector<Observation> & observations)
+// appends the observations from the buffer to the passed vector
+void ObservationBuffer::getObservations(std::vector<Observation::ConstSharedPtr> & observations)
 {
   // first... let's make sure that we don't have any stale observations
   purgeStaleObservations();
 
-  // now we'll just copy the observations for the caller
-  std::list<Observation>::iterator obs_it;
-  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) {
-    observations.push_back(*obs_it);
-  }
+  // now we'll just copy the observation ptrs for the caller
+  observations.insert(observations.end(), observation_list_.cbegin(), observation_list_.cend());
 }
 
 void ObservationBuffer::purgeStaleObservations()
 {
   if (!observation_list_.empty()) {
-    std::list<Observation>::iterator obs_it = observation_list_.begin();
     // if we're keeping observations for no time... then we'll only keep one observation
     if (observation_keep_time_ == rclcpp::Duration(0.0s)) {
-      observation_list_.erase(++obs_it, observation_list_.end());
-      return;
-    }
-
-    // otherwise... we'll have to loop through the observations to see which ones are stale
-    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) {
-      Observation & obs = *obs_it;
-      // check if the observation is out of date... and if it is,
-      // remove it and those that follow from the list
-      if ((clock_->now() - obs.cloud_->header.stamp) >
-        observation_keep_time_)
-      {
-        observation_list_.erase(obs_it, observation_list_.end());
-        return;
-      }
+      observation_list_.erase(std::next(observation_list_.begin()), observation_list_.end());
+    } else {
+      // otherwise remove all observations that are older then the keep time
+      observation_list_.remove_if([now = clock_->now(), this](const auto observation){
+          return (now - observation->cloud_.header.stamp) > observation_keep_time_;
+      });
     }
   }
 }

nav2_costmap_2d/test/testing_helper.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/point_cloud2_iterator.hpp"
@@ -130,7 +131,7 @@ void addObservation(
   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);
+  olayer->addStaticObservation(std::move(obs), marking, clearing);
 }
 
 void addInflationLayer(