diff --git a/autoware_utils/CMakeLists.txt b/autoware_utils/CMakeLists.txt index 21eb4ad..37f9d96 100644 --- a/autoware_utils/CMakeLists.txt +++ b/autoware_utils/CMakeLists.txt @@ -4,8 +4,16 @@ project(autoware_utils) find_package(autoware_cmake REQUIRED) autoware_package() +file(GLOB_RECURSE src_cpps + src/*.cpp + src/geometry/*.cpp + src/math/*.cpp + src/ros/*.cpp + src/system/*.cpp +) + ament_auto_add_library(autoware_utils SHARED - src/autoware_utils.cpp + ${src_cpps} ) if(BUILD_TESTING) diff --git a/autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp b/autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp index 6879818..cdea35f 100644 --- a/autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp +++ b/autoware_utils/include/autoware_utils/geometry/alt_geometry.hpp @@ -35,7 +35,9 @@ 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(); } @@ -99,7 +101,8 @@ class Polygon2d static std::optional create( PointList2d && outer, std::vector && inners) noexcept; - static std::optional create(const autoware_utils::Polygon2d & polygon) noexcept; + static std::optional create( + const autoware_utils::Polygon2d & polygon) noexcept; const PointList2d & outer() const noexcept { return outer_; } @@ -134,7 +137,8 @@ class ConvexPolygon2d : public Polygon2d static std::optional create(PointList2d && vertices) noexcept; - static std::optional create(const autoware_utils::Polygon2d & polygon) noexcept; + static std::optional create( + const autoware_utils::Polygon2d & polygon) noexcept; const PointList2d & vertices() const noexcept { return outer(); } diff --git a/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp b/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp index 3742eb8..b5e39ee 100644 --- a/autoware_utils/include/autoware_utils/geometry/boost_geometry.hpp +++ b/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 diff --git a/autoware_utils/include/autoware_utils/geometry/geometry.hpp b/autoware_utils/include/autoware_utils/geometry/geometry.hpp index e66adc1..3e0cbe6 100644 --- a/autoware_utils/include/autoware_utils/geometry/geometry.hpp +++ b/autoware_utils/include/autoware_utils/geometry/geometry.hpp @@ -27,7 +27,6 @@ #define EIGEN_MPL2_ONLY #include -#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include #include @@ -135,8 +135,7 @@ 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; } @@ -173,8 +172,7 @@ 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; } @@ -199,8 +197,7 @@ 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; } @@ -239,8 +236,7 @@ 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; } @@ -574,8 +570,7 @@ 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 intersect( diff --git a/autoware_utils/include/autoware_utils/geometry/random_concave_polygon.hpp b/autoware_utils/include/autoware_utils/geometry/random_concave_polygon.hpp index 300fa11..0ff4d7b 100644 --- a/autoware_utils/include/autoware_utils/geometry/random_concave_polygon.hpp +++ b/autoware_utils/include/autoware_utils/geometry/random_concave_polygon.hpp @@ -39,8 +39,8 @@ std::optional random_concave_polygon(const size_t vertices, const dou bool test_intersection( const std::vector & polygons1, const std::vector & polygons2, - const std::function< - bool(const autoware_utils::Polygon2d &, const autoware_utils::Polygon2d &)> &); + const std::function &); } // namespace autoware_utils diff --git a/autoware_utils/include/autoware_utils/math/sin_table.hpp b/autoware_utils/include/autoware_utils/math/sin_table.hpp index 3cc5d7a..bb8dac5 100644 --- a/autoware_utils/include/autoware_utils/math/sin_table.hpp +++ b/autoware_utils/include/autoware_utils/math/sin_table.hpp @@ -17,7 +17,7 @@ #include -namespace autowar_utils +namespace autoware_utils { constexpr size_t sin_table_size = 32769; @@ -25,6 +25,6 @@ constexpr size_t discrete_arcs_num_90 = 32768; constexpr size_t discrete_arcs_num_360 = 131072; extern const float g_sin_table[sin_table_size]; -} // namespace autowar_utils +} // namespace autoware_utils #endif // AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp b/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp index 8afdb55..c290d08 100644 --- a/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp +++ b/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp @@ -31,7 +31,8 @@ namespace debug_publisher { template < class T_msg, class T, - std::enable_if_t::value, std::nullptr_t> = + std::enable_if_t< + autoware_utils::debug_traits::is_debug_message::value, std::nullptr_t> = nullptr> T_msg to_debug_msg(const T & data, const rclcpp::Time & stamp) { diff --git a/autoware_utils/include/autoware_utils/ros/update_param.hpp b/autoware_utils/include/autoware_utils/ros/update_param.hpp index e02de83..17b3059 100644 --- a/autoware_utils/include/autoware_utils/ros/update_param.hpp +++ b/autoware_utils/include/autoware_utils/ros/update_param.hpp @@ -23,8 +23,7 @@ namespace autoware_utils { template -bool update_param( - const std::vector & params, const std::string & name, T & value) +bool update_param(const std::vector & params, const std::string & name, T & value) { const auto itr = std::find_if( params.cbegin(), params.cend(), diff --git a/autoware_utils/include/autoware_utils/system/time_keeper.hpp b/autoware_utils/include/autoware_utils/system/time_keeper.hpp index b5f0edc..847b332 100644 --- a/autoware_utils/include/autoware_utils/system/time_keeper.hpp +++ b/autoware_utils/include/autoware_utils/system/time_keeper.hpp @@ -18,9 +18,9 @@ #include +#include #include #include -#include #include #include @@ -61,8 +61,7 @@ class ProcessingTimeNode : public std::enable_shared_from_this Polygon2d::create( return poly; } -std::optional Polygon2d::create(const autoware_utils::Polygon2d & polygon) noexcept +std::optional Polygon2d::create( + const autoware_utils::Polygon2d & polygon) noexcept { PointList2d outer; for (const auto & point : polygon.outer()) { diff --git a/autoware_utils/src/geometry/boost_polygon_utils.cpp b/autoware_utils/src/geometry/boost_polygon_utils.cpp index 4e4bbcc..458a4df 100644 --- a/autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -179,14 +179,18 @@ 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( @@ -202,13 +206,17 @@ 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); diff --git a/autoware_utils/src/geometry/geometry.cpp b/autoware_utils/src/geometry/geometry.cpp index 7b00437..1327cb8 100644 --- a/autoware_utils/src/geometry/geometry.cpp +++ b/autoware_utils/src/geometry/geometry.cpp @@ -182,8 +182,7 @@ 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; @@ -260,7 +259,7 @@ tf2::fromMsg(transform, tf); geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.transform = tf2::toMsg(tf.inverse()); -return transformPose(pose, transform_stamped); +return transform_pose(pose, transform_stamped); } */ @@ -273,7 +272,7 @@ geometry_msgs::msg::Pose inverse_transform_pose( geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped.transform = tf2::toMsg(tf.inverse()); - return transformPose(pose, transform_stamped); + return transform_pose(pose, transform_stamped); } // Transform pose in world coordinates to local coordinates @@ -354,8 +353,7 @@ 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) { diff --git a/autoware_utils/src/geometry/pose_deviation.cpp b/autoware_utils/src/geometry/pose_deviation.cpp index ae1afaa..0563d23 100644 --- a/autoware_utils/src/geometry/pose_deviation.cpp +++ b/autoware_utils/src/geometry/pose_deviation.cpp @@ -68,7 +68,7 @@ double calc_yaw_deviation( { const auto base_yaw = tf2::getYaw(base_pose.orientation); const auto target_yaw = tf2::getYaw(target_pose.orientation); - return normalizeRadian(target_yaw - base_yaw); + return normalize_radian(target_yaw - base_yaw); } PoseDeviation calc_pose_deviation( diff --git a/autoware_utils/src/geometry/random_concave_polygon.cpp b/autoware_utils/src/geometry/random_concave_polygon.cpp index b40e421..62fc632 100644 --- a/autoware_utils/src/geometry/random_concave_polygon.cpp +++ b/autoware_utils/src/geometry/random_concave_polygon.cpp @@ -326,7 +326,8 @@ bool is_convex(const autoware_utils::Polygon2d & polygon) bool test_intersection( const std::vector & polygons1, const std::vector & polygons2, - const std::function & + const std::function & intersection_func) { for (const auto & poly1 : polygons1) { diff --git a/autoware_utils/src/math/trigonometry.cpp b/autoware_utils/src/math/trigonometry.cpp index f7e5a9f..b1733a9 100644 --- a/autoware_utils/src/math/trigonometry.cpp +++ b/autoware_utils/src/math/trigonometry.cpp @@ -25,8 +25,8 @@ namespace autoware_utils float sin(float radian) { - float degree = - radian * (180.f / static_cast(autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + float degree = radian * (180.f / static_cast(autoware_utils::pi)) * + (discrete_arcs_num_360 / 360.f); size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % discrete_arcs_num_360; @@ -86,10 +86,14 @@ std::pair 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(180) / autoware_utils::pi; -static const float atan2_p3 = -0.3258083974640975f * static_cast(180) / autoware_utils::pi; -static const float atan2_p5 = 0.1555786518463281f * static_cast(180) / autoware_utils::pi; -static const float atan2_p7 = -0.04432655554792128f * static_cast(180) / autoware_utils::pi; +static const float atan2_p1 = + 0.9997878412794807f * static_cast(180) / autoware_utils::pi; +static const float atan2_p3 = + -0.3258083974640975f * static_cast(180) / autoware_utils::pi; +static const float atan2_p5 = + 0.1555786518463281f * static_cast(180) / autoware_utils::pi; +static const float atan2_p7 = + -0.04432655554792128f * static_cast(180) / autoware_utils::pi; static const float atan2_DBL_EPSILON = 2.2204460492503131e-016f; } // namespace detail_fast_atan2 diff --git a/autoware_utils/src/system/time_keeper.cpp b/autoware_utils/src/system/time_keeper.cpp index ffe1ea4..bd58b75 100644 --- a/autoware_utils/src/system/time_keeper.cpp +++ b/autoware_utils/src/system/time_keeper.cpp @@ -71,12 +71,10 @@ autoware_internal_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg { autoware_internal_debug_msgs::msg::ProcessingTimeTree time_tree_msg; - std::function + std::function 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_; @@ -128,7 +126,7 @@ void TimeKeeper::add_reporter(std::ostream * os) void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) { reporters_.emplace_back([publisher](const std::shared_ptr & node) { - publisher->publish(node->toMsg()); + publisher->publish(node->to_msg()); }); }