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