From 2d99f34ebd8930e70f3f5058337bc97c0d026695 Mon Sep 17 00:00:00 2001 From: a-maumau Date: Thu, 13 Jun 2024 17:26:00 +0900 Subject: [PATCH] refactor based on linter Signed-off-by: a-maumau --- .../launch/system.launch.xml | 2 +- .../yabloc_common/camera_info_subscriber.hpp | 10 +++---- .../include/yabloc_common/color.hpp | 11 ++++---- .../include/yabloc_common/gamma_converter.hpp | 3 +- .../include/yabloc_common/ground_plane.hpp | 26 ++++++++--------- .../ground_server/filter/moving_averaging.hpp | 2 +- .../ground_server/ground_server.hpp | 4 +-- .../yabloc_common/ground_server/util.hpp | 12 +++++--- .../ll2_decomposer/ll2_decomposer.hpp | 13 +++++---- .../src/camera_info_subscriber.cpp | 4 +-- .../yabloc/yabloc_common/src/color.cpp | 15 +++++----- .../yabloc_common/src/cv_decompress.cpp | 10 +++---- .../src/extract_line_segments.cpp | 11 +++----- .../src/ground_server/ground_server_core.cpp | 24 ++++++++-------- .../src/ground_server/polygon_operation.cpp | 26 +++++++++-------- .../ll2_decomposer/ll2_decomposer_core.cpp | 28 +++++++++---------- .../yabloc_common/src/pose_conversions.cpp | 13 ++++++--- .../src/static_tf_subscriber.cpp | 26 ++++++++--------- 18 files changed, 128 insertions(+), 112 deletions(-) diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 6253dce864ddf..6cd4044dab783 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -29,7 +29,7 @@ - + diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp index f26a63ebd6317..e8bcdf98025cc 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/camera_info_subscriber.hpp +++ b/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::SharedPtr sub_info_; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp index 219c50f387cb5..3a052795d97c1 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp @@ -31,25 +31,26 @@ 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(rgb[2]) / 255.f), g(static_cast(rgb[1]) / 255.f), + b(static_cast(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(r * 255); tmp.rgba[1] = static_cast(g * 255); tmp.rgba[2] = static_cast(b * 255); tmp.rgba[3] = static_cast(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; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp index 98c082e125547..9802c0abe4acd 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/gamma_converter.hpp +++ b/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(0, i) = 256 * std::pow(i / 256.f, gamma); + lut_.at(0, i) = + static_cast(256 * std::pow(static_cast(i) / 256.f, gamma)); } } diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp index 62a3695a9852f..61798ae6a4f04 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_plane.hpp +++ b/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)); diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp index 5b62c8fb79c33..aa90e52920ea5 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/filter/moving_averaging.hpp +++ b/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(buffer_.size()); return mean.normalized(); } diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index 92e5d939d0b7e..e8f7e6e081e43 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/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::SharedPtr sub_map_; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp index fe3285fb08e8a..8f461daa7f09d 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/util.hpp +++ b/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::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(from.x()), static_cast(from.y()), + static_cast(from.z())); + Eigen::Vector3f t(static_cast(to.x()), static_cast(to.y()), + static_cast(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,9 @@ void upsample_line_string( } }; -std::vector merge_indices(const std::vector & indices1, const std::vector & indices2) +std::vector inline merge_indices( + const std::vector & indices1, const std::vector & indices2 +) { std::unordered_set set; for (int i : indices1) set.insert(i); diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp index b044ae985c6c0..02dd1667b3b65 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp +++ b/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 split_line_strings( + static pcl::PointCloud split_line_strings( const lanelet::ConstLineStrings3d & line_strings); pcl::PointCloud load_bounding_boxes(const lanelet::PolygonLayer & polygons) const; - lanelet::ConstLineStrings3d extract_specified_line_string( - const lanelet::LineStringLayer & line_strings, const std::set & visible_labels); - lanelet::ConstPolygons3d extract_specified_polygon( + static lanelet::ConstLineStrings3d extract_specified_line_string( + const lanelet::LineStringLayer & line_string_layer, + const std::set & visible_labels); + static lanelet::ConstPolygons3d extract_specified_polygon( const lanelet::PolygonLayer & polygon_layer, const std::set & visible_labels); MarkerArray make_sign_marker_msg( diff --git a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp b/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp index 33a2279767107..222389c32ef31 100644 --- a/localization/yabloc/yabloc_common/src/camera_info_subscriber.cpp +++ b/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>(opt_info_->k.data()); - return Kd_t.cast().transpose(); + const Eigen::Matrix3d kd_t = Eigen::Map>(opt_info_->k.data()); + return kd_t.cast().transpose(); } } // namespace yabloc::common diff --git a/localization/yabloc/yabloc_common/src/color.cpp b/localization/yabloc/yabloc_common/src/color.cpp index 485f655bd4151..6a421a90429ab 100644 --- a/localization/yabloc/yabloc_common/src/color.cpp +++ b/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); @@ -42,16 +44,15 @@ Color hsv_to_rgb(float h_, float s_, float v_) if (h < 60) return {max, h / 60 * (max - min) + min, min}; - else if (h < 120) + if (h < 120) return {(120 - h) / 60 * (max - min) + min, max, min}; - else if (h < 180) + if (h < 180) return {min, max, (h - 120) / 60 * (max - min) + min}; - else if (h < 240) + if (h < 240) return {min, (240 - h) / 60 * (max - min) + min, max}; - else if (h < 300) + if (h < 300) return {(h - 240) / 60 * (max - min) + min, min, max}; - else - return {max, min, (360 - h) / 60 * (max - min) + min}; + return {max, min, (360 - h) / 60 * (max - min) + min}; } Color blue_red(float value) diff --git a/localization/yabloc/yabloc_common/src/cv_decompress.cpp b/localization/yabloc/yabloc_common/src/cv_decompress.cpp index dc232488e147c..bf26908c9f0b0 100644 --- a/localization/yabloc/yabloc_common/src/cv_decompress.cpp +++ b/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") { diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 9940dca1fd62b..96cb1bfc12845 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -21,11 +21,11 @@ pcl::PointCloud extract_near_line_segments( const float max_range) { constexpr double sqrt_two = 1.41421356237309504880; - const Eigen::Vector3f pose_vector = transform.translation(); + 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. - auto check_intersection = [sqrt_two, max_range, + auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool { const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector; const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector; @@ -42,11 +42,8 @@ pcl::PointCloud extract_near_line_segments( }; pcl::PointCloud 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; } diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index d2d1929991442..3fa7c7705caf9 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -33,8 +34,8 @@ namespace yabloc::ground_server GroundServer::GroundServer(const rclcpp::NodeOptions & options) : Node("ground_server", options), force_zero_tilt_(declare_parameter("force_zero_tilt")), - R(declare_parameter("R")), - K(declare_parameter("K")) + R_(static_cast(declare_parameter("R"))), + K_(static_cast(declare_parameter("K"))) { using std::placeholders::_1; using std::placeholders::_2; @@ -147,8 +148,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(point.x); + const auto y = static_cast(point.y); float height = std::numeric_limits::infinity(); for (const auto & p : cloud_->points) { @@ -186,12 +187,12 @@ std::vector GroundServer::estimate_inliers_by_ransac(const std::vector 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(height_filter_.getValue().value()); + const pcl::PointXYZ xyz(static_cast(point.x), static_cast(point.y), predicted_z); std::vector raw_indices; std::vector distances; - kdtree_->nearestKSearch(xyz, K, raw_indices, distances); + kdtree_->nearestKSearch(xyz, K_, raw_indices, distances); std::vector indices = estimate_inliers_by_ransac(raw_indices); @@ -206,7 +207,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 +230,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(point.x), static_cast(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(point.x) * plane.normal.x(); + float py_ny = static_cast(point.y) * plane.normal.y(); plane.xyz.z() = (inner - px_nx - py_ny) / plane.normal.z(); } diff --git a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp index d4b134cf3a203..e889562e50103 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp @@ -31,9 +31,9 @@ pcl::PointCloud sample_from_polygons(const lanelet::PolygonLayer for (const lanelet::ConstPolygon3d & polygon : polygons) { for (const lanelet::ConstPoint3d & p : polygon) { pcl::PointXYZ xyz; - xyz.x = p.x(); - xyz.y = p.y(); - xyz.z = p.z(); + xyz.x = static_cast(p.x()); + xyz.y = static_cast(p.y()); + xyz.z = static_cast(p.z()); raw_cloud.push_back(xyz); } } @@ -45,9 +45,9 @@ void push_back_line( { Eigen::Vector3f f = from.getVector3fMap(); Eigen::Vector3f t = to.getVector3fMap(); - const float L = (f - t).norm(); + const float vec_len = (f - t).norm(); - for (float l = 0.f; l < L; l += 0.25f) { + for (float l = 0.f; l < vec_len; l += 0.25f) { Eigen::Vector3f xyz = f + (t - f) * l; dst_cloud.emplace_back(xyz.x(), xyz.y(), xyz.z()); } @@ -56,19 +56,23 @@ void push_back_line( void push_back_contour( pcl::PointCloud & dst_cloud, const pcl::PointCloud & vertices) { - const int N = vertices.size(); - for (int i = 0; i < N - 1; ++i) { + const int n = static_cast(vertices.size()); + for (int i = 0; i < n - 1; ++i) { push_back_line(dst_cloud, vertices.at(i), vertices.at(i + 1)); } - push_back_line(dst_cloud, vertices.at(0), vertices.at(N - 1)); + push_back_line(dst_cloud, vertices.at(0), vertices.at(n - 1)); } pcl::PointCloud shrink_vertices( const pcl::PointCloud & vertices, float rate) { - Eigen::Vector3f center = Eigen::Vector3f::Zero(); - for (const pcl::PointXYZ p : vertices) center += p.getVector3fMap(); - center /= vertices.size(); + // Eigen::Vector3f center = Eigen::Vector3f::Zero(); + // for (const pcl::PointXYZ p : vertices) center += p.getVector3fMap(); + Eigen::Vector3f center = std::accumulate( + vertices.begin(), vertices.end(), Eigen::Vector3f::Zero().eval(), + [](const Eigen::Vector3f& acc, const pcl::PointXYZ& p) { return acc + p.getVector3fMap(); } + ); + center /= static_cast(vertices.size()); pcl::PointCloud dst_cloud; for (const pcl::PointXYZ p : vertices) { diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index 1126d6260b911..0289fe755ed60 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -44,7 +44,7 @@ Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to [this](const std::string & param_name, std::set & labels) -> void { this->template declare_parameter>(param_name); auto label_array = get_parameter(param_name).as_string_array(); - for (auto l : label_array) labels.insert(l); + for (const auto& l : label_array) labels.insert(l); }; load_lanelet2_labels("road_marking_labels", road_marking_labels_); @@ -86,14 +86,14 @@ pcl::PointCloud Ll2Decomposer::load_bounding_boxes( for (const lanelet::ConstPolygon3d & polygon : polygons) { if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; - lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + const lanelet::Attribute& attr = polygon.attribute(lanelet::AttributeName::Type); if (bounding_box_labels_.count(attr.value()) == 0) continue; for (const lanelet::ConstPoint3d & p : polygon) { pcl::PointXYZL xyzl; - xyzl.x = p.x(); - xyzl.y = p.y(); - xyzl.z = p.z(); + xyzl.x = static_cast(p.x()); + xyzl.y = static_cast(p.y()); + xyzl.z = static_cast(p.z()); xyzl.label = index; cloud.push_back(xyzl); } @@ -151,7 +151,7 @@ lanelet::ConstLineStrings3d Ll2Decomposer::extract_specified_line_string( lanelet::ConstLineStrings3d line_strings; for (const lanelet::ConstLineString3d & line : line_string_layer) { if (!line.hasAttribute(lanelet::AttributeName::Type)) continue; - lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type); + const lanelet::Attribute& attr = line.attribute(lanelet::AttributeName::Type); if (visible_labels.count(attr.value()) == 0) continue; line_strings.push_back(line); } @@ -164,7 +164,7 @@ lanelet::ConstPolygons3d Ll2Decomposer::extract_specified_polygon( lanelet::ConstPolygons3d polygons; for (const lanelet::ConstPolygon3d & polygon : polygon_layer) { if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; - lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type); + const lanelet::Attribute& attr = polygon.attribute(lanelet::AttributeName::Type); if (visible_labels.count(attr.value()) == 0) continue; polygons.push_back(polygon); } @@ -172,15 +172,15 @@ lanelet::ConstPolygons3d Ll2Decomposer::extract_specified_polygon( } pcl::PointNormal Ll2Decomposer::to_point_normal( - const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) { pcl::PointNormal pn; - pn.x = from.x(); - pn.y = from.y(); - pn.z = from.z(); - pn.normal_x = to.x(); - pn.normal_y = to.y(); - pn.normal_z = to.z(); + pn.x = static_cast(from.x()); + pn.y = static_cast(from.y()); + pn.z = static_cast(from.z()); + pn.normal_x = static_cast(to.x()); + pn.normal_y = static_cast(to.y()); + pn.normal_z = static_cast(to.z()); return pn; } diff --git a/localization/yabloc/yabloc_common/src/pose_conversions.cpp b/localization/yabloc/yabloc_common/src/pose_conversions.cpp index 6ad914eeaf916..3b82bebbe414e 100644 --- a/localization/yabloc/yabloc_common/src/pose_conversions.cpp +++ b/localization/yabloc/yabloc_common/src/pose_conversions.cpp @@ -37,8 +37,11 @@ Eigen::Affine3f pose_to_affine(const geometry_msgs::msg::Pose & pose) { const auto pos = pose.position; const auto ori = pose.orientation; - Eigen::Translation3f t(pos.x, pos.y, pos.z); - Eigen::Quaternionf q(ori.w, ori.x, ori.y, ori.z); + Eigen::Translation3f t(static_cast(pos.x), static_cast(pos.y), + static_cast(pos.z)); + Eigen::Quaternionf q( + static_cast(ori.w), static_cast(ori.x), static_cast(ori.y), + static_cast(ori.z)); return t * q; } @@ -46,8 +49,10 @@ Sophus::SE3f pose_to_se3(const geometry_msgs::msg::Pose & pose) { auto ori = pose.orientation; auto pos = pose.position; - Eigen::Quaternionf q(ori.w, ori.x, ori.y, ori.z); - Eigen::Vector3f t(pos.x, pos.y, pos.z); + Eigen::Quaternionf q(static_cast(ori.w), static_cast(ori.x), + static_cast(ori.y), static_cast(ori.z)); + Eigen::Vector3f t(static_cast(pos.x), static_cast(pos.y), + static_cast(pos.z)); return {q, t}; } diff --git a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp index baf93ad295e9c..3aeaefad12629 100644 --- a/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp +++ b/localization/yabloc/yabloc_common/src/static_tf_subscriber.cpp @@ -35,26 +35,26 @@ std::optional StaticTfSubscriber::se3f( std::optional StaticTfSubscriber::operator()( const std::string & frame_id, const std::string & parent_frame_id) { - std::optional extrinsic_{std::nullopt}; + std::optional extrinsic{std::nullopt}; try { geometry_msgs::msg::TransformStamped ts = tf_buffer_->lookupTransform(parent_frame_id, frame_id, tf2::TimePointZero); Eigen::Vector3f p; - p.x() = ts.transform.translation.x; - p.y() = ts.transform.translation.y; - p.z() = ts.transform.translation.z; + p.x() = static_cast(ts.transform.translation.x); + p.y() = static_cast(ts.transform.translation.y); + p.z() = static_cast(ts.transform.translation.z); Eigen::Quaternionf q; - q.w() = ts.transform.rotation.w; - q.x() = ts.transform.rotation.x; - q.y() = ts.transform.rotation.y; - q.z() = ts.transform.rotation.z; - extrinsic_ = Eigen::Affine3f::Identity(); - extrinsic_->translation() = p; - extrinsic_->matrix().topLeftCorner(3, 3) = q.toRotationMatrix(); - } catch (tf2::TransformException & ex) { + q.w() = static_cast(ts.transform.rotation.w); + q.x() = static_cast(ts.transform.rotation.x); + q.y() = static_cast(ts.transform.rotation.y); + q.z() = static_cast(ts.transform.rotation.z); + extrinsic = Eigen::Affine3f::Identity(); + extrinsic->translation() = p; + extrinsic->matrix().topLeftCorner(3, 3) = q.toRotationMatrix(); + } catch (const tf2::TransformException & ex) { } - return extrinsic_; + return extrinsic; } } // namespace yabloc::common