test(crosswalk): add unit test (#9228)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt

@@ -10,6 +10,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src
 )
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_crosswalk.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+endif()
+
 ament_auto_package(INSTALL_TO_SHARE config)
 
 install(PROGRAMS

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml

@@ -45,6 +45,7 @@
   <depend>tier4_debug_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -62,6 +62,10 @@ using tier4_planning_msgs::msg::PathWithLaneId;
 
 namespace
 {
+/**
+ * @param x_vec Strictly monotone increasing is required.
+ * @param y_vec The same number of elements as x_vec is required.
+ */
 double interpolateEgoPassMargin(
   const std::vector<double> & x_vec, const std::vector<double> & y_vec, const double target_x)
 {
@@ -77,6 +81,10 @@ double interpolateEgoPassMargin(
   return y_vec.back();
 }
 
+/**
+ * @param key_map Strictly monotone increasing should be satisfied.
+ * @param value_map The same number of elements as key_map is required.
+ */
 double InterpolateMap(
   const std::vector<double> & key_map, const std::vector<double> & value_map, const double query)
 {

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_crosswalk.cpp

@@ -0,0 +1,49 @@
+// Copyright 2024 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 "../src/scene_crosswalk.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(CrosswalkTest, CheckInterpolateEgoPassMargin)
+{
+  namespace bvp = autoware::behavior_velocity_planner;
+
+  std::vector<double> x_vec{0.0, 1.0, 10.0};
+  std::vector<double> y_vec{0.0, -1.0, 8.0};
+
+  EXPECT_EQ(0.0, bvp::interpolateEgoPassMargin(x_vec, y_vec, -100.0));
+  EXPECT_EQ(0.0, bvp::interpolateEgoPassMargin(x_vec, y_vec, 0.0));
+  EXPECT_EQ(-0.5, bvp::interpolateEgoPassMargin(x_vec, y_vec, 0.5));
+  EXPECT_EQ(-1.0, bvp::interpolateEgoPassMargin(x_vec, y_vec, 1.0));
+  EXPECT_EQ(-0.5, bvp::interpolateEgoPassMargin(x_vec, y_vec, 1.5));
+  EXPECT_EQ(8.0, bvp::interpolateEgoPassMargin(x_vec, y_vec, 10.0));
+  EXPECT_EQ(8.0, bvp::interpolateEgoPassMargin(x_vec, y_vec, 100.0));
+}
+
+TEST(CrosswalkTest, CheckInterpolateMap)
+{
+  namespace bvp = autoware::behavior_velocity_planner;
+
+  std::vector<double> x_vec{0.0, 1.0, 10.0};
+  std::vector<double> y_vec{0.0, -1.0, 8.0};
+
+  EXPECT_EQ(0.0, bvp::InterpolateMap(x_vec, y_vec, -100.0));
+  EXPECT_EQ(0.0, bvp::InterpolateMap(x_vec, y_vec, 0.0));
+  EXPECT_EQ(-0.5, bvp::InterpolateMap(x_vec, y_vec, 0.5));
+  EXPECT_EQ(-1.0, bvp::InterpolateMap(x_vec, y_vec, 1.0));
+  EXPECT_EQ(-0.5, bvp::InterpolateMap(x_vec, y_vec, 1.5));
+  EXPECT_EQ(8.0, bvp::InterpolateMap(x_vec, y_vec, 10.0));
+  EXPECT_EQ(8.0, bvp::InterpolateMap(x_vec, y_vec, 100.0));
+}