From 41d6312a9edb95b73e41c905bbb753e17011bd27 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 23 Oct 2024 08:07:42 +0900 Subject: [PATCH] feat(autoware_test_utils): add path with lane id parser (#9098) * add path with lane id parser Signed-off-by: Zulfaqar Azmi * refactor parse to use template Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../autoware_test_utils/mock_data_parser.hpp | 58 ++++++++- .../src/autoware_test_utils.cpp | 56 +-------- .../src/mock_data_parser.cpp | 114 ++++++++++++++++-- .../test/test_mock_data_parser.cpp | 61 +++++++++- .../path_with_lane_id_parser_test.yaml | 48 ++++++++ .../test/test_route_handler.hpp | 2 +- .../test/test_lane_change_scene.cpp | 5 +- 7 files changed, 267 insertions(+), 77 deletions(-) create mode 100644 common/autoware_test_utils/test_data/path_with_lane_id_parser_test.yaml diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 47284da447133..84e001aa8806e 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include @@ -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 +T parse(const YAML::Node & node); -LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node); +template <> +Pose parse(const YAML::Node & node); -std::vector parse_lanelet_primitives(const YAML::Node & node); +template <> +LaneletPrimitive parse(const YAML::Node & node); -std::vector parse_segments(const YAML::Node & node); +template <> +std::vector parse(const YAML::Node & node); -LaneletRoute parse_lanelet_route_file(const std::string & filename); +template <> +std::vector parse(const YAML::Node & node); + +template <> +std::vector parse(const YAML::Node & node); + +template <> +Header parse(const YAML::Node & node); + +template <> +std::vector 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 +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_ diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index 549d9e7863fde..fe2d4fbe9351e 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -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(); - path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); - path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); - - // 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(); - point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); - point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); - point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); - point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); - point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); - point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); - point.point.longitudinal_velocity_mps = - point_node["point"]["longitudinal_velocity_mps"].as(); - point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); - point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); - point.point.is_final = point_node["point"]["is_final"].as(); - // Fill the lane_ids - for (const auto & lane_id_node : point_node["lane_ids"]) { - point.lane_ids.push_back(lane_id_node.as()); - } - - 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(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - 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(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.right_bound.push_back(point); - } - return path_msg; + return parse(yaml_path); } } // namespace autoware::test_utils diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index 6ad03071a881a..ef92533e9bf4a 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/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(); @@ -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(); @@ -51,43 +53,131 @@ LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node) return primitive; } -std::vector parse_lanelet_primitives(const YAML::Node & node) +template <> +std::vector parse(const YAML::Node & node) { std::vector 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(p); }); return primitives; } -std::vector parse_segments(const YAML::Node & node) +template <> +std::vector parse(const YAML::Node & node) { std::vector 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(input["preferred_primitive"]); + segment.primitives = parse>(input["primitives"]); return segment; }); return segments; } -// cppcheck-suppress unusedFunction -LaneletRoute parse_lanelet_route_file(const std::string & filename) +template <> +std::vector parse(const YAML::Node & node) +{ + std::vector geom_points; + + std::transform( + node.begin(), node.end(), std::back_inserter(geom_points), [&](const YAML::Node & input) { + Point point; + point.x = input["x"].as(); + point.y = input["y"].as(); + point.z = input["z"].as(); + 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(); + header.stamp.nanosec = node["header"]["stamp"]["nanosec"].as(); + header.frame_id = node["header"]["frame_id"].as(); + + return header; +} + +template <> +std::vector parse>(const YAML::Node & node) +{ + std::vector 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(point_node["pose"]); + + point.point.longitudinal_velocity_mps = point_node["longitudinal_velocity_mps"].as(); + point.point.lateral_velocity_mps = point_node["lateral_velocity_mps"].as(); + point.point.heading_rate_rps = point_node["heading_rate_rps"].as(); + point.point.is_final = point_node["is_final"].as(); + // Fill the lane_ids + for (const auto & lane_id_node : input["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + + 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(config["start_pose"]) : Pose(); + lanelet_route.goal_pose = (config["goal_pose"]) ? parse(config["goal_pose"]) : Pose(); + lanelet_route.segments = parse>(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
(yaml_node); + path.points = parse>(yaml_node); + path.left_bound = parse>(yaml_node["left_bound"]); + path.right_bound = parse>(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 diff --git a/common/autoware_test_utils/test/test_mock_data_parser.cpp b/common/autoware_test_utils/test/test_mock_data_parser.cpp index f9e37a521c42c..cbb93ec96b3de 100644 --- a/common/autoware_test_utils/test/test_mock_data_parser.cpp +++ b/common/autoware_test_utils/test/test_mock_data_parser.cpp @@ -18,8 +18,12 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware_test_utils/mock_data_parser.hpp" +#include + namespace autoware::test_utils { +using tier4_planning_msgs::msg::PathWithLaneId; + // Example YAML structure as a string for testing const char g_complete_yaml[] = R"( start_pose: @@ -58,8 +62,8 @@ 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"]); + Pose start_pose = parse(config["start_pose"]); + Pose goal_pose = parse(config["goal_pose"]); EXPECT_DOUBLE_EQ(start_pose.position.x, 1.0); EXPECT_DOUBLE_EQ(start_pose.position.y, 2.0); @@ -79,7 +83,7 @@ TEST(ParseFunctions, CompleteYAMLTest) EXPECT_DOUBLE_EQ(goal_pose.orientation.w, 0.8); // Test parsing of segments - std::vector segments = parse_segments(config["segments"]); + const auto segments = parse>(config["segments"]); ASSERT_EQ( segments.size(), uint64_t(1)); // Assuming only one segment in the provided YAML for this test @@ -99,7 +103,7 @@ TEST(ParseFunction, CompleteFromFilename) const auto parser_test_route = autoware_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml"; - const auto lanelet_route = parse_lanelet_route_file(parser_test_route); + const auto lanelet_route = parse(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); @@ -132,4 +136,53 @@ TEST(ParseFunction, CompleteFromFilename) EXPECT_EQ(segment1.primitives[3].id, 88); EXPECT_EQ(segment1.primitives[3].primitive_type, "lane"); } + +TEST(ParseFunction, ParsePathWithLaneID) +{ + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto parser_test_path = + autoware_test_utils_dir + "/test_data/path_with_lane_id_parser_test.yaml"; + + const auto path = parse(parser_test_path); + EXPECT_EQ(path.header.stamp.sec, 20); + EXPECT_EQ(path.header.stamp.nanosec, 5); + + const auto path_points = path.points; + const auto & p1 = path_points.front(); + EXPECT_DOUBLE_EQ(p1.point.pose.position.x, 12.9); + EXPECT_DOUBLE_EQ(p1.point.pose.position.y, 3.8); + EXPECT_DOUBLE_EQ(p1.point.pose.position.z, 4.7); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.x, 1.0); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.y, 2.0); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.z, 3.0); + EXPECT_DOUBLE_EQ(p1.point.pose.orientation.w, 4.0); + EXPECT_FLOAT_EQ(p1.point.longitudinal_velocity_mps, 1.2); + EXPECT_FLOAT_EQ(p1.point.lateral_velocity_mps, 3.4); + EXPECT_FLOAT_EQ(p1.point.heading_rate_rps, 5.6); + EXPECT_TRUE(p1.point.is_final); + EXPECT_EQ(p1.lane_ids.front(), 912); + + const auto & p2 = path_points.back(); + EXPECT_DOUBLE_EQ(p2.point.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(p2.point.pose.position.y, 20.5); + EXPECT_DOUBLE_EQ(p2.point.pose.position.z, 90.11); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.x, 4.0); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.y, 3.0); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.z, 2.0); + EXPECT_DOUBLE_EQ(p2.point.pose.orientation.w, 1.0); + EXPECT_FLOAT_EQ(p2.point.longitudinal_velocity_mps, 2.1); + EXPECT_FLOAT_EQ(p2.point.lateral_velocity_mps, 4.3); + EXPECT_FLOAT_EQ(p2.point.heading_rate_rps, 6.5); + EXPECT_FALSE(p2.point.is_final); + EXPECT_EQ(p2.lane_ids.front(), 205); + + EXPECT_DOUBLE_EQ(path.left_bound.front().x, 55.0); + EXPECT_DOUBLE_EQ(path.left_bound.front().y, 66.0); + EXPECT_DOUBLE_EQ(path.left_bound.front().z, 77.0); + + EXPECT_DOUBLE_EQ(path.right_bound.front().x, 0.55); + EXPECT_DOUBLE_EQ(path.right_bound.front().y, 0.66); + EXPECT_DOUBLE_EQ(path.right_bound.front().z, 0.77); +} } // namespace autoware::test_utils diff --git a/common/autoware_test_utils/test_data/path_with_lane_id_parser_test.yaml b/common/autoware_test_utils/test_data/path_with_lane_id_parser_test.yaml new file mode 100644 index 0000000000000..ecd0dd8dd6989 --- /dev/null +++ b/common/autoware_test_utils/test_data/path_with_lane_id_parser_test.yaml @@ -0,0 +1,48 @@ +header: + stamp: + sec: 20 + nanosec: 5 + frame_id: autoware +points: + - point: + pose: + position: + x: 12.9 + y: 3.8 + z: 4.7 + orientation: + x: 1.0 + y: 2.0 + z: 3.0 + w: 4.0 + longitudinal_velocity_mps: 1.2 + lateral_velocity_mps: 3.4 + heading_rate_rps: 5.6 + is_final: true + lane_ids: + - 912 + - point: + pose: + position: + x: 0.0 + y: 20.5 + z: 90.11 + orientation: + x: 4.0 + y: 3.0 + z: 2.0 + w: 1.0 + longitudinal_velocity_mps: 2.1 + lateral_velocity_mps: 4.3 + heading_rate_rps: 6.5 + is_final: false + lane_ids: + - 205 +left_bound: + - x: 55.0 + y: 66.0 + z: 77.0 +right_bound: + - x: 0.55 + y: 0.66 + z: 0.77 diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp index cd7813243c25e..1a9359dc8e17d 100644 --- a/planning/autoware_route_handler/test/test_route_handler.hpp +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -68,7 +68,7 @@ class TestRouteHandler : public ::testing::Test { const auto rh_test_route = get_absolute_path_to_route(autoware_route_handler_dir, route_filename); - route_handler_->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); + route_handler_->setRoute(autoware::test_utils::parse(rh_test_route)); } lanelet::ConstLanelets get_current_lanes() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 01b4c451ebf9e..6f285190c4169 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -21,6 +21,8 @@ #include +#include "autoware_planning_msgs/msg/lanelet_route.hpp" + #include #include @@ -38,6 +40,7 @@ using autoware::test_utils::get_absolute_path_to_config; using autoware::test_utils::get_absolute_path_to_lanelet_map; using autoware::test_utils::get_absolute_path_to_route; using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -116,7 +119,7 @@ class TestNormalLaneChange : public ::testing::Test auto route_handler_ptr = std::make_shared(map_bin_msg); const auto rh_test_route = get_absolute_path_to_route(autoware_route_handler_dir, lane_change_right_test_route_filename); - route_handler_ptr->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); + route_handler_ptr->setRoute(autoware::test_utils::parse(rh_test_route)); return route_handler_ptr; }