diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp index 3a052795d97c1..447c101fd520a 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/color.hpp @@ -31,8 +31,10 @@ struct Color } explicit Color(const cv::Scalar & rgb, float a = 1.0f) - : r(static_cast(rgb[2]) / 255.f), g(static_cast(rgb[1]) / 255.f), - b(static_cast(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) { } 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 8f461daa7f09d..8d9dc06447d62 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 @@ -30,10 +30,10 @@ void inline upsample_line_string( const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to, pcl::PointCloud::Ptr cloud) { - 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())); + 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) { @@ -44,8 +44,7 @@ void inline upsample_line_string( }; std::vector inline merge_indices( - const std::vector & indices1, const std::vector & indices2 -) + 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 02dd1667b3b65..6956a7a7b04bb 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 @@ -55,7 +55,7 @@ class Ll2Decomposer : public rclcpp::Node void on_map(const LaneletMapBin & msg); static pcl::PointNormal to_point_normal( - const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) ; + const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to); static pcl::PointCloud split_line_strings( const lanelet::ConstLineStrings3d & line_strings); diff --git a/localization/yabloc/yabloc_common/src/color.cpp b/localization/yabloc/yabloc_common/src/color.cpp index 6a421a90429ab..6889f7238fb93 100644 --- a/localization/yabloc/yabloc_common/src/color.cpp +++ b/localization/yabloc/yabloc_common/src/color.cpp @@ -42,16 +42,11 @@ 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}; - 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}; + 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}; } diff --git a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp index 96cb1bfc12845..f7326a084e0ff 100644 --- a/localization/yabloc/yabloc_common/src/extract_line_segments.cpp +++ b/localization/yabloc/yabloc_common/src/extract_line_segments.cpp @@ -25,8 +25,7 @@ pcl::PointCloud extract_near_line_segments( // 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 = [max_range, - pose_vector](const pcl::PointNormal & pn) -> bool { + 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,8 +41,9 @@ pcl::PointCloud extract_near_line_segments( }; pcl::PointCloud dst; - std::copy_if(line_segments.begin(), line_segments.end(), std::back_inserter(dst), - [check_intersection](const auto& pn) { return check_intersection(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 3fa7c7705caf9..1366ecaebccf1 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,7 +21,6 @@ #include #include -#include #include #include #include @@ -29,6 +28,8 @@ #include #include +#include + namespace yabloc::ground_server { GroundServer::GroundServer(const rclcpp::NodeOptions & options) @@ -230,8 +231,8 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) const Eigen::Vector3f filt_normal = normal_filter_.update(normal); GroundPlane plane; - plane.xyz = Eigen::Vector3f(static_cast(point.x), static_cast(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 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 dd8287fbcc703..5ce6804c7bdc6 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/polygon_operation.cpp @@ -68,8 +68,7 @@ pcl::PointCloud shrink_vertices( { 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(); } - ); + [](const Eigen::Vector3f & acc, const pcl::PointXYZ & p) { return acc + p.getVector3fMap(); }); center /= static_cast(vertices.size()); pcl::PointCloud dst_cloud; 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 0289fe755ed60..fb7bc53c05e5e 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 (const 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,7 +86,7 @@ pcl::PointCloud Ll2Decomposer::load_bounding_boxes( for (const lanelet::ConstPolygon3d & polygon : polygons) { if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue; - const 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) { @@ -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; - const 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; - const 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); } diff --git a/localization/yabloc/yabloc_common/src/pose_conversions.cpp b/localization/yabloc/yabloc_common/src/pose_conversions.cpp index 3b82bebbe414e..374871bc96f0c 100644 --- a/localization/yabloc/yabloc_common/src/pose_conversions.cpp +++ b/localization/yabloc/yabloc_common/src/pose_conversions.cpp @@ -37,8 +37,8 @@ Eigen::Affine3f pose_to_affine(const geometry_msgs::msg::Pose & pose) { const auto pos = pose.position; const auto ori = pose.orientation; - Eigen::Translation3f t(static_cast(pos.x), static_cast(pos.y), - static_cast(pos.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)); @@ -49,10 +49,11 @@ Sophus::SE3f pose_to_se3(const geometry_msgs::msg::Pose & pose) { auto ori = pose.orientation; auto pos = pose.position; - 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)); + 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}; }