diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp index 9e1356cb6cf..4e02538e997 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/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 #include #include +#include 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 diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index c54d586b729..443e5cca35b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/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 * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier @@ -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 & observations); + void getObservations(std::vector & 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_list_; + std::list 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 diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 16113d61b16..985cf7a29c4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -174,7 +174,7 @@ class ObstacleLayer : public CostmapLayer const std::shared_ptr & 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,7 +184,7 @@ class ObstacleLayer : public CostmapLayer * @return True if all the observation buffers are current, false otherwise */ bool getMarkingObservations( - std::vector & marking_observations) const; + std::vector & marking_observations) const; /** * @brief Get the observations used to clear space @@ -192,7 +192,7 @@ class ObstacleLayer : public CostmapLayer * @return True if all the observation buffers are current, false otherwise */ bool getClearingObservations( - std::vector & clearing_observations) const; + std::vector & 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 static_clearing_observations_; - std::vector static_marking_observations_; + std::vector static_clearing_observations_; + std::vector static_marking_observations_; bool rolling_window_; bool was_reset_; diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 1a089b69122..5d55ff2c3e0 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/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 { diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index bac51b92f92..24e3a451589 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/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( - new ObservationBuffer( - node, topic, observation_keep_time, expected_update_rate, + std::make_shared(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 & buffer) + const std::shared_ptr & 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 & buffer) + const std::shared_ptr & 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 observations, clearing_observations; + std::vector observations, clearing_observations; // get the marking observations current = current && getMarkingObservations(observations); @@ -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::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_; @@ -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 & marking_observations) const +ObstacleLayer::getMarkingObservations( + std::vector & 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 & marking_observa } bool -ObstacleLayer::getClearingObservations(std::vector & clearing_observations) const +ObstacleLayer::getClearingObservations( + std::vector & 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(), @@ -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; @@ -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(); } } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 33bc0fbe7e6..b539e01971e 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/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 observations, clearing_observations; + std::vector observations, clearing_observations; // get the marking observations current = getMarkingObservations(observations) && current; @@ -173,17 +173,15 @@ 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::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_; @@ -268,7 +266,7 @@ void VoxelLayer::raytraceFreespace( { auto clearing_endpoints_ = std::make_unique(); - 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 +300,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 +320,9 @@ void VoxelLayer::raytraceFreespace( double map_end_y = origin_y_ + getSizeInMetersY(); double map_end_z = origin_z_ + getSizeInMetersZ(); - sensor_msgs::PointCloud2ConstIterator iter_x(*(clearing_observation.cloud_), "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(clearing_observation.cloud_, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(clearing_observation.cloud_, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(clearing_observation.cloud_, "z"); for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { double wpx = *iter_x; @@ -422,7 +420,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_)); } diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 21d8c820bc9..79e3baba410 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/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 & observations) +// appends the observations from the buffer to the passed vector +void ObservationBuffer::getObservations(std::vector & 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::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::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_; + }); } } } diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index bf15fb97d7a..5f882b6ed1c 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -18,6 +18,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -127,9 +128,10 @@ 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); + olayer->addStaticObservation(std::move(obs), marking, clearing); } void addInflationLayer( diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 559ef4ae4e2..97d2452a084 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -65,5 +65,9 @@ target_link_libraries(lifecycle_test ament_add_gtest(coordinate_transform_test coordinate_transform_test.cpp) target_link_libraries(coordinate_transform_test + nav2_costmap_2d_core) + +ament_add_gtest(observation_buffer_test observation_buffer_test.cpp) +target_link_libraries(observation_buffer_test nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/observation_buffer_test.cpp b/nav2_costmap_2d/test/unit/observation_buffer_test.cpp new file mode 100644 index 00000000000..645131d6afc --- /dev/null +++ b/nav2_costmap_2d/test/unit/observation_buffer_test.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Fabian Koenig +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "gtest/gtest.h" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "nav2_costmap_2d/observation_buffer.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +class ObservationBufferFixture : public ::testing::Test +{ +protected: + ObservationBufferFixture() + { + auto clock_handle = node_->get_clock()->get_clock_handle(); + auto ret = rcl_enable_ros_time_override(clock_handle); + ret = rcl_set_ros_time_override(clock_handle, 0); + (void) ret; + geometry_msgs::msg::TransformStamped tf_msg{}; + tf_msg.child_frame_id = sensor_frame_; + tf_msg.header.frame_id = global_frame_; + tf_msg.transform = tf2::toMsg(tf2::Transform::getIdentity()); + tf_buffer_.setTransform(tf_msg, "", true); + } + + ~ObservationBufferFixture() + { + node_->shutdown(); + } + + void setClock(const rclcpp::Time & time) + { + auto clock_handle = node_->get_clock()->get_clock_handle(); + auto ret = rcl_set_ros_time_override(clock_handle, time.nanoseconds()); + (void) ret; + } + + static sensor_msgs::msg::PointCloud2 generatePointCloud( + double x, double y, double z, + const rclcpp::Time & stamp) + { + sensor_msgs::msg::PointCloud2 cloud; + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(1); + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + *iter_x = x; + *iter_y = y; + *iter_z = z; + + cloud.header.frame_id = sensor_frame_; + cloud.header.stamp = stamp; + + return cloud; + } + + static sensor_msgs::msg::PointCloud2 generateRandomPointCloud( + size_t num_points, + const rclcpp::Time & stamp) + { + sensor_msgs::msg::PointCloud2 cloud; + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(num_points); + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + std::random_device rand_dev{}; + std::mt19937 generator{rand_dev()}; + std::uniform_int_distribution distr{0, 10000}; + for (auto i = 0u; i < num_points; ++i) { + *iter_x = distr(generator) / 100.0; + *iter_y = distr(generator) / 100.0; + *iter_z = distr(generator) / 100.0; + ++iter_x; + ++iter_y; + ++iter_z; + } + + cloud.header.frame_id = sensor_frame_; + cloud.header.stamp = stamp; + + return cloud; + } + + static constexpr const char * global_frame_{"map"}; + static constexpr const char * sensor_frame_{"sensor"}; + + nav2::LifecycleNode::SharedPtr node_{std::make_shared( + "observation_buffer_test_node")}; + tf2_ros::Buffer tf_buffer_{node_->get_clock()}; + nav2_costmap_2d::ObservationBuffer buffer_{node_, "/observations", 10.0, 0.1, 0.0, + std::numeric_limits::max(), 10.0, 1.0, 5.0, 2.0, tf_buffer_, global_frame_, + sensor_frame_, tf2::Duration{0}}; +}; + +TEST_F(ObservationBufferFixture, isCurrent) +{ + setClock(rclcpp::Time{0, 100000}); + buffer_.bufferCloud(generatePointCloud(0.5, 0.5, 0.5, rclcpp::Time{0, 0})); + // Expect time between clouds is 0.1s, time of the cloud is 0.0s and current time is 0.0001s + // so the buffer is up to date + EXPECT_TRUE(buffer_.isCurrent()); + setClock(rclcpp::Time{1, 0}); + // Now the current time is 1.0s and the buffer is outdated + EXPECT_FALSE(buffer_.isCurrent()); +} + +TEST_F(ObservationBufferFixture, purgeStaleObservations) +{ + setClock(rclcpp::Time{1, 0}); + buffer_.bufferCloud(generatePointCloud(0.5, 0.5, 0.5, rclcpp::Time{1, 0})); + setClock(rclcpp::Time{5, 0}); + buffer_.bufferCloud(generatePointCloud(0.5, 0.5, 0.5, rclcpp::Time{5, 0})); + setClock(rclcpp::Time{10, 0}); + buffer_.bufferCloud(generatePointCloud(0.5, 0.5, 0.5, rclcpp::Time{10, 0})); + std::vector observations{}; + // The observation_keep_time is 10s and the age of the oldest cloud is 9s + // so all 3 added observations should be returned + buffer_.getObservations(observations); + EXPECT_EQ(observations.size(), 3u); + setClock(rclcpp::Time{12, 0}); + observations.clear(); + // Now the oldest cloud is 11s old and only 2 observations should be returned + buffer_.getObservations(observations); + EXPECT_EQ(observations.size(), 2u); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +}