feat(route_handler): add unit tests

planning/planning_test_utils/CMakeLists.txt

@@ -4,7 +4,23 @@ project(planning_test_utils)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+ament_auto_add_library(mock_data_parser SHARED
+  src/mock_data_parser.cpp)
+
+target_link_libraries(mock_data_parser
+  yaml-cpp
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_mock_data_parser
+    test/test_mock_data_parser.cpp)
+
+  target_link_libraries(test_mock_data_parser
+    mock_data_parser)
+endif()
+
 ament_auto_package(INSTALL_TO_SHARE
   config
   test_map
+  test_data
 )

planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp

@@ -0,0 +1,46 @@
+// Copyright 2024 TIER IV
+//
+// 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 PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_
+#define PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_
+
+#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <yaml-cpp/yaml.h>
+
+#include <string>
+#include <vector>
+
+namespace test_utils
+{
+using autoware_planning_msgs::msg::LaneletPrimitive;
+using autoware_planning_msgs::msg::LaneletRoute;
+using autoware_planning_msgs::msg::LaneletSegment;
+using geometry_msgs::msg::Pose;
+
+Pose parse_pose(const YAML::Node & node);
+
+LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node);
+
+std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node);
+
+std::vector<LaneletSegment> parse_segments(const YAML::Node & node);
+
+LaneletRoute parse_lanelet_route_file(const std::string & filename);
+}  // namespace test_utils
+
+#endif  // PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_

planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp

@@ -193,14 +193,10 @@ OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double res
   return costmap_msg;
 }
 
-HADMapBin makeMapBinMsg()
+HADMapBin make_map_bin_msg(
+  const std::string & absolute_path, const double center_line_resolution = 5.0)
 {
-  const auto planning_test_utils_dir =
-    ament_index_cpp::get_package_share_directory("planning_test_utils");
-  const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm";
-  double center_line_resolution = 5.0;
-  // load map from file
-  const auto map = loadMap(lanelet2_path);
+  const auto map = loadMap(absolute_path);
   if (!map) {
     return autoware_auto_mapping_msgs::msg::HADMapBin_<std::allocator<void>>{};
   }
@@ -210,10 +206,20 @@ HADMapBin makeMapBinMsg()
 
   // create map bin msg
   const auto map_bin_msg =
-    convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now());
+    convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now());
   return map_bin_msg;
 }
 
+HADMapBin makeMapBinMsg()
+{
+  const auto planning_test_utils_dir =
+    ament_index_cpp::get_package_share_directory("planning_test_utils");
+  const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm";
+  double center_line_resolution = 5.0;
+
+  return make_map_bin_msg(lanelet2_path, center_line_resolution);
+}
+
 Odometry makeOdometry(const double shift = 0.0)
 {
   Odometry odometry;

planning/planning_test_utils/src/mock_data_parser.cpp

@@ -0,0 +1,92 @@
+// Copyright 2024 TIER IV
+//
+// 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 "planning_test_utils/mock_data_parser.hpp"
+
+#include <rclcpp/logging.hpp>
+
+#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <yaml-cpp/yaml.h>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+namespace test_utils
+{
+Pose parse_pose(const YAML::Node & node)
+{
+  Pose pose;
+  pose.position.x = node["position"]["x"].as<double>();
+  pose.position.y = node["position"]["y"].as<double>();
+  pose.position.z = node["position"]["z"].as<double>();
+  pose.orientation.x = node["orientation"]["x"].as<double>();
+  pose.orientation.y = node["orientation"]["y"].as<double>();
+  pose.orientation.z = node["orientation"]["z"].as<double>();
+  pose.orientation.w = node["orientation"]["w"].as<double>();
+  return pose;
+}
+
+LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node)
+{
+  LaneletPrimitive primitive;
+  primitive.id = node["id"].as<int64_t>();
+  primitive.primitive_type = node["primitive_type"].as<std::string>();
+
+  return primitive;
+}
+
+std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node)
+{
+  std::vector<LaneletPrimitive> primitives;
+  primitives.reserve(node.size());
+  std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) {
+    return parse_lanelet_primitive(p);
+  });
+
+  return primitives;
+}
+
+std::vector<LaneletSegment> parse_segments(const YAML::Node & node)
+{
+  std::vector<LaneletSegment> segments;
+  std::transform(node.begin(), node.end(), std::back_inserter(segments), [&](const auto & input) {
+    LaneletSegment segment;
+    segment.preferred_primitive = parse_lanelet_primitive(input["preferred_primitive"]);
+    segment.primitives = parse_lanelet_primitives(input["primitives"]);
+    return segment;
+  });
+
+  return segments;
+}
+
+LaneletRoute parse_lanelet_route_file(const std::string & filename)
+{
+  LaneletRoute lanelet_route;
+  try {
+    YAML::Node config = YAML::LoadFile(filename);
+
+    lanelet_route.start_pose = (config["start_pose"]) ? parse_pose(config["start_pose"]) : Pose();
+    lanelet_route.goal_pose = (config["goal_pose"]) ? parse_pose(config["goal_pose"]) : Pose();
+    lanelet_route.segments = parse_segments(config["segments"]);
+  } catch (const std::exception & e) {
+    RCLCPP_DEBUG(rclcpp::get_logger("planning_test_utils"), "Exception caught: %s", e.what());
+  }
+  return lanelet_route;
+}
+}  // namespace test_utils

planning/planning_test_utils/test/test_mock_data_parser.cpp

@@ -0,0 +1,134 @@
+// Copyright 2024 TIER IV
+//
+// 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 <gtest/gtest.h>
+
+// Assuming the parseRouteFile function is in 'RouteHandler.h'
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "planning_test_utils/mock_data_parser.hpp"
+
+namespace test_utils
+{
+// Example YAML structure as a string for testing
+const char g_complete_yaml[] = R"(
+start_pose:
+  position:
+    x: 1.0
+    y: 2.0
+    z: 3.0
+  orientation:
+    x: 0.1
+    y: 0.2
+    z: 0.3
+    w: 0.4
+goal_pose:
+  position:
+    x: 4.0
+    y: 5.0
+    z: 6.0
+  orientation:
+    x: 0.5
+    y: 0.6
+    z: 0.7
+    w: 0.8
+segments:
+- preferred_primitive:
+    id: 11
+    primitive_type: ''
+  primitives:
+    - id: 22
+      primitive_type: lane
+    - id: 33
+      primitive_type: lane
+)";
+
+TEST(ParseFunctions, CompleteYAMLTest)
+{
+  YAML::Node config = YAML::Load(g_complete_yaml);
+
+  // Test parsing of start_pose and goal_pose
+  Pose start_pose = parse_pose(config["start_pose"]);
+  Pose goal_pose = parse_pose(config["goal_pose"]);
+
+  EXPECT_DOUBLE_EQ(start_pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(start_pose.position.y, 2.0);
+  EXPECT_DOUBLE_EQ(start_pose.position.z, 3.0);
+
+  EXPECT_DOUBLE_EQ(start_pose.orientation.x, 0.1);
+  EXPECT_DOUBLE_EQ(start_pose.orientation.y, 0.2);
+  EXPECT_DOUBLE_EQ(start_pose.orientation.z, 0.3);
+  EXPECT_DOUBLE_EQ(start_pose.orientation.w, 0.4);
+
+  EXPECT_DOUBLE_EQ(goal_pose.position.x, 4.0);
+  EXPECT_DOUBLE_EQ(goal_pose.position.y, 5.0);
+  EXPECT_DOUBLE_EQ(goal_pose.position.z, 6.0);
+  EXPECT_DOUBLE_EQ(goal_pose.orientation.x, 0.5);
+  EXPECT_DOUBLE_EQ(goal_pose.orientation.y, 0.6);
+  EXPECT_DOUBLE_EQ(goal_pose.orientation.z, 0.7);
+  EXPECT_DOUBLE_EQ(goal_pose.orientation.w, 0.8);
+
+  // Test parsing of segments
+  std::vector<LaneletSegment> segments = parse_segments(config["segments"]);
+  ASSERT_EQ(segments.size(), 1);  // Assuming only one segment in the provided YAML for this test
+
+  const auto & segment0 = segments[0];
+  EXPECT_EQ(segment0.preferred_primitive.id, 11);
+  EXPECT_EQ(segment0.primitives.size(), 2);
+  EXPECT_EQ(segment0.primitives[0].id, 22);
+  EXPECT_EQ(segment0.primitives[0].primitive_type, "lane");
+  EXPECT_EQ(segment0.primitives[1].id, 33);
+  EXPECT_EQ(segment0.primitives[1].primitive_type, "lane");
+}
+
+TEST(ParseFunction, CompleteFromFilename)
+{
+  const auto planning_test_utils_dir =
+    ament_index_cpp::get_package_share_directory("planning_test_utils");
+  const auto parser_test_route =
+    planning_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml";
+
+  const auto lanelet_route = parse_lanelet_route_file(parser_test_route);
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0);
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0);
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0);
+
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1);
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2);
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3);
+  EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4);
+
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0);
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0);
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0);
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5);
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6);
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7);
+  EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8);
+
+  ASSERT_EQ(
+    lanelet_route.segments.size(),
+    2);  // Assuming only one segment in the provided YAML for this test
+  const auto & segment1 = lanelet_route.segments[1];
+  EXPECT_EQ(segment1.preferred_primitive.id, 44);
+  EXPECT_EQ(segment1.primitives.size(), 4);
+  EXPECT_EQ(segment1.primitives[0].id, 55);
+  EXPECT_EQ(segment1.primitives[0].primitive_type, "lane");
+  EXPECT_EQ(segment1.primitives[1].id, 66);
+  EXPECT_EQ(segment1.primitives[1].primitive_type, "lane");
+  EXPECT_EQ(segment1.primitives[2].id, 77);
+  EXPECT_EQ(segment1.primitives[2].primitive_type, "lane");
+  EXPECT_EQ(segment1.primitives[3].id, 88);
+  EXPECT_EQ(segment1.primitives[3].primitive_type, "lane");
+}
+}  // namespace test_utils

planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml

@@ -0,0 +1,41 @@
+start_pose:
+  position:
+    x: 1.0
+    y: 2.0
+    z: 3.0
+  orientation:
+    x: 0.1
+    y: 0.2
+    z: 0.3
+    w: 0.4
+goal_pose:
+  position:
+    x: 4.0
+    y: 5.0
+    z: 6.0
+  orientation:
+    x: 0.5
+    y: 0.6
+    z: 0.7
+    w: 0.8
+segments:
+  - preferred_primitive:
+      id: 11
+      primitive_type: ""
+    primitives:
+      - id: 22
+        primitive_type: lane
+      - id: 33
+        primitive_type: lane
+  - preferred_primitive:
+      id: 44
+      primitive_type: ""
+    primitives:
+      - id: 55
+        primitive_type: lane
+      - id: 66
+        primitive_type: lane
+      - id: 77
+        primitive_type: lane
+      - id: 88
+        primitive_type: lane

planning/route_handler/CMakeLists.txt

@@ -8,4 +8,16 @@ ament_auto_add_library(route_handler SHARED
   src/route_handler.cpp
 )
 
-ament_auto_package()
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_route_handler
+    test/test_route_handler.cpp)
+
+  target_link_libraries(test_route_handler
+    route_handler
+  )
+
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  test_route
+)

planning/route_handler/README.md

@@ -1,3 +1,18 @@
 # route handler
 
 `route_handler` is a library for calculating driving route on the lanelet map.
+
+## Unit Testing
+
+The unit testing depends on `planning_test_utils` package.
+`planning_test_utils` is a library that provides several common functions to simplify unit test creating.
+
+![route_handler_test](./images/route_handler_test.svg)
+
+By default, route file is necessary to create tests. The following illustrates the route that are used in the unit test
+
+### Lane change test route
+
+![lane_change_test_route](./images/lane_change_test_route.svg)
+
+- The route is based on map that can be obtained from `planning_test_utils\test_map`

planning/route_handler/package.xml

@@ -18,6 +18,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
@@ -26,10 +27,12 @@
   <depend>autoware_planning_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
+  <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>yaml-cpp</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

planning/route_handler/test/test_route_handler.cpp

@@ -0,0 +1,110 @@
+// Copyright 2024 TIER IV
+//
+// 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 "test_route_handler.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+namespace route_handler::test
+{
+TEST_F(TestRouteHandler, isRouteHandlerReadyTest)
+{
+  ASSERT_TRUE(route_handler_->isHandlerReady());
+}
+
+TEST_F(TestRouteHandler, checkIfIDReturned)
+{
+  const auto lanelet1 = route_handler_->getLaneletsFromId(4785);
+  const auto is_lanelet1_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet1);
+  ASSERT_TRUE(is_lanelet1_in_goal_route_section);
+
+  const auto lanelet2 = route_handler_->getLaneletsFromId(4780);
+  const auto is_lanelet2_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet2);
+  ASSERT_FALSE(is_lanelet2_in_goal_route_section);
+}
+
+TEST_F(TestRouteHandler, getGoalLaneId)
+{
+  lanelet::ConstLanelet goal_lane;
+
+  const auto goal_lane_obtained = route_handler_->getGoalLanelet(&goal_lane);
+  ASSERT_TRUE(goal_lane_obtained);
+  ASSERT_EQ(goal_lane.id(), 5088);
+}
+
+// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute)
+// {
+//   lanelet::ConstLanelet closest_lane;
+
+//   Pose search_pose;
+
+//   search_pose.position = tier4_autoware_utils::createPoint(-1.0, 1.75, 0);
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//   const auto closest_lane_obtained7 =
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained7);
+//   ASSERT_EQ(closest_lane.id(), 4775);
+
+//   search_pose.position = tier4_autoware_utils::createPoint(-0.5, 1.75, 0);
+//   const auto closest_lane_obtained =
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained);
+//   ASSERT_EQ(closest_lane.id(), 4775);
+
+//   search_pose.position = tier4_autoware_utils::createPoint(-.01, 1.75, 0);
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//   const auto closest_lane_obtained3 =
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained3);
+//   ASSERT_EQ(closest_lane.id(), 4775);
+
+//   search_pose.position = tier4_autoware_utils::createPoint(0.0, 1.75, 0);
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//   const auto closest_lane_obtained1 =
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained1);
+//   ASSERT_EQ(closest_lane.id(), 4775);
+
+//   search_pose.position = tier4_autoware_utils::createPoint(0.01, 1.75, 0);
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//   const auto closest_lane_obtained2 =
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained2);
+//   ASSERT_EQ(closest_lane.id(), 4424);
+
+//   search_pose.position = tier4_autoware_utils::createPoint(0.5, 1.75, 0);
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//   const auto closest_lane_obtained4 =
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained4);
+//   ASSERT_EQ(closest_lane.id(), 4424);
+
+//   search_pose.position = tier4_autoware_utils::createPoint(1.0, 1.75, 0);
+//   search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+//   const auto closest_lane_obtained5 =
+//     route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);
+
+//   ASSERT_TRUE(closest_lane_obtained5);
+//   ASSERT_EQ(closest_lane.id(), 4424);
+// }
+}  // namespace route_handler::test

planning/route_handler/test/test_route_handler.hpp

@@ -0,0 +1,73 @@
+// Copyright 2024 TIER IV
+//
+// 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 TEST_ROUTE_HANDLER_HPP_
+#define TEST_ROUTE_HANDLER_HPP_
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "gtest/gtest.h"
+#include "planning_test_utils/mock_data_parser.hpp"
+#include "planning_test_utils/planning_test_utils.hpp"
+
+#include <lanelet2_extension/io/autoware_osm_parser.hpp>
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/logging.hpp>
+#include <route_handler/route_handler.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+
+#include <lanelet2_io/Io.h>
+
+#include <memory>
+#include <string>
+namespace route_handler::test
+{
+
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+
+class TestRouteHandler : public ::testing::Test
+{
+public:
+  TestRouteHandler()
+  {
+    const auto planning_test_utils_dir =
+      ament_index_cpp::get_package_share_directory("planning_test_utils");
+    const auto lanelet2_path = planning_test_utils_dir + "/test_map/2km_test.osm";
+    constexpr double center_line_resolution = 5.0;
+    const auto map_bin_msg = test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution);
+    route_handler_ = std::make_shared<RouteHandler>(map_bin_msg);
+    set_lane_change_test_route();
+  }
+
+  TestRouteHandler(const TestRouteHandler &) = delete;
+  TestRouteHandler(TestRouteHandler &&) = delete;
+  TestRouteHandler & operator=(const TestRouteHandler &) = delete;
+  TestRouteHandler & operator=(TestRouteHandler &&) = delete;
+  ~TestRouteHandler() override = default;
+
+  void set_lane_change_test_route()
+  {
+    const auto route_handler_dir = ament_index_cpp::get_package_share_directory("route_handler");
+    const auto rh_test_route = route_handler_dir + "/test_route/lane_change_test_route.yaml";
+    route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route));
+  }
+
+  std::shared_ptr<RouteHandler> route_handler_;
+};
+}  // namespace route_handler::test
+
+#endif  // TEST_ROUTE_HANDLER_HPP_

planning/route_handler/test_route/lane_change_test_route.yaml

@@ -0,0 +1,93 @@
+header:
+  stamp:
+    sec: 1715936953
+    nanosec: 67206425
+  frame_id: map
+start_pose:
+  position:
+    x: -50.0
+    y: 1.75
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 1.0
+goal_pose:
+  position:
+    x: 70.0
+    y: -1.75
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 1.0
+segments:
+  - preferred_primitive:
+      id: 4765
+      primitive_type: ""
+    primitives:
+      - id: 4765
+        primitive_type: lane
+      - id: 9590
+        primitive_type: lane
+  - preferred_primitive:
+      id: 4770
+      primitive_type: ""
+    primitives:
+      - id: 4770
+        primitive_type: lane
+      - id: 5072
+        primitive_type: lane
+  - preferred_primitive:
+      id: 4775
+      primitive_type: ""
+    primitives:
+      - id: 4775
+        primitive_type: lane
+      - id: 9594
+        primitive_type: lane
+  - preferred_primitive:
+      id: 9598
+      primitive_type: ""
+    primitives:
+      - id: 4424
+        primitive_type: lane
+      - id: 9598
+        primitive_type: lane
+  - preferred_primitive:
+      id: 5084
+      primitive_type: ""
+    primitives:
+      - id: 4780
+        primitive_type: lane
+      - id: 5084
+        primitive_type: lane
+  - preferred_primitive:
+      id: 5088
+      primitive_type: ""
+    primitives:
+      - id: 4785
+        primitive_type: lane
+      - id: 5088
+        primitive_type: lane
+uuid:
+  uuid:
+    - 231
+    - 254
+    - 143
+    - 227
+    - 194
+    - 8
+    - 220
+    - 88
+    - 30
+    - 194
+    - 172
+    - 147
+    - 127
+    - 143
+    - 176
+    - 133
+allow_modification: false