Skip to content

Commit 4899d24

Browse files
zulfaqar-azmi-t4zulfaqar-azmi-t4
committed
Completed parser
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 5b0f51a commit 4899d24

File tree

5 files changed

+87
-29
lines changed

5 files changed

+87
-29
lines changed

‎planning/route_handler/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -32,4 +32,5 @@ endif()
3232

3333
ament_auto_package(INSTALL_TO_SHARE
3434
test_map
35+
test_route
3536
)

‎planning/route_handler/test/route_parser.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ LaneletRoute parse_route_file(const std::string & filename)
8383

8484
lanelet_route.start_pose = (config["start_pose"]) ? parse_pose(config["start_pose"]) : Pose();
8585
lanelet_route.goal_pose = (config["goal_pose"]) ? parse_pose(config["goal_pose"]) : Pose();
86-
lanelet_route.segments = parse_segments(config["segment"]);
86+
lanelet_route.segments = parse_segments(config["segments"]);
8787
} catch (const std::exception & e) {
8888
RCLCPP_DEBUG(rclcpp::get_logger("route_handler"), "Exception caught: %s", e.what());
8989
}

‎planning/route_handler/test/test_route_handler.hpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -17,21 +17,21 @@
1717

1818
#include "ament_index_cpp/get_package_share_directory.hpp"
1919
#include "gtest/gtest.h"
20+
#include <route_handler/route_handler.hpp>
2021

2122
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
2223
#include <lanelet2_extension/projection/mgrs_projector.hpp>
2324
#include <lanelet2_extension/utility/message_conversion.hpp>
2425
#include <lanelet2_extension/utility/utilities.hpp>
2526
#include <rclcpp/clock.hpp>
2627
#include <rclcpp/logging.hpp>
27-
#include <route_handler/route_handler.hpp>
2828

2929
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
3030

3131
#include <lanelet2_io/Io.h>
3232

33-
#include <memory>
3433
#include <string>
34+
#include <memory>
3535
namespace route_handler::test
3636
{
3737

@@ -40,7 +40,9 @@ using autoware_auto_mapping_msgs::msg::HADMapBin;
4040
class TestRouteHandler : public ::testing::Test
4141
{
4242
public:
43-
TestRouteHandler() { route_handler_ = std::make_shared<RouteHandler>(makeMapBinMsg()); }
43+
TestRouteHandler(){
44+
route_handler_ = std::make_shared<RouteHandler>(makeMapBinMsg());
45+
}
4446
TestRouteHandler(const TestRouteHandler &) = delete;
4547
TestRouteHandler(TestRouteHandler &&) = delete;
4648
TestRouteHandler & operator=(const TestRouteHandler &) = delete;

‎planning/route_handler/test/test_route_parser.cpp

+39-25
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include <gtest/gtest.h>
1616

1717
// Assuming the parseRouteFile function is in 'RouteHandler.h'
18+
#include "ament_index_cpp/get_package_share_directory.hpp"
1819
#include "route_parser.hpp"
1920

2021
namespace route_handler::test
@@ -50,18 +51,6 @@ const std::string g_complete_yaml = R"(
5051
primitive_type: lane
5152
- id: 33
5253
primitive_type: lane
53-
- preferred_primitive:
54-
id: 44
55-
primitive_type: ''
56-
primitives:
57-
- id: 55
58-
primitive_type: lane
59-
- id: 66
60-
primitive_type: lane
61-
- id: 77
62-
primitive_type: lane
63-
- id: 88
64-
primitive_type: lane
6554
)";
6655

6756
TEST(ParseFunctions, CompleteYAMLTest)
@@ -91,7 +80,7 @@ TEST(ParseFunctions, CompleteYAMLTest)
9180

9281
// Test parsing of segments
9382
std::vector<LaneletSegment> segments = parse_segments(config["segments"]);
94-
ASSERT_EQ(segments.size(), 2); // Assuming only one segment in the provided YAML for this test
83+
ASSERT_EQ(segments.size(), 1); // Assuming only one segment in the provided YAML for this test
9584

9685
const auto & segment0 = segments[0];
9786
EXPECT_EQ(segment0.preferred_primitive.id, 11);
@@ -100,20 +89,45 @@ TEST(ParseFunctions, CompleteYAMLTest)
10089
EXPECT_EQ(segment0.primitives[0].primitive_type, "lane");
10190
EXPECT_EQ(segment0.primitives[1].id, 33);
10291
EXPECT_EQ(segment0.primitives[1].primitive_type, "lane");
103-
104-
const auto & segment1 = segments[1];
105-
EXPECT_EQ(segment1.preferred_primitive.id, 44);
106-
EXPECT_EQ(segment1.primitives.size(), 4);
107-
EXPECT_EQ(segment1.primitives[0].id, 55);
108-
EXPECT_EQ(segment1.primitives[0].primitive_type, "lane");
109-
EXPECT_EQ(segment1.primitives[1].id, 66);
110-
EXPECT_EQ(segment1.primitives[1].primitive_type, "lane");
111-
EXPECT_EQ(segment1.primitives[2].id, 77);
112-
EXPECT_EQ(segment1.primitives[2].primitive_type, "lane");
113-
EXPECT_EQ(segment1.primitives[3].id, 88);
114-
EXPECT_EQ(segment1.primitives[3].primitive_type, "lane");
11592
}
11693

94+
TEST(ParseFunction, CompleteFromFilename){
95+
const auto planning_test_utils_dir =
96+
ament_index_cpp::get_package_share_directory("route_handler");
97+
const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route";
98+
99+
100+
const auto lanelet_route = parse_route_file(parser_test_route);
101+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0);
102+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0);
103+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0);
104+
105+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1);
106+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2);
107+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3);
108+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4);
109+
110+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0);
111+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0);
112+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0);
113+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5);
114+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6);
115+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7);
116+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8);
117+
118+
ASSERT_EQ(lanelet_route.segments.size(), 2); // Assuming only one segment in the provided YAML for this test
119+
const auto & segment1 = lanelet_route.segments[1];
120+
EXPECT_EQ(segment1.preferred_primitive.id, 44);
121+
EXPECT_EQ(segment1.primitives.size(), 4);
122+
EXPECT_EQ(segment1.primitives[0].id, 55);
123+
EXPECT_EQ(segment1.primitives[0].primitive_type, "lane");
124+
EXPECT_EQ(segment1.primitives[1].id, 66);
125+
EXPECT_EQ(segment1.primitives[1].primitive_type, "lane");
126+
EXPECT_EQ(segment1.primitives[2].id, 77);
127+
EXPECT_EQ(segment1.primitives[2].primitive_type, "lane");
128+
EXPECT_EQ(segment1.primitives[3].id, 88);
129+
EXPECT_EQ(segment1.primitives[3].primitive_type, "lane");
130+
}
117131
int main(int argc, char ** argv)
118132
{
119133
::testing::InitGoogleTest(&argc, argv);

‎planning/route_handler/test_route/parser_test.route

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
start_pose:
2+
position:
3+
x: 1.0
4+
y: 2.0
5+
z: 3.0
6+
orientation:
7+
x: 0.1
8+
y: 0.2
9+
z: 0.3
10+
w: 0.4
11+
goal_pose:
12+
position:
13+
x: 4.0
14+
y: 5.0
15+
z: 6.0
16+
orientation:
17+
x: 0.5
18+
y: 0.6
19+
z: 0.7
20+
w: 0.8
21+
segments:
22+
- preferred_primitive:
23+
id: 11
24+
primitive_type: ''
25+
primitives:
26+
- id: 22
27+
primitive_type: lane
28+
- id: 33
29+
primitive_type: lane
30+
- preferred_primitive:
31+
id: 44
32+
primitive_type: ''
33+
primitives:
34+
- id: 55
35+
primitive_type: lane
36+
- id: 66
37+
primitive_type: lane
38+
- id: 77
39+
primitive_type: lane
40+
- id: 88
41+
primitive_type: lane

0 commit comments

Comments
 (0)