Add a map reciever

localization/lidar_feature_localization/CMakeLists.txt

@@ -53,20 +53,26 @@ set(EXTRACTION_SOURCE_FILES
   src/extraction/math.cpp
   src/extraction/ring.cpp)
 
+set(ROS_DEPENDENT_SOURCE_FILES
+  src/ros_dependent/map_receiver.cpp)
+
 ament_auto_add_library(library SHARED ${LIBRARY_SOURCE_FILES})
 ament_auto_add_library(localization SHARED ${LOCALIZATION_SOURCE_FILES})
 ament_auto_add_library(extraction SHARED ${EXTRACTION_SOURCE_FILES})
+ament_auto_add_library(ros_dependent SHARED ${ROS_DEPENDENT_SOURCE_FILES})
 
 target_include_directories(library PUBLIC ${INCLUDE_DIRS})
 target_include_directories(localization PUBLIC ${INCLUDE_DIRS})
 target_include_directories(extraction PUBLIC ${INCLUDE_DIRS})
+target_include_directories(ros_dependent PUBLIC ${INCLUDE_DIRS})
 
 target_link_libraries(library ${PCL_LIBRARIES} fmt)
 target_link_libraries(localization fmt)
 target_link_libraries(extraction fmt)
+target_link_libraries(ros_dependent ${PCL_LIBRARIES})
 
 ament_auto_add_executable("lidar_feature_localization" app/localization.cpp)
-target_link_libraries("lidar_feature_localization" library localization extraction)
+target_link_libraries("lidar_feature_localization" library localization extraction ros_dependent)
 
 function(add_localization_testcase filepath)
   get_filename_component(filename ${filepath} NAME)
@@ -98,6 +104,17 @@ function(add_lib_testcase filepath)
   ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
 endfunction()
 
+function(add_ros_dependent_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name_suffix ${filename})
+  set(test_name "ros_dependent_${test_name_suffix}")
+
+  ament_add_gmock(${test_name} ${filepath})
+  target_link_libraries("${test_name}" ros_dependent)
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
+
 if(BUILD_TESTING)
   file(GLOB
     FILES_TO_CHECK
@@ -174,6 +191,13 @@ if(BUILD_TESTING)
   foreach(filepath ${LIB_TEST_FILES})
     add_lib_testcase(${filepath})
   endforeach()
+
+  set(ROS_DEPENDENT_TEST_FILES
+    test/ros_dependent/test_map_reciever.cpp)
+
+  foreach(filepath ${ROS_DEPENDENT_TEST_FILES})
+    add_ros_dependent_testcase(${filepath})
+  endforeach()
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE launch config)

localization/lidar_feature_localization/include/ros_dependent/map_receiver.hpp

@@ -0,0 +1,43 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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 ROS_DEPENDENT__MAP_RECEIVER_HPP_
+#define ROS_DEPENDENT__MAP_RECEIVER_HPP_
+
+#include <memory>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+class MapReceiver
+{
+public:
+  MapReceiver(std::shared_ptr<rclcpp::Node> node, const std::string & topic_name);
+
+  void Callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
+
+  bool IsAvailable() const;
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr Get() const;
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr map_points_ptr_;
+};
+
+#endif  // ROS_DEPENDENT__MAP_RECEIVER_HPP_

localization/lidar_feature_localization/src/ros_dependent/map_receiver.cpp

@@ -0,0 +1,44 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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 "ros_dependent/map_receiver.hpp"
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <string>
+
+
+MapReceiver::MapReceiver(std::shared_ptr<rclcpp::Node> node, const std::string & topic_name)
+: map_points_sub_(
+    node->create_subscription<sensor_msgs::msg::PointCloud2>(
+      topic_name, rclcpp::QoS{1}.transient_local(),
+      std::bind(&MapReceiver::Callback, this, std::placeholders::_1))),
+  map_points_ptr_(new pcl::PointCloud<pcl::PointXYZ>())
+{
+}
+
+void MapReceiver::Callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
+{
+  pcl::fromROSMsg(*map_points_msg_ptr, *(this->map_points_ptr_));
+}
+
+bool MapReceiver::IsAvailable() const
+{
+  return this->map_points_ptr_->size() > 0;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr MapReceiver::Get() const
+{
+  return this->map_points_ptr_;
+}

localization/lidar_feature_localization/test/ros_dependent/test_map_reciever.cpp

@@ -0,0 +1,57 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "ros_dependent/map_receiver.hpp"
+
+
+class MapTestSuite : public ::testing::Test
+{
+protected:
+  void SetUp() {rclcpp::init(0, nullptr);}
+  void TearDown() {(void)rclcpp::shutdown();}
+};
+
+TEST_F(MapTestSuite, MapReciever)
+{
+  pcl::PointCloud<pcl::PointXYZ> pcl_pointcloud;
+  pcl_pointcloud.push_back(pcl::PointXYZ(1., 1., 1.));
+
+  sensor_msgs::msg::PointCloud2 ros_pointcloud;
+  pcl::toROSMsg(pcl_pointcloud, ros_pointcloud);
+
+  rclcpp::NodeOptions node_options;
+  const std::string topic_name = "pointcloud_map";
+  auto node = std::make_shared<rclcpp::Node>("test_map_node", node_options);
+  auto pub_pose = node->create_publisher<sensor_msgs::msg::PointCloud2>(
+    topic_name, rclcpp::QoS{1}.transient_local());
+
+  MapReceiver receiver(node, topic_name);
+  EXPECT_FALSE(receiver.IsAvailable());
+
+  for (int i = 0; i < 20; ++i) {
+    pub_pose->publish(ros_pointcloud);
+    rclcpp::spin_some(node);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  EXPECT_TRUE(receiver.IsAvailable());
+  const auto received_cloud = receiver.Get();
+  EXPECT_GT(received_cloud->size(), 0U);
+  EXPECT_EQ(received_cloud->size(), pcl_pointcloud.size());
+}