feat(autoware_test_utils): add path with lane id parser (#9098)

* add path with lane id parser

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor parse to use template

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp

@@ -19,6 +19,7 @@
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/msg/lanelet_segment.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <yaml-cpp/yaml.h>
 
@@ -30,17 +31,64 @@ namespace autoware::test_utils
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::LaneletSegment;
+using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using std_msgs::msg::Header;
+using tier4_planning_msgs::msg::PathPointWithLaneId;
+using tier4_planning_msgs::msg::PathWithLaneId;
 
-Pose parse_pose(const YAML::Node & node);
+/**
+ * @brief Parses a YAML node and converts it into an object of type T.
+ *
+ * This function extracts data from the given YAML node and converts it into an object of type T.
+ * If no specialization exists for T, it will result in a compile-time error.
+ *
+ * @tparam T The type of object to parse the node contents into.
+ * @param node The YAML node to be parsed.
+ * @return T An object of type T containing the parsed data.
+ */
+template <typename T>
+T parse(const YAML::Node & node);
 
-LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node);
+template <>
+Pose parse(const YAML::Node & node);
 
-std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node);
+template <>
+LaneletPrimitive parse(const YAML::Node & node);
 
-std::vector<LaneletSegment> parse_segments(const YAML::Node & node);
+template <>
+std::vector<LaneletPrimitive> parse(const YAML::Node & node);
 
-LaneletRoute parse_lanelet_route_file(const std::string & filename);
+template <>
+std::vector<LaneletSegment> parse(const YAML::Node & node);
+
+template <>
+std::vector<Point> parse(const YAML::Node & node);
+
+template <>
+Header parse(const YAML::Node & node);
+
+template <>
+std::vector<PathPointWithLaneId> parse(const YAML::Node & node);
+
+/**
+ * @brief Parses a YAML file and converts it into an object of type T.
+ *
+ * This function reads the specified YAML file and converts its contents into an object of the given
+ * type T. If no specialization exists for T, it will result in a compile-time error.
+ *
+ * @tparam T The type of object to parse the file contents into.
+ * @param filename The path to the YAML file to be parsed.
+ * @return T An object of type T containing the parsed data.
+ */
+template <typename T>
+T parse(const std::string & filename);
+
+template <>
+LaneletRoute parse(const std::string & filename);
+
+template <>
+PathWithLaneId parse(const std::string & filename);
 }  // namespace autoware::test_utils
 
 #endif  // AUTOWARE_TEST_UTILS__MOCK_DATA_PARSER_HPP_

common/autoware_test_utils/src/autoware_test_utils.cpp

@@ -14,6 +14,7 @@
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <autoware_test_utils/autoware_test_utils.hpp>
+#include <autoware_test_utils/mock_data_parser.hpp>
 #include <rclcpp/clock.hpp>
 #include <rclcpp/executors/single_threaded_executor.hpp>
 #include <rclcpp/logging.hpp>
@@ -301,60 +302,7 @@ PathWithLaneId loadPathWithLaneIdInYaml()
 {
   const auto yaml_path =
     get_absolute_path_to_config("autoware_test_utils", "path_with_lane_id_data.yaml");
-  YAML::Node yaml_node = YAML::LoadFile(yaml_path);
-  PathWithLaneId path_msg;
-
-  // Convert YAML data to PathWithLaneId message
-  // Fill the header
-  path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as<int>();
-  path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as<uint32_t>();
-  path_msg.header.frame_id = yaml_node["header"]["frame_id"].as<std::string>();
-
-  // Fill the points
-  for (const auto & point_node : yaml_node["points"]) {
-    PathPointWithLaneId point;
-    // Fill the PathPoint data
-    point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as<double>();
-    point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as<double>();
-    point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as<double>();
-    point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as<double>();
-    point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as<double>();
-    point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as<double>();
-    point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as<double>();
-    point.point.longitudinal_velocity_mps =
-      point_node["point"]["longitudinal_velocity_mps"].as<float>();
-    point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as<float>();
-    point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as<float>();
-    point.point.is_final = point_node["point"]["is_final"].as<bool>();
-    // Fill the lane_ids
-    for (const auto & lane_id_node : point_node["lane_ids"]) {
-      point.lane_ids.push_back(lane_id_node.as<int64_t>());
-    }
-
-    path_msg.points.push_back(point);
-  }
-
-  // Fill the left_bound
-  for (const auto & point_node : yaml_node["left_bound"]) {
-    Point point;
-    // Fill the Point data (left_bound)
-    point.x = point_node["x"].as<double>();
-    point.y = point_node["y"].as<double>();
-    point.z = point_node["z"].as<double>();
 
-    path_msg.left_bound.push_back(point);
-  }
-
-  // Fill the right_bound
-  for (const auto & point_node : yaml_node["right_bound"]) {
-    Point point;
-    // Fill the Point data
-    point.x = point_node["x"].as<double>();
-    point.y = point_node["y"].as<double>();
-    point.z = point_node["z"].as<double>();
-
-    path_msg.right_bound.push_back(point);
-  }
-  return path_msg;
+  return parse<PathWithLaneId>(yaml_path);
 }
 }  // namespace autoware::test_utils

common/autoware_test_utils/src/mock_data_parser.cpp

@@ -29,7 +29,8 @@
 
 namespace autoware::test_utils
 {
-Pose parse_pose(const YAML::Node & node)
+template <>
+Pose parse(const YAML::Node & node)
 {
   Pose pose;
   pose.position.x = node["position"]["x"].as<double>();
@@ -42,7 +43,8 @@ Pose parse_pose(const YAML::Node & node)
   return pose;
 }
 
-LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node)
+template <>
+LaneletPrimitive parse(const YAML::Node & node)
 {
   LaneletPrimitive primitive;
   primitive.id = node["id"].as<int64_t>();
@@ -51,43 +53,131 @@ LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node)
   return primitive;
 }
 
-std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node)
+template <>
+std::vector<LaneletPrimitive> parse(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 parse<LaneletPrimitive>(p);
   });
 
   return primitives;
 }
 
-std::vector<LaneletSegment> parse_segments(const YAML::Node & node)
+template <>
+std::vector<LaneletSegment> parse(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"]);
+    segment.preferred_primitive = parse<LaneletPrimitive>(input["preferred_primitive"]);
+    segment.primitives = parse<std::vector<LaneletPrimitive>>(input["primitives"]);
     return segment;
   });
 
   return segments;
 }
 
-// cppcheck-suppress unusedFunction
-LaneletRoute parse_lanelet_route_file(const std::string & filename)
+template <>
+std::vector<Point> parse(const YAML::Node & node)
+{
+  std::vector<Point> geom_points;
+
+  std::transform(
+    node.begin(), node.end(), std::back_inserter(geom_points), [&](const YAML::Node & input) {
+      Point point;
+      point.x = input["x"].as<double>();
+      point.y = input["y"].as<double>();
+      point.z = input["z"].as<double>();
+      return point;
+    });
+
+  return geom_points;
+}
+
+template <>
+Header parse(const YAML::Node & node)
+{
+  Header header;
+
+  if (!node["header"]) {
+    return header;
+  }
+
+  header.stamp.sec = node["header"]["stamp"]["sec"].as<int>();
+  header.stamp.nanosec = node["header"]["stamp"]["nanosec"].as<uint32_t>();
+  header.frame_id = node["header"]["frame_id"].as<std::string>();
+
+  return header;
+}
+
+template <>
+std::vector<PathPointWithLaneId> parse<std::vector<PathPointWithLaneId>>(const YAML::Node & node)
+{
+  std::vector<PathPointWithLaneId> path_points;
+
+  if (!node["points"]) {
+    return path_points;
+  }
+
+  const auto & points = node["points"];
+  path_points.reserve(points.size());
+  std::transform(
+    points.begin(), points.end(), std::back_inserter(path_points), [&](const YAML::Node & input) {
+      PathPointWithLaneId point;
+      if (!input["point"]) {
+        return point;
+      }
+      const auto & point_node = input["point"];
+      point.point.pose = parse<Pose>(point_node["pose"]);
+
+      point.point.longitudinal_velocity_mps = point_node["longitudinal_velocity_mps"].as<float>();
+      point.point.lateral_velocity_mps = point_node["lateral_velocity_mps"].as<float>();
+      point.point.heading_rate_rps = point_node["heading_rate_rps"].as<float>();
+      point.point.is_final = point_node["is_final"].as<bool>();
+      // Fill the lane_ids
+      for (const auto & lane_id_node : input["lane_ids"]) {
+        point.lane_ids.push_back(lane_id_node.as<int64_t>());
+      }
+
+      return point;
+    });
+
+  return path_points;
+}
+
+template <>
+LaneletRoute parse(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"]);
+    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<std::vector<LaneletSegment>>(config["segments"]);
   } catch (const std::exception & e) {
     RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what());
   }
   return lanelet_route;
 }
+
+template <>
+PathWithLaneId parse(const std::string & filename)
+{
+  PathWithLaneId path;
+  try {
+    YAML::Node yaml_node = YAML::LoadFile(filename);
+
+    path.header = parse<Header>(yaml_node);
+    path.points = parse<std::vector<PathPointWithLaneId>>(yaml_node);
+    path.left_bound = parse<std::vector<Point>>(yaml_node["left_bound"]);
+    path.right_bound = parse<std::vector<Point>>(yaml_node["right_bound"]);
+  } catch (const std::exception & e) {
+    RCLCPP_DEBUG(rclcpp::get_logger("autoware_test_utils"), "Exception caught: %s", e.what());
+  }
+
+  return path;
+}
 }  // namespace autoware::test_utils