From f9245bb490bdb6ca602b02c9e7d04d0a9f513a82 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Fri, 13 May 2022 13:50:09 +0900 Subject: [PATCH] feat(map_tf_generator): accelerate the 'viewer' coordinate calculation (#890) * add random point sampling function to quickly calculate the 'viewer' coordinate Signed-off-by: IshitaTakeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Shumpei Wakabayashi --- map/map_tf_generator/CMakeLists.txt | 17 ++++++++ .../map_tf_generator/uniform_random.hpp | 33 ++++++++++++++ map/map_tf_generator/package.xml | 1 + .../src/map_tf_generator_node.cpp | 18 +++++--- .../test/test_uniform_random.cpp | 43 +++++++++++++++++++ 5 files changed, 107 insertions(+), 5 deletions(-) create mode 100644 map/map_tf_generator/include/map_tf_generator/uniform_random.hpp create mode 100644 map/map_tf_generator/test/test_uniform_random.cpp diff --git a/map/map_tf_generator/CMakeLists.txt b/map/map_tf_generator/CMakeLists.txt index 1170dfef00474..506f763c4bb36 100644 --- a/map/map_tf_generator/CMakeLists.txt +++ b/map/map_tf_generator/CMakeLists.txt @@ -16,6 +16,23 @@ rclcpp_components_register_node(map_tf_generator_node EXECUTABLE map_tf_generator ) + +if(BUILD_TESTING) + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gmock(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + add_testcase(test/test_uniform_random.cpp) +endif() + ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp new file mode 100644 index 0000000000000..345703a19bc06 --- /dev/null +++ b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp @@ -0,0 +1,33 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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. + +#ifndef MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ +#define MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ + +#include +#include + +std::vector UniformRandom(const size_t max_exclusive, const size_t n) +{ + std::default_random_engine generator; + std::uniform_int_distribution distribution(0, max_exclusive - 1); + + std::vector v(n); + for (size_t i = 0; i < n; i++) { + v[i] = distribution(generator); + } + return v; +} + +#endif // MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_ diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 9aaf14bc5fda7..3adcf80690668 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -19,6 +19,7 @@ tf2 tf2_ros + ament_cmake_gmock ament_lint_auto autoware_lint_common diff --git a/map/map_tf_generator/src/map_tf_generator_node.cpp b/map/map_tf_generator/src/map_tf_generator_node.cpp index 378d739ffbef3..b99514f18e658 100644 --- a/map/map_tf_generator/src/map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/map_tf_generator_node.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "map_tf_generator/uniform_random.hpp" + #include #include @@ -25,6 +27,8 @@ #include #include +constexpr size_t N_SAMPLES = 20; + class MapTFGeneratorNode : public rclcpp::Node { public: @@ -51,19 +55,23 @@ class MapTFGeneratorNode : public rclcpp::Node void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) { + // fix random seed to produce the same viewer position every time + // 3939 is just the author's favorite number + srand(3939); + PointCloud clouds; pcl::fromROSMsg(*clouds_ros, clouds); - const unsigned int sum = clouds.points.size(); + const std::vector indices = UniformRandom(clouds.size(), N_SAMPLES); double coordinate[3] = {0, 0, 0}; - for (unsigned int i = 0; i < sum; i++) { + for (const auto i : indices) { coordinate[0] += clouds.points[i].x; coordinate[1] += clouds.points[i].y; coordinate[2] += clouds.points[i].z; } - coordinate[0] = coordinate[0] / sum; - coordinate[1] = coordinate[1] / sum; - coordinate[2] = coordinate[2] / sum; + coordinate[0] = coordinate[0] / indices.size(); + coordinate[1] = coordinate[1] / indices.size(); + coordinate[2] = coordinate[2] / indices.size(); geometry_msgs::msg::TransformStamped static_transformStamped; static_transformStamped.header.stamp = this->now(); diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/map_tf_generator/test/test_uniform_random.cpp new file mode 100644 index 0000000000000..76ee174a34871 --- /dev/null +++ b/map/map_tf_generator/test/test_uniform_random.cpp @@ -0,0 +1,43 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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 "map_tf_generator/uniform_random.hpp" + +#include + +using testing::AllOf; +using testing::Each; +using testing::Ge; +using testing::Lt; + +TEST(UniformRandom, UniformRandom) +{ + { + const std::vector random = UniformRandom(4, 0); + ASSERT_EQ(random.size(), static_cast(0)); + } + + // checks if the returned values are in range of [min, max) + // note that the minimun range is always zero and the max value is exclusive + { + const size_t min_inclusive = 0; + const size_t max_exclusive = 4; + + for (int i = 0; i < 50; i++) { + const std::vector random = UniformRandom(4, 10); + ASSERT_EQ(random.size(), 10U); + ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive)))); // in range [0, 4) + } + } +}