From 278fafee6fc0bbceebb0020b9e4b4a8e9a59a390 Mon Sep 17 00:00:00 2001
From: kosuke55 <kosuke.tnp@gmail.com>
Date: Fri, 2 Feb 2024 01:14:42 +0900
Subject: [PATCH] feat(map): add launch_pointcloud_map_loader

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 launch/tier4_map_launch/launch/map.launch.xml   | 5 ++++-
 map/map_height_fitter/src/map_height_fitter.cpp | 5 +++--
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml
index 56ad5b01c5024..5f9c6011b6435 100644
--- a/launch/tier4_map_launch/launch/map.launch.xml
+++ b/launch/tier4_map_launch/launch/map.launch.xml
@@ -17,11 +17,14 @@
   <let name="container_type" value="component_container" unless="$(var use_multithread)"/>
   <let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>
 
+  <!-- whether use pointcloud map -->
+  <arg name="launch_pointcloud_map_loader" default="true" description="launch pointcloud map loader"/>
+
   <group>
     <push-ros-namespace namespace="map"/>
 
     <node_container pkg="rclcpp_components" exec="$(var container_type)" name="map_container" namespace="" output="screen">
-      <composable_node pkg="map_loader" plugin="PointCloudMapLoaderNode" name="pointcloud_map_loader">
+      <composable_node pkg="map_loader" plugin="PointCloudMapLoaderNode" name="pointcloud_map_loader" if="$(var launch_pointcloud_map_loader)">
         <param from="$(var pointcloud_map_loader_param_path)"/>
         <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]"/>
         <param name="pcd_metadata_path" value="$(var pointcloud_map_metadata_path)"/>
diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp
index a05c63d44ebce..5bf677963d557 100644
--- a/map/map_height_fitter/src/map_height_fitter.cpp
+++ b/map/map_height_fitter/src/map_height_fitter.cpp
@@ -72,8 +72,9 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n
 
   const auto map_loader_name = node->declare_parameter<std::string>("map_loader_name");
   params_map_loader_ = rclcpp::AsyncParametersClient::make_shared(node, map_loader_name);
-  params_map_loader_->wait_for_service();
-  params_map_loader_->get_parameters({enable_partial_load}, callback);
+  if (params_map_loader_->wait_for_service(std::chrono::seconds(5.0))) {
+    params_map_loader_->get_parameters({enable_partial_load}, callback);
+  }
 }
 
 void MapHeightFitter::Impl::on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)