feat(map_loader): add partial map loading interface in pointcloud_map…

…_loader (#1938) * first commit Signed-off-by: kminoda <koji.minoda@tier4.jp> * reverted unnecessary modification Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * renamed some classes Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * move autoware_map_msgs to autoware_msgs repos Signed-off-by: kminoda <koji.m.minoda@gmail.com> * catch up with the modification in autoware_map_msgs Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * aligned with autoware_map_msgs change (differential/partial modules seperation) Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * debugged * debugged Signed-off-by: kminoda <koji.minoda@tier4.jp> * added min-max info and others Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * minor fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * already_loaded -> cached Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * load_ -> get_ Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * resolve pre-commit Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * minor fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor fix in readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * grammarly Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * ci(pre-commit): autofix * fix copyright Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix launch file Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove leaf_size param Signed-off-by: kminoda <koji.minoda@tier4.jp> * removed unnecessary things Signed-off-by: kminoda <koji.minoda@tier4.jp> * removed downsample for now Signed-off-by: kminoda <koji.minoda@tier4.jp> * removed differential_map_loader for this PR (would make another PR for this) Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * removed differential_map_loader, debugged Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * removed leaf_size description Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * refactor sphereAndBoxOverlapExists Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * added test for sphereAndBoxOverlapExists Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * remove downsample function for now Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove fmt from target_link_libraries in test Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor fix in cmakelists.txt Signed-off-by: kminoda <koji.minoda@tier4.jp> Signed-off-by: kminoda <koji.minoda@tier4.jp> Signed-off-by: kminoda <koji.m.minoda@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Dec 1, 2022 · 8bbc505 · 8bbc505
1 parent 99f7920
commit 8bbc505
Show file tree

Hide file tree

Showing 17 changed files with 491 additions and 14 deletions.
diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt
@@ -7,4 +7,5 @@ autoware_package()
 ament_auto_package(
   INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml
@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    enable_whole_load: true
+    enable_partial_load: true
diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py
@@ -32,9 +32,14 @@ def launch_setup(context, *args, **kwargs):
     lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform(
         context
     )
+    pointcloud_map_loader_param_path = LaunchConfiguration(
+        "pointcloud_map_loader_param_path"
+    ).perform(context)
 
     with open(lanelet2_map_loader_param_path, "r") as f:
         lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    with open(pointcloud_map_loader_param_path, "r") as f:
+        pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     map_hash_generator = Node(
         package="map_loader",
@@ -77,9 +82,14 @@ def launch_setup(context, *args, **kwargs):
         package="map_loader",
         plugin="PointCloudMapLoaderNode",
         name="pointcloud_map_loader",
-        remappings=[("output/pointcloud_map", "pointcloud_map")],
+        remappings=[
+            ("output/pointcloud_map", "pointcloud_map"),
+            ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"),
+            ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"),
+        ],
         parameters=[
-            {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}
+            {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]},
+            pointcloud_map_loader_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
@@ -149,6 +159,14 @@ def add_launch_arg(name: str, default_value=None, description=None):
         ],
         "path to lanelet2_map_loader param file",
     ),
+    add_launch_arg(
+        "pointcloud_map_loader_param_path",
+        [
+            FindPackageShare("tier4_map_launch"),
+            "/config/pointcloud_map_loader.param.yaml",
+        ],
+        "path to pointcloud_map_loader param file",
+    ),
     add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"),
     add_launch_arg("use_multithread", "false", "use multithread"),
 

diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml
@@ -2,17 +2,18 @@
   <arg name="map_path" default=""/>
   <arg name="lanelet2_map_path" default="$(var map_path)/lanelet2_map.osm"/>
   <arg name="pointcloud_map_path" default="$(var map_path)/pointcloud_map.pcd"/>
+  <arg name="pointcloud_map_loader_param_path" default="$(find-pkg-share tier4_map_launch)/config/pointcloud_map_loader.param.yaml"/>
 
   <group>
     <push-ros-namespace namespace="map"/>
     <include file="$(find-pkg-share map_loader)/launch/lanelet2_map_loader.launch.xml">
       <arg name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
     </include>
 
-    <node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader">
-      <remap from="output/pointcloud_map" to="/map/pointcloud_map"/>
-      <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]"/>
-    </node>
+    <include file="$(find-pkg-share map_loader)/launch/pointcloud_map_loader.launch.xml">
+      <arg name="pointcloud_map_path" value="$(var pointcloud_map_path)"/>
+      <arg name="pointcloud_map_loader_param_path" value="$(var pointcloud_map_loader_param_path)"/>
+    </include>
 
     <include file="$(find-pkg-share map_tf_generator)/launch/map_tf_generator.launch.xml">
       <arg name="input_vector_map_topic" value="/map/vector_map"/>

diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt
@@ -9,6 +9,8 @@ find_package(PCL REQUIRED COMPONENTS io)
 ament_auto_add_library(pointcloud_map_loader_node SHARED
   src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
   src/pointcloud_map_loader/pointcloud_map_loader_module.cpp
+  src/pointcloud_map_loader/partial_map_loader_module.cpp
+  src/pointcloud_map_loader/utils.cpp
 )
 target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})
 
@@ -49,6 +51,19 @@ if(BUILD_TESTING)
     test/data/
     DESTINATION share/${PROJECT_NAME}/test/data/
   )
+
+  function(add_testcase filepath)
+    get_filename_component(filename ${filepath} NAME)
+    string(REGEX REPLACE ".cpp" "" test_name ${filename})
+    ament_add_gmock(${test_name} ${filepath})
+    target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_link_libraries(${test_name} ${${PROJECT_NAME}_LIBRARIES})
+    ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+  endfunction()
+
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_testcase(test/test_sphere_box_overlap.cpp)
 endif()
 
 install(PROGRAMS

diff --git a/map/map_loader/README.md b/map/map_loader/README.md
@@ -6,15 +6,34 @@ This package provides the features of loading various maps.
 
 ### Feature
 
-pointcloud_map_loader loads PointCloud file and publishes the map data as sensor_msgs/PointCloud2 message.
+`pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations.
+Currently, it supports the following two types:
 
-### How to run
+- Publish raw pointcloud map
+- Send partial pointcloud map loading via ROS 2 service
 
-`ros2 run map_loader pointcloud_map_loader --ros-args -p "pcd_paths_or_directory:=[path/to/pointcloud1.pcd, path/to/pointcloud2.pcd, ...]"`
+#### Publish raw pointcloud map (ROS 2 topic)
 
-### Published Topics
+The node publishes the raw pointcloud map loaded from the `.pcd` file(s).
+
+#### Send partial pointcloud map (ROS 2 service)
+
+Here, we assume that the pointcloud maps are divided into grids.
+
+Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area.
+Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details.
+
+### Parameters
+
+| Name                | Type | Description                                    | Default value |
+| :------------------ | :--- | :--------------------------------------------- | :------------ |
+| enable_whole_load   | bool | A flag to enable raw pointcloud map publishing | true          |
+| enable_partial_load | bool | A flag to enable partial pointcloud map server | true          |
+
+### Interfaces
 
-- pointcloud_map (sensor_msgs/PointCloud2) : PointCloud Map
+- `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map
+- `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map
 
 ---
 

diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml
@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    enable_whole_load: true
+    enable_partial_load: true
diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml
@@ -1,8 +1,11 @@
 <launch>
   <arg name="pointcloud_map_path"/>
+  <arg name="pointcloud_map_loader_param_path" default="$(find-pkg-share map_loader)/config/pointcloud_map_loader.param.yaml"/>
 
   <node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader" output="screen">
     <remap from="output/pointcloud_map" to="/map/pointcloud_map"/>
+    <remap from="service/get_partial_pcd_map" to="/map/get_partial_pointcloud_map"/>
     <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]"/>
+    <param from="$(var pointcloud_map_loader_param_path)"/>
   </node>
 </launch>
diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml
@@ -15,11 +15,13 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_map_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libpcl-all-dev</depend>
   <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
@@ -28,6 +30,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
   <test_depend>ros_testing</test_depend>

diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp
@@ -0,0 +1,69 @@
+// Copyright 2022 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 "partial_map_loader_module.hpp"
+
+PartialMapLoaderModule::PartialMapLoaderModule(
+  rclcpp::Node * node, const std::map<std::string, PCDFileMetadata> & pcd_file_metadata_dict)
+: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict)
+{
+  get_partial_pcd_maps_service_ = node->create_service<GetPartialPointCloudMap>(
+    "service/get_partial_pcd_map", std::bind(
+                                     &PartialMapLoaderModule::onServiceGetPartialPointCloudMap,
+                                     this, std::placeholders::_1, std::placeholders::_2));
+}
+
+void PartialMapLoaderModule::partialAreaLoad(
+  const autoware_map_msgs::msg::AreaInfo area,
+  GetPartialPointCloudMap::Response::SharedPtr & response) const
+{
+  // iterate over all the available pcd map grids
+
+  for (const auto & ele : all_pcd_file_metadata_dict_) {
+    std::string path = ele.first;
+    PCDFileMetadata metadata = ele.second;
+
+    // assume that the map ID = map path (for now)
+    std::string map_id = path;
+
+    // skip if the pcd file is not within the queried area
+    if (!isGridWithinQueriedArea(area, metadata)) continue;
+
+    autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
+      loadPointCloudMapCellWithID(path, map_id);
+    response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
+  }
+}
+
+bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap(
+  GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res)
+{
+  auto area = req->area;
+  partialAreaLoad(area, res);
+  res->header.frame_id = "map";
+  return true;
+}
+
+autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID(
+  const std::string path, const std::string map_id) const
+{
+  sensor_msgs::msg::PointCloud2 pcd;
+  if (pcl::io::loadPCDFile(path, pcd) == -1) {
+    RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
+  }
+  autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id;
+  pointcloud_map_cell_with_id.pointcloud = pcd;
+  pointcloud_map_cell_with_id.cell_id = map_id;
+  return pointcloud_map_cell_with_id;
+}
diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp
@@ -0,0 +1,59 @@
+// Copyright 2022 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 POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_
+#define POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_
+
+#include "utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp"
+
+#include <pcl/common/common.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+class PartialMapLoaderModule
+{
+  using GetPartialPointCloudMap = autoware_map_msgs::srv::GetPartialPointCloudMap;
+
+public:
+  explicit PartialMapLoaderModule(
+    rclcpp::Node * node, const std::map<std::string, PCDFileMetadata> & pcd_file_metadata_dict);
+
+private:
+  rclcpp::Logger logger_;
+
+  std::map<std::string, PCDFileMetadata> all_pcd_file_metadata_dict_;
+  rclcpp::Service<GetPartialPointCloudMap>::SharedPtr get_partial_pcd_maps_service_;
+
+  bool onServiceGetPartialPointCloudMap(
+    GetPartialPointCloudMap::Request::SharedPtr req,
+    GetPartialPointCloudMap::Response::SharedPtr res);
+  void partialAreaLoad(
+    const autoware_map_msgs::msg::AreaInfo area,
+    GetPartialPointCloudMap::Response::SharedPtr & response) const;
+  autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID(
+    const std::string path, const std::string map_id) const;
+};
+
+#endif  // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_
diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp
@@ -15,7 +15,6 @@
 #include "pointcloud_map_loader_module.hpp"
 
 #include <fmt/format.h>
-#include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl_conversions/pcl_conversions.h>
 

diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
@@ -15,6 +15,7 @@
 #include "pointcloud_map_loader_node.hpp"
 
 #include <glob.h>
+#include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -48,9 +49,18 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
 {
   const auto pcd_paths =
     getPcdPaths(declare_parameter<std::vector<std::string>>("pcd_paths_or_directory"));
+  bool enable_whole_load = declare_parameter<bool>("enable_whole_load");
+  bool enable_partial_load = declare_parameter<bool>("enable_partial_load");
 
-  std::string publisher_name = "output/pointcloud_map";
-  pcd_map_loader_ = std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name);
+  if (enable_whole_load) {
+    std::string publisher_name = "output/pointcloud_map";
+    pcd_map_loader_ = std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name);
+  }
+
+  if (enable_partial_load) {
+    pcd_metadata_dict_ = generatePCDMetadata(pcd_paths);
+    partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict_);
+  }
 }
 
 std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(
@@ -78,5 +88,21 @@ std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(
   return pcd_paths;
 }
 
+std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::generatePCDMetadata(
+  const std::vector<std::string> & pcd_paths) const
+{
+  pcl::PointCloud<pcl::PointXYZ> partial_pcd;
+  std::map<std::string, PCDFileMetadata> all_pcd_file_metadata_dict;
+  for (const auto & path : pcd_paths) {
+    if (pcl::io::loadPCDFile(path, partial_pcd) == -1) {
+      RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path);
+    }
+    PCDFileMetadata metadata = {};
+    pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max);
+    all_pcd_file_metadata_dict[path] = metadata;
+  }
+  return all_pcd_file_metadata_dict;
+}
+
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode)