diff --git a/costmap_2d/CMakeLists.txt b/costmap_2d/CMakeLists.txt index 39ec3360ff..d72b865a5f 100644 --- a/costmap_2d/CMakeLists.txt +++ b/costmap_2d/CMakeLists.txt @@ -183,6 +183,9 @@ if(CATKIN_ENABLE_TESTING) catkin_add_gtest(array_parser_test test/array_parser_test.cpp) target_link_libraries(array_parser_test costmap_2d) + catkin_add_gtest(observation_buffer_test test/observation_buffer_test.cpp) + target_link_libraries(observation_buffer_test costmap_2d) + catkin_add_gtest(coordinates_test test/coordinates_test.cpp) target_link_libraries(coordinates_test costmap_2d) endif() diff --git a/costmap_2d/include/costmap_2d/observation.h b/costmap_2d/include/costmap_2d/observation.h index 695892bc6a..6194b0a202 100644 --- a/costmap_2d/include/costmap_2d/observation.h +++ b/costmap_2d/include/costmap_2d/observation.h @@ -49,52 +49,29 @@ class Observation /** * @brief Creates an empty observation */ - Observation() : - cloud_(new sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0) + Observation() : obstacle_range_(0.0), raytrace_range_(0.0) { } - virtual ~Observation() - { - delete cloud_; - } + // Keep virtual desctructor in case anyone goes for polymorphism. + virtual ~Observation() = default; /** * @brief Creates an observation from an origin point and a point cloud * @param origin The origin point of the observation * @param cloud The point cloud of the observation - * @param obstacle_range The range out to which an observation should be able to insert obstacles - * @param raytrace_range The range out to which an observation should be able to clear via raytracing + * @param obstacle_range The range up to which an observation should be able to insert obstacles + * @param raytrace_range The range up to which an observation should be able to clear via raytracing */ - Observation(geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud, + Observation(const geometry_msgs::Point& origin, const sensor_msgs::PointCloud2ConstPtr &cloud, double obstacle_range, double raytrace_range) : - origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)), + origin_(origin), cloud_(cloud), obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) { } - /** - * @brief Copy constructor - * @param obs The observation to copy - */ - Observation(const Observation& obs) : - origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))), - obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_) - { - } - - /** - * @brief Creates an observation from a point cloud - * @param cloud The point cloud of the observation - * @param obstacle_range The range out to which an observation should be able to insert obstacles - */ - Observation(const sensor_msgs::PointCloud2 &cloud, double obstacle_range) : - cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0) - { - } - geometry_msgs::Point origin_; - sensor_msgs::PointCloud2* cloud_; + sensor_msgs::PointCloud2ConstPtr cloud_; double obstacle_range_, raytrace_range_; }; diff --git a/costmap_2d/include/costmap_2d/observation_buffer.h b/costmap_2d/include/costmap_2d/observation_buffer.h index 14d7165925..a6503528c6 100644 --- a/costmap_2d/include/costmap_2d/observation_buffer.h +++ b/costmap_2d/include/costmap_2d/observation_buffer.h @@ -51,6 +51,18 @@ namespace costmap_2d { + +/** + * @brief Applies a box filter to the given cloud. + * + * Filter will remove points which are higher than _max_z or lower than _min_z. + * + * @param _cloud The point cloud to filter. + * @param _min_z The lower threshold. + * @param _max_z The upper threshold. + */ +void heightFilterPointCloud(sensor_msgs::PointCloud2& _cloud, double _min_z, double _max_z); + /** * @class ObservationBuffer * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them diff --git a/costmap_2d/include/costmap_2d/testing_helper.h b/costmap_2d/include/costmap_2d/testing_helper.h index 0a2bdc00b4..e7d957a235 100644 --- a/costmap_2d/include/costmap_2d/testing_helper.h +++ b/costmap_2d/include/costmap_2d/testing_helper.h @@ -66,13 +66,13 @@ costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0, double ox = 0.0, double oy = 0.0, double oz = MAX_Z){ - sensor_msgs::PointCloud2 cloud; - sensor_msgs::PointCloud2Modifier modifier(cloud); + sensor_msgs::PointCloud2Ptr cloud(new sensor_msgs::PointCloud2{}); + 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"); + 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; diff --git a/costmap_2d/src/observation_buffer.cpp b/costmap_2d/src/observation_buffer.cpp index 06fb38e878..abba35cd32 100644 --- a/costmap_2d/src/observation_buffer.cpp +++ b/costmap_2d/src/observation_buffer.cpp @@ -36,15 +36,59 @@ *********************************************************************/ #include +#include +#include +#include #include #include -#include + +#include using namespace std; using namespace tf2; namespace costmap_2d { + +void heightFilterPointCloud(sensor_msgs::PointCloud2 &_cloud, double _min_z, double _max_z) +{ + // This would lead to a dead lock. + if(_cloud.point_step == 0) + return; + + // Check if the field z exists: otherwise the iter_z constructor will throw. + if(sensor_msgs::getPointCloud2FieldIndex(_cloud, "z") == -1){ + ROS_DEBUG("The point cloud has no z-field"); + return; + } + + sensor_msgs::PointCloud2Iterator iter_z(_cloud, "z"); + std::vector::iterator begin = _cloud.data.begin(); + std::vector::iterator end = _cloud.data.end(); + for (; begin != end;) + { + if ((*iter_z) <= _max_z && (*iter_z) >= _min_z) + { + // Advance to the next point. + ++iter_z; + begin += _cloud.point_step; + } + else + { + // Swap the current point with the end. + end -= _cloud.point_step; + std::copy_n(end, _cloud.point_step, begin); + } + } + + // Get the point count + const size_t point_count = (end - _cloud.data.begin()) / _cloud.point_step; + // Resize the point-cloud. + sensor_msgs::PointCloud2Modifier modifier(_cloud); + modifier.resize(point_count); + _cloud.data.erase(end, _cloud.data.end()); +} + ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_range, double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame, @@ -90,7 +134,9 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) obs.origin_ = origin.point; // we also need to transform the cloud of the observation to the new global frame - tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame); + sensor_msgs::PointCloud2Ptr transformed_cloud(new sensor_msgs::PointCloud2{}); + tf2_buffer_.transform(*(obs.cloud_), *transformed_cloud, new_global_frame); + obs.cloud_ = transformed_cloud; } catch (TransformException& ex) { @@ -107,80 +153,41 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) { - geometry_msgs::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 string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; + geometry_msgs::PointStamped global_origin; + geometry_msgs::PointStamped local_origin; + local_origin.header.stamp = cloud.header.stamp; + local_origin.header.frame_id = origin_frame; + local_origin.point.x = 0; + local_origin.point.y = 0; + local_origin.point.z = 0; + + sensor_msgs::PointCloud2Ptr global_frame_cloud(new sensor_msgs::PointCloud2()); + try { - // given these observations come from sensors... we'll need to store the origin pt of the sensor - geometry_msgs::PointStamped local_origin; - local_origin.header.stamp = cloud.header.stamp; - local_origin.header.frame_id = origin_frame; - local_origin.point.x = 0; - local_origin.point.y = 0; - local_origin.point.z = 0; + // Transform the sensor origin. tf2_buffer_.transform(local_origin, global_origin, global_frame_); - tf2::convert(global_origin.point, observation_list_.front().origin_); - - // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations - observation_list_.front().raytrace_range_ = raytrace_range_; - observation_list_.front().obstacle_range_ = obstacle_range_; - - sensor_msgs::PointCloud2 global_frame_cloud; // transform the point cloud - tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_); - global_frame_cloud.header.stamp = cloud.header.stamp; - - // now we need to remove observations from the cloud that are below or above our height thresholds - sensor_msgs::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; - observation_cloud.is_bigendian = global_frame_cloud.is_bigendian; - observation_cloud.point_step = global_frame_cloud.point_step; - observation_cloud.row_step = global_frame_cloud.row_step; - observation_cloud.is_dense = global_frame_cloud.is_dense; - - unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width; - sensor_msgs::PointCloud2Modifier modifier(observation_cloud); - modifier.resize(cloud_size); - unsigned int point_count = 0; - - // copy over the points that are within our height bounds - sensor_msgs::PointCloud2Iterator iter_z(global_frame_cloud, "z"); - std::vector::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end(); - std::vector::iterator iter_obs = observation_cloud.data.begin(); - for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step) - { - if ((*iter_z) <= max_obstacle_height_ - && (*iter_z) >= min_obstacle_height_) - { - std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs); - iter_obs += global_frame_cloud.point_step; - ++point_count; - } - } - - // resize the cloud for the number of legal points - modifier.resize(point_count); - observation_cloud.header.stamp = cloud.header.stamp; - observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; + tf2_buffer_.transform(cloud, *global_frame_cloud, global_frame_); + global_frame_cloud->header.stamp = cloud.header.stamp; } catch (TransformException& ex) { - // if an exception occurs, we need to remove the empty observation from the list - observation_list_.pop_front(); ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); return; } + // Apply the filter and remove points outside of the height bounds. + heightFilterPointCloud(*global_frame_cloud, min_obstacle_height_, max_obstacle_height_); + + // Add the new observation. + observation_list_.emplace_front(global_origin.point, global_frame_cloud, obstacle_range_, raytrace_range_); + // if the update was successful, we want to update the last updated time last_updated_ = ros::Time::now(); diff --git a/costmap_2d/test/observation_buffer_test.cpp b/costmap_2d/test/observation_buffer_test.cpp new file mode 100644 index 0000000000..aa43540f6b --- /dev/null +++ b/costmap_2d/test/observation_buffer_test.cpp @@ -0,0 +1,215 @@ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +/** + * @brief Compares the points giving x precedence over y and y precedence over z. + */ +struct compare_points +{ + bool operator()(const geometry_msgs::Point32 &l, const geometry_msgs::Point32 &r) const + { + if (l.x == r.x) + { + if (l.y == r.y) + return l.z < r.z; + return l.y < r.y; + } + return l.x < r.x; + } +}; + +/** + * @brief Sorts the points using the compare_points functor. + */ +std::set +sort_points(const sensor_msgs::PointCloud &cloud) +{ + std::set sorted_points; + for (const auto &p : cloud.points) + sorted_points.insert(p); + + return sorted_points; +} + +bool equal(const sensor_msgs::PointCloud &l, const sensor_msgs::PointCloud &r) +{ + // Sort the points. + const auto l_points = sort_points(l); + const auto r_points = sort_points(r); + + // Compare the results. + return l_points == r_points; +} + +TEST(heightFilterPointCloud, empty) +{ + sensor_msgs::PointCloud2 input; + ASSERT_NO_THROW(costmap_2d::heightFilterPointCloud(input, 0, 1)); +} + +TEST(heightFilterPointCloud, no_z) +{ + sensor_msgs::PointCloud2 input; + input.point_step = 1; + ASSERT_NO_THROW(costmap_2d::heightFilterPointCloud(input, 0, 1)); +} + +/** + * @brief A copying version of the box-filter. + * + * @param _cloud The point-cloud to filter. + * @param _min_z The lower bound. + * @param _max_z The upper bound. + */ +void copyingHeightFilterPointCloud(sensor_msgs::PointCloud &_cloud, + double _min_z, double _max_z) +{ + sensor_msgs::PointCloud out = _cloud; + auto dd = out.points.begin(); + + for (auto ss = _cloud.points.begin(); ss != _cloud.points.end(); ++ss) + { + if (ss->z <= _max_z && ss->z >= _min_z) + { + *dd = *ss; + ++dd; + } + } + + out.points.erase(dd, out.points.end()); + _cloud = out; +} + +TEST(heightFilterPointCloud, no_change) +{ + // Create a simple point cloud. + sensor_msgs::PointCloud points, output; + points.points.resize(100); + + double x = 0; + for (auto &p : points.points) + { + p.x = ++x; + p.z = 1; + } + + sensor_msgs::PointCloud2 input; + sensor_msgs::convertPointCloudToPointCloud2(points, input); + costmap_2d::heightFilterPointCloud(input, 0, 2); + + ASSERT_TRUE(sensor_msgs::convertPointCloud2ToPointCloud(input, output)); + + // We should keep everything. + ASSERT_EQ(points, output); +} + +/// The base fixture defining a pair (min_z, max_z) as parameter. +struct FilterFixture : public testing::TestWithParam> +{ +}; + +using RemoveAllFixture = FilterFixture; +INSTANTIATE_TEST_CASE_P(/**/, RemoveAllFixture, + testing::Values( + std::make_pair(0., 1.9), // all values above + std::make_pair(2.1, 3.) // all values below + )); + +TEST_P(RemoveAllFixture, regression) +{ + // Create a point cloud with uniform points. + sensor_msgs::PointCloud points, output; + geometry_msgs::Point32 point; + point.z = 2; + points.points.resize(100, point); + + // Apply the filter. + sensor_msgs::PointCloud2 input; + sensor_msgs::convertPointCloudToPointCloud2(points, input); + const auto param = GetParam(); + costmap_2d::heightFilterPointCloud(input, param.first, param.second); + + ASSERT_TRUE(sensor_msgs::convertPointCloud2ToPointCloud(input, output)); + + // We should remove everything. + ASSERT_TRUE(output.points.empty()); +} + +using KeepSomeFixture = FilterFixture; +INSTANTIATE_TEST_CASE_P(/**/, KeepSomeFixture, + testing::Values( + std::make_pair(0., 1.), // keep lower first half + std::make_pair(1., 3.), // keep second half + std::make_pair(0.5, 1.5) // keep middle + )); + +TEST_P(KeepSomeFixture, regression) +{ + // Create a point cloud with different x and z values. + sensor_msgs::PointCloud points, output; + points.points.resize(100); + + double x = 0; + for (auto &p : points.points) + { + p.x = ++x; + p.z = x * 0.02; + } + + // Shuffle the points. + std::random_shuffle(points.points.begin(), points.points.end()); + + // Apply the filter. + sensor_msgs::PointCloud2 input; + sensor_msgs::convertPointCloudToPointCloud2(points, input); + const auto param = GetParam(); + costmap_2d::heightFilterPointCloud(input, param.first, param.second); + + ASSERT_TRUE(sensor_msgs::convertPointCloud2ToPointCloud(input, output)); + + // Apply the copying filter + copyingHeightFilterPointCloud(points, param.first, param.second); + + ASSERT_TRUE(equal(points, output)); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}