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 da44047df..384009312 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 "tier4_debug_msgs/msg/float64_stamped.hpp" #include #include @@ -71,7 +71,7 @@ class DeviationEstimator : public rclcpp::Node std::string imu_link_frame_; - std::vector vx_all_; + std::vector vx_all_; std::vector gyro_all_; std::vector pose_buf_; std::vector traj_data_list_for_gyro_; 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 bf4a2f0a7..932393fc3 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 "tier4_debug_msgs/msg/float64_stamped.hpp" #include @@ -39,7 +39,7 @@ struct TrajectoryData { std::vector pose_list; - std::vector vx_list; + std::vector vx_list; std::vector gyro_list; }; @@ -95,12 +95,14 @@ double calculate_std_mean_const(const std::vector & v, const double mean) struct CompareMsgTimestamp { - bool operator()(tier4_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()(tier4_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(); } @@ -163,7 +165,7 @@ double norm_xy(const T p1, const U p2) double clip_radian(const double rad); geometry_msgs::msg::Point integrate_position( - const std::vector & vx_list, + const std::vector & vx_list, const std::vector & gyro_list, const double coef_vx, const double yaw_init); @@ -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 374ce03be..6ea4305e1 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 "tier4_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 c5e3dea01..71b50c9ee 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 - tier4_debug_msgs rosbag2_storage_mcap ament_cmake_gtest diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp index 5296121fb..c3dd02061 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp @@ -202,7 +202,7 @@ void DeviationEstimator::callback_pose_with_covariance( void DeviationEstimator::callback_wheel_odometry( const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr) { - tier4_debug_msgs::msg::Float64Stamped vx; + autoware_internal_debug_msgs::msg::Float64Stamped vx; vx.stamp = wheel_odometry_msg_ptr->header.stamp; vx.data = wheel_odometry_msg_ptr->longitudinal_velocity; diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp index 59f7329e7..d5c755e60 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator_main.cpp @@ -99,7 +99,7 @@ int main(int argc, char ** argv) if (index >= static_cast(trajectory_data_list.size())) { trajectory_data_list.resize(index + 1); } - tier4_debug_msgs::msg::Float64Stamped vx; + autoware_internal_debug_msgs::msg::Float64Stamped vx; vx.stamp = velocity_status_msg->header.stamp; vx.data = velocity_status_msg->longitudinal_velocity; trajectory_data_list[index].vx_list.push_back(vx); diff --git a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp index 726fd7fd8..c0a06a686 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp +++ b/localization/deviation_estimation_tools/deviation_estimator/src/utils.cpp @@ -87,7 +87,7 @@ geometry_msgs::msg::Vector3 interpolate_vector3_stamped( * point */ geometry_msgs::msg::Point integrate_position( - const std::vector & vx_list, + const std::vector & vx_list, const std::vector & gyro_list, const double coef_vx, const double yaw_init) { @@ -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 16110088a..d2cbffdef 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 "tier4_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 69a2c29d2..ca94345ee 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 - tier4_debug_msgs ament_cmake_gtest ament_lint_auto