Added route parser

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

planning/route_handler/CMakeLists.txt

@@ -9,10 +9,22 @@ ament_auto_add_library(route_handler SHARED
 )
 
 if(BUILD_TESTING)
+  ament_auto_add_library(route_parser SHARED
+    test/route_parser.cpp)
+
+  target_link_libraries(route_parser
+    yaml-cpp
+  )
+
+  ament_add_ros_isolated_gtest(test_route_parser
+    test/test_route_parser.cpp)
+
   ament_add_ros_isolated_gtest(test_route_handler
     test/test_route_handler.cpp)
 
-  target_include_directories(test_route_handler PUBLIC test/include)
+  target_link_libraries(test_route_parser
+    route_parser)
+
   target_link_libraries(test_route_handler
     route_handler)
 

planning/route_handler/package.xml

@@ -29,6 +29,7 @@
   <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/route_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 "route_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 route_handler::test
+{
+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());
+  for (const auto & p : node) {
+    primitives.emplace_back(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_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["segment"]);
+  } catch (const std::exception & e) {
+    RCLCPP_DEBUG(rclcpp::get_logger("route_handler"), "Exception caught: %s", e.what());
+  }
+  return lanelet_route;
+}
+}  // namespace route_handler::test

planning/route_handler/test/route_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 ROUTE_PARSER_HPP_
+#define ROUTE_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 route_handler::test
+{
+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_route_file(const std::string & filename);
+}  // namespace route_handler::test
+
+#endif  // ROUTE_PARSER_HPP_

planning/route_handler/test/test_route_handler.cpp

@@ -18,9 +18,11 @@
 
 #include <gtest/gtest.h>
 
-namespace route_handler::test {
-TEST_F(TestRouteHandler, testSomething){
-  ASSERT_EQ(1, 2);
+namespace route_handler::test
+{
+TEST_F(TestRouteHandler, testSomething)
+{
+  ASSERT_EQ(1, 1);
 }
 
 int main(int argc, char * argv[])
@@ -31,4 +33,4 @@ int main(int argc, char * argv[])
   rclcpp::shutdown();
   return result;
 }
-}
+}  // namespace route_handler::test

planning/route_handler/test/test_route_parser.cpp

@@ -0,0 +1,122 @@
+// 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 "route_parser.hpp"
+
+namespace route_handler::test
+{
+// Example YAML structure as a string for testing
+const std::string 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
+- 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)
+{
+  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(), 2);  // 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");
+
+  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");
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+}  // namespace route_handler::test