Skip to content

Commit ecac831

Browse files
zulfaqar-azmi-t4pre-commit-ci[bot]
zulfaqar-azmi-t4
and
pre-commit-ci[bot]
committed
feat(route_handler): add unit tests (#6961)
* feat(route_handler): add unit tests Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Added route parser Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Completed parser Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Added route file for later test Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * pre-commit Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Added test for isHandlerReady Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix some cppcheck error Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Added additional test Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * move to planning test utils Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * renaming route_parser Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * rename function Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Renaming several things Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * style(pre-commit): autofix * remove some function Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * add additional test Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update README Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Add more explanation in the readme Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * style(pre-commit): autofix * fix lane change test route width Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 090f640 commit ecac831

15 files changed

+34889
-9
lines changed

‎planning/planning_test_utils/CMakeLists.txt

+16
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,23 @@ project(planning_test_utils)
44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

7+
ament_auto_add_library(mock_data_parser SHARED
8+
src/mock_data_parser.cpp)
9+
10+
target_link_libraries(mock_data_parser
11+
yaml-cpp
12+
)
13+
14+
if(BUILD_TESTING)
15+
ament_add_ros_isolated_gtest(test_mock_data_parser
16+
test/test_mock_data_parser.cpp)
17+
18+
target_link_libraries(test_mock_data_parser
19+
mock_data_parser)
20+
endif()
21+
722
ament_auto_package(INSTALL_TO_SHARE
823
config
924
test_map
25+
test_data
1026
)

‎planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp

+46
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
// Copyright 2024 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_
16+
#define PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_
17+
18+
#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
19+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
20+
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
21+
#include <geometry_msgs/msg/pose.hpp>
22+
23+
#include <yaml-cpp/yaml.h>
24+
25+
#include <string>
26+
#include <vector>
27+
28+
namespace test_utils
29+
{
30+
using autoware_planning_msgs::msg::LaneletPrimitive;
31+
using autoware_planning_msgs::msg::LaneletRoute;
32+
using autoware_planning_msgs::msg::LaneletSegment;
33+
using geometry_msgs::msg::Pose;
34+
35+
Pose parse_pose(const YAML::Node & node);
36+
37+
LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node);
38+
39+
std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node);
40+
41+
std::vector<LaneletSegment> parse_segments(const YAML::Node & node);
42+
43+
LaneletRoute parse_lanelet_route_file(const std::string & filename);
44+
} // namespace test_utils
45+
46+
#endif // PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_

‎planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp

+14-8
Original file line numberDiff line numberDiff line change
@@ -193,14 +193,10 @@ OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double res
193193
return costmap_msg;
194194
}
195195

196-
HADMapBin makeMapBinMsg()
196+
HADMapBin make_map_bin_msg(
197+
const std::string & absolute_path, const double center_line_resolution = 5.0)
197198
{
198-
const auto planning_test_utils_dir =
199-
ament_index_cpp::get_package_share_directory("planning_test_utils");
200-
const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm";
201-
double center_line_resolution = 5.0;
202-
// load map from file
203-
const auto map = loadMap(lanelet2_path);
199+
const auto map = loadMap(absolute_path);
204200
if (!map) {
205201
return autoware_auto_mapping_msgs::msg::HADMapBin_<std::allocator<void>>{};
206202
}
@@ -210,10 +206,20 @@ HADMapBin makeMapBinMsg()
210206

211207
// create map bin msg
212208
const auto map_bin_msg =
213-
convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now());
209+
convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now());
214210
return map_bin_msg;
215211
}
216212

213+
HADMapBin makeMapBinMsg()
214+
{
215+
const auto planning_test_utils_dir =
216+
ament_index_cpp::get_package_share_directory("planning_test_utils");
217+
const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm";
218+
double center_line_resolution = 5.0;
219+
220+
return make_map_bin_msg(lanelet2_path, center_line_resolution);
221+
}
222+
217223
Odometry makeOdometry(const double shift = 0.0)
218224
{
219225
Odometry odometry;

‎planning/planning_test_utils/src/mock_data_parser.cpp

+92
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
// Copyright 2024 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "planning_test_utils/mock_data_parser.hpp"
16+
17+
#include <rclcpp/logging.hpp>
18+
19+
#include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
20+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
21+
#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
22+
#include <geometry_msgs/msg/pose.hpp>
23+
24+
#include <yaml-cpp/yaml.h>
25+
26+
#include <algorithm>
27+
#include <string>
28+
#include <vector>
29+
30+
namespace test_utils
31+
{
32+
Pose parse_pose(const YAML::Node & node)
33+
{
34+
Pose pose;
35+
pose.position.x = node["position"]["x"].as<double>();
36+
pose.position.y = node["position"]["y"].as<double>();
37+
pose.position.z = node["position"]["z"].as<double>();
38+
pose.orientation.x = node["orientation"]["x"].as<double>();
39+
pose.orientation.y = node["orientation"]["y"].as<double>();
40+
pose.orientation.z = node["orientation"]["z"].as<double>();
41+
pose.orientation.w = node["orientation"]["w"].as<double>();
42+
return pose;
43+
}
44+
45+
LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node)
46+
{
47+
LaneletPrimitive primitive;
48+
primitive.id = node["id"].as<int64_t>();
49+
primitive.primitive_type = node["primitive_type"].as<std::string>();
50+
51+
return primitive;
52+
}
53+
54+
std::vector<LaneletPrimitive> parse_lanelet_primitives(const YAML::Node & node)
55+
{
56+
std::vector<LaneletPrimitive> primitives;
57+
primitives.reserve(node.size());
58+
std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) {
59+
return parse_lanelet_primitive(p);
60+
});
61+
62+
return primitives;
63+
}
64+
65+
std::vector<LaneletSegment> parse_segments(const YAML::Node & node)
66+
{
67+
std::vector<LaneletSegment> segments;
68+
std::transform(node.begin(), node.end(), std::back_inserter(segments), [&](const auto & input) {
69+
LaneletSegment segment;
70+
segment.preferred_primitive = parse_lanelet_primitive(input["preferred_primitive"]);
71+
segment.primitives = parse_lanelet_primitives(input["primitives"]);
72+
return segment;
73+
});
74+
75+
return segments;
76+
}
77+
78+
LaneletRoute parse_lanelet_route_file(const std::string & filename)
79+
{
80+
LaneletRoute lanelet_route;
81+
try {
82+
YAML::Node config = YAML::LoadFile(filename);
83+
84+
lanelet_route.start_pose = (config["start_pose"]) ? parse_pose(config["start_pose"]) : Pose();
85+
lanelet_route.goal_pose = (config["goal_pose"]) ? parse_pose(config["goal_pose"]) : Pose();
86+
lanelet_route.segments = parse_segments(config["segments"]);
87+
} catch (const std::exception & e) {
88+
RCLCPP_DEBUG(rclcpp::get_logger("planning_test_utils"), "Exception caught: %s", e.what());
89+
}
90+
return lanelet_route;
91+
}
92+
} // namespace test_utils

‎planning/planning_test_utils/test/test_mock_data_parser.cpp

+134
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
// Copyright 2024 TIER IV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <gtest/gtest.h>
16+
17+
// Assuming the parseRouteFile function is in 'RouteHandler.h'
18+
#include "ament_index_cpp/get_package_share_directory.hpp"
19+
#include "planning_test_utils/mock_data_parser.hpp"
20+
21+
namespace test_utils
22+
{
23+
// Example YAML structure as a string for testing
24+
const char g_complete_yaml[] = R"(
25+
start_pose:
26+
position:
27+
x: 1.0
28+
y: 2.0
29+
z: 3.0
30+
orientation:
31+
x: 0.1
32+
y: 0.2
33+
z: 0.3
34+
w: 0.4
35+
goal_pose:
36+
position:
37+
x: 4.0
38+
y: 5.0
39+
z: 6.0
40+
orientation:
41+
x: 0.5
42+
y: 0.6
43+
z: 0.7
44+
w: 0.8
45+
segments:
46+
- preferred_primitive:
47+
id: 11
48+
primitive_type: ''
49+
primitives:
50+
- id: 22
51+
primitive_type: lane
52+
- id: 33
53+
primitive_type: lane
54+
)";
55+
56+
TEST(ParseFunctions, CompleteYAMLTest)
57+
{
58+
YAML::Node config = YAML::Load(g_complete_yaml);
59+
60+
// Test parsing of start_pose and goal_pose
61+
Pose start_pose = parse_pose(config["start_pose"]);
62+
Pose goal_pose = parse_pose(config["goal_pose"]);
63+
64+
EXPECT_DOUBLE_EQ(start_pose.position.x, 1.0);
65+
EXPECT_DOUBLE_EQ(start_pose.position.y, 2.0);
66+
EXPECT_DOUBLE_EQ(start_pose.position.z, 3.0);
67+
68+
EXPECT_DOUBLE_EQ(start_pose.orientation.x, 0.1);
69+
EXPECT_DOUBLE_EQ(start_pose.orientation.y, 0.2);
70+
EXPECT_DOUBLE_EQ(start_pose.orientation.z, 0.3);
71+
EXPECT_DOUBLE_EQ(start_pose.orientation.w, 0.4);
72+
73+
EXPECT_DOUBLE_EQ(goal_pose.position.x, 4.0);
74+
EXPECT_DOUBLE_EQ(goal_pose.position.y, 5.0);
75+
EXPECT_DOUBLE_EQ(goal_pose.position.z, 6.0);
76+
EXPECT_DOUBLE_EQ(goal_pose.orientation.x, 0.5);
77+
EXPECT_DOUBLE_EQ(goal_pose.orientation.y, 0.6);
78+
EXPECT_DOUBLE_EQ(goal_pose.orientation.z, 0.7);
79+
EXPECT_DOUBLE_EQ(goal_pose.orientation.w, 0.8);
80+
81+
// Test parsing of segments
82+
std::vector<LaneletSegment> segments = parse_segments(config["segments"]);
83+
ASSERT_EQ(segments.size(), 1); // Assuming only one segment in the provided YAML for this test
84+
85+
const auto & segment0 = segments[0];
86+
EXPECT_EQ(segment0.preferred_primitive.id, 11);
87+
EXPECT_EQ(segment0.primitives.size(), 2);
88+
EXPECT_EQ(segment0.primitives[0].id, 22);
89+
EXPECT_EQ(segment0.primitives[0].primitive_type, "lane");
90+
EXPECT_EQ(segment0.primitives[1].id, 33);
91+
EXPECT_EQ(segment0.primitives[1].primitive_type, "lane");
92+
}
93+
94+
TEST(ParseFunction, CompleteFromFilename)
95+
{
96+
const auto planning_test_utils_dir =
97+
ament_index_cpp::get_package_share_directory("planning_test_utils");
98+
const auto parser_test_route =
99+
planning_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml";
100+
101+
const auto lanelet_route = parse_lanelet_route_file(parser_test_route);
102+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0);
103+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.y, 2.0);
104+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.z, 3.0);
105+
106+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.x, 0.1);
107+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.y, 0.2);
108+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.z, 0.3);
109+
EXPECT_DOUBLE_EQ(lanelet_route.start_pose.orientation.w, 0.4);
110+
111+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.x, 4.0);
112+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.y, 5.0);
113+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.position.z, 6.0);
114+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.x, 0.5);
115+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.y, 0.6);
116+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.z, 0.7);
117+
EXPECT_DOUBLE_EQ(lanelet_route.goal_pose.orientation.w, 0.8);
118+
119+
ASSERT_EQ(
120+
lanelet_route.segments.size(),
121+
2); // Assuming only one segment in the provided YAML for this test
122+
const auto & segment1 = lanelet_route.segments[1];
123+
EXPECT_EQ(segment1.preferred_primitive.id, 44);
124+
EXPECT_EQ(segment1.primitives.size(), 4);
125+
EXPECT_EQ(segment1.primitives[0].id, 55);
126+
EXPECT_EQ(segment1.primitives[0].primitive_type, "lane");
127+
EXPECT_EQ(segment1.primitives[1].id, 66);
128+
EXPECT_EQ(segment1.primitives[1].primitive_type, "lane");
129+
EXPECT_EQ(segment1.primitives[2].id, 77);
130+
EXPECT_EQ(segment1.primitives[2].primitive_type, "lane");
131+
EXPECT_EQ(segment1.primitives[3].id, 88);
132+
EXPECT_EQ(segment1.primitives[3].primitive_type, "lane");
133+
}
134+
} // namespace test_utils

‎planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml

+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)