Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(route_handler): add unit tests #6961

Merged
Changes from 18 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions planning/planning_test_utils/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -4,7 +4,23 @@ project(planning_test_utils)
find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_library(mock_data_parser SHARED
src/mock_data_parser.cpp)

target_link_libraries(mock_data_parser
yaml-cpp
)

if(BUILD_TESTING)
ament_add_ros_isolated_gtest(test_mock_data_parser
test/test_mock_data_parser.cpp)

target_link_libraries(test_mock_data_parser
mock_data_parser)
endif()

ament_auto_package(INSTALL_TO_SHARE
config
test_map
test_data
)
Original file line number Diff line number Diff line change
@@ -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 PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_
#define PLANNING_TEST_UTILS__MOCK_DATA_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 test_utils
{
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_lanelet_route_file(const std::string & filename);
} // namespace test_utils

#endif // PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_
Original file line number Diff line number Diff line change
@@ -193,14 +193,10 @@ OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double res
return costmap_msg;
}

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

// create map bin msg
const auto map_bin_msg =
convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now());
convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now());
return map_bin_msg;
}

HADMapBin makeMapBinMsg()
{
const auto planning_test_utils_dir =
ament_index_cpp::get_package_share_directory("planning_test_utils");
const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm";
double center_line_resolution = 5.0;

return make_map_bin_msg(lanelet2_path, center_line_resolution);
}

Odometry makeOdometry(const double shift = 0.0)
{
Odometry odometry;
92 changes: 92 additions & 0 deletions planning/planning_test_utils/src/mock_data_parser.cpp
Original file line number Diff line number Diff line change
@@ -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 "planning_test_utils/mock_data_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 test_utils
{
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());
std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) {
return 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_lanelet_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["segments"]);
} catch (const std::exception & e) {
RCLCPP_DEBUG(rclcpp::get_logger("planning_test_utils"), "Exception caught: %s", e.what());
}
return lanelet_route;
}
} // namespace test_utils
134 changes: 134 additions & 0 deletions planning/planning_test_utils/test/test_mock_data_parser.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,134 @@
// 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 "ament_index_cpp/get_package_share_directory.hpp"
#include "planning_test_utils/mock_data_parser.hpp"

namespace test_utils
{
// Example YAML structure as a string for testing
const char 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
)";

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(), 1); // 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");
}

TEST(ParseFunction, CompleteFromFilename)
{
const auto planning_test_utils_dir =
ament_index_cpp::get_package_share_directory("planning_test_utils");
const auto parser_test_route =
planning_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml";

const auto lanelet_route = parse_lanelet_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");
}
} // namespace test_utils
Original file line number Diff line number Diff line change
@@ -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
33,390 changes: 33,390 additions & 0 deletions planning/planning_test_utils/test_map/2km_test.osm

Large diffs are not rendered by default.

14 changes: 13 additions & 1 deletion planning/route_handler/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -8,4 +8,16 @@ ament_auto_add_library(route_handler SHARED
src/route_handler.cpp
)

ament_auto_package()
if(BUILD_TESTING)
ament_add_ros_isolated_gtest(test_route_handler
test/test_route_handler.cpp)

target_link_libraries(test_route_handler
route_handler
)

endif()

ament_auto_package(INSTALL_TO_SHARE
test_route
)
15 changes: 15 additions & 0 deletions planning/route_handler/README.md
Original file line number Diff line number Diff line change
@@ -1,3 +1,18 @@
# route handler

`route_handler` is a library for calculating driving route on the lanelet map.

## Unit Testing

The unit testing depends on `planning_test_utils` package.
`planning_test_utils` is a library that provides several common functions to simplify unit test creating.

![route_handler_test](./images/route_handler_test.svg)

By default, route file is necessary to create tests. The following illustrates the route that are used in the unit test

### Lane change test route

![lane_change_test_route](./images/lane_change_test_route.svg)

- The route is based on map that can be obtained from `planning_test_utils\test_map`
618 changes: 618 additions & 0 deletions planning/route_handler/images/lane_change_test_route.svg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
231 changes: 231 additions & 0 deletions planning/route_handler/images/route_handler_test.svg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
3 changes: 3 additions & 0 deletions planning/route_handler/package.xml
Original file line number Diff line number Diff line change
@@ -18,6 +18,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

@@ -26,10 +27,12 @@
<depend>autoware_planning_msgs</depend>
<depend>geometry_msgs</depend>
<depend>lanelet2_extension</depend>
<depend>planning_test_utils</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tf2_ros</depend>
<depend>tier4_autoware_utils</depend>
<depend>yaml-cpp</depend>

<export>
<build_type>ament_cmake</build_type>
110 changes: 110 additions & 0 deletions planning/route_handler/test/test_route_handler.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
// 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 "test_route_handler.hpp"

#include <rclcpp/rclcpp.hpp>

#include <gtest/gtest.h>

namespace route_handler::test
{
TEST_F(TestRouteHandler, isRouteHandlerReadyTest)
{
ASSERT_TRUE(route_handler_->isHandlerReady());
}

TEST_F(TestRouteHandler, checkIfIDReturned)
{
const auto lanelet1 = route_handler_->getLaneletsFromId(4785);
const auto is_lanelet1_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet1);
ASSERT_TRUE(is_lanelet1_in_goal_route_section);

const auto lanelet2 = route_handler_->getLaneletsFromId(4780);
const auto is_lanelet2_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet2);
ASSERT_FALSE(is_lanelet2_in_goal_route_section);
}

TEST_F(TestRouteHandler, getGoalLaneId)
{
lanelet::ConstLanelet goal_lane;

const auto goal_lane_obtained = route_handler_->getGoalLanelet(&goal_lane);
ASSERT_TRUE(goal_lane_obtained);
ASSERT_EQ(goal_lane.id(), 5088);
}

// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute)
// {
// lanelet::ConstLanelet closest_lane;

// Pose search_pose;

// search_pose.position = tier4_autoware_utils::createPoint(-1.0, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained7 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained7);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(-0.5, 1.75, 0);
// const auto closest_lane_obtained =
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(-.01, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained3 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained3);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(0.0, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained1 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained1);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(0.01, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained2 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained2);
// ASSERT_EQ(closest_lane.id(), 4424);

// search_pose.position = tier4_autoware_utils::createPoint(0.5, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained4 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained4);
// ASSERT_EQ(closest_lane.id(), 4424);

// search_pose.position = tier4_autoware_utils::createPoint(1.0, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained5 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained5);
// ASSERT_EQ(closest_lane.id(), 4424);
// }
} // namespace route_handler::test
73 changes: 73 additions & 0 deletions planning/route_handler/test/test_route_handler.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
// 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 TEST_ROUTE_HANDLER_HPP_
#define TEST_ROUTE_HANDLER_HPP_

#include "ament_index_cpp/get_package_share_directory.hpp"
#include "gtest/gtest.h"
#include "planning_test_utils/mock_data_parser.hpp"
#include "planning_test_utils/planning_test_utils.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>
namespace route_handler::test
{

using autoware_auto_mapping_msgs::msg::HADMapBin;

class TestRouteHandler : public ::testing::Test
{
public:
TestRouteHandler()
{
const auto planning_test_utils_dir =
ament_index_cpp::get_package_share_directory("planning_test_utils");
const auto lanelet2_path = planning_test_utils_dir + "/test_map/2km_test.osm";
constexpr double center_line_resolution = 5.0;
const auto map_bin_msg = test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution);
route_handler_ = std::make_shared<RouteHandler>(map_bin_msg);
set_lane_change_test_route();
}

TestRouteHandler(const TestRouteHandler &) = delete;
TestRouteHandler(TestRouteHandler &&) = delete;
TestRouteHandler & operator=(const TestRouteHandler &) = delete;
TestRouteHandler & operator=(TestRouteHandler &&) = delete;
~TestRouteHandler() override = default;

void set_lane_change_test_route()
{
const auto route_handler_dir = ament_index_cpp::get_package_share_directory("route_handler");
const auto rh_test_route = route_handler_dir + "/test_route/lane_change_test_route.yaml";
route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route));
}

std::shared_ptr<RouteHandler> route_handler_;
};
} // namespace route_handler::test

#endif // TEST_ROUTE_HANDLER_HPP_
93 changes: 93 additions & 0 deletions planning/route_handler/test_route/lane_change_test_route.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
header:
stamp:
sec: 1715936953
nanosec: 67206425
frame_id: map
start_pose:
position:
x: -50.0
y: 1.75
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
goal_pose:
position:
x: 70.0
y: -1.75
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
segments:
- preferred_primitive:
id: 4765
primitive_type: ""
primitives:
- id: 4765
primitive_type: lane
- id: 9590
primitive_type: lane
- preferred_primitive:
id: 4770
primitive_type: ""
primitives:
- id: 4770
primitive_type: lane
- id: 5072
primitive_type: lane
- preferred_primitive:
id: 4775
primitive_type: ""
primitives:
- id: 4775
primitive_type: lane
- id: 9594
primitive_type: lane
- preferred_primitive:
id: 9598
primitive_type: ""
primitives:
- id: 4424
primitive_type: lane
- id: 9598
primitive_type: lane
- preferred_primitive:
id: 5084
primitive_type: ""
primitives:
- id: 4780
primitive_type: lane
- id: 5084
primitive_type: lane
- preferred_primitive:
id: 5088
primitive_type: ""
primitives:
- id: 4785
primitive_type: lane
- id: 5088
primitive_type: lane
uuid:
uuid:
- 231
- 254
- 143
- 227
- 194
- 8
- 220
- 88
- 30
- 194
- 172
- 147
- 127
- 143
- 176
- 133
allow_modification: false