feat(pose_initializer): partial map loading (#2500)

* first commit Signed-off-by: kminoda <[email protected]> * move function Signed-off-by: kminoda <[email protected]> * now works Signed-off-by: kminoda <[email protected]> * ci(pre-commit): autofix * update readme Signed-off-by: kminoda <[email protected]> * ci(pre-commit): autofix * clarify how to enable partial mao loading interface Signed-off-by: kminoda <[email protected]> * ci(pre-commit): autofix * update readme Signed-off-by: kminoda <[email protected]> * ci(pre-commit): autofix * Update localization/pose_initializer/config/pose_initializer.param.yaml Co-authored-by: Takagi, Isamu <[email protected]> * fix pre-commit Signed-off-by: kminoda <[email protected]> Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <[email protected]>
autowarefoundation · Dec 16, 2022 · 6731e0c · 6731e0c
1 parent efb4ff1
commit 6731e0c
Show file tree

Hide file tree

Showing 7 changed files with 101 additions and 6 deletions.
diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml
@@ -22,3 +22,7 @@
         0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
         0.0, 0.0, 0.0,  0.0,  0.0,  0.2,
       ]
+
+    # use partial map loading for large maps
+    # Note: You also need to enable partial_map_loading interface in pointcloud_map_loader to use this functionality
+    enable_partial_map_load: false
diff --git a/localization/pose_initializer/docs/map_height_fitter.md b/localization/pose_initializer/docs/map_height_fitter.md
@@ -7,8 +7,21 @@ Use this service as preprocessing for `pose_initializer` when using a initial po
 This service replaces the Z value of the input pose with the lowest point of the map point cloud within a cylinder of XY-radius.
 If no point is found in this range, returns the input pose without changes.
 
+Note that this package supports partial map loading interface, which is disabled by default. The interface is intended to be enabled when
+the pointcloud map is too large to handle. By using the interface, the node will request for a partial map around the requested position
+instead of loading whole map by subscription interface. To use this interface,
+
+1. Set `enable_partial_map_load` in this node to `true`
+2. Set `enable_partial_load` in `pointcloud_map_loader` to `true`
+
 ## Interfaces
 
+### Parameters
+
+| Name                      | Type | Description                                                                           |       |
+| ------------------------- | ---- | ------------------------------------------------------------------------------------- | ----- |
+| `enable_partial_map_load` | bool | If true, use partial map load interface. If false, use topic subscription interaface. | false |
+
 ### Services
 
 | Name                                | Type                                                    | Description          |
@@ -20,3 +33,9 @@ If no point is found in this range, returns the input pose without changes.
 | Name                  | Type                          | Description    |
 | --------------------- | ----------------------------- | -------------- |
 | `/map/pointcloud_map` | sensor_msgs::msg::PointCloud2 | pointcloud map |
+
+### Clients
+
+| Name                              | Type                                            | Description                                  |
+| --------------------------------- | ----------------------------------------------- | -------------------------------------------- |
+| `/map/get_partial_pointcloud_map` | autoware_map_msgs::srv::GetPartialPointCloudMap | client for requesting partial pointcloud map |
diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml
@@ -5,8 +5,10 @@
   <arg name="stop_check_enabled"/>
 
   <!-- for gnss and rviz -->
-  <node pkg="pose_initializer" exec="map_height_fitter" name="map_height_fitter">
+  <node pkg="pose_initializer" exec="map_height_fitter" name="map_height_fitter" output="screen">
+    <param from="$(var config_file)"/>
     <remap from="fit_map_height" to="/localization/util/fit_map_height"/>
+    <remap from="client_partial_map_load" to="/map/get_partial_pointcloud_map"/>
     <remap from="pointcloud_map" to="/map/pointcloud_map"/>
   </node>
 

diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml
@@ -14,6 +14,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_map_msgs</depend>
   <depend>component_interface_specs</depend>
   <depend>component_interface_utils</depend>
   <depend>geometry_msgs</depend>

diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.cpp
@@ -23,17 +23,33 @@
 #endif
 
 #include <algorithm>
+#include <memory>
 
 MapHeightFitter::MapHeightFitter() : Node("map_height_fitter"), tf2_listener_(tf2_buffer_)
 {
+  enable_partial_map_load_ = declare_parameter<bool>("enable_partial_map_load", false);
+
   const auto durable_qos = rclcpp::QoS(1).transient_local();
   using std::placeholders::_1;
   using std::placeholders::_2;
 
-  sub_map_ = create_subscription<sensor_msgs::msg::PointCloud2>(
-    "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1));
   srv_fit_ = create_service<RequestHeightFitting>(
     "fit_map_height", std::bind(&MapHeightFitter::on_fit, this, _1, _2));
+
+  if (!enable_partial_map_load_) {
+    sub_map_ = create_subscription<sensor_msgs::msg::PointCloud2>(
+      "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1));
+  } else {
+    callback_group_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    cli_get_partial_pcd_ = create_client<autoware_map_msgs::srv::GetPartialPointCloudMap>(
+      "client_partial_map_load", rmw_qos_profile_default, callback_group_service_);
+    while (!cli_get_partial_pcd_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "Cannot find partial map loading interface. Please check the setting in "
+        "pointcloud_map_loader to see if the interface is enabled.");
+    }
+  }
 }
 
 void MapHeightFitter::on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
@@ -72,15 +88,62 @@ double MapHeightFitter::get_ground_height(const tf2::Vector3 & point) const
   return std::isfinite(height) ? height : point.getZ();
 }
 
+void MapHeightFitter::get_partial_point_cloud_map(const geometry_msgs::msg::Point & point)
+{
+  if (!cli_get_partial_pcd_) {
+    throw std::runtime_error{"Partial map loading in pointcloud_map_loader is not enabled"};
+  }
+  const auto req = std::make_shared<autoware_map_msgs::srv::GetPartialPointCloudMap::Request>();
+  req->area.center = point;
+  req->area.radius = 50;
+
+  RCLCPP_INFO(this->get_logger(), "Send request to map_loader");
+  auto res{cli_get_partial_pcd_->async_send_request(
+    req, [](rclcpp::Client<autoware_map_msgs::srv::GetPartialPointCloudMap>::SharedFuture) {})};
+
+  std::future_status status = res.wait_for(std::chrono::seconds(0));
+  while (status != std::future_status::ready) {
+    RCLCPP_INFO(this->get_logger(), "waiting response");
+    if (!rclcpp::ok()) {
+      return;
+    }
+    status = res.wait_for(std::chrono::seconds(1));
+  }
+
+  RCLCPP_INFO(
+    this->get_logger(), "Loaded partial pcd map from map_loader (grid size: %d)",
+    static_cast<int>(res.get()->new_pointcloud_with_ids.size()));
+
+  sensor_msgs::msg::PointCloud2 pcd_msg;
+  for (const auto & pcd_with_id : res.get()->new_pointcloud_with_ids) {
+    if (pcd_msg.width == 0) {
+      pcd_msg = pcd_with_id.pointcloud;
+    } else {
+      pcd_msg.width += pcd_with_id.pointcloud.width;
+      pcd_msg.row_step += pcd_with_id.pointcloud.row_step;
+      pcd_msg.data.insert(
+        pcd_msg.data.end(), pcd_with_id.pointcloud.data.begin(), pcd_with_id.pointcloud.data.end());
+    }
+  }
+
+  map_frame_ = res.get()->header.frame_id;
+  map_cloud_ = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(pcd_msg, *map_cloud_);
+}
+
 void MapHeightFitter::on_fit(
   const RequestHeightFitting::Request::SharedPtr req,
-  const RequestHeightFitting::Response::SharedPtr res) const
+  const RequestHeightFitting::Response::SharedPtr res)
 {
   const auto & position = req->pose_with_covariance.pose.pose.position;
   tf2::Vector3 point(position.x, position.y, position.z);
   std::string req_frame = req->pose_with_covariance.header.frame_id;
   res->success = false;
 
+  if (enable_partial_map_load_) {
+    get_partial_point_cloud_map(req->pose_with_covariance.pose.pose.position);
+  }
+
   if (map_cloud_) {
     try {
       const auto stamped = tf2_buffer_.lookupTransform(map_frame_, req_frame, tf2::TimePointZero);

diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_core.hpp
@@ -17,6 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_map_msgs/srv/get_partial_point_cloud_map.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
@@ -40,12 +41,17 @@ class MapHeightFitter : public rclcpp::Node
   std::string map_frame_;
   pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_map_;
+  rclcpp::Client<autoware_map_msgs::srv::GetPartialPointCloudMap>::SharedPtr cli_get_partial_pcd_;
   rclcpp::Service<RequestHeightFitting>::SharedPtr srv_fit_;
 
+  bool enable_partial_map_load_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_service_;
+
   void on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+  void get_partial_point_cloud_map(const geometry_msgs::msg::Point & point);
   void on_fit(
     const RequestHeightFitting::Request::SharedPtr req,
-    const RequestHeightFitting::Response::SharedPtr res) const;
+    const RequestHeightFitting::Response::SharedPtr res);
   double get_ground_height(const tf2::Vector3 & point) const;
 };
 

diff --git a/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp b/localization/pose_initializer/src/map_height_fitter/map_height_fitter_node.cpp
@@ -19,7 +19,7 @@
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   auto node = std::make_shared<MapHeightFitter>();
   executor.add_node(node);
   executor.spin();