Merge pull request #4 from kminoda/feature/debugging_dml_multithreading

Feature/debugging dml multithreading
kminoda · Sep 6, 2022 · 0c337ab · 0c337ab
2 parents d1dbc27 + 92f7451
commit 0c337ab
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 14 deletions.
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -294,7 +294,8 @@ class NDTScanMatcher : public rclcpp::Node
 
   bool ndt_service_align_in_progress_{false};
 
-
+  rclcpp::CallbackGroup::SharedPtr map_callback_group_;
+
 };
 
 #endif  // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -181,6 +181,7 @@ NDTScanMatcher::NDTScanMatcher()
   ndt_ptr_->setRegularizationScaleFactor(regularization_scale_factor_);
 
   copyNDT(ndt_ptr_, backup_ndt_ptr_, ndt_implement_type_);
+
 
   RCLCPP_INFO(
     get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d",
@@ -226,8 +227,8 @@ NDTScanMatcher::NDTScanMatcher()
   rclcpp::CallbackGroup::SharedPtr main_callback_group;
   main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
-  rclcpp::CallbackGroup::SharedPtr map_callback_group;
-  map_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  // rclcpp::CallbackGroup::SharedPtr map_callback_group_;
+  map_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
   auto initial_pose_sub_opt = rclcpp::SubscriptionOptions();
   initial_pose_sub_opt.callback_group = initial_pose_callback_group;
@@ -236,7 +237,7 @@ NDTScanMatcher::NDTScanMatcher()
   main_sub_opt.callback_group = main_callback_group;
 
   auto map_sub_opt = rclcpp::SubscriptionOptions();
-  map_sub_opt.callback_group = map_callback_group;
+  map_sub_opt.callback_group = map_callback_group_;
 
   initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "ekf_pose_with_covariance", 100,
@@ -299,7 +300,7 @@ NDTScanMatcher::NDTScanMatcher()
   service_ = this->create_service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>(
     "ndt_align_srv",
     std::bind(&NDTScanMatcher::serviceNDTAlign, this, std::placeholders::_1, std::placeholders::_2),
-    rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group);
+    rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_);
     // rclcpp::ServicesQoS().get_rmw_qos_profile());
 
   pcd_loader_client_ = this->create_client<autoware_map_msgs::srv::LoadPCDMapsGeneral>(
@@ -316,7 +317,7 @@ NDTScanMatcher::NDTScanMatcher()
   auto period_ns =
     std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(map_update_dt));
   map_update_timer_ = rclcpp::create_timer(
-    this, get_clock(), period_ns, std::bind(&NDTScanMatcher::mapUpdateTimerCallback, this), map_callback_group);
+    this, get_clock(), period_ns, std::bind(&NDTScanMatcher::mapUpdateTimerCallback, this), map_callback_group_);
 
   diagnostic_thread_ = std::thread(&NDTScanMatcher::timerDiagnostic, this);
   diagnostic_thread_.detach();

diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp
@@ -90,21 +90,26 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio
     std::bind(&ElevationMapLoaderNode::onPointcloudMap, this, _1));
   sub_vector_map_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
     "input/vector_map", durable_qos, std::bind(&ElevationMapLoaderNode::onVectorMap, this, _1));
+  // publish();
+
 }
 
 void ElevationMapLoaderNode::publish()
 {
-  struct stat info;
-  if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) {
-    RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
-    createElevationMap();
-  } else if (info.st_mode & S_IFDIR) {
+  // struct stat info;
+  // if (stat(data_manager_.elevation_map_path_->c_str(), &info) != 0) {
+  //   RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map ");
+  //   createElevationMap();
+  // } else if (info.st_mode & S_IFDIR) {
+    data_manager_.elevation_map_path_ = std::make_unique<std::filesystem::path>(
+      std::filesystem::path(elevation_map_directory_) / "elevation_map_eneos/elevation_map_eneos_0.db3"
+    );
     RCLCPP_INFO(
       this->get_logger(), "Load elevation map from: %s",
       data_manager_.elevation_map_path_->c_str());
     grid_map::GridMapRosConverter::loadFromBag(
       *data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
-  }
+  // }
 
   elevation_map_.setFrameId(map_frame_);
   auto msg = grid_map::GridMapRosConverter::toMessage(elevation_map_);
@@ -126,9 +131,9 @@ void ElevationMapLoaderNode::onMapHash(
   const auto elevation_map_hash = map_hash->pcd;
   data_manager_.elevation_map_path_ = std::make_unique<std::filesystem::path>(
     std::filesystem::path(elevation_map_directory_) / elevation_map_hash);
-  if (data_manager_.isInitialized()) {
+  // if (data_manager_.isInitialized()) {
     publish();
-  }
+  // }
 }
 
 void ElevationMapLoaderNode::onPointcloudMap(