test(path_generator): add tests for turn signal activation feature

planning/autoware_path_generator/CMakeLists.txt

@@ -29,6 +29,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
     test/test_path_generator_node_interface.cpp
     test/test_lanelet.cpp
+    test/test_turn_signal.cpp
   )
   target_link_libraries(test_${PROJECT_NAME}
     ${PROJECT_NAME}

planning/autoware_path_generator/test/test_turn_signal.cpp

@@ -0,0 +1,141 @@
+// 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 "utils_test.hpp"
+
+#include <lanelet2_core/geometry/Lanelet.h>
+
+namespace autoware::path_generator
+{
+using autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
+
+struct GetTurnSignalTestParam
+{
+  std::string description;
+  double current_vel;
+  double search_distance;
+  double search_time;
+  double angle_threshold_deg;
+  double base_link_to_front;
+  std::tuple<std::vector<lanelet::Id>, double> current_position_locator;
+  uint8_t expected_turn_signal;
+};
+
+std::ostream & operator<<(std::ostream & os, const GetTurnSignalTestParam & p)
+{
+  return os << p.description;
+}
+
+struct GetTurnSignalTest : public UtilsTest,
+                           public ::testing::WithParamInterface<GetTurnSignalTestParam>
+{
+};
+
+TEST_P(GetTurnSignalTest, getTurnSignal)
+{
+  const auto & p = GetParam();
+
+  geometry_msgs::msg::Pose current_pose;
+  {
+    const auto & [lane_ids, arc_length] = p.current_position_locator;
+
+    lanelet::ConstLanelets lanelets;
+    for (const auto & lane_id : lane_ids) {
+      lanelets.push_back(planner_data_.lanelet_map_ptr->laneletLayer.get(lane_id));
+    }
+
+    const auto current_position = lanelet::geometry::interpolatedPointAtDistance(
+      lanelet::LaneletSequence(lanelets).centerline2d(), arc_length);
+
+    current_pose.position.x = current_position.x();
+    current_pose.position.y = current_position.y();
+  }
+
+  const auto result = utils::get_turn_signal(
+    path_, planner_data_, current_pose, p.current_vel, p.search_distance, p.search_time,
+    p.angle_threshold_deg, p.base_link_to_front);
+
+  ASSERT_EQ(result.command, p.expected_turn_signal);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  , GetTurnSignalTest,
+  ::testing::Values(
+    GetTurnSignalTestParam{
+      "EgoIsStoppingAndBeforeDesiredStartPoint",
+      0.0,
+      30.0,
+      3.0,
+      15.0,
+      3.79,
+      {{125}, -40.0},
+      TurnIndicatorsCommand::NO_COMMAND},
+    GetTurnSignalTestParam{
+      "EgoIsStoppingAndAheadOfDesiredStartPoint",
+      0.0,
+      30.0,
+      3.0,
+      15.0,
+      3.79,
+      {{125}, -20.0},
+      TurnIndicatorsCommand::ENABLE_RIGHT},
+    GetTurnSignalTestParam{
+      "EgoIsMovingAndAheadOfDesiredStartPoint",
+      3.5,
+      30.0,
+      3.0,
+      15.0,
+      3.79,
+      {{125}, -40.0},
+      TurnIndicatorsCommand::ENABLE_RIGHT},
+    GetTurnSignalTestParam{
+      "EgoIsInRequiredSection",
+      0.0,
+      30.0,
+      3.0,
+      15.0,
+      3.79,
+      {{50}, -10.0},
+      TurnIndicatorsCommand::ENABLE_RIGHT},
+    GetTurnSignalTestParam{
+      "EgoIsInDesiredSection",
+      0.0,
+      30.0,
+      3.0,
+      15.0,
+      3.79,
+      {{50}, -1.0},
+      TurnIndicatorsCommand::ENABLE_RIGHT},
+    GetTurnSignalTestParam{
+      "EgoIsAheadOfDesiredEndPoint",
+      0.0,
+      30.0,
+      3.0,
+      15.0,
+      3.79,
+      {{122}, 1.0},
+      TurnIndicatorsCommand::NO_COMMAND}),
+  ::testing::PrintToStringParamName{});
+
+TEST_F(UtilsTest, getTurnSignalRequiredEndPoint)
+{
+  constexpr lanelet::Id lane_id = 50;
+  constexpr double angle_threshold_deg = 15.0;
+
+  const auto result = utils::get_turn_signal_required_end_point(
+    planner_data_.lanelet_map_ptr->laneletLayer.get(lane_id), angle_threshold_deg);
+
+  ASSERT_TRUE(result);
+}
+}  // namespace autoware::path_generator