refactor(ndt_scan_matcher): fixed as pointed out by clang-tidy (autow…

…arefoundation#4286)

Fixed as pointed out by clang-tidy

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp

@@ -39,7 +39,7 @@ class MapModule
     rclcpp::CallbackGroup::SharedPtr map_callback_group);
 
 private:
-  void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
+  void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_points_sub_;
   std::shared_ptr<NormalDistributionsTransform> ndt_ptr_;

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -63,7 +63,7 @@ class MapUpdateModule
     const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & maps_to_add,
     const std::vector<std::string> & map_ids_to_remove);
   void update_map(const geometry_msgs::msg::Point & position);
-  bool should_update_map(const geometry_msgs::msg::Point & position) const;
+  [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position) const;
   void publish_partial_pcd_map();
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr loaded_pcd_pub_;

localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp

@@ -30,18 +30,18 @@ class PoseArrayInterpolator
 
 public:
   PoseArrayInterpolator(
-    rclcpp::Node * node, const rclcpp::Time target_ros_time,
+    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,
+    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();
+  [[nodiscard]] bool is_success() const;
 
 private:
   rclcpp::Logger logger_;
@@ -51,10 +51,10 @@ class PoseArrayInterpolator
   PoseWithCovarianceStamped::SharedPtr new_pose_ptr_;
   bool success_;
 
-  bool validate_time_stamp_difference(
+  [[nodiscard]] 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(
+  [[nodiscard]] bool validate_position_difference(
     const geometry_msgs::msg::Point & target_point,
     const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
 };

localization/ndt_scan_matcher/src/debug.cpp

@@ -30,7 +30,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers(
   marker.type = visualization_msgs::msg::Marker::ARROW;
   marker.action = visualization_msgs::msg::Marker::ADD;
   marker.scale = scale;
-  marker.id = i;
+  marker.id = static_cast<int32_t>(i);
   marker.lifetime = rclcpp::Duration::from_seconds(10.0);  // 10.0 is the lifetime in seconds.
 
   marker.ns = "initial_pose_transform_probability_color_marker";
@@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers(
 
   marker.ns = "initial_pose_index_color_marker";
   marker.pose = particle.initial_pose;
-  marker.color = exchange_color_crc((1.0 * i) / 100);
+  marker.color = exchange_color_crc(static_cast<double>(i) / 100.0);
   marker_array.markers.push_back(marker);
 
   marker.ns = "result_pose_transform_probability_color_marker";
@@ -60,7 +60,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers(
 
   marker.ns = "result_pose_index_color_marker";
   marker.pose = particle.result_pose;
-  marker.color = exchange_color_crc((1.0 * i) / 100);
+  marker.color = exchange_color_crc(static_cast<double>(i) / 100.0);
   marker_array.markers.push_back(marker);
 
   return marker_array;

localization/ndt_scan_matcher/src/map_module.cpp

@@ -18,7 +18,7 @@ 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)
+: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex)
 {
   auto map_sub_opt = rclcpp::SubscriptionOptions();
   map_sub_opt.callback_group = map_callback_group;

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -28,13 +28,13 @@ MapUpdateModule::MapUpdateModule(
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
   rclcpp::CallbackGroup::SharedPtr main_callback_group,
   std::shared_ptr<std::map<std::string, std::string>> state_ptr)
-: ndt_ptr_(ndt_ptr),
+: ndt_ptr_(std::move(ndt_ptr)),
   ndt_ptr_mutex_(ndt_ptr_mutex),
-  map_frame_(map_frame),
+  map_frame_(std::move(map_frame)),
   logger_(node->get_logger()),
   clock_(node->get_clock()),
-  tf2_listener_module_(tf2_listener_module),
-  state_ptr_(state_ptr),
+  tf2_listener_module_(std::move(tf2_listener_module)),
+  state_ptr_(std::move(state_ptr)),
   dynamic_map_loading_update_distance_(
     node->declare_parameter<double>("dynamic_map_loading_update_distance")),
   dynamic_map_loading_map_radius_(
@@ -112,9 +112,9 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi
 void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
 {
   auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
-  request->area.center_x = position.x;
-  request->area.center_y = position.y;
-  request->area.radius = dynamic_map_loading_map_radius_;
+  request->area.center_x = static_cast<float>(position.x);
+  request->area.center_y = static_cast<float>(position.y);
+  request->area.radius = static_cast<float>(dynamic_map_loading_map_radius_);
   request->cached_ids = ndt_ptr_->getCurrentMapIDs();
 
   // // send a request to map_loader
@@ -163,9 +163,9 @@ void MapUpdateModule::update_ndt(
   backup_ndt.createVoxelKdtree();
 
   const auto exe_end_time = std::chrono::system_clock::now();
-  const double exe_time =
-    std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count() /
-    1000.0;
+  const auto duration_micro_sec =
+    std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
+  const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
   RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
 
   // swap

localization/ndt_scan_matcher/src/pose_array_interpolator.cpp

@@ -15,7 +15,7 @@
 #include "ndt_scan_matcher/pose_array_interpolator.hpp"
 
 PoseArrayInterpolator::PoseArrayInterpolator(
-  rclcpp::Node * node, const rclcpp::Time target_ros_time,
+  rclcpp::Node * node, const rclcpp::Time & target_ros_time,
   const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array)
 : logger_(node->get_logger()),
   clock_(*node->get_clock()),
@@ -33,7 +33,7 @@ PoseArrayInterpolator::PoseArrayInterpolator(
 }
 
 PoseArrayInterpolator::PoseArrayInterpolator(
-  rclcpp::Node * node, const rclcpp::Time target_ros_time,
+  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(node, target_ros_time, pose_msg_ptr_array)
@@ -50,7 +50,7 @@ PoseArrayInterpolator::PoseArrayInterpolator(
     pose_distance_tolerance_meters);
 
   // all validations must be true
-  if (!(is_old_pose_valid & is_new_pose_valid & is_pose_diff_valid)) {
+  if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) {
     RCLCPP_WARN(logger_, "Validation error.");
   }
 }
@@ -70,7 +70,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pos
   return *new_pose_ptr_;
 }
 
-bool PoseArrayInterpolator::is_success()
+bool PoseArrayInterpolator::is_success() const
 {
   return success_;
 }

localization/ndt_scan_matcher/src/util_func.cpp

@@ -28,19 +28,19 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x)
 
   if (x <= 0.25) {
     color.b = 1.0;
-    color.g = std::sin(x * 2.0 * M_PI);
+    color.g = static_cast<float>(std::sin(x * 2.0 * M_PI));
     color.r = 0;
   } else if (x > 0.25 && x <= 0.5) {
-    color.b = std::sin(x * 2 * M_PI);
+    color.b = static_cast<float>(std::sin(x * 2 * M_PI));
     color.g = 1.0;
     color.r = 0;
   } else if (x > 0.5 && x <= 0.75) {
     color.b = 0;
     color.g = 1.0;
-    color.r = -std::sin(x * 2.0 * M_PI);
+    color.r = static_cast<float>(-std::sin(x * 2.0 * M_PI));
   } else {
     color.b = 0;
-    color.g = -std::sin(x * 2.0 * M_PI);
+    color.g = static_cast<float>(-std::sin(x * 2.0 * M_PI));
     color.r = 1.0;
   }
   color.a = 0.999;
@@ -58,9 +58,9 @@ double calc_diff_for_radian(const double lhs_rad, const double rhs_rad)
   return diff_rad;
 }
 
-Eigen::Map<const RowMatrixXd> makeEigenCovariance(const std::array<double, 36> & covariance)
+Eigen::Map<const RowMatrixXd> make_eigen_covariance(const std::array<double, 36> & covariance)
 {
-  return Eigen::Map<const RowMatrixXd>(covariance.data(), 6, 6);
+  return {covariance.data(), 6, 6};
 }
 
 // x: roll, y: pitch, z: yaw
@@ -242,7 +242,7 @@ std::vector<geometry_msgs::msg::Pose> create_random_pose_array(
 {
   std::default_random_engine engine(seed_gen());
   const Eigen::Map<const RowMatrixXd> covariance =
-    makeEigenCovariance(base_pose_with_cov.pose.covariance);
+    make_eigen_covariance(base_pose_with_cov.pose.covariance);
 
   std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0)));
   std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1)));