diff --git a/costmap_cspace/include/costmap_cspace/pointcloud_accumulator.h b/costmap_cspace/include/costmap_cspace/pointcloud_accumulator.h index 722fdafa7..f7d5b41b5 100644 --- a/costmap_cspace/include/costmap_cspace/pointcloud_accumulator.h +++ b/costmap_cspace/include/costmap_cspace/pointcloud_accumulator.h @@ -37,7 +37,7 @@ namespace costmap_cspace { template -class PointcloudAccumurator +class PointcloudAccumulator { public: class Points : public T @@ -52,11 +52,11 @@ class PointcloudAccumurator } }; - PointcloudAccumurator() + PointcloudAccumulator() { } - explicit PointcloudAccumurator(const ros::Duration& duration) + explicit PointcloudAccumulator(const ros::Duration& duration) { reset(duration); } @@ -108,6 +108,11 @@ class PointcloudAccumurator ros::Duration time_to_hold_; std::list points_; }; + +// to keep backward compatibility with code base using PointcloudAccumurator +template +using PointcloudAccumurator + [[deprecated("Use costmap_cspace::PointcloudAccumulator instead.")]] = PointcloudAccumulator; } // namespace costmap_cspace #endif // COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_H diff --git a/costmap_cspace/src/laserscan_to_map.cpp b/costmap_cspace/src/laserscan_to_map.cpp index bfdae3b14..ed8f77ed4 100644 --- a/costmap_cspace/src/laserscan_to_map.cpp +++ b/costmap_cspace/src/laserscan_to_map.cpp @@ -66,7 +66,7 @@ class LaserscanToMapNode float origin_x_; float origin_y_; - costmap_cspace::PointcloudAccumurator accum_; + costmap_cspace::PointcloudAccumulator accum_; public: LaserscanToMapNode() diff --git a/costmap_cspace/src/pointcloud2_to_map.cpp b/costmap_cspace/src/pointcloud2_to_map.cpp index 46981ebf9..2d4ec5c9d 100644 --- a/costmap_cspace/src/pointcloud2_to_map.cpp +++ b/costmap_cspace/src/pointcloud2_to_map.cpp @@ -65,7 +65,7 @@ class Pointcloud2ToMapNode float origin_x_; float origin_y_; - std::vector> accums_; + std::vector> accums_; public: Pointcloud2ToMapNode() @@ -130,7 +130,7 @@ class Pointcloud2ToMapNode tf2::doTransform(*cloud, cloud_global, trans); const int buffer = singleshot ? 1 : 0; - accums_[buffer].push(costmap_cspace::PointcloudAccumurator::Points( + accums_[buffer].push(costmap_cspace::PointcloudAccumulator::Points( cloud_global, cloud_global.header.stamp)); ros::Time now = cloud->header.stamp; diff --git a/costmap_cspace/test/CMakeLists.txt b/costmap_cspace/test/CMakeLists.txt index eb8dd433e..ade366800 100644 --- a/costmap_cspace/test/CMakeLists.txt +++ b/costmap_cspace/test/CMakeLists.txt @@ -1,2 +1,5 @@ catkin_add_gtest(test_costmap_3d src/test_costmap_3d.cpp) target_link_libraries(test_costmap_3d ${catkin_LIBRARIES}) + +catkin_add_gtest(test_pointcloud_accumulator src/test_pointcloud_accumulator.cpp) +target_link_libraries(test_pointcloud_accumulator ${catkin_LIBRARIES}) diff --git a/costmap_cspace/test/src/test_pointcloud_accumulator.cpp b/costmap_cspace/test/src/test_pointcloud_accumulator.cpp new file mode 100644 index 000000000..5f646fa94 --- /dev/null +++ b/costmap_cspace/test/src/test_pointcloud_accumulator.cpp @@ -0,0 +1,132 @@ +/* + * Copyright (c) 2021, the neonavigation authors + * All rights reserved. + * + * 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 copyright holder 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 + +void fillInPointcloudMsg(sensor_msgs::PointCloud2& cloud, const std::initializer_list& points) +{ + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2Fields( + 3, + "x", 1, sensor_msgs::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32); + modifier.resize(points.size() / 3); + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; + cloud.width = points.size(); + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + std::initializer_list::iterator it; + for (it = points.begin(); it != points.end(); it += 3) + { + *iter_x = *it; + *iter_y = *(it + 1); + *iter_z = *(it + 2); + ++iter_x; + ++iter_y; + ++iter_z; + } +} + +TEST(PointcloudAccumulator, PushPointCloud) +{ + ros::Duration accum_duration(1.0); + costmap_cspace::PointcloudAccumulator accum(accum_duration); + + ros::Time stamp(0); + ros::Duration dt(0.25); + sensor_msgs::PointCloud2 cloud; + for (int i = 0; i < 10; i++) + { + fillInPointcloudMsg(cloud, {static_cast(i), 0, 0}); // NOLINT + cloud.header.stamp = stamp; + accum.push(costmap_cspace::PointcloudAccumulator::Points(cloud, stamp)); + // check the number of clouds accumulated so far + ASSERT_EQ(i < 5 ? i + 1 : 5, std::distance(accum.begin(), accum.end())); + stamp += dt; + } + + // check the timestamp difference between the oldest and latest clouds in the accumulator + const auto& oldest = accum.begin(); + const auto& latest = std::prev(accum.end()); + ASSERT_LE(latest->header.stamp - oldest->header.stamp, accum_duration); + + // check the content of the clouds + float expected_xs[] = {5.0, 6.0, 7.0, 8.0, 9.0}; + int idx = 0; + for (auto& pc : accum) + { + sensor_msgs::PointCloud2Iterator it_x(pc, "x"); + ASSERT_EQ(expected_xs[idx++], *it_x); + } + + // check the accumulator is empty after clearing it + accum.clear(); + ASSERT_EQ(0, std::distance(accum.begin(), accum.end())); + + // check the accumulation difference is properly updated after calling reset + accum.reset(ros::Duration(2.0)); + for (int i = 0; i < 10; i++) + { + fillInPointcloudMsg(cloud, {static_cast(i), 0, 0}); // NOLINT + cloud.header.stamp = stamp; + accum.push(costmap_cspace::PointcloudAccumulator::Points(cloud, stamp)); + stamp += dt; + } + ASSERT_EQ(9, std::distance(accum.begin(), accum.end())); +} + +TEST(PointcloudAccumulator, BackwardCompatibility) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + // check accumulator can be instantiated using the PointcloudAccumurator class + costmap_cspace::PointcloudAccumurator accum; + accum.reset(ros::Duration(1.0)); +#pragma GCC diagnostic pop +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +}