refactor(yabloc_common): apply static analysis (#7481)

* refactor based on linter

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* restore unwanted change

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* remove unnecessary comment

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* style(pre-commit): autofix

* change to const double

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* add static cast

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

---------

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: 2 people

localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp

@@ -31,17 +31,17 @@ class CameraInfoSubscriber
 
   explicit CameraInfoSubscriber(rclcpp::Node * node);
 
-  bool is_camera_info_ready() const;
+  [[nodiscard]] bool is_camera_info_ready() const;
 
-  bool is_camera_info_nullopt() const;
+  [[nodiscard]] bool is_camera_info_nullopt() const;
 
-  Eigen::Vector2i size() const;
+  [[nodiscard]] Eigen::Vector2i size() const;
 
-  Eigen::Matrix3f intrinsic() const;
+  [[nodiscard]] Eigen::Matrix3f intrinsic() const;
 
   // This member function DOES NOT check isCameraInfoReady()
   // If it it not ready, throw bad optional access
-  std::string get_frame_id() const;
+  [[nodiscard]] std::string get_frame_id() const;
 
 private:
   rclcpp::Subscription<CameraInfo>::SharedPtr sub_info_;

localization/yabloc/yabloc_common/include/yabloc_common/color.hpp

@@ -31,25 +31,28 @@ struct Color
   }
 
   explicit Color(const cv::Scalar & rgb, float a = 1.0f)
-  : r(rgb[2] / 255.f), g(rgb[1] / 255.f), b(rgb[0] / 255.f), a(a)
+  : r(static_cast<float>(rgb[2]) / 255.f),
+    g(static_cast<float>(rgb[1]) / 255.f),
+    b(static_cast<float>(rgb[0]) / 255.f),
+    a(a)
   {
   }
 
-  operator uint32_t() const
+  explicit operator uint32_t() const
   {
     union uchar4_uint32 {
       uint8_t rgba[4];
       uint32_t u32;
     };
-    uchar4_uint32 tmp;
+    uchar4_uint32 tmp{};
     tmp.rgba[0] = static_cast<uint8_t>(r * 255);
     tmp.rgba[1] = static_cast<uint8_t>(g * 255);
     tmp.rgba[2] = static_cast<uint8_t>(b * 255);
     tmp.rgba[3] = static_cast<uint8_t>(a * 255);
     return tmp.u32;
   }
-  operator const cv::Scalar() const { return cv::Scalar(255 * b, 255 * g, 255 * r); }
-  operator const std_msgs::msg::ColorRGBA() const
+  explicit operator cv::Scalar() const { return {255 * b, 255 * g, 255 * r}; }
+  explicit operator std_msgs::msg::ColorRGBA() const
   {
     std_msgs::msg::ColorRGBA rgba;
     rgba.a = a;

localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp

@@ -27,7 +27,8 @@ class GammaConverter
   {
     lut_ = cv::Mat(1, 256, CV_8UC1);
     for (int i = 0; i < 256; i++) {
-      lut_.at<uchar>(0, i) = 256 * std::pow(i / 256.f, gamma);
+      lut_.at<uchar>(0, i) =
+        static_cast<unsigned char>(256 * std::pow(static_cast<float>(i) / 256.f, gamma));
     }
   }
 

localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp

@@ -46,39 +46,39 @@ struct GroundPlane
     }
   }
 
-  float height() const { return xyz.z(); }
+  [[nodiscard]] float height() const { return xyz.z(); }
 
-  Eigen::Quaternionf align_with_slope(const Eigen::Quaternionf & q) const
+  [[nodiscard]] Eigen::Quaternionf align_with_slope(const Eigen::Quaternionf & q) const
   {
     return Eigen::Quaternionf{align_with_slope(q.toRotationMatrix())};
   }
 
-  Eigen::Matrix3f align_with_slope(const Eigen::Matrix3f & R) const
+  [[nodiscard]] Eigen::Matrix3f align_with_slope(const Eigen::Matrix3f & R) const
   {
-    Eigen::Matrix3f R_;
+    Eigen::Matrix3f r;
     Eigen::Vector3f rz = this->normal;
     Eigen::Vector3f azimuth = R * Eigen::Vector3f::UnitX();
     Eigen::Vector3f ry = (rz.cross(azimuth)).normalized();
     Eigen::Vector3f rx = ry.cross(rz);
-    R_.col(0) = rx;
-    R_.col(1) = ry;
-    R_.col(2) = rz;
-    return R_;
+    r.col(0) = rx;
+    r.col(1) = ry;
+    r.col(2) = rz;
+    return r;
   }
 
-  Sophus::SE3f align_with_slope(const Sophus::SE3f & pose) const
+  [[nodiscard]] Sophus::SE3f align_with_slope(const Sophus::SE3f & pose) const
   {
     return {align_with_slope(pose.rotationMatrix()), pose.translation()};
   }
 
-  Eigen::Affine3f align_with_slope(const Eigen::Affine3f & pose) const
+  [[nodiscard]] Eigen::Affine3f align_with_slope(const Eigen::Affine3f & pose) const
   {
-    Eigen::Matrix3f R = pose.rotation();
+    Eigen::Matrix3f r = pose.rotation();
     Eigen::Vector3f t = pose.translation();
-    return Eigen::Translation3f(t) * align_with_slope(R);
+    return Eigen::Translation3f(t) * align_with_slope(r);
   }
 
-  Float32Array msg() const
+  [[nodiscard]] Float32Array msg() const
   {
     Float32Array array;
     for (int i = 0; i < 3; i++) array.data.push_back(xyz(i));

localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp

@@ -32,7 +32,7 @@ class MovingAveraging
 
     Eigen::Vector3f mean = Eigen::Vector3f::Zero();
     for (const Eigen::Vector3f & v : buffer_) mean += v;
-    mean /= buffer_.size();
+    mean /= static_cast<float>(buffer_.size());
 
     return mean.normalized();
   }

localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp

@@ -61,8 +61,8 @@ class GroundServer : public rclcpp::Node
 
 private:
   const bool force_zero_tilt_;
-  const float R;
-  const int K;
+  const float R_;
+  const int K_;
 
   // Subscriber
   rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;

localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp

@@ -26,12 +26,14 @@
 
 namespace yabloc::ground_server
 {
-void upsample_line_string(
+void inline upsample_line_string(
   const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to,
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
 {
-  Eigen::Vector3f f(from.x(), from.y(), from.z());
-  Eigen::Vector3f t(to.x(), to.y(), to.z());
+  Eigen::Vector3f f(
+    static_cast<float>(from.x()), static_cast<float>(from.y()), static_cast<float>(from.z()));
+  Eigen::Vector3f t(
+    static_cast<float>(to.x()), static_cast<float>(to.y()), static_cast<float>(to.z()));
   float length = (t - f).norm();
   Eigen::Vector3f d = (t - f).normalized();
   for (float l = 0; l < length; l += 0.5f) {
@@ -41,7 +43,8 @@ void upsample_line_string(
   }
 };
 
-std::vector<int> merge_indices(const std::vector<int> & indices1, const std::vector<int> & indices2)
+std::vector<int> inline merge_indices(
+  const std::vector<int> & indices1, const std::vector<int> & indices2)
 {
   std::unordered_set<int> set;
   for (int i : indices1) set.insert(i);

localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp

@@ -54,17 +54,18 @@ class Ll2Decomposer : public rclcpp::Node
 
   void on_map(const LaneletMapBin & msg);
 
-  pcl::PointNormal to_point_normal(
-    const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const;
+  static pcl::PointNormal to_point_normal(
+    const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to);
 
-  pcl::PointCloud<pcl::PointNormal> split_line_strings(
+  static pcl::PointCloud<pcl::PointNormal> split_line_strings(
     const lanelet::ConstLineStrings3d & line_strings);
 
   pcl::PointCloud<pcl::PointXYZL> load_bounding_boxes(const lanelet::PolygonLayer & polygons) const;
 
-  lanelet::ConstLineStrings3d extract_specified_line_string(
-    const lanelet::LineStringLayer & line_strings, const std::set<std::string> & visible_labels);
-  lanelet::ConstPolygons3d extract_specified_polygon(
+  static lanelet::ConstLineStrings3d extract_specified_line_string(
+    const lanelet::LineStringLayer & line_string_layer,
+    const std::set<std::string> & visible_labels);
+  static lanelet::ConstPolygons3d extract_specified_polygon(
     const lanelet::PolygonLayer & polygon_layer, const std::set<std::string> & visible_labels);
 
   MarkerArray make_sign_marker_msg(

localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp

@@ -51,8 +51,8 @@ Eigen::Matrix3f CameraInfoSubscriber::intrinsic() const
   if (!opt_info_.has_value()) {
     throw std::runtime_error("camera_info is not ready but it's accessed");
   }
-  const Eigen::Matrix3d Kd_t = Eigen::Map<const Eigen::Matrix<double, 3, 3>>(opt_info_->k.data());
-  return Kd_t.cast<float>().transpose();
+  const Eigen::Matrix3d kd_t = Eigen::Map<const Eigen::Matrix<double, 3, 3>>(opt_info_->k.data());
+  return kd_t.cast<float>().transpose();
 }
 
 }  // namespace yabloc::common

localization/yabloc/yabloc_common/src/color.cpp

@@ -19,7 +19,9 @@ namespace yabloc::common::color_scale
 Color rainbow(float value)
 {
   // clang-format off
-  float r = 1.0f, g = 1.0f, b = 1.0f;
+  float r = 1.0f;
+  float g = 1.0f;
+  float b = 1.0f;
   value = std::clamp(value, 0.0f, 1.0f);
   if (value < 0.25f) {
     r = 0; g = 4 * (value);
@@ -40,18 +42,12 @@ Color hsv_to_rgb(float h_, float s_, float v_)
   const float max = v_;
   const float min = max * (1.0f - s_);
 
-  if (h < 60)
-    return {max, h / 60 * (max - min) + min, min};
-  else if (h < 120)
-    return {(120 - h) / 60 * (max - min) + min, max, min};
-  else if (h < 180)
-    return {min, max, (h - 120) / 60 * (max - min) + min};
-  else if (h < 240)
-    return {min, (240 - h) / 60 * (max - min) + min, max};
-  else if (h < 300)
-    return {(h - 240) / 60 * (max - min) + min, min, max};
-  else
-    return {max, min, (360 - h) / 60 * (max - min) + min};
+  if (h < 60) return {max, h / 60 * (max - min) + min, min};
+  if (h < 120) return {(120 - h) / 60 * (max - min) + min, max, min};
+  if (h < 180) return {min, max, (h - 120) / 60 * (max - min) + min};
+  if (h < 240) return {min, (240 - h) / 60 * (max - min) + min, max};
+  if (h < 300) return {(h - 240) / 60 * (max - min) + min, min, max};
+  return {max, min, (360 - h) / 60 * (max - min) + min};
 }
 
 Color blue_red(float value)

localization/yabloc/yabloc_common/src/cv_decompress.cpp

@@ -28,16 +28,16 @@ cv::Mat decompress_image(const sensor_msgs::msg::CompressedImage & compressed_im
   cv::Mat raw_image;
 
   const std::string & format = compressed_img.format;
-  const std::string encoding = format.substr(0, format.find(";"));
+  const std::string encoding = format.substr(0, format.find(';'));
 
-  constexpr int DECODE_GRAY = 0;
-  constexpr int DECODE_RGB = 1;
+  constexpr int decode_gray = 0;
+  constexpr int decode_rgb = 1;
 
   bool encoding_is_bayer = encoding.find("bayer") != std::string::npos;
   if (!encoding_is_bayer) {
-    return cv::imdecode(cv::Mat(compressed_img.data), DECODE_RGB);
+    return cv::imdecode(cv::Mat(compressed_img.data), decode_rgb);
   }
-  raw_image = cv::imdecode(cv::Mat(compressed_img.data), DECODE_GRAY);
+  raw_image = cv::imdecode(cv::Mat(compressed_img.data), decode_gray);
 
   // TODO(KYabuuchi) integrate with implementation in the sensing/perception component
   if (encoding == "bayer_rggb8") {

localization/yabloc/yabloc_common/src/extract_line_segments.cpp

@@ -20,8 +20,8 @@ pcl::PointCloud<pcl::PointNormal> extract_near_line_segments(
   const pcl::PointCloud<pcl::PointNormal> & line_segments, const Sophus::SE3f & transform,
   const float max_range)
 {
-  constexpr double sqrt_two = 1.41421356237309504880;
-  const Eigen::Vector3f pose_vector = transform.translation();
+  const double sqrt_two = 1.41421356237309504880;
+  const Eigen::Vector3f & pose_vector = transform.translation();
 
   // All line segments contained in a square with max_range on one side must be taken out,
   // so pick up those that are closer than the **diagonals** of the square.
@@ -42,11 +42,9 @@ pcl::PointCloud<pcl::PointNormal> extract_near_line_segments(
   };
 
   pcl::PointCloud<pcl::PointNormal> dst;
-  for (const pcl::PointNormal & pn : line_segments) {
-    if (check_intersection(pn)) {
-      dst.push_back(pn);
-    }
-  }
+  std::copy_if(
+    line_segments.begin(), line_segments.end(), std::back_inserter(dst),
+    [check_intersection](const auto & pn) { return check_intersection(pn); });
   return dst;
 }
 

localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp

@@ -28,13 +28,15 @@
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/segmentation/sac_segmentation.h>
 
+#include <cmath>
+
 namespace yabloc::ground_server
 {
 GroundServer::GroundServer(const rclcpp::NodeOptions & options)
 : Node("ground_server", options),
   force_zero_tilt_(declare_parameter<bool>("force_zero_tilt")),
-  R(declare_parameter<int>("R")),
-  K(declare_parameter<int>("K"))
+  R_(static_cast<float>(declare_parameter<int>("R"))),
+  K_(static_cast<int>(declare_parameter<int>("K")))
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -147,8 +149,8 @@ float GroundServer::estimate_height_simply(const geometry_msgs::msg::Point & poi
 {
   // NOTE: Sometimes it might give not-accurate height
   constexpr float sq_radius = 3.0 * 3.0;
-  const float x = point.x;
-  const float y = point.y;
+  const auto x = static_cast<float>(point.x);
+  const auto y = static_cast<float>(point.y);
 
   float height = std::numeric_limits<float>::infinity();
   for (const auto & p : cloud_->points) {
@@ -186,12 +188,12 @@ std::vector<int> GroundServer::estimate_inliers_by_ransac(const std::vector<int>
 GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
 {
   // Because height_filter_ is always initialized, getValue does not return nullopt
-  const float predicted_z = height_filter_.getValue().value();
-  const pcl::PointXYZ xyz(point.x, point.y, predicted_z);
+  const float predicted_z = static_cast<float>(height_filter_.getValue().value());
+  const pcl::PointXYZ xyz(static_cast<float>(point.x), static_cast<float>(point.y), predicted_z);
 
   std::vector<int> raw_indices;
   std::vector<float> distances;
-  kdtree_->nearestKSearch(xyz, K, raw_indices, distances);
+  kdtree_->nearestKSearch(xyz, K_, raw_indices, distances);
 
   std::vector<int> indices = estimate_inliers_by_ransac(raw_indices);
 
@@ -206,7 +208,7 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
 
   // NOTE: I forgot why I don't use coefficients computed by SACSegmentation
   Eigen::Vector4f plane_parameter;
-  float curvature;
+  float curvature = NAN;
   pcl::solvePlaneParameters(covariance, centroid, plane_parameter, curvature);
   Eigen::Vector3f normal = plane_parameter.topRows(3).normalized();
 
@@ -229,15 +231,16 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
   const Eigen::Vector3f filt_normal = normal_filter_.update(normal);
 
   GroundPlane plane;
-  plane.xyz = Eigen::Vector3f(point.x, point.y, predicted_z);
+  plane.xyz =
+    Eigen::Vector3f(static_cast<float>(point.x), static_cast<float>(point.y), predicted_z);
   plane.normal = filt_normal;
 
   // Compute z value by intersection of estimated plane and orthogonal line
   {
     Eigen::Vector3f center = centroid.topRows(3);
     float inner = center.dot(plane.normal);
-    float px_nx = point.x * plane.normal.x();
-    float py_ny = point.y * plane.normal.y();
+    float px_nx = static_cast<float>(point.x) * plane.normal.x();
+    float py_ny = static_cast<float>(point.y) * plane.normal.y();
     plane.xyz.z() = (inner - px_nx - py_ny) / plane.normal.z();
   }