refactor(ndt_scan_matcher): refactor callbackSensorPoints (#1757)

* now works

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* refactored alignUsingMonteCarlo

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* output from screen to log

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* lowerCamelCase to snake_case

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* remove iteration_num_max from NdtResult

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* minor fix

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* remove publish_scalars

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* move publish_tf out from publish_pose

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* simplified the function transform_sensor_points

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* WIP pose_array_interpolator

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* added pose_array_interpolator class

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* remove unnecessary code

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* removed std::pow(*, 2)

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* reflected comments

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* minor fix in publish_point_cloud (seperated pcd transform and publish)

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* removed unnecessary commentout

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

* changed Eigen::Vector3f to Eigen::Vector3d

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* reverted No pose warning for initial pose

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

* from_HOGE_to_FUGA to HOGE_to_FUGA

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

* validate_* functions should not change member variables implicitely

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

* ci(pre-commit): autofix

* removed member function that implicitely changes the member variables

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

* ci(pre-commit): autofix

* remote 'ros' and 'eigen' prefix from utiility function name

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

* ci(pre-commit): autofix

* debugged

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

* ci(pre-commit): autofix

* added a name for each validation booleans

Signed-off-by: kminoda <koji.m.minoda@gmail.com>

* ci(pre-commit): autofix

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

localization/ndt_scan_matcher/CMakeLists.txt

@@ -27,6 +27,7 @@ ament_auto_add_executable(ndt_scan_matcher
   src/ndt_scan_matcher_node.cpp
   src/ndt_scan_matcher_core.cpp
   src/util_func.cpp
+  src/pose_array_interpolator.cpp
 )
 
 ament_auto_package(

localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp

@@ -21,7 +21,7 @@
 
 #include <string>
 
-visualization_msgs::msg::MarkerArray makeDebugMarkers(
+visualization_msgs::msg::MarkerArray make_debug_markers(
   const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_,
   const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i);
 

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -68,8 +68,17 @@ enum class ConvergedParamType {
   NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
 };
 
+struct NdtResult
+{
+  geometry_msgs::msg::Pose pose;
+  float transform_probability;
+  float nearest_voxel_transformation_likelihood;
+  int iteration_num;
+  std::vector<geometry_msgs::msg::Pose> transformation_array;
+};
+
 template <typename PointSource, typename PointTarget>
-std::shared_ptr<NormalDistributionsTransformBase<PointSource, PointTarget>> getNDT(
+std::shared_ptr<NormalDistributionsTransformBase<PointSource, PointTarget>> get_ndt(
   const NDTImplementType & ndt_mode)
 {
   std::shared_ptr<NormalDistributionsTransformBase<PointSource, PointTarget>> ndt_ptr;
@@ -107,40 +116,59 @@ class NDTScanMatcher : public rclcpp::Node
   NDTScanMatcher();
 
 private:
-  void serviceNDTAlign(
+  void service_ndt_align(
     const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
     tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
 
-  void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
-  void callbackSensorPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
-  void callbackInitialPose(
+  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);
-  void callbackRegularizationPose(
+  void callback_regularization_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
 
-  geometry_msgs::msg::PoseWithCovarianceStamped alignUsingMonteCarlo(
+  NdtResult align(const geometry_msgs::msg::Pose & initial_pose_msg);
+  geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo(
     const std::shared_ptr<NormalDistributionsTransformBase<PointSource, PointTarget>> & ndt_ptr,
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
 
-  void updateTransforms();
-
-  void publishTF(
-    const std::string & child_frame_id, const geometry_msgs::msg::PoseStamped & pose_msg);
-  bool getTransform(
+  void transform_sensor_measurement(
+    const std::string source_frame, const std::string target_frame,
+    const pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_input_ptr,
+    pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_output_ptr);
+  void update_transforms();
+
+  void publish_tf(
+    const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg);
+  void publish_pose(
+    const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg,
+    const bool is_converged);
+  void publish_point_cloud(
+    const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
+    const std::shared_ptr<const pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr);
+  void publish_marker(
+    const rclcpp::Time & sensor_ros_time, const std::vector<geometry_msgs::msg::Pose> & pose_array);
+  void publish_initial_to_result_distances(
+    const rclcpp::Time & sensor_ros_msg, const geometry_msgs::msg::Pose & result_pose_msg,
+    const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg,
+    const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg,
+    const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg);
+
+  bool get_transform(
     const std::string & target_frame, const std::string & source_frame,
     const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr);
 
-  bool validateTimeStampDifference(
-    const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
-    const double time_tolerance_sec);
-  bool validatePositionDifference(
-    const geometry_msgs::msg::Point & target_point,
-    const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_);
+  bool validate_num_iteration(const int iter_num, const int max_iter_num);
+  bool validate_score(
+    const double score, const double score_threshold, const std::string score_name);
+  bool validate_converged_param(
+    const double & transform_probability, const double & nearest_voxel_transformation_likelihood);
 
-  std::optional<Eigen::Matrix4f> interpolateRegularizationPose(
+  std::optional<Eigen::Matrix4f> interpolate_regularization_pose(
     const rclcpp::Time & sensor_ros_time);
+  void add_regularization_pose(const rclcpp::Time & sensor_ros_time);
 
-  void timerDiagnostic();
+  void timer_diagnostic();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
@@ -197,7 +225,7 @@ class NDTScanMatcher : public rclcpp::Node
 
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
     initial_pose_msg_ptr_array_;
-  std::mutex ndt_map_mtx_;
+  std::mutex ndt_ptr_mtx_;
   std::mutex initial_pose_array_mtx_;
 
   OMPParams omp_params_;

localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp

@@ -0,0 +1,62 @@
+// Copyright 2015-2019 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__POSE_ARRAY_INTERPOLATOR_HPP_
+#define NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_
+
+#include "ndt_scan_matcher/util_func.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <deque>
+
+class PoseArrayInterpolator
+{
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+
+public:
+  PoseArrayInterpolator(
+    rclcpp::Node * node, const rclcpp::Time target_ros_time,
+    const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array,
+    const double & pose_timeout_sec, const double & pose_distance_tolerance_meters);
+
+  PoseArrayInterpolator(
+    rclcpp::Node * node, const rclcpp::Time target_ros_time,
+    const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array);
+
+  PoseWithCovarianceStamped get_current_pose();
+  PoseWithCovarianceStamped get_old_pose();
+  PoseWithCovarianceStamped get_new_pose();
+  bool is_success();
+
+private:
+  rclcpp::Logger logger_;
+  rclcpp::Clock clock_;
+  const PoseWithCovarianceStamped::SharedPtr current_pose_ptr_;
+  PoseWithCovarianceStamped::SharedPtr old_pose_ptr_;
+  PoseWithCovarianceStamped::SharedPtr new_pose_ptr_;
+  bool success_;
+
+  bool validate_time_stamp_difference(
+    const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
+    const double time_tolerance_sec) const;
+  bool validate_position_difference(
+    const geometry_msgs::msg::Point & target_point,
+    const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
+};
+
+#endif  // NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp

@@ -36,41 +36,44 @@
 #include <vector>
 
 // ref by http://takacity.blog.fc2.com/blog-entry-69.html
-std_msgs::msg::ColorRGBA ExchangeColorCrc(double x);
+std_msgs::msg::ColorRGBA exchange_color_crc(double x);
 
-double calcDiffForRadian(const double lhs_rad, const double rhs_rad);
+double calc_diff_for_radian(const double lhs_rad, const double rhs_rad);
 
 // x: roll, y: pitch, z: yaw
-geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose);
-geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose);
-geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
 
-geometry_msgs::msg::Twist calcTwist(
+geometry_msgs::msg::Twist calc_twist(
   const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
 
-void getNearestTimeStampPose(
+void get_nearest_timestamp_pose(
   const std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr> &
     pose_cov_msg_ptr_array,
   const rclcpp::Time & time_stamp,
   geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr,
   geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr);
 
-geometry_msgs::msg::PoseStamped interpolatePose(
+geometry_msgs::msg::PoseStamped interpolate_pose(
   const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
   const rclcpp::Time & time_stamp);
 
-geometry_msgs::msg::PoseStamped interpolatePose(
+geometry_msgs::msg::PoseStamped interpolate_pose(
   const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
   const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
 
-void popOldPose(
+void pop_old_pose(
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr> &
     pose_cov_msg_ptr_array,
   const rclcpp::Time & time_stamp);
 
-Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose);
+Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
+Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
+geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
+Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
 
-std::vector<geometry_msgs::msg::Pose> createRandomPoseArray(
+std::vector<geometry_msgs::msg::Pose> create_random_pose_array(
   const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num);
 
 template <class T>
@@ -81,4 +84,6 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf
   return output;
 }
 
+double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
+
 #endif  // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_

localization/ndt_scan_matcher/src/debug.cpp

@@ -16,7 +16,7 @@
 
 #include "ndt_scan_matcher/util_func.hpp"
 
-visualization_msgs::msg::MarkerArray makeDebugMarkers(
+visualization_msgs::msg::MarkerArray make_debug_markers(
   const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_,
   const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i)
 {
@@ -34,32 +34,32 @@ visualization_msgs::msg::MarkerArray makeDebugMarkers(
 
   marker.ns = "initial_pose_transform_probability_color_marker";
   marker.pose = particle.initial_pose;
-  marker.color = ExchangeColorCrc(particle.score / 4.5);
+  marker.color = exchange_color_crc(particle.score / 4.5);
   marker_array.markers.push_back(marker);
 
   marker.ns = "initial_pose_iteration_color_marker";
   marker.pose = particle.initial_pose;
-  marker.color = ExchangeColorCrc((1.0 * particle.iteration) / 30.0);
+  marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0);
   marker_array.markers.push_back(marker);
 
   marker.ns = "initial_pose_index_color_marker";
   marker.pose = particle.initial_pose;
-  marker.color = ExchangeColorCrc((1.0 * i) / 100);
+  marker.color = exchange_color_crc((1.0 * i) / 100);
   marker_array.markers.push_back(marker);
 
   marker.ns = "result_pose_transform_probability_color_marker";
   marker.pose = particle.result_pose;
-  marker.color = ExchangeColorCrc(particle.score / 4.5);
+  marker.color = exchange_color_crc(particle.score / 4.5);
   marker_array.markers.push_back(marker);
 
   marker.ns = "result_pose_iteration_color_marker";
   marker.pose = particle.result_pose;
-  marker.color = ExchangeColorCrc((1.0 * particle.iteration) / 30.0);
+  marker.color = exchange_color_crc((1.0 * particle.iteration) / 30.0);
   marker_array.markers.push_back(marker);
 
   marker.ns = "result_pose_index_color_marker";
   marker.pose = particle.result_pose;
-  marker.color = ExchangeColorCrc((1.0 * i) / 100);
+  marker.color = exchange_color_crc((1.0 * i) / 100);
   marker_array.markers.push_back(marker);
 
   return marker_array;