feat(map_tf_generator): accelerate the 'viewer' coordinate calculation (

tier4#890)

* add random point sampling function to quickly calculate the 'viewer' coordinate

Signed-off-by: IshitaTakeshi <ishitah.takeshi@gmail.com>
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>

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
 )

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 <random>
+#include <vector>
+
+std::vector<size_t> UniformRandom(const size_t max_exclusive, const size_t n)
+{
+  std::default_random_engine generator;
+  std::uniform_int_distribution<size_t> distribution(0, max_exclusive - 1);
+
+  std::vector<size_t> v(n);
+  for (size_t i = 0; i < n; i++) {
+    v[i] = distribution(generator);
+  }
+  return v;
+}
+
+#endif  // MAP_TF_GENERATOR__UNIFORM_RANDOM_HPP_

map/map_tf_generator/package.xml

@@ -19,6 +19,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

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 <rclcpp/rclcpp.hpp>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -25,6 +27,8 @@
 #include <memory>
 #include <string>
 
+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<pcl::PointXYZ>(*clouds_ros, clouds);
 
-    const unsigned int sum = clouds.points.size();
+    const std::vector<size_t> 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();

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 <gmock/gmock.h>
+
+using testing::AllOf;
+using testing::Each;
+using testing::Ge;
+using testing::Lt;
+
+TEST(UniformRandom, UniformRandom)
+{
+  {
+    const std::vector<size_t> random = UniformRandom(4, 0);
+    ASSERT_EQ(random.size(), static_cast<size_t>(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<size_t> random = UniformRandom(4, 10);
+      ASSERT_EQ(random.size(), 10U);
+      ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive))));  // in range [0, 4)
+    }
+  }
+}