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

autowarefoundation#890)

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

Signed-off-by: IshitaTakeshi <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kenji Miyake <[email protected]>
Signed-off-by: Shumpei Wakabayashi <[email protected]>

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)
+ }
+ }
+}