diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp index 59e95f7b..38400931 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp @@ -24,13 +24,13 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" #include "sensor_msgs/msg/imu.hpp" #include "std_msgs/msg/float64.hpp" -#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include #include diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp index e13dc4b6..932393fc 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/utils.hpp @@ -18,10 +18,10 @@ #include "deviation_estimator/autoware_universe_utils.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include @@ -95,12 +95,14 @@ double calculate_std_mean_const(const std::vector & v, const double mean) struct CompareMsgTimestamp { - bool operator()(autoware_internal_debug_msgs::msg::Float64Stamped const & t1, double const & t2) const + bool operator()( + autoware_internal_debug_msgs::msg::Float64Stamped const & t1, double const & t2) const { return rclcpp::Time(t1.stamp).seconds() < t2; } - bool operator()(autoware_internal_debug_msgs::msg::Float64Stamped const & t1, rclcpp::Time const & t2) const + bool operator()( + autoware_internal_debug_msgs::msg::Float64Stamped const & t1, rclcpp::Time const & t2) const { return rclcpp::Time(t1.stamp).seconds() < t2.seconds(); } @@ -176,9 +178,11 @@ geometry_msgs::msg::Vector3 integrate_orientation( const std::vector & gyro_list, const geometry_msgs::msg::Vector3 & gyro_bias); -double get_mean_abs_vx(const std::vector & vx_list); +double get_mean_abs_vx( + const std::vector & vx_list); double get_mean_abs_wz(const std::vector & gyro_list); -double get_mean_accel(const std::vector & vx_list); +double get_mean_accel( + const std::vector & vx_list); geometry_msgs::msg::Vector3 transform_vector3( const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform); diff --git a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp index ad25fde4..6ea4305e 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp +++ b/localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/velocity_coef_module.hpp @@ -17,9 +17,9 @@ #include "deviation_estimator/utils.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/vector3_stamped.hpp" -#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include #include diff --git a/localization/deviation_estimation_tools/deviation_estimator/package.xml b/localization/deviation_estimation_tools/deviation_estimator/package.xml index a94028cd..71b50c9e 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/package.xml +++ b/localization/deviation_estimation_tools/deviation_estimator/package.xml @@ -15,6 +15,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_universe_utils autoware_vehicle_msgs geometry_msgs @@ -28,7 +29,6 @@ tf2 tf2_geometry_msgs tf2_ros - autoware_internal_debug_msgs rosbag2_storage_mcap ament_cmake_gtest diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp index f0478603..c0a06a68 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp @@ -153,7 +153,8 @@ geometry_msgs::msg::Vector3 integrate_orientation( /** * @brief calculate mean of |vx| */ -double get_mean_abs_vx(const std::vector & vx_list) +double get_mean_abs_vx( + const std::vector & vx_list) { double mean_abs_vx = 0; for (const auto & msg : vx_list) { @@ -179,7 +180,8 @@ double get_mean_abs_wz(const std::vector & g /** * @brief calculate mean of acceleration */ -double get_mean_accel(const std::vector & vx_list) +double get_mean_accel( + const std::vector & vx_list) { const double dt = (rclcpp::Time(vx_list.back().stamp) - rclcpp::Time(vx_list.front().stamp)).seconds(); diff --git a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp index 8f57c403..d2cbffde 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp +++ b/localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/LinearMath/Quaternion.h" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" @@ -27,7 +28,6 @@ #include "sensor_msgs/msg/imu.hpp" #include "std_msgs/msg/float64.hpp" #include "std_srvs/srv/set_bool.hpp" -#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include diff --git a/localization/deviation_estimation_tools/deviation_evaluator/package.xml b/localization/deviation_estimation_tools/deviation_evaluator/package.xml index 13515202..ca94345e 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/package.xml +++ b/localization/deviation_estimation_tools/deviation_evaluator/package.xml @@ -15,6 +15,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_universe_utils geometry_msgs nav_msgs @@ -25,7 +26,6 @@ std_srvs tf2 tf2_ros - autoware_internal_debug_msgs ament_cmake_gtest ament_lint_auto