Skip to content

Refactor/observation buffer #4890

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
+254 −152
Open

Refactor/observation buffer #4890

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bfc3cf7
Apply rule of zero for Observation and do not use ptr for cloud
kfabian Jan 30, 2025
3bc462b
ObservationBuffer stores shared ptrs of observations to avoid copies …
kfabian Jan 31, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 15 additions & 50 deletions nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down
9 changes: 2 additions & 7 deletions nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down
10 changes: 5 additions & 5 deletions nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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
Expand Down Expand Up @@ -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_;
Expand Down
2 changes: 1 addition & 1 deletion nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
67 changes: 32 additions & 35 deletions nav2_costmap_2d/plugins/obstacle_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -500,17 +498,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_;
Expand Down Expand Up @@ -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);
}
}

Expand All @@ -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(),
Expand All @@ -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(),
Expand All @@ -686,7 +685,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;
Expand Down Expand Up @@ -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();
}
}

Expand Down
Loading
Loading