refactor(ndt_scan_matcher): modularize map_callback (autowarefoundati…

…on#2249)

* still work in progress

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* ptr_ptr yameru

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* remove unnecessary newline

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* revert launch setting

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* ci(pre-commit): autofix

* reverted debugging line

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* pre-commit

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* remove format inclusion

Signed-off-by: kminoda <koji.minoda@tier4.jp>

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

localization/ndt_scan_matcher/CMakeLists.txt

@@ -32,6 +32,7 @@ ament_auto_add_executable(ndt_scan_matcher
   src/util_func.cpp
   src/pose_array_interpolator.cpp
   src/tf2_listener_module.cpp
+  src/map_module.cpp
 )
 
 link_directories(${PCL_LIBRARY_DIRS})

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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 NDT_SCAN_MATCHER__MAP_MODULE_HPP_
+#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pclomp/ndt_omp.h>
+
+#include <memory>
+
+class MapModule
+{
+  using PointSource = pcl::PointXYZ;
+  using PointTarget = pcl::PointXYZ;
+  using NormalDistributionsTransform =
+    pclomp::NormalDistributionsTransform<PointSource, PointTarget>;
+
+public:
+  MapModule(
+    rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
+    std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
+    rclcpp::CallbackGroup::SharedPtr map_callback_group);
+
+private:
+  void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
+  std::shared_ptr<NormalDistributionsTransform> ndt_ptr_;
+  std::mutex * ndt_ptr_mutex_;
+};
+
+#endif  // NDT_SCAN_MATCHER__MAP_MODULE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -17,6 +17,7 @@
 
 #define FMT_HEADER_ONLY
 
+#include "ndt_scan_matcher/map_module.hpp"
 #include "ndt_scan_matcher/particle.hpp"
 #include "ndt_scan_matcher/tf2_listener_module.hpp"
 
@@ -84,7 +85,6 @@ class NDTScanMatcher : public rclcpp::Node
     const std_srvs::srv::SetBool::Request::SharedPtr req,
     std_srvs::srv::SetBool::Response::SharedPtr res);
 
-  void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
   void callback_sensor_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
   void callback_initial_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
@@ -130,7 +130,6 @@ class NDTScanMatcher : public rclcpp::Node
   void timer_diagnostic();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     regularization_pose_sub_;
@@ -195,6 +194,7 @@ class NDTScanMatcher : public rclcpp::Node
 
   bool is_activated_;
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
+  std::unique_ptr<MapModule> map_module_;
 };
 
 #endif  // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_

localization/ndt_scan_matcher/src/map_module.cpp

@@ -0,0 +1,49 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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 "ndt_scan_matcher/map_module.hpp"
+
+MapModule::MapModule(
+  rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
+  std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
+  rclcpp::CallbackGroup::SharedPtr map_callback_group)
+: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex)
+{
+  auto map_sub_opt = rclcpp::SubscriptionOptions();
+  map_sub_opt.callback_group = map_callback_group;
+
+  map_points_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
+    "pointcloud_map", rclcpp::QoS{1}.transient_local(),
+    std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt);
+}
+
+void MapModule::callback_map_points(
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
+{
+  NormalDistributionsTransform new_ndt;
+  new_ndt.setParams(ndt_ptr_->getParams());
+
+  pcl::shared_ptr<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);
+  pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
+  new_ndt.setInputTarget(map_points_ptr);
+  // create Thread
+  // detach
+  auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
+  new_ndt.align(*output_cloud);
+
+  // swap
+  ndt_ptr_mutex_->lock();
+  *ndt_ptr_ = new_ndt;
+  ndt_ptr_mutex_->unlock();
+}

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -168,9 +168,6 @@ NDTScanMatcher::NDTScanMatcher()
     "ekf_pose_with_covariance", 100,
     std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1),
     initial_pose_sub_opt);
-  map_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-    "pointcloud_map", rclcpp::QoS{1}.transient_local(),
-    std::bind(&NDTScanMatcher::callback_map_points, this, std::placeholders::_1), main_sub_opt);
   sensor_points_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size),
     std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt);
@@ -227,6 +224,7 @@ NDTScanMatcher::NDTScanMatcher()
   diagnostic_thread_.detach();
 
   tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);
+  map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group);
 }
 
 void NDTScanMatcher::timer_diagnostic()
@@ -354,26 +352,6 @@ void NDTScanMatcher::callback_regularization_pose(
   regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr);
 }
 
-void NDTScanMatcher::callback_map_points(
-  sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
-{
-  std::shared_ptr<NormalDistributionsTransform> new_ndt_ptr(new NormalDistributionsTransform);
-  new_ndt_ptr->setParams(ndt_ptr_->getParams());
-
-  pcl::shared_ptr<pcl::PointCloud<PointTarget>> map_points_ptr(new pcl::PointCloud<PointTarget>);
-  pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
-  new_ndt_ptr->setInputTarget(map_points_ptr);
-  // create Thread
-  // detach
-  auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
-  new_ndt_ptr->align(*output_cloud);
-
-  // swap
-  ndt_ptr_mtx_.lock();
-  ndt_ptr_ = new_ndt_ptr;
-  ndt_ptr_mtx_.unlock();
-}
-
 void NDTScanMatcher::callback_sensor_points(
   sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr)
 {