Skip to content

Commit 3c2ec5a

Browse files
pre-commit
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 9e95813 commit 3c2ec5a

File tree

3 files changed

+43
-43
lines changed

3 files changed

+43
-43
lines changed

planning/route_handler/test/test_route_handler.hpp

+3-5
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>
2120

2221
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
2322
#include <lanelet2_extension/projection/mgrs_projector.hpp>
2423
#include <lanelet2_extension/utility/message_conversion.hpp>
2524
#include <lanelet2_extension/utility/utilities.hpp>
2625
#include <rclcpp/clock.hpp>
2726
#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 <string>
3433
#include <memory>
34+
#include <string>
3535
namespace route_handler::test
3636
{
3737

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

planning/route_handler/test/test_route_parser.cpp

+39-37
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121
namespace route_handler::test
2222
{
2323
// Example YAML structure as a string for testing
24-
const std::string g_complete_yaml = R"(
24+
const char g_complete_yaml[] = R"(
2525
start_pose:
2626
position:
2727
x: 1.0
@@ -91,42 +91,44 @@ TEST(ParseFunctions, CompleteYAMLTest)
9191
EXPECT_EQ(segment0.primitives[1].primitive_type, "lane");
9292
}
9393

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");
94+
TEST(ParseFunction, CompleteFromFilename)
95+
{
96+
const auto planning_test_utils_dir =
97+
ament_index_cpp::get_package_share_directory("route_handler");
98+
const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route";
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(
119+
lanelet_route.segments.size(),
120+
2); // Assuming only one segment in the provided YAML for this test
121+
const auto & segment1 = lanelet_route.segments[1];
122+
EXPECT_EQ(segment1.preferred_primitive.id, 44);
123+
EXPECT_EQ(segment1.primitives.size(), 4);
124+
EXPECT_EQ(segment1.primitives[0].id, 55);
125+
EXPECT_EQ(segment1.primitives[0].primitive_type, "lane");
126+
EXPECT_EQ(segment1.primitives[1].id, 66);
127+
EXPECT_EQ(segment1.primitives[1].primitive_type, "lane");
128+
EXPECT_EQ(segment1.primitives[2].id, 77);
129+
EXPECT_EQ(segment1.primitives[2].primitive_type, "lane");
130+
EXPECT_EQ(segment1.primitives[3].id, 88);
131+
EXPECT_EQ(segment1.primitives[3].primitive_type, "lane");
130132
}
131133
int main(int argc, char ** argv)
132134
{

planning/route_handler/test_route/rh_test.route

+1-1
Original file line numberDiff line numberDiff line change
@@ -250,4 +250,4 @@ segments:
250250
- id: 4870
251251
primitive_type: lane
252252
- id: 5156
253-
primitive_type: lane
253+
primitive_type: lane

0 commit comments

Comments
 (0)