From d50614338d54d8f1c61cbdad1417a1cda6322f02 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 9 May 2024 18:41:57 +0900 Subject: [PATCH 01/19] feat(route_handler): add unit tests Signed-off-by: Muhammad Zulfaqar Azmi --- planning/route_handler/CMakeLists.txt | 14 +- planning/route_handler/package.xml | 1 + .../route_handler/test/test_route_handler.cpp | 34 + .../route_handler/test/test_route_handler.hpp | 109 + .../route_handler/test_map/lanelet2_map.osm | 33390 ++++++++++++++++ 5 files changed, 33547 insertions(+), 1 deletion(-) create mode 100644 planning/route_handler/test/test_route_handler.cpp create mode 100644 planning/route_handler/test/test_route_handler.hpp create mode 100644 planning/route_handler/test_map/lanelet2_map.osm diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 54b34f11f5363..946de98a26815 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/planning/route_handler/CMakeLists.txt @@ -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_include_directories(test_route_handler PUBLIC test/include) + target_link_libraries(test_route_handler + route_handler) + +endif() + +ament_auto_package(INSTALL_TO_SHARE + test_map +) diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 22db01d833023..33c905c163e7b 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -18,6 +18,7 @@ ament_cmake_auto autoware_cmake + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp new file mode 100644 index 0000000000000..d416d9bd69c77 --- /dev/null +++ b/planning/route_handler/test/test_route_handler.cpp @@ -0,0 +1,34 @@ +// 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 + +#include + +namespace route_handler::test { +TEST_F(TestRouteHandler, testSomething){ + ASSERT_EQ(1, 2); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} +} diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp new file mode 100644 index 0000000000000..9db07b2020fd8 --- /dev/null +++ b/planning/route_handler/test/test_route_handler.hpp @@ -0,0 +1,109 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +namespace route_handler::test +{ + +using autoware_auto_mapping_msgs::msg::HADMapBin; + +class TestRouteHandler : public ::testing::Test +{ +public: + TestRouteHandler() { route_handler_ = std::make_shared(makeMapBinMsg()); } + TestRouteHandler(const TestRouteHandler &) = delete; + TestRouteHandler(TestRouteHandler &&) = delete; + TestRouteHandler & operator=(const TestRouteHandler &) = delete; + TestRouteHandler & operator=(TestRouteHandler &&) = delete; + ~TestRouteHandler() override = default; + + static lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) + { + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + auto map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; + } + + static HADMapBin makeMapBinMsg() + { + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("route_handler"); + 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); + if (!map) { + return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; + } + + static HADMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now) + { + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + HADMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.format_version = format_version; + map_bin_msg.map_version = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; + } + + std::shared_ptr route_handler_; +}; +} // namespace route_handler::test + +#endif // TEST_ROUTE_HANDLER_HPP_ diff --git a/planning/route_handler/test_map/lanelet2_map.osm b/planning/route_handler/test_map/lanelet2_map.osm new file mode 100644 index 0000000000000..43c22f8de6bb1 --- /dev/null +++ b/planning/route_handler/test_map/lanelet2_map.osm @@ -0,0 +1,33390 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 84bb9cb7e1cf3a71815aa5028ce594671706b535 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 9 May 2024 20:46:24 +0900 Subject: [PATCH 02/19] Added route parser Signed-off-by: Muhammad Zulfaqar Azmi --- planning/route_handler/CMakeLists.txt | 14 +- planning/route_handler/package.xml | 1 + planning/route_handler/test/route_parser.cpp | 92 +++++++++++++ planning/route_handler/test/route_parser.hpp | 46 +++++++ .../route_handler/test/test_route_handler.cpp | 10 +- .../route_handler/test/test_route_parser.cpp | 122 ++++++++++++++++++ 6 files changed, 280 insertions(+), 5 deletions(-) create mode 100644 planning/route_handler/test/route_parser.cpp create mode 100644 planning/route_handler/test/route_parser.hpp create mode 100644 planning/route_handler/test/test_route_parser.cpp diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 946de98a26815..5178acc171f1f 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/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) diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 33c905c163e7b..38d875e830e02 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -31,6 +31,7 @@ rclcpp_components tf2_ros tier4_autoware_utils + yaml-cpp ament_cmake diff --git a/planning/route_handler/test/route_parser.cpp b/planning/route_handler/test/route_parser.cpp new file mode 100644 index 0000000000000..7a5da4e2c1ea4 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace route_handler::test +{ +Pose parse_pose(const YAML::Node & node) +{ + Pose pose; + pose.position.x = node["position"]["x"].as(); + pose.position.y = node["position"]["y"].as(); + pose.position.z = node["position"]["z"].as(); + pose.orientation.x = node["orientation"]["x"].as(); + pose.orientation.y = node["orientation"]["y"].as(); + pose.orientation.z = node["orientation"]["z"].as(); + pose.orientation.w = node["orientation"]["w"].as(); + return pose; +} + +LaneletPrimitive parse_lanelet_primitive(const YAML::Node & node) +{ + LaneletPrimitive primitive; + primitive.id = node["id"].as(); + primitive.primitive_type = node["primitive_type"].as(); + + return primitive; +} + +std::vector parse_lanelet_primitives(const YAML::Node & node) +{ + std::vector primitives; + primitives.reserve(node.size()); + for (const auto & p : node) { + primitives.emplace_back(parse_lanelet_primitive(p)); + } + + return primitives; +} + +std::vector parse_segments(const YAML::Node & node) +{ + std::vector 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 diff --git a/planning/route_handler/test/route_parser.hpp b/planning/route_handler/test/route_parser.hpp new file mode 100644 index 0000000000000..e23b14107ce26 --- /dev/null +++ b/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 +#include +#include +#include + +#include + +#include +#include + +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 parse_lanelet_primitives(const YAML::Node & node); + +std::vector parse_segments(const YAML::Node & node); + +LaneletRoute parse_route_file(const std::string & filename); +} // namespace route_handler::test + +#endif // ROUTE_PARSER_HPP_ diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index d416d9bd69c77..8a8230669ec79 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -18,9 +18,11 @@ #include -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 diff --git a/planning/route_handler/test/test_route_parser.cpp b/planning/route_handler/test/test_route_parser.cpp new file mode 100644 index 0000000000000..5952fe04a86f4 --- /dev/null +++ b/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 + +// 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 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 From 6b8dc2460fb332b638c4f384fb2a6cf61005731e Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 9 May 2024 20:59:01 +0900 Subject: [PATCH 03/19] Completed parser Signed-off-by: Muhammad Zulfaqar Azmi --- planning/route_handler/CMakeLists.txt | 1 + planning/route_handler/test/route_parser.cpp | 2 +- .../route_handler/test/test_route_handler.hpp | 8 ++- .../route_handler/test/test_route_parser.cpp | 64 +++++++++++-------- .../test_route/parser_test.route | 41 ++++++++++++ 5 files changed, 87 insertions(+), 29 deletions(-) create mode 100644 planning/route_handler/test_route/parser_test.route diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 5178acc171f1f..05370f1ec8f53 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/planning/route_handler/CMakeLists.txt @@ -32,4 +32,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE test_map + test_route ) diff --git a/planning/route_handler/test/route_parser.cpp b/planning/route_handler/test/route_parser.cpp index 7a5da4e2c1ea4..c8651a9f33b67 100644 --- a/planning/route_handler/test/route_parser.cpp +++ b/planning/route_handler/test/route_parser.cpp @@ -83,7 +83,7 @@ LaneletRoute parse_route_file(const std::string & 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"]); + lanelet_route.segments = parse_segments(config["segments"]); } catch (const std::exception & e) { RCLCPP_DEBUG(rclcpp::get_logger("route_handler"), "Exception caught: %s", e.what()); } diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 9db07b2020fd8..64b5aa280eee1 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -17,6 +17,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "gtest/gtest.h" +#include #include #include @@ -24,14 +25,13 @@ #include #include #include -#include #include #include -#include #include +#include namespace route_handler::test { @@ -40,7 +40,9 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; class TestRouteHandler : public ::testing::Test { public: - TestRouteHandler() { route_handler_ = std::make_shared(makeMapBinMsg()); } + TestRouteHandler(){ + route_handler_ = std::make_shared(makeMapBinMsg()); + } TestRouteHandler(const TestRouteHandler &) = delete; TestRouteHandler(TestRouteHandler &&) = delete; TestRouteHandler & operator=(const TestRouteHandler &) = delete; diff --git a/planning/route_handler/test/test_route_parser.cpp b/planning/route_handler/test/test_route_parser.cpp index 5952fe04a86f4..45cb6229be3e9 100644 --- a/planning/route_handler/test/test_route_parser.cpp +++ b/planning/route_handler/test/test_route_parser.cpp @@ -15,6 +15,7 @@ #include // Assuming the parseRouteFile function is in 'RouteHandler.h' +#include "ament_index_cpp/get_package_share_directory.hpp" #include "route_parser.hpp" namespace route_handler::test @@ -50,18 +51,6 @@ const std::string g_complete_yaml = R"( 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) @@ -91,7 +80,7 @@ TEST(ParseFunctions, CompleteYAMLTest) // Test parsing of segments std::vector segments = parse_segments(config["segments"]); - ASSERT_EQ(segments.size(), 2); // Assuming only one segment in the provided YAML for this test + 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); @@ -100,20 +89,45 @@ TEST(ParseFunctions, CompleteYAMLTest) 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"); } +TEST(ParseFunction, CompleteFromFilename){ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("route_handler"); + const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route"; + + + const auto lanelet_route = parse_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"); +} int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/planning/route_handler/test_route/parser_test.route b/planning/route_handler/test_route/parser_test.route new file mode 100644 index 0000000000000..eaf98b8a3bcf3 --- /dev/null +++ b/planning/route_handler/test_route/parser_test.route @@ -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 From b5a0854e9226c208d8fc23a7f1d25dabf90297f9 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 9 May 2024 20:59:56 +0900 Subject: [PATCH 04/19] Added route file for later test Signed-off-by: Muhammad Zulfaqar Azmi --- .../route_handler/test_route/rh_test.route | 253 ++++++++++++++++++ 1 file changed, 253 insertions(+) create mode 100644 planning/route_handler/test_route/rh_test.route diff --git a/planning/route_handler/test_route/rh_test.route b/planning/route_handler/test_route/rh_test.route new file mode 100644 index 0000000000000..dc2be043195bc --- /dev/null +++ b/planning/route_handler/test_route/rh_test.route @@ -0,0 +1,253 @@ +start_pose: + position: + x: -200.0 + y: 1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +goal_pose: + position: + x: 500.0 + y: -1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +segments: +- preferred_primitive: + id: 4735 + primitive_type: '' + primitives: + - id: 4735 + primitive_type: lane + - id: 5044 + primitive_type: lane +- preferred_primitive: + id: 4740 + primitive_type: '' + primitives: + - id: 4740 + primitive_type: lane + - id: 5048 + primitive_type: lane +- preferred_primitive: + id: 4745 + primitive_type: '' + primitives: + - id: 4745 + primitive_type: lane + - id: 5052 + primitive_type: lane +- preferred_primitive: + id: 4750 + primitive_type: '' + primitives: + - id: 4750 + primitive_type: lane + - id: 5056 + primitive_type: lane +- preferred_primitive: + id: 4755 + primitive_type: '' + primitives: + - id: 4755 + primitive_type: lane + - id: 5060 + primitive_type: lane +- preferred_primitive: + id: 4760 + primitive_type: '' + primitives: + - id: 4760 + primitive_type: lane + - id: 5064 + primitive_type: lane +- 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: 9594 + 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 +- preferred_primitive: + id: 5092 + primitive_type: '' + primitives: + - id: 4790 + primitive_type: lane + - id: 5092 + primitive_type: lane +- preferred_primitive: + id: 5096 + primitive_type: '' + primitives: + - id: 4795 + primitive_type: lane + - id: 5096 + primitive_type: lane +- preferred_primitive: + id: 5104 + primitive_type: '' + primitives: + - id: 4800 + primitive_type: lane + - id: 5104 + primitive_type: lane +- preferred_primitive: + id: 5100 + primitive_type: '' + primitives: + - id: 4805 + primitive_type: lane + - id: 5100 + primitive_type: lane +- preferred_primitive: + id: 9813 + primitive_type: '' + primitives: + - id: 4810 + primitive_type: lane + - id: 9813 + primitive_type: lane +- preferred_primitive: + id: 5112 + primitive_type: '' + primitives: + - id: 4815 + primitive_type: lane + - id: 5112 + primitive_type: lane +- preferred_primitive: + id: 5116 + primitive_type: '' + primitives: + - id: 4820 + primitive_type: lane + - id: 5116 + primitive_type: lane +- preferred_primitive: + id: 5120 + primitive_type: '' + primitives: + - id: 4825 + primitive_type: lane + - id: 5120 + primitive_type: lane +- preferred_primitive: + id: 5124 + primitive_type: '' + primitives: + - id: 4830 + primitive_type: lane + - id: 5124 + primitive_type: lane +- preferred_primitive: + id: 5128 + primitive_type: '' + primitives: + - id: 4835 + primitive_type: lane + - id: 5128 + primitive_type: lane +- preferred_primitive: + id: 5132 + primitive_type: '' + primitives: + - id: 4840 + primitive_type: lane + - id: 5132 + primitive_type: lane +- preferred_primitive: + id: 5136 + primitive_type: '' + primitives: + - id: 4845 + primitive_type: lane + - id: 5136 + primitive_type: lane +- preferred_primitive: + id: 5140 + primitive_type: '' + primitives: + - id: 4850 + primitive_type: lane + - id: 5140 + primitive_type: lane +- preferred_primitive: + id: 9817 + primitive_type: '' + primitives: + - id: 4855 + primitive_type: lane + - id: 9817 + primitive_type: lane +- preferred_primitive: + id: 9821 + primitive_type: '' + primitives: + - id: 4860 + primitive_type: lane + - id: 9821 + primitive_type: lane +- preferred_primitive: + id: 9825 + primitive_type: '' + primitives: + - id: 4865 + primitive_type: lane + - id: 9825 + primitive_type: lane +- preferred_primitive: + id: 5156 + primitive_type: '' + primitives: + - id: 4870 + primitive_type: lane + - id: 5156 + primitive_type: lane \ No newline at end of file From b54a704fdd5adcf2250a195343b6800f65f1c241 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 9 May 2024 21:03:14 +0900 Subject: [PATCH 05/19] pre-commit Signed-off-by: Muhammad Zulfaqar Azmi --- .../route_handler/test/test_route_handler.hpp | 8 +- .../route_handler/test/test_route_parser.cpp | 76 ++++++++++--------- .../route_handler/test_route/rh_test.route | 2 +- 3 files changed, 43 insertions(+), 43 deletions(-) diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 64b5aa280eee1..9db07b2020fd8 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -17,7 +17,6 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "gtest/gtest.h" -#include #include #include @@ -25,13 +24,14 @@ #include #include #include +#include #include #include -#include #include +#include namespace route_handler::test { @@ -40,9 +40,7 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; class TestRouteHandler : public ::testing::Test { public: - TestRouteHandler(){ - route_handler_ = std::make_shared(makeMapBinMsg()); - } + TestRouteHandler() { route_handler_ = std::make_shared(makeMapBinMsg()); } TestRouteHandler(const TestRouteHandler &) = delete; TestRouteHandler(TestRouteHandler &&) = delete; TestRouteHandler & operator=(const TestRouteHandler &) = delete; diff --git a/planning/route_handler/test/test_route_parser.cpp b/planning/route_handler/test/test_route_parser.cpp index 45cb6229be3e9..86ebe3a6a131d 100644 --- a/planning/route_handler/test/test_route_parser.cpp +++ b/planning/route_handler/test/test_route_parser.cpp @@ -21,7 +21,7 @@ namespace route_handler::test { // Example YAML structure as a string for testing -const std::string g_complete_yaml = R"( +const char g_complete_yaml[] = R"( start_pose: position: x: 1.0 @@ -91,42 +91,44 @@ TEST(ParseFunctions, CompleteYAMLTest) EXPECT_EQ(segment0.primitives[1].primitive_type, "lane"); } -TEST(ParseFunction, CompleteFromFilename){ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("route_handler"); - const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route"; - - - const auto lanelet_route = parse_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"); +TEST(ParseFunction, CompleteFromFilename) +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("route_handler"); + const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route"; + + const auto lanelet_route = parse_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"); } int main(int argc, char ** argv) { diff --git a/planning/route_handler/test_route/rh_test.route b/planning/route_handler/test_route/rh_test.route index dc2be043195bc..82f00352f6234 100644 --- a/planning/route_handler/test_route/rh_test.route +++ b/planning/route_handler/test_route/rh_test.route @@ -250,4 +250,4 @@ segments: - id: 4870 primitive_type: lane - id: 5156 - primitive_type: lane \ No newline at end of file + primitive_type: lane From 2e2cfe7f1f916ed0570764d91b1871deec13cdd4 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 9 May 2024 21:09:06 +0900 Subject: [PATCH 06/19] Added test for isHandlerReady Signed-off-by: Muhammad Zulfaqar Azmi --- planning/route_handler/CMakeLists.txt | 4 +++- planning/route_handler/test/test_route_handler.cpp | 11 +++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 05370f1ec8f53..78d1736f8f17a 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/planning/route_handler/CMakeLists.txt @@ -26,7 +26,9 @@ if(BUILD_TESTING) route_parser) target_link_libraries(test_route_handler - route_handler) + route_handler + route_parser + ) endif() diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index 8a8230669ec79..16a80debbfb80 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -14,15 +14,22 @@ #include "test_route_handler.hpp" +#include "route_parser.hpp" + #include #include namespace route_handler::test { -TEST_F(TestRouteHandler, testSomething) +TEST_F(TestRouteHandler, isRouteHandlerReadyTest) { - ASSERT_EQ(1, 1); + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("route_handler"); + const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; + ASSERT_FALSE(route_handler_->isHandlerReady()); + route_handler_->setRoute(parse_route_file(rh_test_route)); + ASSERT_TRUE(route_handler_->isHandlerReady()); } int main(int argc, char * argv[]) From ea95fd5845d91d3488f7fbd1b0071ab76326d731 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 10 May 2024 14:07:32 +0900 Subject: [PATCH 07/19] fix some cppcheck error Signed-off-by: Zulfaqar Azmi --- planning/route_handler/test/route_parser.cpp | 6 +++--- planning/route_handler/test/test_route_handler.hpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/route_handler/test/route_parser.cpp b/planning/route_handler/test/route_parser.cpp index c8651a9f33b67..ccbf86b700b9e 100644 --- a/planning/route_handler/test/route_parser.cpp +++ b/planning/route_handler/test/route_parser.cpp @@ -55,9 +55,9 @@ std::vector parse_lanelet_primitives(const YAML::Node & node) { std::vector primitives; primitives.reserve(node.size()); - for (const auto & p : node) { - primitives.emplace_back(parse_lanelet_primitive(p)); - } + std::transform(node.begin(), node.end(), std::back_inserter(primitives), [&](const auto & p) { + return parse_lanelet_primitive(p); + }); return primitives; } diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 9db07b2020fd8..d8bd41d608c37 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -40,7 +40,7 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; class TestRouteHandler : public ::testing::Test { public: - TestRouteHandler() { route_handler_ = std::make_shared(makeMapBinMsg()); } + TestRouteHandler() : route_handler_(std::make_shared(makeMapBinMsg())) {} TestRouteHandler(const TestRouteHandler &) = delete; TestRouteHandler(TestRouteHandler &&) = delete; TestRouteHandler & operator=(const TestRouteHandler &) = delete; From 55b58b00dc75e9269de132df9ea3606e3fe183c9 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 10 May 2024 17:25:42 +0900 Subject: [PATCH 08/19] Added additional test Signed-off-by: Zulfaqar Azmi --- .../route_handler/test/test_route_handler.cpp | 17 +++++++++++------ .../route_handler/test/test_route_parser.cpp | 5 ----- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index 16a80debbfb80..e698c6def91a3 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -32,12 +32,17 @@ TEST_F(TestRouteHandler, isRouteHandlerReadyTest) ASSERT_TRUE(route_handler_->isHandlerReady()); } -int main(int argc, char * argv[]) +TEST_F(TestRouteHandler, checkIfIDReturned) { - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - bool result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("route_handler"); + const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; + + route_handler_->setRoute(parse_route_file(rh_test_route)); + const auto lanelet = route_handler_->getLaneletsFromId(4870); + + const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet); + + ASSERT_TRUE(is_in_goal_route_section); } } // namespace route_handler::test diff --git a/planning/route_handler/test/test_route_parser.cpp b/planning/route_handler/test/test_route_parser.cpp index 86ebe3a6a131d..9e56f562e8d41 100644 --- a/planning/route_handler/test/test_route_parser.cpp +++ b/planning/route_handler/test/test_route_parser.cpp @@ -130,9 +130,4 @@ TEST(ParseFunction, CompleteFromFilename) 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 From 9a9d61a44a72d426c0488fa0d7a30f92f8ef939c Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 17 May 2024 11:15:55 +0900 Subject: [PATCH 09/19] move to planning test utils Signed-off-by: Muhammad Zulfaqar Azmi --- planning/planning_test_utils/CMakeLists.txt | 16 ++++++++ .../planning_test_utils}/route_parser.hpp | 10 ++--- .../src}/route_parser.cpp | 6 +-- .../test/test_route_parser.cpp | 11 ++--- .../test_data/lanelet_route_parser_test.yaml | 41 +++++++++++++++++++ .../test_map/2km_test.osm} | 0 planning/route_handler/CMakeLists.txt | 15 ------- planning/route_handler/package.xml | 1 + .../route_handler/test/test_route_handler.cpp | 6 +-- .../route_handler/test/test_route_handler.hpp | 4 +- .../test_route/parser_test.route | 41 ------------------- 11 files changed, 77 insertions(+), 74 deletions(-) rename planning/{route_handler/test => planning_test_utils/include/planning_test_utils}/route_parser.hpp (87%) rename planning/{route_handler/test => planning_test_utils/src}/route_parser.cpp (96%) rename planning/{route_handler => planning_test_utils}/test/test_route_parser.cpp (94%) create mode 100644 planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml rename planning/{route_handler/test_map/lanelet2_map.osm => planning_test_utils/test_map/2km_test.osm} (100%) delete mode 100644 planning/route_handler/test_route/parser_test.route diff --git a/planning/planning_test_utils/CMakeLists.txt b/planning/planning_test_utils/CMakeLists.txt index 29ee79c2cab1a..e65344b2b8fae 100644 --- a/planning/planning_test_utils/CMakeLists.txt +++ b/planning/planning_test_utils/CMakeLists.txt @@ -4,7 +4,23 @@ project(planning_test_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(route_parser SHARED + src/route_parser.cpp) + +target_link_libraries(route_parser + yaml-cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_route_parser + test/test_route_parser.cpp) + + target_link_libraries(test_route_parser + route_parser) +endif() + ament_auto_package(INSTALL_TO_SHARE config test_map + test_data ) diff --git a/planning/route_handler/test/route_parser.hpp b/planning/planning_test_utils/include/planning_test_utils/route_parser.hpp similarity index 87% rename from planning/route_handler/test/route_parser.hpp rename to planning/planning_test_utils/include/planning_test_utils/route_parser.hpp index e23b14107ce26..86f860df04754 100644 --- a/planning/route_handler/test/route_parser.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/route_parser.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROUTE_PARSER_HPP_ -#define ROUTE_PARSER_HPP_ +#ifndef PLANNING_TEST_UTILS__ROUTE_PARSER_HPP_ +#define PLANNING_TEST_UTILS__ROUTE_PARSER_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace route_handler::test +namespace test_utils { using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; @@ -41,6 +41,6 @@ std::vector parse_lanelet_primitives(const YAML::Node & node); std::vector parse_segments(const YAML::Node & node); LaneletRoute parse_route_file(const std::string & filename); -} // namespace route_handler::test +} // namespace test_utils -#endif // ROUTE_PARSER_HPP_ +#endif // PLANNING_TEST_UTILS__ROUTE_PARSER_HPP_ diff --git a/planning/route_handler/test/route_parser.cpp b/planning/planning_test_utils/src/route_parser.cpp similarity index 96% rename from planning/route_handler/test/route_parser.cpp rename to planning/planning_test_utils/src/route_parser.cpp index ccbf86b700b9e..7cec2c3d61e0d 100644 --- a/planning/route_handler/test/route_parser.cpp +++ b/planning/planning_test_utils/src/route_parser.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "route_parser.hpp" +#include "planning_test_utils/route_parser.hpp" #include @@ -27,7 +27,7 @@ #include #include -namespace route_handler::test +namespace test_utils { Pose parse_pose(const YAML::Node & node) { @@ -89,4 +89,4 @@ LaneletRoute parse_route_file(const std::string & filename) } return lanelet_route; } -} // namespace route_handler::test +} // namespace test_utils diff --git a/planning/route_handler/test/test_route_parser.cpp b/planning/planning_test_utils/test/test_route_parser.cpp similarity index 94% rename from planning/route_handler/test/test_route_parser.cpp rename to planning/planning_test_utils/test/test_route_parser.cpp index 9e56f562e8d41..5a95b07875ee7 100644 --- a/planning/route_handler/test/test_route_parser.cpp +++ b/planning/planning_test_utils/test/test_route_parser.cpp @@ -16,9 +16,9 @@ // Assuming the parseRouteFile function is in 'RouteHandler.h' #include "ament_index_cpp/get_package_share_directory.hpp" -#include "route_parser.hpp" +#include "planning_test_utils/route_parser.hpp" -namespace route_handler::test +namespace test_utils { // Example YAML structure as a string for testing const char g_complete_yaml[] = R"( @@ -94,8 +94,9 @@ TEST(ParseFunctions, CompleteYAMLTest) TEST(ParseFunction, CompleteFromFilename) { const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("route_handler"); - const auto parser_test_route = planning_test_utils_dir + "/test_route/parser_test.route"; + 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_route_file(parser_test_route); EXPECT_DOUBLE_EQ(lanelet_route.start_pose.position.x, 1.0); @@ -130,4 +131,4 @@ TEST(ParseFunction, CompleteFromFilename) EXPECT_EQ(segment1.primitives[3].id, 88); EXPECT_EQ(segment1.primitives[3].primitive_type, "lane"); } -} // namespace route_handler::test +} // namespace test_utils diff --git a/planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml b/planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml new file mode 100644 index 0000000000000..06ed15d20d3c9 --- /dev/null +++ b/planning/planning_test_utils/test_data/lanelet_route_parser_test.yaml @@ -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 diff --git a/planning/route_handler/test_map/lanelet2_map.osm b/planning/planning_test_utils/test_map/2km_test.osm similarity index 100% rename from planning/route_handler/test_map/lanelet2_map.osm rename to planning/planning_test_utils/test_map/2km_test.osm diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 78d1736f8f17a..414489e899fff 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/planning/route_handler/CMakeLists.txt @@ -9,30 +9,15 @@ 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_link_libraries(test_route_parser - route_parser) - target_link_libraries(test_route_handler route_handler - route_parser ) endif() ament_auto_package(INSTALL_TO_SHARE - test_map test_route ) diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 38d875e830e02..097bc62f8bf39 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -27,6 +27,7 @@ autoware_planning_msgs geometry_msgs lanelet2_extension + planning_test_utils rclcpp rclcpp_components tf2_ros diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index e698c6def91a3..f3cfbdb48acf4 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -14,7 +14,7 @@ #include "test_route_handler.hpp" -#include "route_parser.hpp" +#include "planning_test_utils/route_parser.hpp" #include @@ -28,7 +28,7 @@ TEST_F(TestRouteHandler, isRouteHandlerReadyTest) ament_index_cpp::get_package_share_directory("route_handler"); const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; ASSERT_FALSE(route_handler_->isHandlerReady()); - route_handler_->setRoute(parse_route_file(rh_test_route)); + route_handler_->setRoute(test_utils::parse_route_file(rh_test_route)); ASSERT_TRUE(route_handler_->isHandlerReady()); } @@ -38,7 +38,7 @@ TEST_F(TestRouteHandler, checkIfIDReturned) ament_index_cpp::get_package_share_directory("route_handler"); const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; - route_handler_->setRoute(parse_route_file(rh_test_route)); + route_handler_->setRoute(test_utils::parse_route_file(rh_test_route)); const auto lanelet = route_handler_->getLaneletsFromId(4870); const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet); diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index d8bd41d608c37..b7f3c44b420b1 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -65,8 +65,8 @@ class TestRouteHandler : public ::testing::Test static HADMapBin makeMapBinMsg() { const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("route_handler"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/2km_test.osm"; double center_line_resolution = 5.0; // load map from file const auto map = loadMap(lanelet2_path); diff --git a/planning/route_handler/test_route/parser_test.route b/planning/route_handler/test_route/parser_test.route deleted file mode 100644 index eaf98b8a3bcf3..0000000000000 --- a/planning/route_handler/test_route/parser_test.route +++ /dev/null @@ -1,41 +0,0 @@ -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 From e126afa37ff98325edcada89fde27be3e8bc85d1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 15:42:06 +0900 Subject: [PATCH 10/19] renaming route_parser Signed-off-by: Zulfaqar Azmi --- planning/planning_test_utils/CMakeLists.txt | 14 +++++++------- .../{route_parser.hpp => mock_data_parser.hpp} | 0 .../src/{route_parser.cpp => mock_data_parser.cpp} | 2 +- ..._route_parser.cpp => test_mock_data_parser.cpp} | 2 +- planning/route_handler/test/test_route_handler.cpp | 2 +- 5 files changed, 10 insertions(+), 10 deletions(-) rename planning/planning_test_utils/include/planning_test_utils/{route_parser.hpp => mock_data_parser.hpp} (100%) rename planning/planning_test_utils/src/{route_parser.cpp => mock_data_parser.cpp} (98%) rename planning/planning_test_utils/test/{test_route_parser.cpp => test_mock_data_parser.cpp} (98%) diff --git a/planning/planning_test_utils/CMakeLists.txt b/planning/planning_test_utils/CMakeLists.txt index e65344b2b8fae..56e985c7fa2b7 100644 --- a/planning/planning_test_utils/CMakeLists.txt +++ b/planning/planning_test_utils/CMakeLists.txt @@ -4,19 +4,19 @@ project(planning_test_utils) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(route_parser SHARED - src/route_parser.cpp) +ament_auto_add_library(mock_data_parser SHARED + src/mock_data_parser.cpp) -target_link_libraries(route_parser +target_link_libraries(mock_data_parser yaml-cpp ) if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_route_parser - test/test_route_parser.cpp) + ament_add_ros_isolated_gtest(test_mock_data_parser + test/test_mock_data_parser.cpp) - target_link_libraries(test_route_parser - route_parser) + target_link_libraries(test_mock_data_parser + mock_data_parser) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/planning/planning_test_utils/include/planning_test_utils/route_parser.hpp b/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp similarity index 100% rename from planning/planning_test_utils/include/planning_test_utils/route_parser.hpp rename to planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp diff --git a/planning/planning_test_utils/src/route_parser.cpp b/planning/planning_test_utils/src/mock_data_parser.cpp similarity index 98% rename from planning/planning_test_utils/src/route_parser.cpp rename to planning/planning_test_utils/src/mock_data_parser.cpp index 7cec2c3d61e0d..d51a277743d5e 100644 --- a/planning/planning_test_utils/src/route_parser.cpp +++ b/planning/planning_test_utils/src/mock_data_parser.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_test_utils/route_parser.hpp" +#include "planning_test_utils/mock_data_parser.hpp" #include diff --git a/planning/planning_test_utils/test/test_route_parser.cpp b/planning/planning_test_utils/test/test_mock_data_parser.cpp similarity index 98% rename from planning/planning_test_utils/test/test_route_parser.cpp rename to planning/planning_test_utils/test/test_mock_data_parser.cpp index 5a95b07875ee7..44cb32444ba41 100644 --- a/planning/planning_test_utils/test/test_route_parser.cpp +++ b/planning/planning_test_utils/test/test_mock_data_parser.cpp @@ -16,7 +16,7 @@ // Assuming the parseRouteFile function is in 'RouteHandler.h' #include "ament_index_cpp/get_package_share_directory.hpp" -#include "planning_test_utils/route_parser.hpp" +#include "planning_test_utils/mock_data_parser.hpp" namespace test_utils { diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index f3cfbdb48acf4..5258f8c363dfa 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -14,7 +14,7 @@ #include "test_route_handler.hpp" -#include "planning_test_utils/route_parser.hpp" +#include "planning_test_utils/mock_data_parser.hpp" #include From 5563a6d449ebe2d10afc321f2f49ed9ca3239869 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 15:43:49 +0900 Subject: [PATCH 11/19] rename function Signed-off-by: Zulfaqar Azmi --- .../include/planning_test_utils/mock_data_parser.hpp | 8 ++++---- planning/planning_test_utils/src/mock_data_parser.cpp | 2 +- .../planning_test_utils/test/test_mock_data_parser.cpp | 2 +- planning/route_handler/test/test_route_handler.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp b/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp index 86f860df04754..09d4474dda15d 100644 --- a/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/mock_data_parser.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TEST_UTILS__ROUTE_PARSER_HPP_ -#define PLANNING_TEST_UTILS__ROUTE_PARSER_HPP_ +#ifndef PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_ +#define PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_ #include #include @@ -40,7 +40,7 @@ std::vector parse_lanelet_primitives(const YAML::Node & node); std::vector parse_segments(const YAML::Node & node); -LaneletRoute parse_route_file(const std::string & filename); +LaneletRoute parse_lanelet_route_file(const std::string & filename); } // namespace test_utils -#endif // PLANNING_TEST_UTILS__ROUTE_PARSER_HPP_ +#endif // PLANNING_TEST_UTILS__MOCK_DATA_PARSER_HPP_ diff --git a/planning/planning_test_utils/src/mock_data_parser.cpp b/planning/planning_test_utils/src/mock_data_parser.cpp index d51a277743d5e..f2686d720a5c7 100644 --- a/planning/planning_test_utils/src/mock_data_parser.cpp +++ b/planning/planning_test_utils/src/mock_data_parser.cpp @@ -75,7 +75,7 @@ std::vector parse_segments(const YAML::Node & node) return segments; } -LaneletRoute parse_route_file(const std::string & filename) +LaneletRoute parse_lanelet_route_file(const std::string & filename) { LaneletRoute lanelet_route; try { diff --git a/planning/planning_test_utils/test/test_mock_data_parser.cpp b/planning/planning_test_utils/test/test_mock_data_parser.cpp index 44cb32444ba41..4582762ac53c7 100644 --- a/planning/planning_test_utils/test/test_mock_data_parser.cpp +++ b/planning/planning_test_utils/test/test_mock_data_parser.cpp @@ -98,7 +98,7 @@ TEST(ParseFunction, CompleteFromFilename) const auto parser_test_route = planning_test_utils_dir + "/test_data/lanelet_route_parser_test.yaml"; - const auto lanelet_route = parse_route_file(parser_test_route); + 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); diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index 5258f8c363dfa..fce695fbe6bca 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -28,7 +28,7 @@ TEST_F(TestRouteHandler, isRouteHandlerReadyTest) ament_index_cpp::get_package_share_directory("route_handler"); const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; ASSERT_FALSE(route_handler_->isHandlerReady()); - route_handler_->setRoute(test_utils::parse_route_file(rh_test_route)); + route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route)); ASSERT_TRUE(route_handler_->isHandlerReady()); } @@ -38,7 +38,7 @@ TEST_F(TestRouteHandler, checkIfIDReturned) ament_index_cpp::get_package_share_directory("route_handler"); const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; - route_handler_->setRoute(test_utils::parse_route_file(rh_test_route)); + route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route)); const auto lanelet = route_handler_->getLaneletsFromId(4870); const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet); From 159cfe8cfa25231c1cddfc79f38cfe3dc7e281ca Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 17:01:34 +0900 Subject: [PATCH 12/19] Renaming several things Signed-off-by: Zulfaqar Azmi --- .../planning_test_utils.hpp | 22 ++++++++++++------- .../src/mock_data_parser.cpp | 2 +- .../route_handler/test/test_route_handler.cpp | 13 ----------- .../route_handler/test/test_route_handler.hpp | 21 +++++++++++++++++- ...test.route => lane_change_test_route.yaml} | 0 5 files changed, 35 insertions(+), 23 deletions(-) rename planning/route_handler/test_route/{rh_test.route => lane_change_test_route.yaml} (100%) diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index c47404b45bb5d..6af1ce165d7ab 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -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_>{}; } @@ -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; diff --git a/planning/planning_test_utils/src/mock_data_parser.cpp b/planning/planning_test_utils/src/mock_data_parser.cpp index f2686d720a5c7..f9f9a7b0b1594 100644 --- a/planning/planning_test_utils/src/mock_data_parser.cpp +++ b/planning/planning_test_utils/src/mock_data_parser.cpp @@ -85,7 +85,7 @@ LaneletRoute parse_lanelet_route_file(const std::string & filename) 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("route_handler"), "Exception caught: %s", e.what()); + RCLCPP_DEBUG(rclcpp::get_logger("planning_test_utils"), "Exception caught: %s", e.what()); } return lanelet_route; } diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index fce695fbe6bca..f347430e765f7 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -14,8 +14,6 @@ #include "test_route_handler.hpp" -#include "planning_test_utils/mock_data_parser.hpp" - #include #include @@ -24,23 +22,12 @@ namespace route_handler::test { TEST_F(TestRouteHandler, isRouteHandlerReadyTest) { - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("route_handler"); - const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; - ASSERT_FALSE(route_handler_->isHandlerReady()); - route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route)); ASSERT_TRUE(route_handler_->isHandlerReady()); } TEST_F(TestRouteHandler, checkIfIDReturned) { - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("route_handler"); - const auto rh_test_route = planning_test_utils_dir + "/test_route/rh_test.route"; - - route_handler_->setRoute(test_utils::parse_lanelet_route_file(rh_test_route)); const auto lanelet = route_handler_->getLaneletsFromId(4870); - const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet); ASSERT_TRUE(is_in_goal_route_section); diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index b7f3c44b420b1..02eb3744931a9 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -17,6 +17,8 @@ #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 #include @@ -40,7 +42,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; class TestRouteHandler : public ::testing::Test { public: - TestRouteHandler() : route_handler_(std::make_shared(makeMapBinMsg())) {} + 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(map_bin_msg); + set_lane_change_test_route(); + } + TestRouteHandler(const TestRouteHandler &) = delete; TestRouteHandler(TestRouteHandler &&) = delete; TestRouteHandler & operator=(const TestRouteHandler &) = delete; @@ -102,6 +114,13 @@ class TestRouteHandler : public ::testing::Test return map_bin_msg; } + 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 route_handler_; }; } // namespace route_handler::test diff --git a/planning/route_handler/test_route/rh_test.route b/planning/route_handler/test_route/lane_change_test_route.yaml similarity index 100% rename from planning/route_handler/test_route/rh_test.route rename to planning/route_handler/test_route/lane_change_test_route.yaml From 8c2d371afc8fd30178ebc15c5d7aa3cddde2c97c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 May 2024 08:03:57 +0000 Subject: [PATCH 13/19] style(pre-commit): autofix --- .../test_route/lane_change_test_route.yaml | 464 +++++++++--------- 1 file changed, 232 insertions(+), 232 deletions(-) diff --git a/planning/route_handler/test_route/lane_change_test_route.yaml b/planning/route_handler/test_route/lane_change_test_route.yaml index 82f00352f6234..34d0080354ee8 100644 --- a/planning/route_handler/test_route/lane_change_test_route.yaml +++ b/planning/route_handler/test_route/lane_change_test_route.yaml @@ -19,235 +19,235 @@ goal_pose: z: 0.0 w: 1.0 segments: -- preferred_primitive: - id: 4735 - primitive_type: '' - primitives: - - id: 4735 - primitive_type: lane - - id: 5044 - primitive_type: lane -- preferred_primitive: - id: 4740 - primitive_type: '' - primitives: - - id: 4740 - primitive_type: lane - - id: 5048 - primitive_type: lane -- preferred_primitive: - id: 4745 - primitive_type: '' - primitives: - - id: 4745 - primitive_type: lane - - id: 5052 - primitive_type: lane -- preferred_primitive: - id: 4750 - primitive_type: '' - primitives: - - id: 4750 - primitive_type: lane - - id: 5056 - primitive_type: lane -- preferred_primitive: - id: 4755 - primitive_type: '' - primitives: - - id: 4755 - primitive_type: lane - - id: 5060 - primitive_type: lane -- preferred_primitive: - id: 4760 - primitive_type: '' - primitives: - - id: 4760 - primitive_type: lane - - id: 5064 - primitive_type: lane -- 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: 9594 - 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 -- preferred_primitive: - id: 5092 - primitive_type: '' - primitives: - - id: 4790 - primitive_type: lane - - id: 5092 - primitive_type: lane -- preferred_primitive: - id: 5096 - primitive_type: '' - primitives: - - id: 4795 - primitive_type: lane - - id: 5096 - primitive_type: lane -- preferred_primitive: - id: 5104 - primitive_type: '' - primitives: - - id: 4800 - primitive_type: lane - - id: 5104 - primitive_type: lane -- preferred_primitive: - id: 5100 - primitive_type: '' - primitives: - - id: 4805 - primitive_type: lane - - id: 5100 - primitive_type: lane -- preferred_primitive: - id: 9813 - primitive_type: '' - primitives: - - id: 4810 - primitive_type: lane - - id: 9813 - primitive_type: lane -- preferred_primitive: - id: 5112 - primitive_type: '' - primitives: - - id: 4815 - primitive_type: lane - - id: 5112 - primitive_type: lane -- preferred_primitive: - id: 5116 - primitive_type: '' - primitives: - - id: 4820 - primitive_type: lane - - id: 5116 - primitive_type: lane -- preferred_primitive: - id: 5120 - primitive_type: '' - primitives: - - id: 4825 - primitive_type: lane - - id: 5120 - primitive_type: lane -- preferred_primitive: - id: 5124 - primitive_type: '' - primitives: - - id: 4830 - primitive_type: lane - - id: 5124 - primitive_type: lane -- preferred_primitive: - id: 5128 - primitive_type: '' - primitives: - - id: 4835 - primitive_type: lane - - id: 5128 - primitive_type: lane -- preferred_primitive: - id: 5132 - primitive_type: '' - primitives: - - id: 4840 - primitive_type: lane - - id: 5132 - primitive_type: lane -- preferred_primitive: - id: 5136 - primitive_type: '' - primitives: - - id: 4845 - primitive_type: lane - - id: 5136 - primitive_type: lane -- preferred_primitive: - id: 5140 - primitive_type: '' - primitives: - - id: 4850 - primitive_type: lane - - id: 5140 - primitive_type: lane -- preferred_primitive: - id: 9817 - primitive_type: '' - primitives: - - id: 4855 - primitive_type: lane - - id: 9817 - primitive_type: lane -- preferred_primitive: - id: 9821 - primitive_type: '' - primitives: - - id: 4860 - primitive_type: lane - - id: 9821 - primitive_type: lane -- preferred_primitive: - id: 9825 - primitive_type: '' - primitives: - - id: 4865 - primitive_type: lane - - id: 9825 - primitive_type: lane -- preferred_primitive: - id: 5156 - primitive_type: '' - primitives: - - id: 4870 - primitive_type: lane - - id: 5156 - primitive_type: lane + - preferred_primitive: + id: 4735 + primitive_type: "" + primitives: + - id: 4735 + primitive_type: lane + - id: 5044 + primitive_type: lane + - preferred_primitive: + id: 4740 + primitive_type: "" + primitives: + - id: 4740 + primitive_type: lane + - id: 5048 + primitive_type: lane + - preferred_primitive: + id: 4745 + primitive_type: "" + primitives: + - id: 4745 + primitive_type: lane + - id: 5052 + primitive_type: lane + - preferred_primitive: + id: 4750 + primitive_type: "" + primitives: + - id: 4750 + primitive_type: lane + - id: 5056 + primitive_type: lane + - preferred_primitive: + id: 4755 + primitive_type: "" + primitives: + - id: 4755 + primitive_type: lane + - id: 5060 + primitive_type: lane + - preferred_primitive: + id: 4760 + primitive_type: "" + primitives: + - id: 4760 + primitive_type: lane + - id: 5064 + primitive_type: lane + - 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: 9594 + 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 + - preferred_primitive: + id: 5092 + primitive_type: "" + primitives: + - id: 4790 + primitive_type: lane + - id: 5092 + primitive_type: lane + - preferred_primitive: + id: 5096 + primitive_type: "" + primitives: + - id: 4795 + primitive_type: lane + - id: 5096 + primitive_type: lane + - preferred_primitive: + id: 5104 + primitive_type: "" + primitives: + - id: 4800 + primitive_type: lane + - id: 5104 + primitive_type: lane + - preferred_primitive: + id: 5100 + primitive_type: "" + primitives: + - id: 4805 + primitive_type: lane + - id: 5100 + primitive_type: lane + - preferred_primitive: + id: 9813 + primitive_type: "" + primitives: + - id: 4810 + primitive_type: lane + - id: 9813 + primitive_type: lane + - preferred_primitive: + id: 5112 + primitive_type: "" + primitives: + - id: 4815 + primitive_type: lane + - id: 5112 + primitive_type: lane + - preferred_primitive: + id: 5116 + primitive_type: "" + primitives: + - id: 4820 + primitive_type: lane + - id: 5116 + primitive_type: lane + - preferred_primitive: + id: 5120 + primitive_type: "" + primitives: + - id: 4825 + primitive_type: lane + - id: 5120 + primitive_type: lane + - preferred_primitive: + id: 5124 + primitive_type: "" + primitives: + - id: 4830 + primitive_type: lane + - id: 5124 + primitive_type: lane + - preferred_primitive: + id: 5128 + primitive_type: "" + primitives: + - id: 4835 + primitive_type: lane + - id: 5128 + primitive_type: lane + - preferred_primitive: + id: 5132 + primitive_type: "" + primitives: + - id: 4840 + primitive_type: lane + - id: 5132 + primitive_type: lane + - preferred_primitive: + id: 5136 + primitive_type: "" + primitives: + - id: 4845 + primitive_type: lane + - id: 5136 + primitive_type: lane + - preferred_primitive: + id: 5140 + primitive_type: "" + primitives: + - id: 4850 + primitive_type: lane + - id: 5140 + primitive_type: lane + - preferred_primitive: + id: 9817 + primitive_type: "" + primitives: + - id: 4855 + primitive_type: lane + - id: 9817 + primitive_type: lane + - preferred_primitive: + id: 9821 + primitive_type: "" + primitives: + - id: 4860 + primitive_type: lane + - id: 9821 + primitive_type: lane + - preferred_primitive: + id: 9825 + primitive_type: "" + primitives: + - id: 4865 + primitive_type: lane + - id: 9825 + primitive_type: lane + - preferred_primitive: + id: 5156 + primitive_type: "" + primitives: + - id: 4870 + primitive_type: lane + - id: 5156 + primitive_type: lane From 00235b9b1dc5cb7bc21101c8462907c8ec5067f6 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 17:06:15 +0900 Subject: [PATCH 14/19] remove some function Signed-off-by: Zulfaqar Azmi --- .../route_handler/test/test_route_handler.hpp | 55 ------------------- 1 file changed, 55 deletions(-) diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 02eb3744931a9..3c6893da3f9d1 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -59,61 +59,6 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler & operator=(TestRouteHandler &&) = delete; ~TestRouteHandler() override = default; - static lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) - { - lanelet::ErrorMessages errors{}; - lanelet::projection::MGRSProjector projector{}; - auto map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); - } - return nullptr; - } - - static 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/2km_test.osm"; - double center_line_resolution = 5.0; - // load map from file - const auto map = loadMap(lanelet2_path); - if (!map) { - return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; - } - - // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); - - // create map bin msg - const auto map_bin_msg = - convertToMapBinMsg(map, lanelet2_path, rclcpp::Clock(RCL_ROS_TIME).now()); - return map_bin_msg; - } - - static HADMapBin convertToMapBinMsg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, - const rclcpp::Time & now) - { - std::string format_version{}; - std::string map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); - - HADMapBin map_bin_msg; - map_bin_msg.header.stamp = now; - map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; - lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); - - return map_bin_msg; - } - void set_lane_change_test_route() { const auto route_handler_dir = ament_index_cpp::get_package_share_directory("route_handler"); From 16579ec33613bf29abf216656b069b187d260ee5 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 18:51:31 +0900 Subject: [PATCH 15/19] add additional test Signed-off-by: Zulfaqar Azmi --- .../route_handler/test/test_route_handler.cpp | 81 ++++++- .../test_route/lane_change_test_route.yaml | 214 +++--------------- 2 files changed, 105 insertions(+), 190 deletions(-) diff --git a/planning/route_handler/test/test_route_handler.cpp b/planning/route_handler/test/test_route_handler.cpp index f347430e765f7..212fc0ce84d89 100644 --- a/planning/route_handler/test/test_route_handler.cpp +++ b/planning/route_handler/test/test_route_handler.cpp @@ -27,9 +27,84 @@ TEST_F(TestRouteHandler, isRouteHandlerReadyTest) TEST_F(TestRouteHandler, checkIfIDReturned) { - const auto lanelet = route_handler_->getLaneletsFromId(4870); - const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet); + 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); - ASSERT_TRUE(is_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 diff --git a/planning/route_handler/test_route/lane_change_test_route.yaml b/planning/route_handler/test_route/lane_change_test_route.yaml index 34d0080354ee8..8b4c423c622a0 100644 --- a/planning/route_handler/test_route/lane_change_test_route.yaml +++ b/planning/route_handler/test_route/lane_change_test_route.yaml @@ -1,6 +1,11 @@ +header: + stamp: + sec: 1715936953 + nanosec: 67206425 + frame_id: map start_pose: position: - x: -200.0 + x: -50.0 y: 1.75 z: 0.0 orientation: @@ -10,7 +15,7 @@ start_pose: w: 1.0 goal_pose: position: - x: 500.0 + x: 70.0 y: -1.75 z: 0.0 orientation: @@ -19,54 +24,6 @@ goal_pose: z: 0.0 w: 1.0 segments: - - preferred_primitive: - id: 4735 - primitive_type: "" - primitives: - - id: 4735 - primitive_type: lane - - id: 5044 - primitive_type: lane - - preferred_primitive: - id: 4740 - primitive_type: "" - primitives: - - id: 4740 - primitive_type: lane - - id: 5048 - primitive_type: lane - - preferred_primitive: - id: 4745 - primitive_type: "" - primitives: - - id: 4745 - primitive_type: lane - - id: 5052 - primitive_type: lane - - preferred_primitive: - id: 4750 - primitive_type: "" - primitives: - - id: 4750 - primitive_type: lane - - id: 5056 - primitive_type: lane - - preferred_primitive: - id: 4755 - primitive_type: "" - primitives: - - id: 4755 - primitive_type: lane - - id: 5060 - primitive_type: lane - - preferred_primitive: - id: 4760 - primitive_type: "" - primitives: - - id: 4760 - primitive_type: lane - - id: 5064 - primitive_type: lane - preferred_primitive: id: 4765 primitive_type: "" @@ -84,7 +41,7 @@ segments: - id: 5072 primitive_type: lane - preferred_primitive: - id: 9594 + id: 4775 primitive_type: "" primitives: - id: 4775 @@ -115,139 +72,22 @@ segments: primitive_type: lane - id: 5088 primitive_type: lane - - preferred_primitive: - id: 5092 - primitive_type: "" - primitives: - - id: 4790 - primitive_type: lane - - id: 5092 - primitive_type: lane - - preferred_primitive: - id: 5096 - primitive_type: "" - primitives: - - id: 4795 - primitive_type: lane - - id: 5096 - primitive_type: lane - - preferred_primitive: - id: 5104 - primitive_type: "" - primitives: - - id: 4800 - primitive_type: lane - - id: 5104 - primitive_type: lane - - preferred_primitive: - id: 5100 - primitive_type: "" - primitives: - - id: 4805 - primitive_type: lane - - id: 5100 - primitive_type: lane - - preferred_primitive: - id: 9813 - primitive_type: "" - primitives: - - id: 4810 - primitive_type: lane - - id: 9813 - primitive_type: lane - - preferred_primitive: - id: 5112 - primitive_type: "" - primitives: - - id: 4815 - primitive_type: lane - - id: 5112 - primitive_type: lane - - preferred_primitive: - id: 5116 - primitive_type: "" - primitives: - - id: 4820 - primitive_type: lane - - id: 5116 - primitive_type: lane - - preferred_primitive: - id: 5120 - primitive_type: "" - primitives: - - id: 4825 - primitive_type: lane - - id: 5120 - primitive_type: lane - - preferred_primitive: - id: 5124 - primitive_type: "" - primitives: - - id: 4830 - primitive_type: lane - - id: 5124 - primitive_type: lane - - preferred_primitive: - id: 5128 - primitive_type: "" - primitives: - - id: 4835 - primitive_type: lane - - id: 5128 - primitive_type: lane - - preferred_primitive: - id: 5132 - primitive_type: "" - primitives: - - id: 4840 - primitive_type: lane - - id: 5132 - primitive_type: lane - - preferred_primitive: - id: 5136 - primitive_type: "" - primitives: - - id: 4845 - primitive_type: lane - - id: 5136 - primitive_type: lane - - preferred_primitive: - id: 5140 - primitive_type: "" - primitives: - - id: 4850 - primitive_type: lane - - id: 5140 - primitive_type: lane - - preferred_primitive: - id: 9817 - primitive_type: "" - primitives: - - id: 4855 - primitive_type: lane - - id: 9817 - primitive_type: lane - - preferred_primitive: - id: 9821 - primitive_type: "" - primitives: - - id: 4860 - primitive_type: lane - - id: 9821 - primitive_type: lane - - preferred_primitive: - id: 9825 - primitive_type: "" - primitives: - - id: 4865 - primitive_type: lane - - id: 9825 - primitive_type: lane - - preferred_primitive: - id: 5156 - primitive_type: "" - primitives: - - id: 4870 - primitive_type: lane - - id: 5156 - primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false From d5b65e3b4cf69df5157b5b6a25a485b571fdc579 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 20:21:06 +0900 Subject: [PATCH 16/19] Update README Signed-off-by: Zulfaqar Azmi --- planning/route_handler/README.md | 10 + .../images/lane_change_test_route.svg | 618 ++++++++++++++++++ 2 files changed, 628 insertions(+) create mode 100644 planning/route_handler/images/lane_change_test_route.svg diff --git a/planning/route_handler/README.md b/planning/route_handler/README.md index 4d369349bf364..e11e274163fa8 100644 --- a/planning/route_handler/README.md +++ b/planning/route_handler/README.md @@ -1,3 +1,13 @@ # route handler `route_handler` is a library for calculating driving route on the lanelet map. + +## Unit Testing + +The following illustrates the route that are used in the unit test + +### Lane change test route + +![debug](./images/lane_change_test_route.svg) + +- The route is based on map that can be obtained from `planning_test_utils\test_map` diff --git a/planning/route_handler/images/lane_change_test_route.svg b/planning/route_handler/images/lane_change_test_route.svg new file mode 100644 index 0000000000000..dfd57000713ad --- /dev/null +++ b/planning/route_handler/images/lane_change_test_route.svg @@ -0,0 +1,618 @@ + + + + + + + + + + + + + +
+
+
+ 4765 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ 4770 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4775 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4424 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4780 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5088 +
+
+
+ + + + + + + + + + + + +
+
+
+
goal pose
+ (70.0, -1.75) +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ 9590 +
+
+
+ + + + + + + + + + + + +
+
+
+ 9594 +
+
+
+ + + + + + + + + + + + +
+
+
+ 9598 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5072 +
+
+
+ + + + + + + + + + + + +
+
+
+ 5084 +
+
+
+ + + + + + + + + + + + +
+
+
+ 4765 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 3.0 meter +
+
+
+ + + + + + + + + +
+
+
+ preferred primitive +
+
+
+ + + + + + + + + +
+
+
+ primitive +
+
+
+ + + + + + + + + + + + +
+
+
+ pose1 +
+ (0.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ check point +
+ (25.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+ start pose +
+ (-50.0, 1.75) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + From 2aefa35f0104a1d9dadd2c68abe81efd1e1d3938 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 17 May 2024 20:29:01 +0900 Subject: [PATCH 17/19] Add more explanation in the readme Signed-off-by: Zulfaqar Azmi --- planning/route_handler/README.md | 9 +++++++-- planning/route_handler/images/route_handler_test.svg | 3 +++ 2 files changed, 10 insertions(+), 2 deletions(-) create mode 100644 planning/route_handler/images/route_handler_test.svg diff --git a/planning/route_handler/README.md b/planning/route_handler/README.md index e11e274163fa8..e8df4fd087930 100644 --- a/planning/route_handler/README.md +++ b/planning/route_handler/README.md @@ -4,10 +4,15 @@ ## Unit Testing -The following illustrates the route that are used in the unit test +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 -![debug](./images/lane_change_test_route.svg) +![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` diff --git a/planning/route_handler/images/route_handler_test.svg b/planning/route_handler/images/route_handler_test.svg new file mode 100644 index 0000000000000..278f6a093ab80 --- /dev/null +++ b/planning/route_handler/images/route_handler_test.svg @@ -0,0 +1,3 @@ + + +
route msg in yaml format
(to simulate /planning/mission_planner/route topic)
mock_data_parser
LaneletRoute
lanelet file
HADMapBin
test route handler
planning_test_utils
 \ No newline at end of file From 7e399bee327dd501193d8af29542fb3f7637a54f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 May 2024 11:31:14 +0000 Subject: [PATCH 18/19] style(pre-commit): autofix --- .../images/route_handler_test.svg | 230 +++++++++++++++++- 1 file changed, 229 insertions(+), 1 deletion(-) diff --git a/planning/route_handler/images/route_handler_test.svg b/planning/route_handler/images/route_handler_test.svg index 278f6a093ab80..38b2a97da0432 100644 --- a/planning/route_handler/images/route_handler_test.svg +++ b/planning/route_handler/images/route_handler_test.svg @@ -1,3 +1,231 @@ -
route msg in yaml format
(to simulate /planning/mission_planner/route topic)
mock_data_parser
LaneletRoute
lanelet file
HADMapBin
test route handler
planning_test_utils
 \ No newline at end of file + + + + + + + + + + + + + + + +
+
+
+ + route msg in yaml format +
+ + (to simulate /planning/mission_planner/route topic) +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ mock_data_parser +
+
+
+ + + + + + + + + + + + +
+
+
+ LaneletRoute +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ lanelet file +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ HADMapBin +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ test route handler +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ planning_test_utils +
+
+
+ + + + + + + From f136c6fb06c78fa8f8f563d6f9960333ab86ecad Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 21 May 2024 10:35:19 +0900 Subject: [PATCH 19/19] fix lane change test route width Signed-off-by: Muhammad Zulfaqar Azmi --- .../route_handler/images/lane_change_test_route.svg | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/route_handler/images/lane_change_test_route.svg b/planning/route_handler/images/lane_change_test_route.svg index dfd57000713ad..e71c8e13b1760 100644 --- a/planning/route_handler/images/lane_change_test_route.svg +++ b/planning/route_handler/images/lane_change_test_route.svg @@ -43,7 +43,7 @@
-
+
- 3.0 meter + 3.5 meter