From 3158f9d7929fb793ee02edae997df2e6cd3dbcbd Mon Sep 17 00:00:00 2001
From: Anh Nguyen <anh.nguyen.2@tier4.jp>
Date: Mon, 17 Jun 2024 07:58:22 +0700
Subject: [PATCH] Testing

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>
---
 launch/tier4_map_launch/launch/map.launch.xml    |  2 +-
 .../launch/ndt_scan_matcher.launch.xml           | 16 ++++++++++++++++
 .../ndt_scan_matcher/src/map_update_module.cpp   | 16 ++++++++++++++++
 3 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml
index c58c0848eb5b3..6d6f9282f5989 100644
--- a/launch/tier4_map_launch/launch/map.launch.xml
+++ b/launch/tier4_map_launch/launch/map.launch.xml
@@ -15,7 +15,7 @@
   <arg name="use_intra_process" default="false"/>
 
   <!-- select container type -->
-  <arg name="use_multithread" default="false"/>
+  <arg name="use_multithread" default="true"/>
   <let name="container_type" value="component_container" unless="$(var use_multithread)"/>
   <let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>
 
diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
index 671e2ace56cad..8b6928929ef0d 100644
--- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
+++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
@@ -14,6 +14,22 @@
 
   <arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node"/>
 
+  <load_composable_node target="/map/map_container">
+    <composable_node pkg="ndt_scan_matcher" plugin="NDTScanMatcher" name="$(var node_name)">
+      <remap from="points_raw" to="$(var input_pointcloud)"/>
+      <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
+      <remap from="regularization_pose_with_covariance" to="$(var input_regularization_pose_topic)"/>
+      <remap from="trigger_node_srv" to="$(var input_service_trigger_node)"/>
+
+      <remap from="ndt_pose" to="$(var output_pose_topic)"/>
+      <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)"/>
+
+      <remap from="pcd_loader_service" to="$(var client_map_loader)"/>
+
+      <param from="$(var param_file)"/>
+    </composable_node>
+  </load_composable_node>
+
   <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher_node" name="$(var node_name)" output="both">
     <remap from="points_raw" to="$(var input_pointcloud)"/>
     <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index 32e5a0f2a7c3c..0b9a6327adffa 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -62,6 +62,8 @@ void MapUpdateModule::callback_timer(
     return;
   }
 
+  std::ofstream test("/home/anh/Work/autoware/wait_time.txt", std::ios::app);
+
   // check is_set_last_update_position
   const bool is_set_last_update_position = (position != std::nullopt);
   diagnostics_ptr->add_key_value("is_set_last_update_position", is_set_last_update_position);
@@ -75,8 +77,13 @@ void MapUpdateModule::callback_timer(
   }
 
   if (should_update_map(position.value(), diagnostics_ptr)) {
+    test << "Update" << std::endl;
     update_map(position.value(), diagnostics_ptr);
   }
+  else
+  {
+    test << "Skip" << std::endl;
+  }
 }
 
 bool MapUpdateModule::should_update_map(
@@ -208,6 +215,8 @@ bool MapUpdateModule::update_ndt(
     return false;
   }
 
+  auto wait_start_time = std::chrono::system_clock::now();
+
   // send a request to map_loader
   auto result{pcd_loader_client_->async_send_request(
     request,
@@ -227,6 +236,13 @@ bool MapUpdateModule::update_ndt(
     }
     status = result.wait_for(std::chrono::seconds(1));
   }
+
+  auto wait_end_time = std::chrono::system_clock::now();
+
+  std::ofstream test("/home/anh/Work/autoware/wait_time.txt", std::ios::app);
+
+  test << std::chrono::duration_cast<std::chrono::microseconds>(wait_end_time - wait_start_time).count() << std::endl;
+
   diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true);
 
   auto & maps_to_add = result.get()->new_pointcloud_with_ids;