Completed parser

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/route_handler/CMakeLists.txt

@@ -32,4 +32,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   test_map
+  test_route
 )

planning/route_handler/test/route_parser.cpp

@@ -83,7 +83,7 @@ LaneletRoute parse_route_file(const std::string & 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["segment"]);
+    lanelet_route.segments = parse_segments(config["segments"]);
   } catch (const std::exception & e) {
     RCLCPP_DEBUG(rclcpp::get_logger("route_handler"), "Exception caught: %s", e.what());
   }

planning/route_handler/test/test_route_handler.hpp

@@ -17,21 +17,21 @@
 
 #include "ament_index_cpp/get_package_share_directory.hpp"
 #include "gtest/gtest.h"
+#include <route_handler/route_handler.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>
+#include <memory>
 namespace route_handler::test
 {
 
@@ -40,7 +40,9 @@ using autoware_auto_mapping_msgs::msg::HADMapBin;
 class TestRouteHandler : public ::testing::Test
 {
 public:
-  TestRouteHandler() { route_handler_ = std::make_shared<RouteHandler>(makeMapBinMsg()); }
+  TestRouteHandler(){
+    route_handler_ = std::make_shared<RouteHandler>(makeMapBinMsg());
+  }
   TestRouteHandler(const TestRouteHandler &) = delete;
   TestRouteHandler(TestRouteHandler &&) = delete;
   TestRouteHandler & operator=(const TestRouteHandler &) = delete;

planning/route_handler/test/test_route_parser.cpp

@@ -15,6 +15,7 @@
 #include <gtest/gtest.h>
 
 // Assuming the parseRouteFile function is in 'RouteHandler.h'
+#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "route_parser.hpp"
 
 namespace route_handler::test
@@ -50,18 +51,6 @@ const std::string g_complete_yaml = R"(
       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
 )";
 
 TEST(ParseFunctions, CompleteYAMLTest)
@@ -91,7 +80,7 @@ TEST(ParseFunctions, CompleteYAMLTest)
 
   // Test parsing of segments
   std::vector<LaneletSegment> segments = parse_segments(config["segments"]);
-  ASSERT_EQ(segments.size(), 2);  // Assuming only one segment in the provided YAML for this test
+  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);
@@ -100,20 +89,45 @@ TEST(ParseFunctions, CompleteYAMLTest)
   EXPECT_EQ(segment0.primitives[0].primitive_type, "lane");
   EXPECT_EQ(segment0.primitives[1].id, 33);
   EXPECT_EQ(segment0.primitives[1].primitive_type, "lane");
-
-  const auto & segment1 = 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");
 }
 
+TEST(ParseFunction, CompleteFromFilename){
+    const auto planning_test_utils_dir =
+      ament_index_cpp::get_package_share_directory("route_handler");
+    const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route";
+
+
+    const auto lanelet_route = parse_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");
+}
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

planning/route_handler/test_route/parser_test.route

@@ -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