refactor(yabloc_pose_initializer): apply static analysis (autowarefou…

…ndation#7719)

* refactor based on linter

Signed-off-by: a-maumau <[email protected]>

* style(pre-commit): autofix

---------

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

localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp

@@ -65,9 +65,9 @@ class CameraPoseInitializer : public rclcpp::Node
 
   void on_map(const LaneletMapBin & msg);
   void on_service(
-    const RequestPoseAlignment::Request::SharedPtr,
-    RequestPoseAlignment::Response::SharedPtr request);
-  PoseCovStamped create_rectified_initial_pose(
+    const RequestPoseAlignment::Request::SharedPtr request,
+    RequestPoseAlignment::Response::SharedPtr response);
+  static PoseCovStamped create_rectified_initial_pose(
     const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg);
 
   std::optional<double> estimate_pose(

localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp

@@ -31,13 +31,13 @@ class SemanticSegmentation
   static void print_error_message(const rclcpp::Logger & logger);
 
 private:
-  cv::Mat make_blob(const cv::Mat & image);
+  static cv::Mat make_blob(const cv::Mat & image);
 
-  cv::Mat convert_blob_to_image(const cv::Mat & blob);
+  static cv::Mat convert_blob_to_image(const cv::Mat & blob);
 
-  cv::Mat normalize(const cv::Mat & mask, double score_threshold = 0.5);
+  static cv::Mat normalize(const cv::Mat & mask, double score_threshold = 0.5);
 
-  cv::Mat draw_overlay(const cv::Mat & image, const cv::Mat & segmentation);
+  static cv::Mat draw_overlay(const cv::Mat & image, const cv::Mat & segmentation);
 
   struct Impl;
   std::shared_ptr<Impl> impl_{nullptr};

localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp

@@ -32,7 +32,7 @@ namespace yabloc
 {
 CameraPoseInitializer::CameraPoseInitializer(const rclcpp::NodeOptions & options)
 : Node("camera_pose_initializer", options),
-  angle_resolution_(declare_parameter<int>("angle_resolution"))
+  angle_resolution_(static_cast<int>(declare_parameter<int>("angle_resolution")))
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -62,7 +62,7 @@ CameraPoseInitializer::CameraPoseInitializer(const rclcpp::NodeOptions & options
   }
 }
 
-cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2)
+cv::Mat bitwise_and_3ch(const cv::Mat & src1, const cv::Mat & src2)
 {
   std::vector<cv::Mat> src1_array;
   std::vector<cv::Mat> src2_array;
@@ -79,7 +79,7 @@ cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2)
   return merged;
 }
 
-int count_non_zero(cv::Mat image_3ch)
+int count_non_zero(const cv::Mat & image_3ch)
 {
   std::vector<cv::Mat> images;
   cv::split(image_3ch, images);
@@ -153,13 +153,13 @@ std::optional<double> CameraPoseInitializer::estimate_pose(
     // consider lanelet direction
     float gain = 1;
     if (lane_angle_rad) {
-      gain = 2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0);
+      gain = static_cast<float>(2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0));
     }
     // If count_non_zero() returns 0 everywhere, the orientation is chosen by the only gain
-    const float score = gain * (1 + count_non_zero(dst));
+    const float score = gain * static_cast<float>(1 + count_non_zero(dst));
 
     scores.push_back(score);
-    angles_rad.push_back(angle_rad);
+    angles_rad.push_back(static_cast<float>(angle_rad));
   }
 
   marker_module_->publish_marker(scores, angles_rad, position);
@@ -176,7 +176,7 @@ void CameraPoseInitializer::on_map(const LaneletMapBin & msg)
   lane_image_ = std::make_unique<LaneImage>(lanelet_map);
 
   const_lanelets_.clear();
-  for (auto l : lanelet_map->laneletLayer) {
+  for (const auto & l : lanelet_map->laneletLayer) {
     const_lanelets_.push_back(l);
   }
 }
@@ -191,7 +191,9 @@ void CameraPoseInitializer::on_service(
   const auto query_pos = request->pose_with_covariance.pose.pose.position;
   const auto orientation = request->pose_with_covariance.pose.pose.orientation;
   const double yaw_std_rad = std::sqrt(query_pos_with_cov.pose.covariance.at(35));
-  const Eigen::Vector3f pos_vec3f(query_pos.x, query_pos.y, query_pos.z);
+  const Eigen::Vector3f pos_vec3f(
+    static_cast<float>(query_pos.x), static_cast<float>(query_pos.y),
+    static_cast<float>(query_pos.z));
   RCLCPP_INFO_STREAM(get_logger(), "Given initial position " << pos_vec3f.transpose());
 
   // Estimate orientation

localization/yabloc/yabloc_pose_initializer/src/camera/lane_image.cpp

@@ -27,22 +27,22 @@ namespace yabloc
 {
 namespace bg = boost::geometry;
 
-typedef bg::model::d2::point_xy<double> point_t;
-typedef bg::model::box<point_t> box_t;
-typedef bg::model::polygon<point_t> polygon_t;
+using point_t = bg::model::d2::point_xy<double>;
+using box_t = bg::model::box<point_t>;
+using polygon_t = bg::model::polygon<point_t>;
 
-LaneImage::LaneImage(lanelet::LaneletMapPtr map) : map_(map)
+LaneImage::LaneImage(lanelet::LaneletMapPtr map) : map_(std::move(map))
 {
 }
 
 cv::Point2i to_cv_point(const Eigen::Vector3f & v)
 {
-  const float image_size_ = 800;
-  const float max_range_ = 30;
+  const float image_size = 800;
+  const float max_range = 30;
 
   cv::Point pt;
-  pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f;
-  pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f;
+  pt.x = static_cast<int>(-v.y() / max_range * image_size * 0.5f + image_size / 2.f);
+  pt.y = static_cast<int>(-v.x() / max_range * image_size * 0.5f + image_size / 2.f);
   return pt;
 }
 
@@ -59,7 +59,8 @@ void draw_lane(cv::Mat & image, const polygon_t & polygon)
 {
   std::vector<cv::Point> contour;
   for (auto p : polygon.outer()) {
-    cv::Point2i pt = to_cv_point(Eigen::Vector3f(p.x(), p.y(), 0));
+    cv::Point2i pt =
+      to_cv_point(Eigen::Vector3f(static_cast<float>(p.x()), static_cast<float>(p.y()), 0));
     contour.push_back(pt);
   }
 
@@ -71,8 +72,9 @@ void draw_lane(cv::Mat & image, const polygon_t & polygon)
 void draw_line(cv::Mat & image, const lanelet::LineString2d & line, geometry_msgs::msg::Point xyz)
 {
   std::vector<cv::Point> contour;
-  for (auto p : line) {
-    cv::Point2i pt = to_cv_point(Eigen::Vector3f(p.x() - xyz.x, p.y() - xyz.y, 0));
+  for (const auto & p : line) {
+    cv::Point2i pt = to_cv_point(
+      Eigen::Vector3f(static_cast<float>(p.x() - xyz.x), static_cast<float>(p.y() - xyz.y), 0));
     contour.push_back(pt);
   }
   cv::polylines(image, contour, false, cv::Scalar(0, 0, 255), 2);
@@ -83,15 +85,17 @@ cv::Mat LaneImage::get_image(const Pose & pose)
   const auto xyz = pose.position;
   box_t box(point_t(-20, -20), point_t(20, 20));
 
-  cv::Mat image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3);
+  cv::Mat image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3);  // NOLINT
+                                                                // suppress hicpp-signed-bitwise
+                                                                // from OpenCV
 
   std::vector<lanelet::Lanelet> joint_lanes;
   for (auto lanelet : map_->laneletLayer) {
     polygon_t polygon;
     for (auto right : lanelet.rightBound2d()) {
       polygon.outer().push_back(point_t(right.x() - xyz.x, right.y() - xyz.y));
     }
-    for (auto left : boost::adaptors::reverse(lanelet.leftBound2d())) {
+    for (const auto & left : boost::adaptors::reverse(lanelet.leftBound2d())) {
       polygon.outer().push_back(point_t(left.x() - xyz.x, left.y() - xyz.y));
     }
 

localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp

@@ -28,22 +28,22 @@ void MarkerModule::publish_marker(
   const std::vector<float> & scores, const std::vector<float> & angles,
   const Eigen::Vector3f & position)
 {
-  const int N = scores.size();
+  const int n = static_cast<int>(scores.size());
   auto minmax = std::minmax_element(scores.begin(), scores.end());
   auto normalize = [minmax](int score) -> float {
-    return static_cast<float>(score - *minmax.first) /
+    return static_cast<float>(static_cast<float>(score) - *minmax.first) /
            std::max(1e-4f, static_cast<float>(*minmax.second - *minmax.first));
   };
 
   MarkerArray array;
-  for (int i = 0; i < N; i++) {
+  for (int i = 0; i < n; i++) {
     Marker marker;
     marker.header.frame_id = "map";
     marker.type = Marker::ARROW;
     marker.id = i;
     marker.ns = "arrow";
-    marker.color =
-      static_cast<std_msgs::msg::ColorRGBA>(common::color_scale::rainbow(normalize(scores.at(i))));
+    marker.color = static_cast<std_msgs::msg::ColorRGBA>(
+      common::color_scale::rainbow(normalize(static_cast<int>(scores.at(i)))));
     marker.color.a = 0.5;
 
     marker.pose.position.x = position.x();

localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp

@@ -27,12 +27,12 @@ ProjectorModule::ProjectorModule(rclcpp::Node * node)
 
 cv::Point2i to_cv_point(const Eigen::Vector3f & v)
 {
-  const float image_size_ = 800;
-  const float max_range_ = 30;
+  const float image_size = 800;
+  const float max_range = 30;
 
   cv::Point pt;
-  pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f;
-  pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_ / 2.f;
+  pt.x = static_cast<int>(-v.y() / max_range * image_size * 0.5f + image_size / 2.f);
+  pt.y = static_cast<int>(-v.x() / max_range * image_size * 0.5f + image_size / 2.f);
   return pt;
 }
 
@@ -44,7 +44,10 @@ cv::Mat ProjectorModule::project_image(const cv::Mat & mask_image)
   std::vector<cv::Scalar> colors = {
     cv::Scalar(255, 0, 0), cv::Scalar(0, 255, 0), cv::Scalar(0, 0, 255)};
 
-  cv::Mat projected_image = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3);
+  cv::Mat projected_image =
+    cv::Mat::zeros(cv::Size(800, 800), CV_8UC3);  // NOLINT
+                                                  // suppress hicpp-signed-bitwise
+                                                  // from OpenCV
   for (int i = 0; i < 3; i++) {
     std::vector<std::vector<cv::Point> > contours;
     cv::findContours(masks[i], contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
@@ -90,7 +93,7 @@ bool ProjectorModule::define_project_func()
 
   // TODO(KYabuuchi) This will take into account ground tilt and camera vibration someday.
   project_func_ = [intrinsic_inv, q, t](const cv::Point & u) -> std::optional<Eigen::Vector3f> {
-    Eigen::Vector3f u3(u.x, u.y, 1);
+    Eigen::Vector3f u3(static_cast<float>(u.x), static_cast<float>(u.y), 1);
     Eigen::Vector3f u_bearing = (q * intrinsic_inv * u3).normalized();
     if (u_bearing.z() > -0.01) return std::nullopt;
     float u_distance = -t.z() / u_bearing.z();

localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp

@@ -53,7 +53,8 @@ cv::Mat SemanticSegmentation::convert_blob_to_image(const cv::Mat & blob)
   const int channels = blob.size[1];
   const int height = blob.size[2];
   const int width = blob.size[3];
-  cv::Mat image = cv::Mat(height, width, CV_32FC4);
+  cv::Mat image = cv::Mat(height, width, CV_32FC4);  // NOLINT
+                                                     // suppress hicpp-signed-bitwise from OpenCV
 
   for (int h = 0; h < height; ++h) {
     for (int w = 0; w < width; ++w) {
@@ -92,7 +93,8 @@ cv::Mat SemanticSegmentation::normalize(const cv::Mat & mask, double score_thres
   for (size_t i = 1; i < masks.size(); ++i) {
     cv::Mat bin_mask;
     cv::threshold(masks[i], bin_mask, score_threshold, 255, cv::THRESH_BINARY_INV);
-    bin_mask.convertTo(bin_mask, CV_8UC1);
+    bin_mask.convertTo(bin_mask, CV_8UC1);  // NOLINT
+                                            // suppress hicpp-signed-bitwise from OpenCV
     bin_masks.push_back(255 - bin_mask);
   }
 
@@ -109,11 +111,11 @@ cv::Mat SemanticSegmentation::draw_overlay(const cv::Mat & image, const cv::Mat
 
 void SemanticSegmentation::print_error_message(const rclcpp::Logger & logger)
 {
-  const std::string ERROR_MESSAGE =
+  const std::string error_message =
     R"(The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly.
 Please check the README of yabloc_pose_initializer to know how download models.)";
 
-  std::istringstream stream(ERROR_MESSAGE);
+  std::istringstream stream(error_message);
   std::string line;
   while (std::getline(stream, line)) {
     RCLCPP_ERROR_STREAM(logger, line);