From 45339f63a05434cceaabbb2fa2d162b96e11d275 Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Tue, 10 Oct 2023 18:16:58 +0800 Subject: [PATCH] feat(autoware_auto_msgs_adapter): add map adapter (#4700) * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware_auto_msgs_adapter/CMakeLists.txt | 1 + .../config/adapter_map.param.yaml | 5 + .../autoware_auto_msgs_adapter.launch.xml | 4 + system/autoware_auto_msgs_adapter/package.xml | 4 +- .../autoware_auto_msgs_adapter.schema.json | 1 + .../src/autoware_auto_msgs_adapter_core.cpp | 5 + .../src/include/adapter_map.hpp | 59 ++++++++++ .../autoware_auto_msgs_adapter_core.hpp | 1 + .../test/test_msg_had_map_bin.cpp | 104 ++++++++++++++++++ 9 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml create mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp create mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index ff8dcf6cedbb3..8b1d31ff2d01b 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${NODE_NAME} src/include/adapter_base.hpp src/include/adapter_base_interface.hpp src/include/adapter_control.hpp + src/include/adapter_map.hpp src/include/adapter_perception.hpp src/include/autoware_auto_msgs_adapter_core.hpp src/autoware_auto_msgs_adapter_core.cpp) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml new file mode 100644 index 0000000000000..dcaa49e089b44 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" + topic_name_source: "/map/vector_map" + topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index f914fe6e824c4..d80c1bd8cbedf 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,11 +1,15 @@ + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index de19032e1aee9..99a94fa043565 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -10,8 +10,6 @@ Apache License 2.0 - M. Fatih Cırıt - ament_cmake_auto autoware_cmake @@ -22,8 +20,10 @@ autoware_lint_common autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_control_msgs + autoware_map_msgs autoware_perception_msgs rclcpp rclcpp_components diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json index 35e3aef1511cc..f6aa87f774528 100644 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -11,6 +11,7 @@ "description": "Target message type", "enum": [ "autoware_auto_control_msgs/msg/AckermannControlCommand", + "autoware_auto_mapping_msgs/msg/HADMapBin", "autoware_auto_perception_msgs/msg/PredictedObjects" ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp index 0f65571aba07b..15e3c90d227dd 100644 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -57,6 +57,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( return std::static_pointer_cast( std::make_shared(*this, topic_name_source, topic_name_target)); }}, + {"autoware_auto_mapping_msgs/msg/HADMapBin", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, {"autoware_auto_perception_msgs/msg/PredictedObjects", [&] { return std::static_pointer_cast( diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp new file mode 100644 index 0000000000000..8150b7d7dba08 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef ADAPTER_MAP_HPP_ +#define ADAPTER_MAP_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; + +class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterMap( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + HADMapBin convert(const LaneletMapBin & msg_source) override + { + autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; + msg_auto.header = msg_source.header; + msg_auto.format_version = msg_source.version_map_format; + msg_auto.map_version = msg_source.version_map; + msg_auto.map_format = 0; + msg_auto.data = msg_source.data; + + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp index f595359dcc1e7..f0e28f48aacd8 100644 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -15,6 +15,7 @@ #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ #include "adapter_control.hpp" +#include "adapter_map.hpp" #include "adapter_perception.hpp" #include diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp new file mode 100644 index 0000000000000..b312bd144f6f3 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 + +#include + +#include + +autoware_map_msgs::msg::LaneletMapBin generate_map_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_map_msgs::msg::LaneletMapBin msg_map; + msg_map.header.stamp = rclcpp::Time(rand_int()); + msg_map.header.frame_id = "test_frame"; + + msg_map.version_map_format = "1.1.1"; + msg_map.version_map = "1.0.0"; + msg_map.name_map = "florence-prato-city-center"; + msg_map.data.push_back(rand_int()); + + return msg_map; +} + +TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_map = generate_map_msg(); + auto sub = node_subscriber->create_subscription( + topic_name_target, rclcpp::QoS{1}, + [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); + + EXPECT_EQ(msg->map_format, 0); + EXPECT_EQ(msg->format_version, msg_map.version_map_format); + EXPECT_EQ(msg->map_version, msg_map.version_map); + EXPECT_EQ(msg->data, msg_map.data); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_map); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +}