Merge branch 'main' of github.com:JianKangEgon/autoware_utils into main

autoware_utils/CMakeLists.txt

@@ -4,7 +4,7 @@ project(autoware_utils)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-file(GLOB_RECURSE src_cpps 
+file(GLOB_RECURSE src_cpps
     src/*.cpp
     src/geometry/*.cpp
     src/math/*.cpp

autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp

@@ -35,9 +35,7 @@ class Vector2d
 
   Vector2d(const double x, const double y) : x_(x), y_(y) {}
 
-  explicit Vector2d(const autoware_utils::Point2d & point) : x_(point.x()), y_(point.y())
-  {
-  }
+  explicit Vector2d(const autoware_utils::Point2d & point) : x_(point.x()), y_(point.y()) {}
 
   double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); }
 
@@ -101,8 +99,7 @@ class Polygon2d
   static std::optional<Polygon2d> create(
     PointList2d && outer, std::vector<PointList2d> && inners) noexcept;
 
-  static std::optional<Polygon2d> create(
-    const autoware_utils::Polygon2d & polygon) noexcept;
+  static std::optional<Polygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;
 
   const PointList2d & outer() const noexcept { return outer_; }
 
@@ -137,8 +134,7 @@ class ConvexPolygon2d : public Polygon2d
 
   static std::optional<ConvexPolygon2d> create(PointList2d && vertices) noexcept;
 
-  static std::optional<ConvexPolygon2d> create(
-    const autoware_utils::Polygon2d & polygon) noexcept;
+  static std::optional<ConvexPolygon2d> create(const autoware_utils::Polygon2d & polygon) noexcept;
 
   const PointList2d & vertices() const noexcept { return outer(); }
 

autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp

@@ -95,9 +95,9 @@ inline Point3d from_msg(const geometry_msgs::msg::Point & msg)
 }
 }  // namespace autoware_utils
 
-BOOST_GEOMETRY_REGISTER_POINT_2D(                                           // NOLINT
+BOOST_GEOMETRY_REGISTER_POINT_2D(                                 // NOLINT
   autoware_utils::Point2d, double, cs::cartesian, x(), y())       // NOLINT
-BOOST_GEOMETRY_REGISTER_POINT_3D(                                           // NOLINT
+BOOST_GEOMETRY_REGISTER_POINT_3D(                                 // NOLINT
   autoware_utils::Point3d, double, cs::cartesian, x(), y(), z())  // NOLINT
 BOOST_GEOMETRY_REGISTER_RING(autoware_utils::LinearRing2d)        // NOLINT
 

autoware_utils/include/autoware_utils/geometry/geometry.hpp

@@ -27,6 +27,7 @@
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 
+#include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_planning_msgs/msg/path.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <geometry_msgs/msg/point32.hpp>
@@ -38,7 +39,6 @@
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <tf2/utils.h>
 
@@ -135,7 +135,8 @@ inline geometry_msgs::msg::Point get_point(const autoware_planning_msgs::msg::Pa
 }
 
 template <>
-inline geometry_msgs::msg::Point get_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
+inline geometry_msgs::msg::Point get_point(
+  const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return p.point.pose.position;
 }
@@ -172,7 +173,8 @@ inline geometry_msgs::msg::Pose get_pose(const autoware_planning_msgs::msg::Path
 }
 
 template <>
-inline geometry_msgs::msg::Pose get_pose(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
+inline geometry_msgs::msg::Pose get_pose(
+  const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return p.point.pose;
 }
@@ -197,7 +199,8 @@ inline double get_longitudinal_velocity(const autoware_planning_msgs::msg::PathP
 }
 
 template <>
-inline double get_longitudinal_velocity(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
+inline double get_longitudinal_velocity(
+  const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return p.point.longitudinal_velocity_mps;
 }
@@ -236,7 +239,8 @@ inline void set_pose(
 
 template <>
 inline void set_pose(
-  const geometry_msgs::msg::Pose & pose, autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
+  const geometry_msgs::msg::Pose & pose,
+  autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   p.point.pose = pose;
 }
@@ -570,7 +574,8 @@ inline double calc_norm(const geometry_msgs::msg::Vector3 & v)
  * @return If all element of covariance is 0, return false.
  */
 //
-bool is_twist_covariance_valid(const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance);
+bool is_twist_covariance_valid(
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance);
 
 // NOTE: much faster than boost::geometry::intersects()
 std::optional<geometry_msgs::msg::Point> intersect(

autoware_utils/include/autoware_utils/geometry/random_concave_polygon.hpp

@@ -39,8 +39,8 @@ std::optional<Polygon2d> random_concave_polygon(const size_t vertices, const dou
 bool test_intersection(
   const std::vector<autoware_utils::Polygon2d> & polygons1,
   const std::vector<autoware_utils::Polygon2d> & polygons2,
-  const std::function<bool(
-    const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &);
+  const std::function<
+    bool(const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &);
 
 }  // namespace autoware_utils
 

autoware_utils/include/autoware_utils/ros/debug_publisher.hpp

@@ -31,8 +31,7 @@ namespace debug_publisher
 {
 template <
   class T_msg, class T,
-  std::enable_if_t<
-    autoware_utils::debug_traits::is_debug_message<T_msg>::value, std::nullptr_t> =
+  std::enable_if_t<autoware_utils::debug_traits::is_debug_message<T_msg>::value, std::nullptr_t> =
     nullptr>
 T_msg to_debug_msg(const T & data, const rclcpp::Time & stamp)
 {

autoware_utils/include/autoware_utils/ros/update_param.hpp

@@ -23,7 +23,8 @@
 namespace autoware_utils
 {
 template <class T>
-bool update_param(const std::vector<rclcpp::Parameter> & params, const std::string & name, T & value)
+bool update_param(
+  const std::vector<rclcpp::Parameter> & params, const std::string & name, T & value)
 {
   const auto itr = std::find_if(
     params.cbegin(), params.cend(),

autoware_utils/include/autoware_utils/system/time_keeper.hpp

@@ -18,9 +18,9 @@
 
 #include <rclcpp/publisher.hpp>
 
-#include <std_msgs/msg/string.hpp>
 #include <autoware_internal_debug_msgs/msg/processing_time_node.hpp>
 #include <autoware_internal_debug_msgs/msg/processing_time_tree.hpp>
+#include <std_msgs/msg/string.hpp>
 
 #include <memory>
 #include <ostream>
@@ -61,7 +61,8 @@ class ProcessingTimeNode : public std::enable_shared_from_this<ProcessingTimeNod
   /**
    * @brief Construct a ProcessingTimeTree message from the node and its children
    *
-   * @return autoware_internal_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message
+   * @return autoware_internal_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree
+   * message
    */
   autoware_internal_debug_msgs::msg::ProcessingTimeTree to_msg() const;
 
@@ -111,7 +112,8 @@ class ProcessingTimeNode : public std::enable_shared_from_this<ProcessingTimeNod
 };
 
 using ProcessingTimeDetail =
-  autoware_internal_debug_msgs::msg::ProcessingTimeTree;  //!< Alias for the ProcessingTimeTree message
+  autoware_internal_debug_msgs::msg::ProcessingTimeTree;  //!< Alias for the ProcessingTimeTree
+                                                          //!< message
 
 /**
  * @brief Class for tracking and reporting the processing time of various functions

autoware_utils/src/geometry/alt_geometry.cpp

@@ -62,8 +62,7 @@ std::optional<Polygon2d> Polygon2d::create(
   return poly;
 }
 
-std::optional<Polygon2d> Polygon2d::create(
-  const autoware_utils::Polygon2d & polygon) noexcept
+std::optional<Polygon2d> Polygon2d::create(const autoware_utils::Polygon2d & polygon) noexcept
 {
   PointList2d outer;
   for (const auto & point : polygon.outer()) {

autoware_utils/src/geometry/boost_polygon_utils.cpp

@@ -179,18 +179,14 @@ Polygon2d to_polygon2d(
   return is_clockwise(polygon) ? polygon : inverse_clockwise(polygon);
 }
 
-autoware_utils::Polygon2d to_polygon2d(
-  const autoware_perception_msgs::msg::DetectedObject & object)
+autoware_utils::Polygon2d to_polygon2d(const autoware_perception_msgs::msg::DetectedObject & object)
 {
-  return autoware_utils::to_polygon2d(
-    object.kinematics.pose_with_covariance.pose, object.shape);
+  return autoware_utils::to_polygon2d(object.kinematics.pose_with_covariance.pose, object.shape);
 }
 
-autoware_utils::Polygon2d to_polygon2d(
-  const autoware_perception_msgs::msg::TrackedObject & object)
+autoware_utils::Polygon2d to_polygon2d(const autoware_perception_msgs::msg::TrackedObject & object)
 {
-  return autoware_utils::to_polygon2d(
-    object.kinematics.pose_with_covariance.pose, object.shape);
+  return autoware_utils::to_polygon2d(object.kinematics.pose_with_covariance.pose, object.shape);
 }
 
 autoware_utils::Polygon2d to_polygon2d(
@@ -206,17 +202,13 @@ Polygon2d to_footprint(
 {
   Polygon2d polygon;
   const auto point0 =
-    autoware_utils::calc_offset_pose(base_link_pose, base_to_front, width / 2.0, 0.0)
-      .position;
+    autoware_utils::calc_offset_pose(base_link_pose, base_to_front, width / 2.0, 0.0).position;
   const auto point1 =
-    autoware_utils::calc_offset_pose(base_link_pose, base_to_front, -width / 2.0, 0.0)
-      .position;
+    autoware_utils::calc_offset_pose(base_link_pose, base_to_front, -width / 2.0, 0.0).position;
   const auto point2 =
-    autoware_utils::calc_offset_pose(base_link_pose, -base_to_rear, -width / 2.0, 0.0)
-      .position;
+    autoware_utils::calc_offset_pose(base_link_pose, -base_to_rear, -width / 2.0, 0.0).position;
   const auto point3 =
-    autoware_utils::calc_offset_pose(base_link_pose, -base_to_rear, width / 2.0, 0.0)
-      .position;
+    autoware_utils::calc_offset_pose(base_link_pose, -base_to_rear, width / 2.0, 0.0).position;
 
   append_point_to_polygon(polygon, point0);
   append_point_to_polygon(polygon, point1);

autoware_utils/src/geometry/geometry.cpp

@@ -182,7 +182,8 @@ Point2d transform_point(const Point2d & point, const geometry_msgs::msg::Transfo
   return Point2d{transformed.x(), transformed.y()};
 }
 
-Eigen::Vector3d transform_point(const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose)
+Eigen::Vector3d transform_point(
+  const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose)
 {
   geometry_msgs::msg::Transform transform;
   transform.translation.x = pose.position.x;
@@ -353,7 +354,8 @@ geometry_msgs::msg::Pose calc_offset_pose(
  * @return If all element of covariance is 0, return false.
  */
 //
-bool is_twist_covariance_valid(const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance)
+bool is_twist_covariance_valid(
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance)
 {
   for (const auto & c : twist_with_covariance.covariance) {
     if (c != 0.0) {

autoware_utils/src/geometry/random_concave_polygon.cpp

@@ -326,8 +326,7 @@ bool is_convex(const autoware_utils::Polygon2d & polygon)
 bool test_intersection(
   const std::vector<autoware_utils::Polygon2d> & polygons1,
   const std::vector<autoware_utils::Polygon2d> & polygons2,
-  const std::function<bool(
-    const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &
+  const std::function<bool(const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &
     intersection_func)
 {
   for (const auto & poly1 : polygons1) {

autoware_utils/src/math/trigonometry.cpp

@@ -25,8 +25,8 @@ namespace autoware_utils
 
 float sin(float radian)
 {
-  float degree = radian * (180.f / static_cast<float>(autoware_utils::pi)) *
-                 (discrete_arcs_num_360 / 360.f);
+  float degree =
+    radian * (180.f / static_cast<float>(autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
   size_t idx =
     (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
     discrete_arcs_num_360;
@@ -86,14 +86,10 @@ std::pair<float, float> sin_and_cos(float radian)
 // 2. output of the function is changed from degrees to radians.
 namespace detail_fast_atan2
 {
-static const float atan2_p1 =
-  0.9997878412794807f * static_cast<float>(180) / autoware_utils::pi;
-static const float atan2_p3 =
-  -0.3258083974640975f * static_cast<float>(180) / autoware_utils::pi;
-static const float atan2_p5 =
-  0.1555786518463281f * static_cast<float>(180) / autoware_utils::pi;
-static const float atan2_p7 =
-  -0.04432655554792128f * static_cast<float>(180) / autoware_utils::pi;
+static const float atan2_p1 = 0.9997878412794807f * static_cast<float>(180) / autoware_utils::pi;
+static const float atan2_p3 = -0.3258083974640975f * static_cast<float>(180) / autoware_utils::pi;
+static const float atan2_p5 = 0.1555786518463281f * static_cast<float>(180) / autoware_utils::pi;
+static const float atan2_p7 = -0.04432655554792128f * static_cast<float>(180) / autoware_utils::pi;
 static const float atan2_DBL_EPSILON = 2.2204460492503131e-016f;
 }  // namespace detail_fast_atan2
 

autoware_utils/src/system/time_keeper.cpp

@@ -71,10 +71,12 @@ autoware_internal_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg
 {
   autoware_internal_debug_msgs::msg::ProcessingTimeTree time_tree_msg;
 
-  std::function<void(const ProcessingTimeNode &, autoware_internal_debug_msgs::msg::ProcessingTimeTree &, int)>
+  std::function<void(
+    const ProcessingTimeNode &, autoware_internal_debug_msgs::msg::ProcessingTimeTree &, int)>
     construct_msg = [&](
                       const ProcessingTimeNode & node,
-                      autoware_internal_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) {
+                      autoware_internal_debug_msgs::msg::ProcessingTimeTree & tree_msg,
+                      int parent_id) {
       autoware_internal_debug_msgs::msg::ProcessingTimeNode time_node_msg;
       time_node_msg.name = node.name_;
       time_node_msg.processing_time = node.processing_time_;