feat(autoware_auto_msgs_adapter): add map adapter (#4700)

* feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu <[email protected]> * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu <[email protected]> * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu <[email protected]> * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu <[email protected]> * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Oct 10, 2023 · 45339f6 · 45339f6
1 parent b36bc09
commit 45339f6
Show file tree

Hide file tree

Showing 9 changed files with 182 additions and 2 deletions.
diff --git 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
@@ -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
@@ -1,11 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="control_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_control.param.yaml"/>
+  <arg name="map_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_map.param.yaml"/>
   <arg name="perception_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_perception.param.yaml"/>
 
   <node pkg="autoware_auto_msgs_adapter" exec="autoware_auto_msgs_adapter_exe" name="autoware_auto_msgs_adapter_control" output="screen">
     <param from="$(var control_param_path)"/>
   </node>
+  <node pkg="autoware_auto_msgs_adapter" exec="autoware_auto_msgs_adapter_exe" name="autoware_auto_msgs_adapter_map" output="screen">
+    <param from="$(var map_param_path)"/>
+  </node>
   <node pkg="autoware_auto_msgs_adapter" exec="autoware_auto_msgs_adapter_exe" name="autoware_auto_msgs_adapter_perception" output="screen">
     <param from="$(var perception_param_path)"/>
   </node>

diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml
@@ -10,8 +10,6 @@
 
   <license>Apache License 2.0</license>
 
-  <author email="[email protected]">M. Fatih Cırıt</author>
-
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
@@ -22,8 +20,10 @@
   <test_depend>autoware_lint_common</test_depend>
 
   <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_control_msgs</depend>
+  <depend>autoware_map_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

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
@@ -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
@@ -57,6 +57,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map(
        return std::static_pointer_cast<AdapterBaseInterface>(
          std::make_shared<AdapterControl>(*this, topic_name_source, topic_name_target));
      }},
+    {"autoware_auto_mapping_msgs/msg/HADMapBin",
+     [&] {
+       return std::static_pointer_cast<AdapterBaseInterface>(
+         std::make_shared<AdapterMap>(*this, topic_name_source, topic_name_target));
+     }},
     {"autoware_auto_perception_msgs/msg/PredictedObjects",
      [&] {
        return std::static_pointer_cast<AdapterBaseInterface>(

diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp 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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
+
+#include <string>
+
+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<LaneletMapBin, HADMapBin>
+{
+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
@@ -15,6 +15,7 @@
 #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_
 
 #include "adapter_control.hpp"
+#include "adapter_map.hpp"
 #include "adapter_perception.hpp"
 
 #include <rclcpp/rclcpp.hpp>

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
@@ -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 <autoware_auto_msgs_adapter_core.hpp>
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+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<AutowareAutoMsgsAdapterNode>(node_options);
+
+  std::cout << "Creating the subscriber node..." << std::endl;
+
+  auto node_subscriber = std::make_shared<rclcpp::Node>("node_subscriber", rclcpp::NodeOptions{});
+
+  bool test_completed = false;
+
+  const auto msg_map = generate_map_msg();
+  auto sub = node_subscriber->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+    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<rclcpp::Node>("node_publisher", rclcpp::NodeOptions{});
+  auto pub = node_publisher->create_publisher<autoware_map_msgs::msg::LaneletMapBin>(
+    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();
+}