From d88ddd5c14d987733baaf2dc0ddc1d970da7f40c Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 15 Jun 2023 19:18:26 +0900 Subject: [PATCH 01/51] feat(map_projection_loader): add map_projection_loader Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 60 ++++++++ .../map_projection_loader.hpp | 29 ++++ .../launch/map_projection_loader.launch.xml | 7 + map/map_projection_loader/package.xml | 25 ++++ .../src/map_projection_loader.cpp | 51 +++++++ .../src/map_projection_loader_node.cpp | 23 +++ .../test/data/projection_info_mgrs.yaml | 6 + .../test_map_projection_loader_mgrs.test.py | 140 ++++++++++++++++++ 8 files changed, 341 insertions(+) create mode 100644 map/map_projection_loader/CMakeLists.txt create mode 100644 map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp create mode 100644 map/map_projection_loader/launch/map_projection_loader.launch.xml create mode 100644 map/map_projection_loader/package.xml create mode 100644 map/map_projection_loader/src/map_projection_loader.cpp create mode 100644 map/map_projection_loader/src/map_projection_loader_node.cpp create mode 100644 map/map_projection_loader/test/data/projection_info_mgrs.yaml create mode 100644 map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt new file mode 100644 index 0000000000000..65b8443d06e86 --- /dev/null +++ b/map/map_projection_loader/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.14) +project(map_projection_loader) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS io) + +ament_auto_add_library(map_projection_loader SHARED + src/map_projection_loader_node.cpp +) +target_link_libraries(map_projection_loader yaml-cpp) + +# ament_auto_find_build_dependencies() + +# ament_auto_add_library(map_projection_loader_lib SHARED +# src/map_projection_loader.cpp +# ) + +# target_link_libraries(map_projection_loader_lib yaml-cpp) + +# ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) + +# target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) + +# target_link_libraries(map_projection_loader map_projection_loader_lib) +# target_include_directories(map_projection_loader PUBLIC include) + +# function(add_testcase filepath) +# get_filename_component(filename ${filepath} NAME) +# string(REGEX REPLACE ".cpp" "" test_name ${filename}) + +# ament_add_gtest(${test_name} ${filepath}) +# target_link_libraries("${test_name}" map_projection_loader_lib) +# ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +# endfunction() + +if(BUILD_TESTING) + add_launch_test( + test/test_map_projection_loader_launch.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) + # find_package(ament_cmake_gtest REQUIRED) + + # set(TEST_FILES + # test/test_map_projection_loader.cpp) + + # foreach(filepath ${TEST_FILES}) + # add_testcase(${filepath}) + # endforeach() +endif() + + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp new file mode 100644 index 0000000000000..4cc94ca4a0e56 --- /dev/null +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "rclcpp/rclcpp.hpp" +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_yaml_file(const std::string & filename); + +class MapProjectionLoader : public rclcpp::Node +{ +public: + MapProjectionLoader(); + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; \ No newline at end of file diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml new file mode 100644 index 0000000000000..1d9be0f1b3275 --- /dev/null +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml new file mode 100644 index 0000000000000..35514d7864c8a --- /dev/null +++ b/map/map_projection_loader/package.xml @@ -0,0 +1,25 @@ + + + + map_projection_loader + 0.1.0 + map_projection_loader package as a ROS 2 node + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + rclcpp_components + tier4_map_msgs + + ament_cmake_gmock + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp new file mode 100644 index 0000000000000..9bfea2e57eaf3 --- /dev/null +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -0,0 +1,51 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "map_projection_loader/map_projection_loader.hpp" + +#include +#include + + +tier4_map_msgs::msg::MapProjectorInfo load_yaml_file(const std::string & filename) +{ + YAML::Node data = YAML::LoadFile(filename); + + tier4_map_msgs::msg::MapProjectorInfo msg; + + // Assume the YAML file has a "type" key for map_projector_info. + msg.type = data["type"].as(); + msg.mgrs_grid = data["mgrs_grid"].as(); + + return msg; +} + +MapProjectionLoader::MapProjectionLoader() + : Node("map_projection_loader") +{ + publisher_ = this->create_publisher("map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + + std::string filename = this->declare_parameter("yaml_file_path", ""); + + std::ifstream file(filename); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filename.c_str()); + return; + } + std::cout << "Load !!!!!!!!!!!!! " << filename << std::endl; + const auto msg = load_yaml_file(filename); + + // Publish the message + publisher_->publish(msg); +} \ No newline at end of file diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp new file mode 100644 index 0000000000000..e0c5a440d91d3 --- /dev/null +++ b/map/map_projection_loader/src/map_projection_loader_node.cpp @@ -0,0 +1,23 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "map_projection_loader/map_projection_loader.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml new file mode 100644 index 0000000000000..4329599a8c1fc --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_mgrs.yaml @@ -0,0 +1,6 @@ +type: "MGRS" +mgrs_grid: "54SUE" +map_origin: + latitude: 0.0 + longitude: 0.0 + altitude: 0.0 diff --git a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py new file mode 100644 index 0000000000000..86e7b2a2ecb00 --- /dev/null +++ b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.logging import get_logger +import launch_testing +import pytest +import yaml +import rclpy +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy +from tier4_map_msgs.msg import MapProjectorInfo + + +logger = get_logger(__name__) + +YAML_FILE_PATH = 'test/data/projection_info_mgrs.yaml' + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + {"yaml_file_path": map_projection_info_path}, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.29 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "map_projector_info", + self.callback, + custom_qos_profile + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, + rclpy.task.Future(), + timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") + self.assertEqual(self.received_message.type, yaml_data['type']) + self.assertEqual(self.received_message.mgrs_grid, yaml_data['mgrs_grid']) + self.assertEqual(self.received_message.map_origin.latitude, yaml_data['map_origin']['latitude']) + self.assertEqual(self.received_message.map_origin.longitude, yaml_data['map_origin']['longitude']) + self.assertEqual(self.received_message.map_origin.altitude, yaml_data['map_origin']['altitude']) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From b8b98aa369b115a41393ff2852ec8a859a0923cd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 15 Jun 2023 10:20:31 +0000 Subject: [PATCH 02/51] style(pre-commit): autofix --- .../map_projection_loader.hpp | 3 +- .../launch/map_projection_loader.launch.xml | 2 +- .../src/map_projection_loader.cpp | 10 +- .../src/map_projection_loader_node.cpp | 2 +- .../test_map_projection_loader_mgrs.test.py | 198 +++++++++--------- 5 files changed, 111 insertions(+), 104 deletions(-) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 4cc94ca4a0e56..d4e06a2321068 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -13,6 +13,7 @@ // limitations under the License. #include "rclcpp/rclcpp.hpp" + #include "tier4_map_msgs/msg/map_projector_info.hpp" #include @@ -26,4 +27,4 @@ class MapProjectionLoader : public rclcpp::Node private: rclcpp::Publisher::SharedPtr publisher_; -}; \ No newline at end of file +}; diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 1d9be0f1b3275..14bf98434f275 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9bfea2e57eaf3..75a6f4456ca3d 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -15,8 +15,8 @@ #include "map_projection_loader/map_projection_loader.hpp" #include -#include +#include tier4_map_msgs::msg::MapProjectorInfo load_yaml_file(const std::string & filename) { @@ -31,10 +31,10 @@ tier4_map_msgs::msg::MapProjectorInfo load_yaml_file(const std::string & filenam return msg; } -MapProjectionLoader::MapProjectionLoader() - : Node("map_projection_loader") +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") { - publisher_ = this->create_publisher("map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + publisher_ = this->create_publisher( + "map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); std::string filename = this->declare_parameter("yaml_file_path", ""); @@ -48,4 +48,4 @@ MapProjectionLoader::MapProjectionLoader() // Publish the message publisher_->publish(msg); -} \ No newline at end of file +} diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp index e0c5a440d91d3..1d9336be0d9dd 100644 --- a/map/map_projection_loader/src/map_projection_loader_node.cpp +++ b/map/map_projection_loader/src/map_projection_loader_node.cpp @@ -20,4 +20,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py index 86e7b2a2ecb00..b4ec5cea57e16 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py @@ -20,121 +20,127 @@ from ament_index_python import get_package_share_directory import launch from launch import LaunchDescription -from launch_ros.actions import Node from launch.logging import get_logger +from launch_ros.actions import Node import launch_testing import pytest -import yaml import rclpy -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile from tier4_map_msgs.msg import MapProjectorInfo - +import yaml logger = get_logger(__name__) -YAML_FILE_PATH = 'test/data/projection_info_mgrs.yaml' +YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" + @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH - ) - - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - {"yaml_file_path": map_projection_info_path}, - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.29 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, - "map_projector_info", - self.callback, - custom_qos_profile + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, - rclpy.task.Future(), - timeout_sec=self.evaluation_time + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + {"yaml_file_path": map_projection_info_path}, + ], ) - # Load the yaml file directly - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, ) - with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) - # Test if message received - self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") - self.assertEqual(self.received_message.type, yaml_data['type']) - self.assertEqual(self.received_message.mgrs_grid, yaml_data['mgrs_grid']) - self.assertEqual(self.received_message.map_origin.latitude, yaml_data['map_origin']['latitude']) - self.assertEqual(self.received_message.map_origin.longitude, yaml_data['map_origin']['longitude']) - self.assertEqual(self.received_message.map_origin.altitude, yaml_data['map_origin']['altitude']) - self.test_node.destroy_subscription(subscription) +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.29 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, "map_projector_info", self.callback, custom_qos_profile + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + self.assertEqual( + self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] + ) + + self.test_node.destroy_subscription(subscription) @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From c3bbd3dd8206daed7155704aa09976db034b4450 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Jun 2023 13:07:32 +0900 Subject: [PATCH 03/51] Update default algorithm Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 46 +--- .../map_projection_loader.hpp | 2 - .../launch/map_projection_loader.launch.xml | 9 +- .../src/map_projection_loader.cpp | 48 ++--- ...test_map_projection_loader_default.test.py | 140 ++++++++++++ .../test_map_projection_loader_mgrs.test.py | 201 +++++++++--------- 6 files changed, 275 insertions(+), 171 deletions(-) create mode 100644 map/map_projection_loader/test/test_map_projection_loader_default.test.py diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 65b8443d06e86..4bae2c317cf86 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -4,57 +4,29 @@ project(map_projection_loader) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(PCL REQUIRED COMPONENTS io) - -ament_auto_add_library(map_projection_loader SHARED +ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp + src/map_projection_loader.cpp ) -target_link_libraries(map_projection_loader yaml-cpp) - -# ament_auto_find_build_dependencies() - -# ament_auto_add_library(map_projection_loader_lib SHARED -# src/map_projection_loader.cpp -# ) - -# target_link_libraries(map_projection_loader_lib yaml-cpp) - -# ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) - -# target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) - -# target_link_libraries(map_projection_loader map_projection_loader_lib) -# target_include_directories(map_projection_loader PUBLIC include) +ament_target_dependencies(map_projection_loader) -# function(add_testcase filepath) -# get_filename_component(filename ${filepath} NAME) -# string(REGEX REPLACE ".cpp" "" test_name ${filename}) - -# ament_add_gtest(${test_name} ${filepath}) -# target_link_libraries("${test_name}" map_projection_loader_lib) -# ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) -# endfunction() +target_link_libraries(map_projection_loader yaml-cpp) if(BUILD_TESTING) add_launch_test( - test/test_map_projection_loader_launch.test.py + test/test_map_projection_loader_mgrs.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_map_projection_loader_default.test.py TIMEOUT "30" ) install(DIRECTORY test/data/ DESTINATION share/${PROJECT_NAME}/test/data/ ) - # find_package(ament_cmake_gtest REQUIRED) - - # set(TEST_FILES - # test/test_map_projection_loader.cpp) - - # foreach(filepath ${TEST_FILES}) - # add_testcase(${filepath}) - # endforeach() endif() - ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index d4e06a2321068..4561a11684f19 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -18,8 +18,6 @@ #include -tier4_map_msgs::msg::MapProjectorInfo load_yaml_file(const std::string & filename); - class MapProjectionLoader : public rclcpp::Node { public: diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 14bf98434f275..25c7d80034992 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -1,7 +1,8 @@ - - - - + + + + + diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 75a6f4456ca3d..6052ef288c73b 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -15,37 +15,33 @@ #include "map_projection_loader/map_projection_loader.hpp" #include - #include -tier4_map_msgs::msg::MapProjectorInfo load_yaml_file(const std::string & filename) +MapProjectionLoader::MapProjectionLoader() + : Node("map_projection_loader") { - YAML::Node data = YAML::LoadFile(filename); - tier4_map_msgs::msg::MapProjectorInfo msg; - - // Assume the YAML file has a "type" key for map_projector_info. - msg.type = data["type"].as(); - msg.mgrs_grid = data["mgrs_grid"].as(); - - return msg; -} - -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") -{ - publisher_ = this->create_publisher( - "map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); - - std::string filename = this->declare_parameter("yaml_file_path", ""); - - std::ifstream file(filename); - if (!file.is_open()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filename.c_str()); - return; + if (this->declare_parameter("use_local_projector")) + { + RCLCPP_INFO(this->get_logger(), "Use default value for map_projector_info instead of loading file. NOTE: Please consider providing map_projector_info.yaml and setting `load_info_from_file` to false"); + msg.type = "local"; + } + else + { + std::string filename = this->declare_parameter("map_projector_info_path"); + std::ifstream file(filename); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s.", filename.c_str()); + return; + } + YAML::Node data = YAML::LoadFile(filename); + + // Assume the YAML file has a "type" key for map_projector_info. + msg.type = data["type"].as(); + msg.mgrs_grid = data["mgrs_grid"].as(); } - std::cout << "Load !!!!!!!!!!!!! " << filename << std::endl; - const auto msg = load_yaml_file(filename); // Publish the message + publisher_ = this->create_publisher("map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); publisher_->publish(msg); -} +} \ No newline at end of file diff --git a/map/map_projection_loader/test/test_map_projection_loader_default.test.py b/map/map_projection_loader/test/test_map_projection_loader_default.test.py new file mode 100644 index 0000000000000..f66c93aaa3410 --- /dev/null +++ b/map/map_projection_loader/test/test_map_projection_loader_default.test.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.logging import get_logger +import launch_testing +import pytest +import yaml +import rclpy +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy +from tier4_map_msgs.msg import MapProjectorInfo + + +logger = get_logger(__name__) + +YAML_FILE_PATH = 'test/data/projection_info_mgrs.yaml' + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "use_local_projector": True, + }, + ], + ) + + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.29 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "map_projector_info", + self.callback, + custom_qos_profile + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, + rclpy.task.Future(), + timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") + self.assertEqual(self.received_message.type, "local") + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py index b4ec5cea57e16..1f287d1733fb9 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py @@ -20,127 +20,124 @@ from ament_index_python import get_package_share_directory import launch from launch import LaunchDescription -from launch.logging import get_logger from launch_ros.actions import Node +from launch.logging import get_logger import launch_testing import pytest +import yaml import rclpy -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy from tier4_map_msgs.msg import MapProjectorInfo -import yaml -logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" +logger = get_logger(__name__) +YAML_FILE_PATH = 'test/data/projection_info_mgrs.yaml' @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL ) - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - {"yaml_file_path": map_projection_info_path}, - ], + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "map_projector_info", + self.callback, + custom_qos_profile ) - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, + rclpy.task.Future(), + timeout_sec=self.evaluation_time ) + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.29 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, "map_projector_info", self.callback, custom_qos_profile - ) - - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time - ) - - # Load the yaml file directly - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH - ) - with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) - - # Test if message received - self.assertIsNotNone( - self.received_message, "No message received on map_projector_info topic" - ) - self.assertEqual(self.received_message.type, yaml_data["type"]) - self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) - self.assertEqual( - self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] - ) - self.assertEqual( - self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] - ) - self.assertEqual( - self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] - ) - - self.test_node.destroy_subscription(subscription) + # Test if message received + self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") + self.assertEqual(self.received_message.type, yaml_data['type']) + self.assertEqual(self.received_message.mgrs_grid, yaml_data['mgrs_grid']) + self.assertEqual(self.received_message.map_origin.latitude, yaml_data['map_origin']['latitude']) + self.assertEqual(self.received_message.map_origin.longitude, yaml_data['map_origin']['longitude']) + self.assertEqual(self.received_message.map_origin.altitude, yaml_data['map_origin']['altitude']) + + self.test_node.destroy_subscription(subscription) @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From 1fa4347b6a590174129037c159668c81110ed181 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Jun 2023 13:12:38 +0900 Subject: [PATCH 04/51] fix test Signed-off-by: kminoda --- .../test_map_projection_loader_default.test.py | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/map/map_projection_loader/test/test_map_projection_loader_default.test.py b/map/map_projection_loader/test/test_map_projection_loader_default.test.py index f66c93aaa3410..7b6a47ee2d940 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_default.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_default.test.py @@ -32,21 +32,14 @@ logger = get_logger(__name__) -YAML_FILE_PATH = 'test/data/projection_info_mgrs.yaml' - @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH - ) - map_projection_loader_node = Node( package="map_projection_loader", executable="map_projection_loader", output="screen", parameters=[ { - "map_projector_info_path": map_projection_info_path, "use_local_projector": True, }, ], @@ -82,7 +75,7 @@ def tearDownClass(cls): def setUp(self): # Create a ROS node for tests self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.29 # 200ms + self.evaluation_time = 0.2 # 200ms self.received_message = None def tearDown(self): @@ -119,13 +112,6 @@ def test_node_link(self): timeout_sec=self.evaluation_time ) - # Load the yaml file directly - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH - ) - with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) - # Test if message received self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") self.assertEqual(self.received_message.type, "local") From 37f9619bb5fce54dd45816e879bb2fe886e27ad4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 16 Jun 2023 04:14:52 +0000 Subject: [PATCH 05/51] style(pre-commit): autofix --- .../launch/map_projection_loader.launch.xml | 2 +- .../src/map_projection_loader.cpp | 21 +- ...test_map_projection_loader_default.test.py | 169 ++++++++------- .../test_map_projection_loader_mgrs.test.py | 204 +++++++++--------- 4 files changed, 201 insertions(+), 195 deletions(-) diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 25c7d80034992..186d4d59ca464 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 6052ef288c73b..9b2421b5c31c8 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -15,19 +15,19 @@ #include "map_projection_loader/map_projection_loader.hpp" #include + #include -MapProjectionLoader::MapProjectionLoader() - : Node("map_projection_loader") +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") { tier4_map_msgs::msg::MapProjectorInfo msg; - if (this->declare_parameter("use_local_projector")) - { - RCLCPP_INFO(this->get_logger(), "Use default value for map_projector_info instead of loading file. NOTE: Please consider providing map_projector_info.yaml and setting `load_info_from_file` to false"); + if (this->declare_parameter("use_local_projector")) { + RCLCPP_INFO( + this->get_logger(), + "Use default value for map_projector_info instead of loading file. NOTE: Please consider " + "providing map_projector_info.yaml and setting `load_info_from_file` to false"); msg.type = "local"; - } - else - { + } else { std::string filename = this->declare_parameter("map_projector_info_path"); std::ifstream file(filename); if (!file.is_open()) { @@ -42,6 +42,7 @@ MapProjectionLoader::MapProjectionLoader() } // Publish the message - publisher_ = this->create_publisher("map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + publisher_ = this->create_publisher( + "map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); publisher_->publish(msg); -} \ No newline at end of file +} diff --git a/map/map_projection_loader/test/test_map_projection_loader_default.test.py b/map/map_projection_loader/test/test_map_projection_loader_default.test.py index 7b6a47ee2d940..00845cacaf947 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_default.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_default.test.py @@ -20,107 +20,106 @@ from ament_index_python import get_package_share_directory import launch from launch import LaunchDescription -from launch_ros.actions import Node from launch.logging import get_logger +from launch_ros.actions import Node import launch_testing import pytest -import yaml import rclpy -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile from tier4_map_msgs.msg import MapProjectorInfo - +import yaml logger = get_logger(__name__) + @pytest.mark.launch_test def generate_test_description(): - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - { - "use_local_projector": True, - }, - ], - ) - - - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, - "map_projector_info", - self.callback, - custom_qos_profile + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "use_local_projector": True, + }, + ], ) - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, - rclpy.task.Future(), - timeout_sec=self.evaluation_time + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, ) - # Test if message received - self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") - self.assertEqual(self.received_message.type, "local") - self.test_node.destroy_subscription(subscription) +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, "map_projector_info", self.callback, custom_qos_profile + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, "local") + + self.test_node.destroy_subscription(subscription) @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py index 1f287d1733fb9..cf8b225acbb34 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py @@ -20,124 +20,130 @@ from ament_index_python import get_package_share_directory import launch from launch import LaunchDescription -from launch_ros.actions import Node from launch.logging import get_logger +from launch_ros.actions import Node import launch_testing import pytest -import yaml import rclpy -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile from tier4_map_msgs.msg import MapProjectorInfo - +import yaml logger = get_logger(__name__) -YAML_FILE_PATH = 'test/data/projection_info_mgrs.yaml' +YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" + @pytest.mark.launch_test def generate_test_description(): - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH - ) - - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - { - "map_projector_info_path": map_projection_info_path, - "use_local_projector": False, - }, - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, - "map_projector_info", - self.callback, - custom_qos_profile + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH ) - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, - rclpy.task.Future(), - timeout_sec=self.evaluation_time + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "use_local_projector": False, + }, + ], ) - # Load the yaml file directly - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, ) - with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) - # Test if message received - self.assertIsNotNone(self.received_message, "No message received on map_projector_info topic") - self.assertEqual(self.received_message.type, yaml_data['type']) - self.assertEqual(self.received_message.mgrs_grid, yaml_data['mgrs_grid']) - self.assertEqual(self.received_message.map_origin.latitude, yaml_data['map_origin']['latitude']) - self.assertEqual(self.received_message.map_origin.longitude, yaml_data['map_origin']['longitude']) - self.assertEqual(self.received_message.map_origin.altitude, yaml_data['map_origin']['altitude']) - self.test_node.destroy_subscription(subscription) +class TestEKFLocalizer(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, "map_projector_info", self.callback, custom_qos_profile + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + self.assertEqual( + self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] + ) + + self.test_node.destroy_subscription(subscription) @launch_testing.post_shutdown_test() class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From a6ac915a447304bc1c9e64b930cfcae19b986cd3 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 16 Jun 2023 13:50:44 +0900 Subject: [PATCH 06/51] add readme Signed-off-by: kminoda --- map/map_projection_loader/README.md | 16 ++++++++++++++++ .../launch/map_projection_loader.launch.xml | 1 + .../src/map_projection_loader.cpp | 2 +- 3 files changed, 18 insertions(+), 1 deletion(-) create mode 100644 map/map_projection_loader/README.md diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md new file mode 100644 index 0000000000000..4074b5cc2735c --- /dev/null +++ b/map/map_projection_loader/README.md @@ -0,0 +1,16 @@ +# map_projection_loader + +## Feature +`map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate the Autoware is operating on. +This is a necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. + +## Published Topics + +- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information + +## Parameters + +| Name | Type | Description | Default value | +| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | +| use_local_projector | bool | A flag for defining whether to use local coordinate | false | +| map_projector_info_path | std::string | A path to map_projector_info.yaml (used only when `use_local_projector` is false) | N/A | diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 186d4d59ca464..ffdb0f36bbfc2 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -2,6 +2,7 @@ + diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9b2421b5c31c8..9f408abbfd228 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -43,6 +43,6 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") // Publish the message publisher_ = this->create_publisher( - "map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + "~/map_projector_info", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); publisher_->publish(msg); } From 7c1b2d147611bb430eff4bd220527d3348bec510 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 16 Jun 2023 04:52:20 +0000 Subject: [PATCH 07/51] style(pre-commit): autofix --- map/map_projection_loader/README.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 4074b5cc2735c..e35b4db9d9751 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -1,6 +1,7 @@ # map_projection_loader ## Feature + `map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate the Autoware is operating on. This is a necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. @@ -10,7 +11,7 @@ This is a necessary information especially when you want to convert from global ## Parameters -| Name | Type | Description | Default value | -| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | -| use_local_projector | bool | A flag for defining whether to use local coordinate | false | -| map_projector_info_path | std::string | A path to map_projector_info.yaml (used only when `use_local_projector` is false) | N/A | +| Name | Type | Description | Default value | +| :---------------------- | :---------- | :-------------------------------------------------------------------------------- | :------------ | +| use_local_projector | bool | A flag for defining whether to use local coordinate | false | +| map_projector_info_path | std::string | A path to map_projector_info.yaml (used only when `use_local_projector` is false) | N/A | From e1f373870bf9d76e02dc0b03e6d5d7bb54064542 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 20 Jul 2023 16:05:45 +0900 Subject: [PATCH 08/51] fix launch file and fix map_loader Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.py | 20 +++++++- .../map_loader/lanelet2_map_loader_node.hpp | 3 -- .../lanelet2_map_loader_node.cpp | 27 +---------- .../map_projection_loader.hpp | 3 ++ .../launch/map_projection_loader.launch.xml | 2 + .../src/map_projection_loader.cpp | 47 ++++++++++++------- 6 files changed, 55 insertions(+), 47 deletions(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index f9459be3b8924..e2ee86376aca3 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ament_index_python import get_package_share_directory + import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -25,8 +27,10 @@ from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource import yaml - +import os def launch_setup(context, *args, **kwargs): lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( @@ -110,6 +114,19 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + map_projection_loader_launch_file = os.path.join( + get_package_share_directory("map_projection_loader"), + "launch", + "map_projection_loader.launch.xml", + ) + map_projection_loader = IncludeLaunchDescription( + AnyLaunchDescriptionSource(map_projection_loader_launch_file), + launch_arguments={ + 'map_projector_info_path': 'value1', + 'lanelet2_map_path': LaunchConfiguration("lanelet2_map_path"), + }.items(), + ) + container = ComposableNodeContainer( name="map_container", namespace="", @@ -129,6 +146,7 @@ def launch_setup(context, *args, **kwargs): PushRosNamespace("map"), container, map_hash_generator, + map_projection_loader, ] ) diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 16681f3bd290d..72f54efd75ae3 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -36,9 +36,6 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0); - static const MapProjectorInfo get_map_projector_type( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index aafa66a35be84..3d4f122291ab2 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -69,16 +69,11 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options // create map bin msg const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); - const auto map_projector_type_msg = get_map_projector_type( - lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon); // create publisher and publish pub_map_bin_ = create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); - // create publisher and publish - pub_map_projector_type_ = - create_publisher("map_projector_type", rclcpp::QoS{1}.transient_local()); - pub_map_projector_type_->publish(map_projector_type_msg); + } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( @@ -137,26 +132,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -const MapProjectorInfo Lanelet2MapLoaderNode::get_map_projector_type( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon) -{ - lanelet::ErrorMessages errors{}; - MapProjectorInfo map_projector_type_msg; - if (lanelet2_map_projector_type == "MGRS") { - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - map_projector_type_msg.type = "MGRS"; - map_projector_type_msg.mgrs_grid = projector.getProjectedMGRSGrid(); - } else if (lanelet2_map_projector_type == "UTM") { - map_projector_type_msg.type = "UTM"; - map_projector_type_msg.map_origin.latitude = map_origin_lat; - map_projector_type_msg.map_origin.longitude = map_origin_lon; - } else { - map_projector_type_msg.type = "local"; - } - return map_projector_type_msg; -} HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 4561a11684f19..f4ade774180e3 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -18,6 +18,9 @@ #include +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); + class MapProjectionLoader : public rclcpp::Node { public: diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index ffdb0f36bbfc2..64167586c2d4f 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -1,9 +1,11 @@ + + diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9f408abbfd228..8072ca8482c5a 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -18,27 +18,40 @@ #include +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) +{ + YAML::Node data = YAML::LoadFile(filename); + + tier4_map_msgs::msg::MapProjectorInfo msg; + msg.type = data["type"].as(); + msg.mgrs_grid = data["mgrs_grid"].as(); + + return msg; +} + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +{ + (void)filename; + tier4_map_msgs::msg::MapProjectorInfo msg; + return msg; +} + + MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") { + std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); + std::ifstream file(yaml_filename); + tier4_map_msgs::msg::MapProjectorInfo msg; - if (this->declare_parameter("use_local_projector")) { - RCLCPP_INFO( - this->get_logger(), - "Use default value for map_projector_info instead of loading file. NOTE: Please consider " - "providing map_projector_info.yaml and setting `load_info_from_file` to false"); - msg.type = "local"; + + bool use_yaml_file = file.is_open(); + if (use_yaml_file) { + RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + msg = load_info_from_yaml(yaml_filename); } else { - std::string filename = this->declare_parameter("map_projector_info_path"); - std::ifstream file(filename); - if (!file.is_open()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s.", filename.c_str()); - return; - } - YAML::Node data = YAML::LoadFile(filename); - - // Assume the YAML file has a "type" key for map_projector_info. - msg.type = data["type"].as(); - msg.mgrs_grid = data["mgrs_grid"].as(); + RCLCPP_WARN(this->get_logger(), "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. Please use map_projector_info.yaml instead."); + msg = load_info_from_lanelet2_map(yaml_filename); } // Publish the message From 1c4fa44fa559500f5ece0f50c3647716ee60fbcf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 20 Jul 2023 07:07:27 +0000 Subject: [PATCH 09/51] style(pre-commit): autofix --- launch/tier4_map_launch/launch/map.launch.py | 13 +++++++------ .../lanelet2_map_loader_node.cpp | 2 -- .../src/map_projection_loader.cpp | 6 ++++-- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index e2ee86376aca3..f8e5e8bc20b15 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -12,25 +12,26 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_index_python import get_package_share_directory +import os +from ament_index_python import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition +from launch.launch_description_sources import AnyLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import AnyLaunchDescriptionSource import yaml -import os + def launch_setup(context, *args, **kwargs): lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( @@ -122,8 +123,8 @@ def launch_setup(context, *args, **kwargs): map_projection_loader = IncludeLaunchDescription( AnyLaunchDescriptionSource(map_projection_loader_launch_file), launch_arguments={ - 'map_projector_info_path': 'value1', - 'lanelet2_map_path': LaunchConfiguration("lanelet2_map_path"), + "map_projector_info_path": "value1", + "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), }.items(), ) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 3d4f122291ab2..f2e3c0b525968 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -73,7 +73,6 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options pub_map_bin_ = create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); - } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( @@ -132,7 +131,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } - HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 8072ca8482c5a..be1943001fbc1 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -36,7 +36,6 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str return msg; } - MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") { std::string yaml_filename = this->declare_parameter("map_projector_info_path"); @@ -50,7 +49,10 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); msg = load_info_from_yaml(yaml_filename); } else { - RCLCPP_WARN(this->get_logger(), "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. Please use map_projector_info.yaml instead."); + RCLCPP_WARN( + this->get_logger(), + "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead."); msg = load_info_from_lanelet2_map(yaml_filename); } From 3257cf540e74f3bf2f8e3e7a57342d4a0145aea1 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 20 Jul 2023 16:33:57 +0900 Subject: [PATCH 10/51] update lanelet2 Signed-off-by: kminoda --- .../load_info_from_lanelet2_map.hpp | 42 +++++++++++++++++++ .../map_projection_loader.hpp | 1 - map/map_projection_loader/package.xml | 1 + .../src/map_projection_loader.cpp | 15 ++----- 4 files changed, 47 insertions(+), 12 deletions(-) create mode 100644 map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp new file mode 100644 index 0000000000000..97df162fa2502 --- /dev/null +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -0,0 +1,42 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "tier4_map_msgs/msg/map_projector_info.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +{ + tier4_map_msgs::msg::MapProjectorInfo msg; + + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); + if (!errors.empty()) { + throw std::runtime_error("Error occurred while loading lanelet2 map"); + } + + msg.type = "MGRS"; + msg.mgrs_grid = projector.getProjectedMGRSGrid(); + return msg; +} \ No newline at end of file diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index f4ade774180e3..991757bb271b6 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -19,7 +19,6 @@ #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); -tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); class MapProjectionLoader : public rclcpp::Node { diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 35514d7864c8a..b952667da94d5 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -13,6 +13,7 @@ rclcpp rclcpp_components tier4_map_msgs + lanelet2_extension ament_cmake_gmock ament_lint_auto diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index be1943001fbc1..0634a811dd9a1 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "map_projection_loader/map_projection_loader.hpp" +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include @@ -29,12 +30,6 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } -tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) -{ - (void)filename; - tier4_map_msgs::msg::MapProjectorInfo msg; - return msg; -} MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") { @@ -49,11 +44,9 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); msg = load_info_from_yaml(yaml_filename); } else { - RCLCPP_WARN( - this->get_logger(), - "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead."); - msg = load_info_from_lanelet2_map(yaml_filename); + RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); + RCLCPP_WARN(this->get_logger(), "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. Please use map_projector_info.yaml instead."); + msg = load_info_from_lanelet2_map(lanelet2_map_filename); } // Publish the message From 7973c971908cb91fb6bc5e34598138286cfeec89 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 20 Jul 2023 16:36:31 +0900 Subject: [PATCH 11/51] fill yaml file path Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index f8e5e8bc20b15..f3ae5873f0968 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -123,7 +123,7 @@ def launch_setup(context, *args, **kwargs): map_projection_loader = IncludeLaunchDescription( AnyLaunchDescriptionSource(map_projection_loader_launch_file), launch_arguments={ - "map_projector_info_path": "value1", + "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), }.items(), ) @@ -178,6 +178,11 @@ def add_launch_arg(name: str, default_value=None, description=None): [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], "path to pointcloud map metadata file", ), + add_launch_arg( + "map_projector_info_path", + [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], + "path to map projector info yaml file", + ), add_launch_arg( "lanelet2_map_loader_param_path", [ From b7175a0b6534b692f7cc8655f177fbf134f9ab25 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 20 Jul 2023 07:37:04 +0000 Subject: [PATCH 12/51] style(pre-commit): autofix --- .../load_info_from_lanelet2_map.hpp | 11 ++++++----- map/map_projection_loader/package.xml | 2 +- .../src/map_projection_loader.cpp | 7 +++++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp index 97df162fa2502..5597dab3d17e5 100644 --- a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_map_msgs/msg/map_projector_info.hpp" - -#include -#include #include #include #include #include + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + #include #include #include #include +#include + tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) { tier4_map_msgs::msg::MapProjectorInfo msg; @@ -39,4 +40,4 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str msg.type = "MGRS"; msg.mgrs_grid = projector.getProjectedMGRSGrid(); return msg; -} \ No newline at end of file +} diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b952667da94d5..0cd6f56ab83a3 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -10,10 +10,10 @@ ament_cmake_auto autoware_cmake + lanelet2_extension rclcpp rclcpp_components tier4_map_msgs - lanelet2_extension ament_cmake_gmock ament_lint_auto diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 0634a811dd9a1..48f5ca1e483ef 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "map_projection_loader/map_projection_loader.hpp" + #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include @@ -30,7 +31,6 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } - MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") { std::string yaml_filename = this->declare_parameter("map_projector_info_path"); @@ -45,7 +45,10 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") msg = load_info_from_yaml(yaml_filename); } else { RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); - RCLCPP_WARN(this->get_logger(), "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. Please use map_projector_info.yaml instead."); + RCLCPP_WARN( + this->get_logger(), + "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead."); msg = load_info_from_lanelet2_map(lanelet2_map_filename); } From 1d3e8c22cb7fcadbf9b1932ffa745de486455447 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 20 Jul 2023 17:01:39 +0900 Subject: [PATCH 13/51] update readme Signed-off-by: kminoda --- map/map_loader/README.md | 1 + map/map_projection_loader/README.md | 59 +++++++++++++++++++++++++++-- 2 files changed, 56 insertions(+), 4 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 0494b57dcfab9..2dcb041e685d1 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -69,6 +69,7 @@ sample-map-rosbag │ ├── B.pcd │ ├── C.pcd │ └── ... +├── map_projector_info.yaml └── pointcloud_map_metadata.yaml ``` diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index e35b4db9d9751..5dcc5113c221a 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -5,13 +5,64 @@ `map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate the Autoware is operating on. This is a necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. +- If `map_projector_info_path` DOES exist, this node loads it and publish the map projection information accordingly. +- If `map_projector_info_path` does NOT exist, the node assumes that you are using `MGRS` projection type, and loads lanelet2 map instead to extract MGRS grid. + - **DEPRECATED WARNING: This interface that uses lanelet2 map is not recommended. Please prepare YAML file instead.** + + +## Map projector info file specification +You need to provide a YAML file, namely `map_projector_info.yaml` under the `map_path` directory. For `pointcloud_map_metadata.yaml`, please refer to the Readme of `map_loader`. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +├── map_projector_info.yaml +└── pointcloud_map_metadata.yaml +``` + +### Using local coordinate +```yaml +type: "Local" +``` + +### Using MGRS +If you want to use MGRS, please specify MGRS grid as well. +```yaml +type: "MGRS" +mgrs_grid: "54SUE" +``` + +### Using UTM +If you want to use UTM, please specify the map origin as well. +``` +type: "UTM" +map_origin: + latitude: 35.6092 + longitude: 139.7303 + altitude: 0.0 +``` + +### Using Transverse Mercator +If you want to use Transverse Mercator projection, please specify the map origin as well. + +``` +type: "TransverseMercator" +map_origin: + latitude: 36.1148 + longitude: 137.9532 + altitude: 0.0 +``` + + ## Published Topics - ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information ## Parameters -| Name | Type | Description | Default value | -| :---------------------- | :---------- | :-------------------------------------------------------------------------------- | :------------ | -| use_local_projector | bool | A flag for defining whether to use local coordinate | false | -| map_projector_info_path | std::string | A path to map_projector_info.yaml (used only when `use_local_projector` is false) | N/A | +| Name | Type | Description | +| :---------------------- | :---------- | :-------------------------------------------------------------------------------- | +| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | +| lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) | + From 046dc1115d289445b01f7fea5f3cc22c3b99cd76 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 20 Jul 2023 08:03:33 +0000 Subject: [PATCH 14/51] style(pre-commit): autofix --- map/map_projection_loader/README.md | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 5dcc5113c221a..5fd8d53956e59 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -9,8 +9,8 @@ This is a necessary information especially when you want to convert from global - If `map_projector_info_path` does NOT exist, the node assumes that you are using `MGRS` projection type, and loads lanelet2 map instead to extract MGRS grid. - **DEPRECATED WARNING: This interface that uses lanelet2 map is not recommended. Please prepare YAML file instead.** - ## Map projector info file specification + You need to provide a YAML file, namely `map_projector_info.yaml` under the `map_path` directory. For `pointcloud_map_metadata.yaml`, please refer to the Readme of `map_loader`. ```bash @@ -22,19 +22,24 @@ sample-map-rosbag ``` ### Using local coordinate + ```yaml type: "Local" ``` ### Using MGRS + If you want to use MGRS, please specify MGRS grid as well. + ```yaml type: "MGRS" mgrs_grid: "54SUE" ``` ### Using UTM + If you want to use UTM, please specify the map origin as well. + ``` type: "UTM" map_origin: @@ -44,6 +49,7 @@ map_origin: ``` ### Using Transverse Mercator + If you want to use Transverse Mercator projection, please specify the map origin as well. ``` @@ -54,15 +60,13 @@ map_origin: altitude: 0.0 ``` - ## Published Topics - ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information ## Parameters -| Name | Type | Description | -| :---------------------- | :---------- | :-------------------------------------------------------------------------------- | -| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | +| Name | Type | Description | +| :---------------------- | :---------- | :------------------------------------------------------------------------------- | +| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | | lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) | - From 9a89f2f7221ceadaec2e1a42679d30fe96b98918 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 24 Jul 2023 11:16:02 +0900 Subject: [PATCH 15/51] minor fix Signed-off-by: kminoda --- map/map_projection_loader/README.md | 9 ++++++--- .../test/test_map_projection_loader_default.test.py | 1 - 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 5fd8d53956e59..ce90a3cb385a0 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -24,6 +24,7 @@ sample-map-rosbag ### Using local coordinate ```yaml +# map_projector_info.yaml type: "Local" ``` @@ -32,6 +33,7 @@ type: "Local" If you want to use MGRS, please specify MGRS grid as well. ```yaml +# map_projector_info.yaml type: "MGRS" mgrs_grid: "54SUE" ``` @@ -39,8 +41,8 @@ mgrs_grid: "54SUE" ### Using UTM If you want to use UTM, please specify the map origin as well. - -``` +```yaml +# map_projector_info.yaml type: "UTM" map_origin: latitude: 35.6092 @@ -52,7 +54,8 @@ map_origin: If you want to use Transverse Mercator projection, please specify the map origin as well. -``` +```yaml +# map_projector_info.yaml type: "TransverseMercator" map_origin: latitude: 36.1148 diff --git a/map/map_projection_loader/test/test_map_projection_loader_default.test.py b/map/map_projection_loader/test/test_map_projection_loader_default.test.py index 00845cacaf947..6f67835526f14 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_default.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_default.test.py @@ -14,7 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os import unittest from ament_index_python import get_package_share_directory From 4fb2cebaf5bdead3fbf6be8ae86693e68bed4978 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jul 2023 02:18:42 +0000 Subject: [PATCH 16/51] style(pre-commit): autofix --- map/map_projection_loader/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index ce90a3cb385a0..e026f2a88f585 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -41,6 +41,7 @@ mgrs_grid: "54SUE" ### Using UTM If you want to use UTM, please specify the map origin as well. + ```yaml # map_projector_info.yaml type: "UTM" From 331ced782781713198bb7235a795eadcd16ddf74 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 24 Jul 2023 11:23:42 +0900 Subject: [PATCH 17/51] fix test Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 6 +- ...test_map_projection_loader_default.test.py | 124 ------------------ ...t_map_projection_loader_from_yaml.test.py} | 3 +- 3 files changed, 3 insertions(+), 130 deletions(-) delete mode 100644 map/map_projection_loader/test/test_map_projection_loader_default.test.py rename map/map_projection_loader/test/{test_map_projection_loader_mgrs.test.py => test_map_projection_loader_from_yaml.test.py} (96%) diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 4bae2c317cf86..2b22d347de030 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -14,11 +14,7 @@ target_link_libraries(map_projection_loader yaml-cpp) if(BUILD_TESTING) add_launch_test( - test/test_map_projection_loader_mgrs.test.py - TIMEOUT "30" - ) - add_launch_test( - test/test_map_projection_loader_default.test.py + test/test_map_projection_loader_from_yaml.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/map/map_projection_loader/test/test_map_projection_loader_default.test.py b/map/map_projection_loader/test/test_map_projection_loader_default.test.py deleted file mode 100644 index 6f67835526f14..0000000000000 --- a/map/map_projection_loader/test/test_map_projection_loader_default.test.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 TIER IV, Inc. -# -# 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. - -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch.logging import get_logger -from launch_ros.actions import Node -import launch_testing -import pytest -import rclpy -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo -import yaml - -logger = get_logger(__name__) - - -@pytest.mark.launch_test -def generate_test_description(): - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - { - "use_local_projector": True, - }, - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -class TestEKFLocalizer(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, "map_projector_info", self.callback, custom_qos_profile - ) - - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time - ) - - # Test if message received - self.assertIsNotNone( - self.received_message, "No message received on map_projector_info topic" - ) - self.assertEqual(self.received_message.type, "local") - - self.test_node.destroy_subscription(subscription) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py b/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py similarity index 96% rename from map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py rename to map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py index cf8b225acbb34..9f05393de7152 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_mgrs.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py @@ -49,6 +49,7 @@ def generate_test_description(): parameters=[ { "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", "use_local_projector": False, }, ], @@ -108,7 +109,7 @@ def test_node_link(self): # Create subscription to map_projector_info topic subscription = self.test_node.create_subscription( - MapProjectorInfo, "map_projector_info", self.callback, custom_qos_profile + MapProjectorInfo, "/map_projection_loader/map_projector_info", self.callback, custom_qos_profile ) # Give time for the message to be received and processed From a439d6cd56d2bf282b8a46ab6e81f44b67209177 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jul 2023 02:25:37 +0000 Subject: [PATCH 18/51] style(pre-commit): autofix --- .../test/test_map_projection_loader_from_yaml.test.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py b/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py index 9f05393de7152..ece0e28067ceb 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py +++ b/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py @@ -109,7 +109,10 @@ def test_node_link(self): # Create subscription to map_projector_info topic subscription = self.test_node.create_subscription( - MapProjectorInfo, "/map_projection_loader/map_projector_info", self.callback, custom_qos_profile + MapProjectorInfo, + "/map_projection_loader/map_projector_info", + self.callback, + custom_qos_profile, ) # Give time for the message to be received and processed From 02c3c1d963969a12dabd70f8acdce6defa8e93eb Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 24 Jul 2023 11:26:47 +0900 Subject: [PATCH 19/51] add include guard Signed-off-by: kminoda --- .../map_projection_loader/load_info_from_lanelet2_map.hpp | 5 +++++ .../include/map_projection_loader/map_projection_loader.hpp | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp index 5597dab3d17e5..7c41fd0055b46 100644 --- a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ +#define MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ + #include #include #include @@ -41,3 +44,5 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str msg.mgrs_grid = projector.getProjectedMGRSGrid(); return msg; } + +#endif // MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 991757bb271b6..ce08cae007752 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ +#define MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ + #include "rclcpp/rclcpp.hpp" #include "tier4_map_msgs/msg/map_projector_info.hpp" @@ -28,3 +31,5 @@ class MapProjectionLoader : public rclcpp::Node private: rclcpp::Publisher::SharedPtr publisher_; }; + +#endif MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ From 4aea8760f04a426f0b81234a98a67328bf727237 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jul 2023 02:28:53 +0000 Subject: [PATCH 20/51] style(pre-commit): autofix --- .../include/map_projection_loader/map_projection_loader.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index ce08cae007752..de19ac5dabda4 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -32,4 +32,4 @@ class MapProjectionLoader : public rclcpp::Node rclcpp::Publisher::SharedPtr publisher_; }; -#endif MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ +#endif // MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ From f2d407d5eb85088d525c2ceaae75ab740a817994 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 27 Jul 2023 15:15:53 +0900 Subject: [PATCH 21/51] update test Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 6 +- .../src/map_projection_loader.cpp | 9 +- .../projection_info_transverse_mercator.yaml | 5 + ...om_yaml.test.py => test_load_mgrs.test.py} | 2 +- .../test_load_transverse_mercator.test.py | 152 ++++++++++++++++++ 5 files changed, 170 insertions(+), 4 deletions(-) create mode 100644 map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml rename map/map_projection_loader/test/{test_map_projection_loader_from_yaml.test.py => test_load_mgrs.test.py} (99%) create mode 100644 map/map_projection_loader/test/test_load_transverse_mercator.test.py diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 2b22d347de030..f7a68f68e60c1 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -14,7 +14,11 @@ target_link_libraries(map_projection_loader yaml-cpp) if(BUILD_TESTING) add_launch_test( - test/test_map_projection_loader_from_yaml.test.py + test/test_load_mgrs.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_load_transverse_mercator.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 48f5ca1e483ef..86b4e28a325b2 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -26,8 +26,13 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi tier4_map_msgs::msg::MapProjectorInfo msg; msg.type = data["type"].as(); - msg.mgrs_grid = data["mgrs_grid"].as(); - + if (msg.type == "MGRS") { + msg.mgrs_grid = data["mgrs_grid"].as(); + } else if (msg.type == "TransverseMercator") { + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); + } return msg; } diff --git a/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml b/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml new file mode 100644 index 0000000000000..bfc7c4be81177 --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml @@ -0,0 +1,5 @@ +type: "TransverseMercator" +map_origin: + latitude: 0.0 + longitude: 0.0 + altitude: 0.0 diff --git a/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py b/map/map_projection_loader/test/test_load_mgrs.test.py similarity index 99% rename from map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py rename to map/map_projection_loader/test/test_load_mgrs.test.py index ece0e28067ceb..c623be1331fb1 100644 --- a/map/map_projection_loader/test/test_map_projection_loader_from_yaml.test.py +++ b/map/map_projection_loader/test/test_load_mgrs.test.py @@ -71,7 +71,7 @@ def generate_test_description(): ) -class TestEKFLocalizer(unittest.TestCase): +class TestLoadMGRS(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context for the test node diff --git a/map/map_projection_loader/test/test_load_transverse_mercator.test.py b/map/map_projection_loader/test/test_load_transverse_mercator.test.py new file mode 100644 index 0000000000000..febcad95fd327 --- /dev/null +++ b/map/map_projection_loader/test/test_load_transverse_mercator.test.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/projection_info_transverse_mercator.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadTransverseMercator(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map_projection_loader/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + self.assertEqual( + self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From 76215548a6b35165c4ef2a94a3067da892d5c77c Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 27 Jul 2023 18:55:59 +0900 Subject: [PATCH 22/51] update map_loader Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.py | 5 +++- .../map_loader/lanelet2_map_loader_node.hpp | 4 +++- .../lanelet2_map_loader_node.cpp | 24 +++++++++++++++---- 3 files changed, 27 insertions(+), 6 deletions(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index f3ae5873f0968..169c53814df22 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -62,7 +62,10 @@ def launch_setup(context, *args, **kwargs): package="map_loader", plugin="Lanelet2MapLoaderNode", name="lanelet2_map_loader", - remappings=[("output/lanelet2_map", "vector_map")], + remappings=[ + ("output/lanelet2_map", "vector_map"), + ("input/map_projector_info", "map_projector_type"), + ], parameters=[ { "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 72f54efd75ae3..2fb8b893d8fce 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -41,8 +41,10 @@ class Lanelet2MapLoaderNode : public rclcpp::Node const rclcpp::Time & now); private: + void on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg); + + rclcpp::Subscription::SharedPtr sub_map_projector_type_; rclcpp::Publisher::SharedPtr pub_map_bin_; - rclcpp::Publisher::SharedPtr pub_map_projector_type_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index f2e3c0b525968..81912fe6d4e75 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -49,16 +50,21 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) +{ + // subscription + sub_map_projector_type_ = create_subscription( + "input/map_projector_info", rclcpp::QoS{1}.transient_local(), + [this](const MapProjectorInfo::ConstSharedPtr msg) {on_map_projector_info(msg);}); +} + +void Lanelet2MapLoaderNode::on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg) { const auto lanelet2_filename = declare_parameter("lanelet2_map_path", ""); - const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS"); const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0); - const double map_origin_lat = declare_parameter("latitude", 0.0); - const double map_origin_lon = declare_parameter("longitude", 0.0); // load map from file const auto map = - load_map(lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon); + load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude); if (!map) { return; } @@ -80,6 +86,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( const double & map_origin_lat, const double & map_origin_lon) { lanelet::ErrorMessages errors{}; + std::cout << "KOJI!!!!!!!!!!! " << lanelet2_map_projector_type << std::endl; if (lanelet2_map_projector_type == "MGRS") { lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); @@ -90,7 +97,15 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; + const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + } else if (lanelet2_map_projector_type == "TransverseMercator") { + lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; + lanelet::Origin origin{position}; + lanelet::projection::TransverseMercatorProjector projector{origin}; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); if (errors.empty()) { return map; @@ -120,6 +135,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( } return map; + } else { RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); return nullptr; From 085afcbb295f1365ffb45bf3912d0fb4f1865f0b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Jul 2023 09:57:49 +0000 Subject: [PATCH 23/51] style(pre-commit): autofix --- .../src/lanelet2_map_loader/lanelet2_map_loader_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 81912fe6d4e75..7b34a1671c6cd 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -54,7 +54,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options // subscription sub_map_projector_type_ = create_subscription( "input/map_projector_info", rclcpp::QoS{1}.transient_local(), - [this](const MapProjectorInfo::ConstSharedPtr msg) {on_map_projector_info(msg);}); + [this](const MapProjectorInfo::ConstSharedPtr msg) { on_map_projector_info(msg); }); } void Lanelet2MapLoaderNode::on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg) @@ -135,7 +135,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( } return map; - + } else { RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); return nullptr; From 7c849fcac08b9c81afbd50d9ec543bb2be3496be Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 27 Jul 2023 19:21:32 +0900 Subject: [PATCH 24/51] update docs --- map/map_loader/README.md | 9 +++++---- map/map_loader/config/lanelet2_map_loader.param.yaml | 4 ---- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 2dcb041e685d1..6c1f80b671db1 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -139,12 +139,16 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. -The node projects lan/lon coordinates into MGRS coordinates. +The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_type` from `map_projection_loader`. ### How to run `ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` +### Subscribed Topics +- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware + + ### Published Topics - ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map @@ -177,8 +181,5 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa | Name | Type | Description | Default value | | :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | -| lanelet2_map_projector_type | std::string | The type of the map projector using, can be MGRS, UTM, local | MGRS | -| latitude | double | Latitude of map_origin, only using in UTM map projector | 0.0 | -| longitude | double | Longitude of map_origin, only using in UTM map projector | 0.0 | | center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | | lanelet2_map_path | std::string | The lanelet2 map path | None | diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 971af0147b0fe..24d2b0e8ed7a8 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,7 +1,3 @@ /**: ros__parameters: - lanelet2_map_projector_type: MGRS # Options: MGRS, UTM, local - latitude: 40.816617984672746 # Latitude of map_origin, using in UTM - longitude: 29.360491808334285 # Longitude of map_origin, using in UTM - center_line_resolution: 5.0 # [m] From d32d01a59c9b94ebb52d47f32379a55da6dfa683 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Jul 2023 10:23:43 +0000 Subject: [PATCH 25/51] style(pre-commit): autofix --- map/map_loader/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 6c1f80b671db1..7226c672c8455 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -146,8 +146,8 @@ The node projects lan/lon coordinates into arbitrary coordinates defined in `/ma `ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` ### Subscribed Topics -- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware +- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware ### Published Topics @@ -179,7 +179,7 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Parameters -| Name | Type | Description | Default value | -| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | -| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | -| lanelet2_map_path | std::string | The lanelet2 map path | None | +| Name | Type | Description | Default value | +| :--------------------- | :---------- | :----------------------------------------------- | :------------ | +| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | +| lanelet2_map_path | std::string | The lanelet2 map path | None | From 53f6c927d0111515b2d71e966ace88efbb00557b Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 27 Jul 2023 19:30:47 +0900 Subject: [PATCH 26/51] update Signed-off-by: kminoda --- .../src/lanelet2_map_loader/lanelet2_map_loader_node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 7b34a1671c6cd..c0b60e08eeb86 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -86,7 +86,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( const double & map_origin_lat, const double & map_origin_lon) { lanelet::ErrorMessages errors{}; - std::cout << "KOJI!!!!!!!!!!! " << lanelet2_map_projector_type << std::endl; if (lanelet2_map_projector_type == "MGRS") { lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); @@ -135,7 +134,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( } return map; - } else { RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); return nullptr; From 604fc25a50bf3786df84189dac9220a25b370096 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 28 Jul 2023 17:50:20 +0900 Subject: [PATCH 27/51] add dependency Signed-off-by: kminoda --- launch/tier4_map_launch/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 6f75950838267..745d527a2f122 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -14,6 +14,7 @@ map_loader map_tf_generator + map_projection_loader ament_lint_auto autoware_lint_common From aab33259cd1e93f527d1d811bdb11d4349ac8368 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Jul 2023 08:55:10 +0000 Subject: [PATCH 28/51] style(pre-commit): autofix --- launch/tier4_map_launch/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 745d527a2f122..d8f69c124526a 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -13,8 +13,8 @@ autoware_cmake map_loader - map_tf_generator map_projection_loader + map_tf_generator ament_lint_auto autoware_lint_common From 412a6406a211eccc8be162f8109d6d242cd28b99 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 28 Jul 2023 18:40:19 +0900 Subject: [PATCH 29/51] remove unnecessary parameter Signed-off-by: kminoda --- .../launch/map_projection_loader.launch.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 64167586c2d4f..6448675c4a75f 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -6,6 +6,5 @@ - From c5b1a50754030100fb63e16b464fea8b6d6ab577 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 2 Aug 2023 15:56:44 +0900 Subject: [PATCH 30/51] update Signed-off-by: kminoda --- map/map_loader/README.md | 3 +- map/map_projection_loader/CMakeLists.txt | 12 +- .../test/data/lanelet2_map_local.osm | 21 +++ .../test/data/lanelet2_map_mgrs.osm | 21 +++ .../test_load_local_from_lanelet2_map.test.py | 136 ++++++++++++++++ .../test_load_mgrs_from_lanelet2_map.test.py | 153 ++++++++++++++++++ ...st.py => test_load_mgrs_from_yaml.test.py} | 2 +- ...oad_transverse_mercator_from_yaml.test.py} | 2 +- 8 files changed, 345 insertions(+), 5 deletions(-) create mode 100644 map/map_projection_loader/test/data/lanelet2_map_local.osm create mode 100644 map/map_projection_loader/test/data/lanelet2_map_mgrs.osm create mode 100644 map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py create mode 100644 map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py rename map/map_projection_loader/test/{test_load_mgrs.test.py => test_load_mgrs_from_yaml.test.py} (99%) rename map/map_projection_loader/test/{test_load_transverse_mercator.test.py => test_load_transverse_mercator_from_yaml.test.py} (98%) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 7226c672c8455..2d8b61489d8fe 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -159,10 +159,11 @@ The node projects lan/lon coordinates into arbitrary coordinates defined in `/ma ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are 3 types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package. +lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are four types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package. - MGRS - UTM +- TransverseMercator - local ### How to Run diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index f7a68f68e60c1..d0f7081ddc6bf 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -14,11 +14,19 @@ target_link_libraries(map_projection_loader yaml-cpp) if(BUILD_TESTING) add_launch_test( - test/test_load_mgrs.test.py + test/test_load_mgrs_from_yaml.test.py TIMEOUT "30" ) add_launch_test( - test/test_load_transverse_mercator.test.py + test/test_load_transverse_mercator_from_yaml.test.py + TIMEOUT "30" + ) + # add_launch_test( + # test/test_load_mgrs_from_lanelet2_map.test.py + # TIMEOUT "30" + # ) + add_launch_test( + test/test_load_local_from_lanelet2_map.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/map/map_projection_loader/test/data/lanelet2_map_local.osm b/map/map_projection_loader/test/data/lanelet2_map_local.osm new file mode 100644 index 0000000000000..19cbb0f5432c5 --- /dev/null +++ b/map/map_projection_loader/test/data/lanelet2_map_local.osm @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/map/map_projection_loader/test/data/lanelet2_map_mgrs.osm b/map/map_projection_loader/test/data/lanelet2_map_mgrs.osm new file mode 100644 index 0000000000000..08dc222dc1183 --- /dev/null +++ b/map/map_projection_loader/test/data/lanelet2_map_mgrs.osm @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py b/map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py new file mode 100644 index 0000000000000..845c8e6d3187e --- /dev/null +++ b/map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +LANELET2_MAP_PATH = "test/data/lanelet2_map_local.osm" + + +@pytest.mark.launch_test +def generate_test_description(): + lanelet2_map_path = os.path.join( + get_package_share_directory("map_projection_loader"), LANELET2_MAP_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": "", + "lanelet2_map_path": lanelet2_map_path, + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadMGRS(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map_projection_loader/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, "local") + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py b/map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py new file mode 100644 index 0000000000000..72e4132a898e8 --- /dev/null +++ b/map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +LANELET2_MAP_PATH = "test/data/lanelet2_map_mgrs.osm" + + +@pytest.mark.launch_test +def generate_test_description(): + lanelet2_map_path = os.path.join( + get_package_share_directory("map_projection_loader"), LANELET2_MAP_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": "", + "lanelet2_map_path": lanelet2_map_path, + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadMGRS(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map_projection_loader/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + self.assertEqual( + self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_load_mgrs.test.py b/map/map_projection_loader/test/test_load_mgrs_from_yaml.test.py similarity index 99% rename from map/map_projection_loader/test/test_load_mgrs.test.py rename to map/map_projection_loader/test/test_load_mgrs_from_yaml.test.py index c623be1331fb1..1504f4abee3bd 100644 --- a/map/map_projection_loader/test/test_load_mgrs.test.py +++ b/map/map_projection_loader/test/test_load_mgrs_from_yaml.test.py @@ -71,7 +71,7 @@ def generate_test_description(): ) -class TestLoadMGRS(unittest.TestCase): +class TestLoadMGRSFromYaml(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context for the test node diff --git a/map/map_projection_loader/test/test_load_transverse_mercator.test.py b/map/map_projection_loader/test/test_load_transverse_mercator_from_yaml.test.py similarity index 98% rename from map/map_projection_loader/test/test_load_transverse_mercator.test.py rename to map/map_projection_loader/test/test_load_transverse_mercator_from_yaml.test.py index febcad95fd327..a78a359017669 100644 --- a/map/map_projection_loader/test/test_load_transverse_mercator.test.py +++ b/map/map_projection_loader/test/test_load_transverse_mercator_from_yaml.test.py @@ -71,7 +71,7 @@ def generate_test_description(): ) -class TestLoadTransverseMercator(unittest.TestCase): +class TestLoadTransverseMercatorFromYaml(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context for the test node From 108d7fc3ef0b48ab87ba47bd935b2a665ab539c5 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 2 Aug 2023 18:32:32 +0900 Subject: [PATCH 31/51] update test Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 55 ++++--- map/map_projection_loader/package.xml | 1 + .../test/data/lanelet2_map_local.osm | 21 --- .../test/data/lanelet2_map_mgrs.osm | 21 --- .../test/test_load_info_from_lanelet2_map.cpp | 69 ++++++++ .../test_load_local_from_lanelet2_map.test.py | 136 ---------------- .../test_load_mgrs_from_lanelet2_map.test.py | 153 ------------------ ... => test_node_load_mgrs_from_yaml.test.py} | 0 ...oad_transverse_mercator_from_yaml.test.py} | 0 9 files changed, 104 insertions(+), 352 deletions(-) delete mode 100644 map/map_projection_loader/test/data/lanelet2_map_local.osm delete mode 100644 map/map_projection_loader/test/data/lanelet2_map_mgrs.osm create mode 100644 map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp delete mode 100644 map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py delete mode 100644 map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py rename map/map_projection_loader/test/{test_load_mgrs_from_yaml.test.py => test_node_load_mgrs_from_yaml.test.py} (100%) rename map/map_projection_loader/test/{test_load_transverse_mercator_from_yaml.test.py => test_node_load_transverse_mercator_from_yaml.test.py} (100%) diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index d0f7081ddc6bf..2a259389eac83 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -4,35 +4,48 @@ project(map_projection_loader) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(map_projection_loader - src/map_projection_loader_node.cpp +ament_auto_find_build_dependencies() + +ament_auto_add_library(map_projection_loader_lib SHARED src/map_projection_loader.cpp ) -ament_target_dependencies(map_projection_loader) -target_link_libraries(map_projection_loader yaml-cpp) +target_link_libraries(map_projection_loader_lib yaml-cpp) + +ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) + +target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) + +target_link_libraries(map_projection_loader map_projection_loader_lib) +target_include_directories(map_projection_loader PUBLIC include) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" map_projection_loader_lib) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() if(BUILD_TESTING) - add_launch_test( - test/test_load_mgrs_from_yaml.test.py - TIMEOUT "30" - ) - add_launch_test( - test/test_load_transverse_mercator_from_yaml.test.py - TIMEOUT "30" - ) + # # Test python scripts # add_launch_test( - # test/test_load_mgrs_from_lanelet2_map.test.py + # test/test_node_load_mgrs_from_yaml.test.py # TIMEOUT "30" # ) - add_launch_test( - test/test_load_local_from_lanelet2_map.test.py - TIMEOUT "30" - ) - install(DIRECTORY - test/data/ - DESTINATION share/${PROJECT_NAME}/test/data/ - ) + # add_launch_test( + # test/test_node_load_transverse_mercator_from_yaml.test.py + # TIMEOUT "30" + # ) + # install(DIRECTORY + # test/data/ + # DESTINATION share/${PROJECT_NAME}/test/data/ + # ) + + # Test c++ scripts + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_load_info_from_lanelet2_map.cpp) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 0cd6f56ab83a3..ad64f0f9ca4ea 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -16,6 +16,7 @@ tier4_map_msgs ament_cmake_gmock + ament_cmake_gtest ament_lint_auto autoware_lint_common ros_testing diff --git a/map/map_projection_loader/test/data/lanelet2_map_local.osm b/map/map_projection_loader/test/data/lanelet2_map_local.osm deleted file mode 100644 index 19cbb0f5432c5..0000000000000 --- a/map/map_projection_loader/test/data/lanelet2_map_local.osm +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/map/map_projection_loader/test/data/lanelet2_map_mgrs.osm b/map/map_projection_loader/test/data/lanelet2_map_mgrs.osm deleted file mode 100644 index 08dc222dc1183..0000000000000 --- a/map/map_projection_loader/test/data/lanelet2_map_mgrs.osm +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp new file mode 100644 index 0000000000000..e73383c6384e3 --- /dev/null +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include + +#include +#include +#include + + +void save_dummy_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) +{ + int zone = std::stoi(mgrs_coord.substr(0, 2)); + bool is_north = false; + int precision = 0; + double utm_x, utm_y; + double lat, lon; + GeographicLib::MGRS::Reverse(mgrs_coord, zone, is_north, utm_x, utm_y, precision, false); + GeographicLib::UTMUPS::Reverse(zone, is_north, utm_x, utm_y, lat, lon); + + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << ""; + + file.close(); +} + +TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) +{ + // Save dummy lanelet2 map + const std::string mgrs_grid = "54SUE"; + const std::string mgrs_coord = mgrs_grid + "1000010000"; + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_lanelet2_map(mgrs_coord, output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py b/map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py deleted file mode 100644 index 845c8e6d3187e..0000000000000 --- a/map/map_projection_loader/test/test_load_local_from_lanelet2_map.test.py +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 TIER IV, Inc. -# -# 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. - -import os -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch.logging import get_logger -from launch_ros.actions import Node -import launch_testing -import pytest -import rclpy -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo -import yaml - -logger = get_logger(__name__) - -LANELET2_MAP_PATH = "test/data/lanelet2_map_local.osm" - - -@pytest.mark.launch_test -def generate_test_description(): - lanelet2_map_path = os.path.join( - get_package_share_directory("map_projection_loader"), LANELET2_MAP_PATH - ) - - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - { - "map_projector_info_path": "", - "lanelet2_map_path": lanelet2_map_path, - "use_local_projector": False, - }, - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -class TestLoadMGRS(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, - "/map_projection_loader/map_projector_info", - self.callback, - custom_qos_profile, - ) - - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time - ) - - # Test if message received - self.assertIsNotNone( - self.received_message, "No message received on map_projector_info topic" - ) - self.assertEqual(self.received_message.type, "local") - - self.test_node.destroy_subscription(subscription) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py b/map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py deleted file mode 100644 index 72e4132a898e8..0000000000000 --- a/map/map_projection_loader/test/test_load_mgrs_from_lanelet2_map.test.py +++ /dev/null @@ -1,153 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 TIER IV, Inc. -# -# 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. - -import os -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch.logging import get_logger -from launch_ros.actions import Node -import launch_testing -import pytest -import rclpy -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from tier4_map_msgs.msg import MapProjectorInfo -import yaml - -logger = get_logger(__name__) - -LANELET2_MAP_PATH = "test/data/lanelet2_map_mgrs.osm" - - -@pytest.mark.launch_test -def generate_test_description(): - lanelet2_map_path = os.path.join( - get_package_share_directory("map_projection_loader"), LANELET2_MAP_PATH - ) - - map_projection_loader_node = Node( - package="map_projection_loader", - executable="map_projection_loader", - output="screen", - parameters=[ - { - "map_projector_info_path": "", - "lanelet2_map_path": lanelet2_map_path, - "use_local_projector": False, - }, - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -class TestLoadMGRS(unittest.TestCase): - @classmethod - def setUpClass(cls): - # Initialize the ROS context for the test node - rclpy.init() - - @classmethod - def tearDownClass(cls): - # Shutdown the ROS context - rclpy.shutdown() - - def setUp(self): - # Create a ROS node for tests - self.test_node = rclpy.create_node("test_node") - self.evaluation_time = 0.2 # 200ms - self.received_message = None - - def tearDown(self): - self.test_node.destroy_node() - - def callback(self, msg): - self.received_message = msg - - @staticmethod - def print_message(stat): - logger.debug("===========================") - logger.debug(stat) - - def test_node_link(self): - # Create custom QoS profile for subscription - custom_qos_profile = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - ) - - # Create subscription to map_projector_info topic - subscription = self.test_node.create_subscription( - MapProjectorInfo, - "/map_projection_loader/map_projector_info", - self.callback, - custom_qos_profile, - ) - - # Give time for the message to be received and processed - rclpy.spin_until_future_complete( - self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time - ) - - # Load the yaml file directly - map_projection_info_path = os.path.join( - get_package_share_directory("map_projection_loader"), YAML_FILE_PATH - ) - with open(map_projection_info_path) as f: - yaml_data = yaml.load(f) - - # Test if message received - self.assertIsNotNone( - self.received_message, "No message received on map_projector_info topic" - ) - self.assertEqual(self.received_message.type, yaml_data["type"]) - self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) - self.assertEqual( - self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] - ) - self.assertEqual( - self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] - ) - self.assertEqual( - self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] - ) - - self.test_node.destroy_subscription(subscription) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py similarity index 100% rename from map/map_projection_loader/test/test_load_mgrs_from_yaml.test.py rename to map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py diff --git a/map/map_projection_loader/test/test_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py similarity index 100% rename from map/map_projection_loader/test/test_load_transverse_mercator_from_yaml.test.py rename to map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py From 2727ecc421d3799c9a0cd9dbda9d327e86706715 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 2 Aug 2023 09:34:24 +0000 Subject: [PATCH 32/51] style(pre-commit): autofix --- .../test/test_load_info_from_lanelet2_map.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index e73383c6384e3..fb458e1af0a2b 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -14,12 +14,12 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" -#include - -#include #include #include +#include + +#include void save_dummy_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) { @@ -66,4 +66,3 @@ int main(int argc, char ** argv) ::testing::InitGoogleMock(&argc, argv); return RUN_ALL_TESTS(); } - From 6817598bb8351cfc3102bb16df5bd053ac465cd3 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 2 Aug 2023 18:34:32 +0900 Subject: [PATCH 33/51] add url Signed-off-by: kminoda --- map/map_projection_loader/src/map_projection_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 86b4e28a325b2..b1c49d473800e 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -53,7 +53,7 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") RCLCPP_WARN( this->get_logger(), "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead."); + "Please use map_projector_info.yaml instead. For more info, visit https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/README.md"); msg = load_info_from_lanelet2_map(lanelet2_map_filename); } From 73e45b3411c64744ccf6f598e15f2fba15420d22 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 2 Aug 2023 18:35:40 +0900 Subject: [PATCH 34/51] enable python tests Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 26 ++++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 2a259389eac83..60404c1bc6efc 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -28,19 +28,19 @@ function(add_testcase filepath) endfunction() if(BUILD_TESTING) - # # Test python scripts - # add_launch_test( - # test/test_node_load_mgrs_from_yaml.test.py - # TIMEOUT "30" - # ) - # add_launch_test( - # test/test_node_load_transverse_mercator_from_yaml.test.py - # TIMEOUT "30" - # ) - # install(DIRECTORY - # test/data/ - # DESTINATION share/${PROJECT_NAME}/test/data/ - # ) + # Test python scripts + add_launch_test( + test/test_node_load_mgrs_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_transverse_mercator_from_yaml.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) # Test c++ scripts find_package(ament_lint_auto REQUIRED) From 2f7fca13fdcd6320ad1d3be491932a6ea0ba6dc0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 2 Aug 2023 09:37:43 +0000 Subject: [PATCH 35/51] style(pre-commit): autofix --- map/map_projection_loader/src/map_projection_loader.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index b1c49d473800e..74ea5dcf74231 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -53,7 +53,9 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") RCLCPP_WARN( this->get_logger(), "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead. For more info, visit https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/README.md"); + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" + "README.md"); msg = load_info_from_lanelet2_map(lanelet2_map_filename); } From e41bd9a49eb1e63a8ebdaa1906e414cf9115f8e5 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 2 Aug 2023 19:36:10 +0900 Subject: [PATCH 36/51] small fix Signed-off-by: kminoda --- .../map_projection_loader/load_info_from_lanelet2_map.hpp | 3 +-- map/map_projection_loader/test/data/projection_info_mgrs.yaml | 4 ++-- .../test/data/projection_info_transverse_mercator.yaml | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp index 7c41fd0055b46..122e5dfcb3fce 100644 --- a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -31,8 +31,6 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) { - tier4_map_msgs::msg::MapProjectorInfo msg; - lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); @@ -40,6 +38,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str throw std::runtime_error("Error occurred while loading lanelet2 map"); } + tier4_map_msgs::msg::MapProjectorInfo msg; msg.type = "MGRS"; msg.mgrs_grid = projector.getProjectedMGRSGrid(); return msg; diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml index 4329599a8c1fc..61f00bc10970b 100644 --- a/map/map_projection_loader/test/data/projection_info_mgrs.yaml +++ b/map/map_projection_loader/test/data/projection_info_mgrs.yaml @@ -1,5 +1,5 @@ -type: "MGRS" -mgrs_grid: "54SUE" +type: MGRS +mgrs_grid: 54SUE map_origin: latitude: 0.0 longitude: 0.0 diff --git a/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml b/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml index bfc7c4be81177..4fe34193924d1 100644 --- a/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml +++ b/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml @@ -1,4 +1,4 @@ -type: "TransverseMercator" +type: TransverseMercator map_origin: latitude: 0.0 longitude: 0.0 From d365f2a790dcc5cde9d170bdf0ae771c7732355a Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 2 Aug 2023 19:39:05 +0900 Subject: [PATCH 37/51] fix grammar Signed-off-by: kminoda --- map/map_projection_loader/README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index e026f2a88f585..ecaec0fd91b08 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -2,12 +2,12 @@ ## Feature -`map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate the Autoware is operating on. -This is a necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. +`map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate Autoware is operating. +This is necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. -- If `map_projector_info_path` DOES exist, this node loads it and publish the map projection information accordingly. -- If `map_projector_info_path` does NOT exist, the node assumes that you are using `MGRS` projection type, and loads lanelet2 map instead to extract MGRS grid. - - **DEPRECATED WARNING: This interface that uses lanelet2 map is not recommended. Please prepare YAML file instead.** +- If `map_projector_info_path` DOES exist, this node loads it and publishes the map projection information accordingly. +- If `map_projector_info_path` does NOT exist, the node assumes that you are using the `MGRS` projection type, and loads the lanelet2 map instead to extract the MGRS grid. + - **DEPRECATED WARNING: This interface that uses the lanelet2 map is not recommended. Please prepare the YAML file instead.** ## Map projector info file specification @@ -30,7 +30,7 @@ type: "Local" ### Using MGRS -If you want to use MGRS, please specify MGRS grid as well. +If you want to use MGRS, please specify the MGRS grid as well. ```yaml # map_projector_info.yaml From 87ae213667f7eb270a9ce9cf5995cbcb09f86512 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 3 Aug 2023 14:43:04 +0900 Subject: [PATCH 38/51] remove transverse mercator Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 2 +- map/map_projection_loader/README.md | 12 ------------ .../src/map_projection_loader.cpp | 4 ---- ...rse_mercator.yaml => projection_info_local.yaml} | 3 ++- ...st.py => test_node_load_local_from_yaml.test.py} | 13 ++----------- 5 files changed, 5 insertions(+), 29 deletions(-) rename map/map_projection_loader/test/data/{projection_info_transverse_mercator.yaml => projection_info_local.yaml} (68%) rename map/map_projection_loader/test/{test_node_load_transverse_mercator_from_yaml.test.py => test_node_load_local_from_yaml.test.py} (89%) diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 60404c1bc6efc..c7ceab2064001 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -34,7 +34,7 @@ if(BUILD_TESTING) TIMEOUT "30" ) add_launch_test( - test/test_node_load_transverse_mercator_from_yaml.test.py + test/test_node_load_local_from_yaml.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index ecaec0fd91b08..18eb9419708f2 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -51,18 +51,6 @@ map_origin: altitude: 0.0 ``` -### Using Transverse Mercator - -If you want to use Transverse Mercator projection, please specify the map origin as well. - -```yaml -# map_projector_info.yaml -type: "TransverseMercator" -map_origin: - latitude: 36.1148 - longitude: 137.9532 - altitude: 0.0 -``` ## Published Topics diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 74ea5dcf74231..18e97cc482a34 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -28,10 +28,6 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.type = data["type"].as(); if (msg.type == "MGRS") { msg.mgrs_grid = data["mgrs_grid"].as(); - } else if (msg.type == "TransverseMercator") { - msg.map_origin.latitude = data["map_origin"]["latitude"].as(); - msg.map_origin.longitude = data["map_origin"]["longitude"].as(); - msg.map_origin.altitude = data["map_origin"]["altitude"].as(); } return msg; } diff --git a/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml b/map/map_projection_loader/test/data/projection_info_local.yaml similarity index 68% rename from map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml rename to map/map_projection_loader/test/data/projection_info_local.yaml index 4fe34193924d1..61f00bc10970b 100644 --- a/map/map_projection_loader/test/data/projection_info_transverse_mercator.yaml +++ b/map/map_projection_loader/test/data/projection_info_local.yaml @@ -1,4 +1,5 @@ -type: TransverseMercator +type: MGRS +mgrs_grid: 54SUE map_origin: latitude: 0.0 longitude: 0.0 diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py similarity index 89% rename from map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py rename to map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index a78a359017669..aa0e3245565b5 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -33,7 +33,7 @@ logger = get_logger(__name__) -YAML_FILE_PATH = "test/data/projection_info_transverse_mercator.yaml" +YAML_FILE_PATH = "test/data/projection_info_local.yaml" @pytest.mark.launch_test @@ -71,7 +71,7 @@ def generate_test_description(): ) -class TestLoadTransverseMercatorFromYaml(unittest.TestCase): +class TestLoadLocalFromYaml(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context for the test node @@ -132,15 +132,6 @@ def test_node_link(self): self.received_message, "No message received on map_projector_info topic" ) self.assertEqual(self.received_message.type, yaml_data["type"]) - self.assertEqual( - self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] - ) - self.assertEqual( - self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] - ) - self.assertEqual( - self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] - ) self.test_node.destroy_subscription(subscription) From 9d87a537808027ad73f9cfbb8eb62adf19b30819 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 3 Aug 2023 05:44:42 +0000 Subject: [PATCH 39/51] style(pre-commit): autofix --- map/map_projection_loader/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 18eb9419708f2..9d3eed9c07177 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -51,7 +51,6 @@ map_origin: altitude: 0.0 ``` - ## Published Topics - ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information From 7097b2de3c5b951400e1cbabfd56779ff638231e Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 4 Aug 2023 15:27:01 +0900 Subject: [PATCH 40/51] add rule in map Signed-off-by: kminoda --- map/map_loader/README.md | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 2d8b61489d8fe..98d7d8edab2df 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,11 +20,12 @@ Currently, it supports the following two types: You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: -1. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. -2. **The division size along each axis should be equal.** -3. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). -4. **All the split maps should not overlap with each other.** -5. **Metadata file should also be provided.** The metadata structure description is provided below. +1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). +2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. +3. **The division size along each axis should be equal.** +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). +5. **All the split maps should not overlap with each other.** +6. **Metadata file should also be provided.** The metadata structure description is provided below. Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). From 4b8ddbbb0bd9f0870cd822993b0d3fc44c866fbe Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 4 Aug 2023 15:44:47 +0900 Subject: [PATCH 41/51] fix readme of map loader Signed-off-by: kminoda --- map/map_loader/README.md | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 98d7d8edab2df..546c28dcfa808 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -141,6 +141,11 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_type` from `map_projection_loader`. +The node supports the following three types of coordinate systems: + +- MGRS +- UTM +- local ### How to run @@ -154,18 +159,20 @@ The node projects lan/lon coordinates into arbitrary coordinates defined in `/ma - ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +### Parameters + +| Name | Type | Description | Default value | +| :--------------------- | :---------- | :----------------------------------------------- | :------------ | +| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | +| lanelet2_map_path | std::string | The lanelet2 map path | None | + --- ## lanelet2_map_visualization ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are four types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package. - -- MGRS -- UTM -- TransverseMercator -- local +lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -178,10 +185,3 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Published Topics - ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz - -### Parameters - -| Name | Type | Description | Default value | -| :--------------------- | :---------- | :----------------------------------------------- | :------------ | -| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | -| lanelet2_map_path | std::string | The lanelet2 map path | None | From 7d56305d22fbc80883179122bf50c9c81283bc46 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 4 Aug 2023 15:45:11 +0900 Subject: [PATCH 42/51] remove transverse mercator for now Signed-off-by: kminoda --- .../src/lanelet2_map_loader/lanelet2_map_loader_node.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index c0b60e08eeb86..42c46e2b5e96c 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -100,15 +100,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (errors.empty()) { return map; } - } else if (lanelet2_map_projector_type == "TransverseMercator") { - lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; - lanelet::Origin origin{position}; - - lanelet::projection::TransverseMercatorProjector projector{origin}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } } else if (lanelet2_map_projector_type == "local") { // Use MGRSProjector as parser lanelet::projection::MGRSProjector projector{}; From b358ab93eb297997ec416269b7738fac822a4733 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 4 Aug 2023 15:46:01 +0900 Subject: [PATCH 43/51] add utm Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 4 + .../src/map_projection_loader.cpp | 7 + .../test/data/projection_info_local.yaml | 7 +- .../test/data/projection_info_mgrs.yaml | 4 - .../test/data/projection_info_utm.yaml | 4 + .../test_node_load_mgrs_from_yaml.test.py | 9 -- .../test/test_node_load_utm_from_yaml.test.py | 149 ++++++++++++++++++ 7 files changed, 165 insertions(+), 19 deletions(-) create mode 100644 map/map_projection_loader/test/data/projection_info_utm.yaml create mode 100644 map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index c7ceab2064001..1e7fa3aee5c39 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -37,6 +37,10 @@ if(BUILD_TESTING) test/test_node_load_local_from_yaml.test.py TIMEOUT "30" ) + add_launch_test( + test/test_node_load_utm_from_yaml.test.py + TIMEOUT "30" + ) install(DIRECTORY test/data/ DESTINATION share/${PROJECT_NAME}/test/data/ diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 18e97cc482a34..773d21b70baae 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -28,6 +28,13 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.type = data["type"].as(); if (msg.type == "MGRS") { msg.mgrs_grid = data["mgrs_grid"].as(); + } else if (msg.type == "UTM") { + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + } else if (msg.type == "local") { + continue; // do nothing + } else { + throw std::runtime_error("Invalid map projector type. Currently supported types: MGRS, UTM, and local"); } return msg; } diff --git a/map/map_projection_loader/test/data/projection_info_local.yaml b/map/map_projection_loader/test/data/projection_info_local.yaml index 61f00bc10970b..cea4aaf31d439 100644 --- a/map/map_projection_loader/test/data/projection_info_local.yaml +++ b/map/map_projection_loader/test/data/projection_info_local.yaml @@ -1,6 +1 @@ -type: MGRS -mgrs_grid: 54SUE -map_origin: - latitude: 0.0 - longitude: 0.0 - altitude: 0.0 +type: local diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml index 61f00bc10970b..d7687521be5fa 100644 --- a/map/map_projection_loader/test/data/projection_info_mgrs.yaml +++ b/map/map_projection_loader/test/data/projection_info_mgrs.yaml @@ -1,6 +1,2 @@ type: MGRS mgrs_grid: 54SUE -map_origin: - latitude: 0.0 - longitude: 0.0 - altitude: 0.0 diff --git a/map/map_projection_loader/test/data/projection_info_utm.yaml b/map/map_projection_loader/test/data/projection_info_utm.yaml new file mode 100644 index 0000000000000..002383c97a474 --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_utm.yaml @@ -0,0 +1,4 @@ +type: UTM +map_origin: + latitude: 35.6762 + longitude: 139.6503 diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index 1504f4abee3bd..ae29b4b06c61c 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -133,15 +133,6 @@ def test_node_link(self): ) self.assertEqual(self.received_message.type, yaml_data["type"]) self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) - self.assertEqual( - self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] - ) - self.assertEqual( - self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] - ) - self.assertEqual( - self.received_message.map_origin.altitude, yaml_data["map_origin"]["altitude"] - ) self.test_node.destroy_subscription(subscription) diff --git a/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py new file mode 100644 index 0000000000000..8da98bbb8b25d --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/projection_info_utm.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map_projection_loader/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From ea735e218e0c931dc0f1d680235e3eb7b6a79f17 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 4 Aug 2023 15:46:19 +0900 Subject: [PATCH 44/51] remove altitude from current projection loader Signed-off-by: kminoda --- map/map_projection_loader/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 9d3eed9c07177..bbf15f34929da 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -48,7 +48,6 @@ type: "UTM" map_origin: latitude: 35.6092 longitude: 139.7303 - altitude: 0.0 ``` ## Published Topics From 08b6ded92d4aa82b5c79cc3ce11945e6ea1b8508 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 4 Aug 2023 06:46:23 +0000 Subject: [PATCH 45/51] style(pre-commit): autofix --- map/map_loader/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 546c28dcfa808..44f215b2d8c01 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -172,7 +172,7 @@ The node supports the following three types of coordinate systems: ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. ### How to Run From 081cc0bb04e142f43b5e350077224ac2f70858be Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 7 Aug 2023 11:43:45 +0900 Subject: [PATCH 46/51] fix pre-commit Signed-off-by: kminoda --- map/map_projection_loader/src/map_projection_loader.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 773d21b70baae..89701ad6eab4c 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -34,7 +34,8 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi } else if (msg.type == "local") { continue; // do nothing } else { - throw std::runtime_error("Invalid map projector type. Currently supported types: MGRS, UTM, and local"); + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, UTM, and local"); } return msg; } From 0702af6c7688c4b25c4c64da7d329e60d2a3fa4a Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 7 Aug 2023 15:12:33 +0900 Subject: [PATCH 47/51] fix build error Signed-off-by: kminoda --- map/map_projection_loader/src/map_projection_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 89701ad6eab4c..ccea8d7417cbc 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -32,7 +32,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); } else if (msg.type == "local") { - continue; // do nothing + ; // do nothing } else { throw std::runtime_error( "Invalid map projector type. Currently supported types: MGRS, UTM, and local"); From 378d9dc8ae70613bd382f248e88f04a56a1a4197 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 7 Aug 2023 21:06:16 +0900 Subject: [PATCH 48/51] fix: remove package.xml Signed-off-by: kminoda --- map/map_projection_loader/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index ad64f0f9ca4ea..84f66583ed840 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -5,6 +5,8 @@ 0.1.0 map_projection_loader package as a ROS 2 node Koji Minoda + Yamato Ando + Masahiro Sakamoto Apache License 2.0 ament_cmake_auto From 0509fe19224e2180784a55a4f6a496d328fe343c Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Aug 2023 14:26:09 +0900 Subject: [PATCH 49/51] fix clang-tidy Signed-off-by: kminoda --- map/map_projection_loader/CMakeLists.txt | 1 + .../load_info_from_lanelet2_map.hpp | 20 +--------- .../src/load_info_from_lanelet2_map.cpp | 39 +++++++++++++++++++ .../test/test_load_info_from_lanelet2_map.cpp | 6 ++- 4 files changed, 45 insertions(+), 21 deletions(-) create mode 100644 map/map_projection_loader/src/load_info_from_lanelet2_map.cpp diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 1e7fa3aee5c39..a53cdddf93b5b 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -8,6 +8,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(map_projection_loader_lib SHARED src/map_projection_loader.cpp + src/load_info_from_lanelet2_map.cpp ) target_link_libraries(map_projection_loader_lib yaml-cpp) diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp index 122e5dfcb3fce..746f16e0f6b33 100644 --- a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -22,26 +22,8 @@ #include "tier4_map_msgs/msg/map_projector_info.hpp" -#include -#include -#include -#include - #include -tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) -{ - lanelet::ErrorMessages errors{}; - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); - if (!errors.empty()) { - throw std::runtime_error("Error occurred while loading lanelet2 map"); - } - - tier4_map_msgs::msg::MapProjectorInfo msg; - msg.type = "MGRS"; - msg.mgrs_grid = projector.getProjectedMGRSGrid(); - return msg; -} +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); #endif // MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp new file mode 100644 index 0000000000000..343442c0fcbd2 --- /dev/null +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include +#include +#include +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); + if (!errors.empty()) { + throw std::runtime_error("Error occurred while loading lanelet2 map"); + } + + tier4_map_msgs::msg::MapProjectorInfo msg; + msg.type = "MGRS"; + msg.mgrs_grid = projector.getProjectedMGRSGrid(); + return msg; +} \ No newline at end of file diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index fb458e1af0a2b..52abacf67f344 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -26,8 +26,10 @@ void save_dummy_lanelet2_map(const std::string & mgrs_coord, const std::string & int zone = std::stoi(mgrs_coord.substr(0, 2)); bool is_north = false; int precision = 0; - double utm_x, utm_y; - double lat, lon; + double utm_x{}; + double utm_y{}; + double lat{}; + double lon{}; GeographicLib::MGRS::Reverse(mgrs_coord, zone, is_north, utm_x, utm_y, precision, false); GeographicLib::UTMUPS::Reverse(zone, is_north, utm_x, utm_y, lat, lon); From 05cf612fe4d3f2831fcb625a30ab75aba924c7f4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 9 Aug 2023 05:28:39 +0000 Subject: [PATCH 50/51] style(pre-commit): autofix --- map/map_projection_loader/src/load_info_from_lanelet2_map.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp index 343442c0fcbd2..a93a94f296d45 100644 --- a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -36,4 +36,4 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str msg.type = "MGRS"; msg.mgrs_grid = projector.getProjectedMGRSGrid(); return msg; -} \ No newline at end of file +} From 97b7ab473a9f5fcae9a36a888ffb8fa6c3acd75d Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 10 Aug 2023 09:32:32 +0900 Subject: [PATCH 51/51] feat!: change map_projector_type topic name Signed-off-by: kminoda --- .../include/component_interface_specs/map.hpp | 2 +- launch/tier4_map_launch/launch/map.launch.py | 2 +- map/map_loader/README.md | 2 +- map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp | 2 +- .../src/lanelet2_map_loader/lanelet2_map_loader_node.cpp | 2 +- .../launch/map_projection_loader.launch.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/common/component_interface_specs/include/component_interface_specs/map.hpp b/common/component_interface_specs/include/component_interface_specs/map.hpp index f0cce7a7f97a2..dc162d4a7267a 100644 --- a/common/component_interface_specs/include/component_interface_specs/map.hpp +++ b/common/component_interface_specs/include/component_interface_specs/map.hpp @@ -25,7 +25,7 @@ namespace map_interface struct MapProjectorInfo { using Message = tier4_map_msgs::msg::MapProjectorInfo; - static constexpr char name[] = "/map/map_projector_type"; + static constexpr char name[] = "/map/map_projector_info"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 169c53814df22..315ac2cf2eb90 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -64,7 +64,7 @@ def launch_setup(context, *args, **kwargs): name="lanelet2_map_loader", remappings=[ ("output/lanelet2_map", "vector_map"), - ("input/map_projector_info", "map_projector_type"), + ("input/map_projector_info", "map_projector_info"), ], parameters=[ { diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 44f215b2d8c01..4c64eb1eaecae 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -140,7 +140,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. -The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_type` from `map_projection_loader`. +The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. The node supports the following three types of coordinate systems: - MGRS diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 2fb8b893d8fce..b4e903eaa7561 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -43,7 +43,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node private: void on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg); - rclcpp::Subscription::SharedPtr sub_map_projector_type_; + rclcpp::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Publisher::SharedPtr pub_map_bin_; }; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 42c46e2b5e96c..ac910b8f769df 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -52,7 +52,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options : Node("lanelet2_map_loader", options) { // subscription - sub_map_projector_type_ = create_subscription( + sub_map_projector_info_ = create_subscription( "input/map_projector_info", rclcpp::QoS{1}.transient_local(), [this](const MapProjectorInfo::ConstSharedPtr msg) { on_map_projector_info(msg); }); } diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index 6448675c4a75f..6536dad6a4e21 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -3,7 +3,7 @@ - +