From 5b951a74578081bbe0d9598e6820a1fcbabc0b0e Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:14:42 +0900 Subject: [PATCH 01/10] feat(deviation_estimation_tools)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- .../deviation_estimator/deviation_estimator.hpp | 4 ++-- .../include/deviation_estimator/utils.hpp | 14 +++++++------- .../deviation_estimator/velocity_coef_module.hpp | 2 +- .../deviation_estimator/package.xml | 2 +- .../src/deviation_estimator.cpp | 2 +- .../src/deviation_estimator_main.cpp | 2 +- .../deviation_estimator/src/utils.cpp | 6 +++--- .../deviation_evaluator/deviation_evaluator.hpp | 2 +- .../deviation_evaluator/package.xml | 2 +- 9 files changed, 18 insertions(+), 18 deletions(-) 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 da44047d..59e95f7b 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 @@ -30,7 +30,7 @@ #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 "autoware_internal_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 bf4a2f0a..e13dc4b6 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 @@ -21,7 +21,7 @@ #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 "autoware_internal_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,12 @@ 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 +163,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 +176,9 @@ 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 374ce03b..ad25fde4 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 @@ -19,7 +19,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/vector3_stamped.hpp" -#include "tier4_debug_msgs/msg/float64_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 c5e3dea0..a94028cd 100644 --- a/localization/deviation_estimation_tools/deviation_estimator/package.xml +++ b/localization/deviation_estimation_tools/deviation_estimator/package.xml @@ -28,7 +28,7 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_debug_msgs + autoware_internal_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 5296121f..c3dd0206 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 59f7329e..d5c755e6 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 726fd7fd..f0478603 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,7 @@ 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 +179,7 @@ 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 16110088..8f57c403 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 @@ -27,7 +27,7 @@ #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 "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 69a2c29d..13515202 100644 --- a/localization/deviation_estimation_tools/deviation_evaluator/package.xml +++ b/localization/deviation_estimation_tools/deviation_evaluator/package.xml @@ -25,7 +25,7 @@ std_srvs tf2 tf2_ros - tier4_debug_msgs + autoware_internal_debug_msgs ament_cmake_gtest ament_lint_auto From 21935b287a8fbf1852ce0e4a6e4e7e6100a7e65d Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:15:49 +0900 Subject: [PATCH 02/10] feat(tier4_debug_rviz_plugin)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- common/tier4_debug_rviz_plugin/README.md | 2 +- .../float32_multi_array_stamped_pie_chart.hpp | 6 +++--- common/tier4_debug_rviz_plugin/package.xml | 2 +- .../tier4_debug_rviz_plugin/plugins/plugin_description.xml | 2 +- .../src/float32_multi_array_stamped_pie_chart.cpp | 6 +++--- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/common/tier4_debug_rviz_plugin/README.md b/common/tier4_debug_rviz_plugin/README.md index 91898161..df23bdb1 100644 --- a/common/tier4_debug_rviz_plugin/README.md +++ b/common/tier4_debug_rviz_plugin/README.md @@ -7,6 +7,6 @@ Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. ### Float32MultiArrayStampedPieChart -Pie chart from `tier4_debug_msgs::msg::Float32MultiArrayStamped`. +Pie chart from `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped`. ![float32_multi_array_stamped_pie_chart](./images/float32_multi_array_stamped_pie_chart.png) diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp index c8267c70..a5013dec 100644 --- a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp @@ -60,7 +60,7 @@ #include #include -#include +#include #include @@ -87,7 +87,7 @@ class Float32MultiArrayStampedPieChartDisplay : public rviz_common::Display virtual void onDisable(); virtual void onInitialize(); virtual void processMessage( - const tier4_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg); + const autoware_internal_debug_msgs::msg::Float32MultiArrayStamped::ConstSharedPtr msg); virtual void drawPlot(double val); virtual void update(float wall_dt, float ros_dt); // properties @@ -114,7 +114,7 @@ class Float32MultiArrayStampedPieChartDisplay : public rviz_common::Display rviz_common::properties::FloatProperty * med_color_threshold_property_; rviz_common::properties::BoolProperty * clockwise_rotate_property_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; int left_; int top_; uint16_t texture_size_; diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml index 7e15e1a3..bb1bf348 100644 --- a/common/tier4_debug_rviz_plugin/package.xml +++ b/common/tier4_debug_rviz_plugin/package.xml @@ -19,7 +19,7 @@ rviz_common rviz_default_plugins rviz_rendering - tier4_debug_msgs + autoware_internal_debug_msgs ament_lint_auto autoware_lint_common diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml index 72864327..57205a43 100644 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -2,7 +2,7 @@ - Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + Display drivable area of autoware_internal_debug_msgs::msg::Float32MultiArrayStamped lock(mutex_); @@ -297,7 +297,7 @@ void Float32MultiArrayStampedPieChartDisplay::subscribe() if (topic_name.length() > 0 && topic_name != "/") { rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - sub_ = raw_node->create_subscription( + sub_ = raw_node->create_subscription( topic_name, 1, std::bind( &Float32MultiArrayStampedPieChartDisplay::processMessage, this, std::placeholders::_1)); From aba16ba6c30fea7986a14673ef54397c0bf0a347 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:16:43 +0900 Subject: [PATCH 03/10] feat(tier4_debug_tools)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- .../tier4_debug_tools/lateral_error_publisher.hpp | 8 ++++---- common/tier4_debug_tools/package.xml | 2 +- .../src/lateral_error_publisher.cpp | 12 ++++++------ 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp index 6089aff7..afa0ae61 100644 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include class LateralErrorPublisher : public rclcpp::Node { @@ -50,11 +50,11 @@ class LateralErrorPublisher : public rclcpp::Node sub_vehicle_pose_; //!< @brief subscription for vehicle pose rclcpp::Subscription::SharedPtr sub_ground_truth_pose_; //!< @brief subscription for gnss pose - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_control_lateral_error_; //!< @brief publisher for control lateral error - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_localization_lateral_error_; //!< @brief publisher for localization lateral error - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_lateral_error_; //!< @brief publisher for lateral error (control + localization) /** diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index 53454235..509660ae 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -17,7 +17,7 @@ rclcpp rclcpp_components tf2_ros - tier4_debug_msgs + autoware_internal_debug_msgs launch_ros python3-rtree diff --git a/common/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp index bd8ab750..60c67adf 100644 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -36,11 +36,11 @@ LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_op "~/input/ground_truth_pose_with_covariance", rclcpp::QoS{1}, std::bind(&LateralErrorPublisher::onGroundTruthPose, this, _1)); pub_control_lateral_error_ = - create_publisher("~/control_lateral_error", 1); + create_publisher("~/control_lateral_error", 1); pub_localization_lateral_error_ = - create_publisher("~/localization_lateral_error", 1); + create_publisher("~/localization_lateral_error", 1); pub_lateral_error_ = - create_publisher("~/lateral_error", 1); + create_publisher("~/lateral_error", 1); } void LateralErrorPublisher::onTrajectory( @@ -131,17 +131,17 @@ void LateralErrorPublisher::onGroundTruthPose( RCLCPP_DEBUG(this->get_logger(), "localization_error: %f", lateral_error); // Publish lateral errors - tier4_debug_msgs::msg::Float32Stamped control_msg; + autoware_internal_debug_msgs::msg::Float32Stamped control_msg; control_msg.stamp = this->now(); control_msg.data = static_cast(control_lateral_error); pub_control_lateral_error_->publish(control_msg); - tier4_debug_msgs::msg::Float32Stamped localization_msg; + autoware_internal_debug_msgs::msg::Float32Stamped localization_msg; localization_msg.stamp = this->now(); localization_msg.data = static_cast(localization_lateral_error); pub_localization_lateral_error_->publish(localization_msg); - tier4_debug_msgs::msg::Float32Stamped sum_msg; + autoware_internal_debug_msgs::msg::Float32Stamped sum_msg; sum_msg.stamp = this->now(); sum_msg.data = static_cast(lateral_error); pub_lateral_error_->publish(sum_msg); From 98bb6c1b0ca1059bff27107adb94ecfaf55b8f13 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:17:17 +0900 Subject: [PATCH 04/10] feat(stop_accel_evaluator)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- .../stop_accel_evaluator/stop_accel_evaluator_node.hpp | 8 ++++---- control/stop_accel_evaluator/package.xml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp index 32c90570..83f006de 100644 --- a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp +++ b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp @@ -22,8 +22,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include #include @@ -34,8 +34,8 @@ using autoware::universe_utils::SelfPoseListener; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; class StopAccelEvaluatorNode : public rclcpp::Node { diff --git a/control/stop_accel_evaluator/package.xml b/control/stop_accel_evaluator/package.xml index 06828fe4..9f978aa7 100644 --- a/control/stop_accel_evaluator/package.xml +++ b/control/stop_accel_evaluator/package.xml @@ -18,7 +18,7 @@ rclcpp_components sensor_msgs std_msgs - tier4_debug_msgs + autoware_internal_debug_msgs ament_lint_auto autoware_lint_common From 8fd1201ca180ccef6d1a22a399dc908ce5e2e0d9 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:18:49 +0900 Subject: [PATCH 05/10] feat(vehicle_cmd_analyzer)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- .../include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp | 4 ++-- control/vehicle_cmd_analyzer/package.xml | 2 +- control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp index 4deb2732..7822c278 100644 --- a/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp +++ b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp @@ -21,7 +21,7 @@ #include #include "autoware_control_msgs/msg/control.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include #include @@ -33,7 +33,7 @@ class VehicleCmdAnalyzer : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::TimerBase::SharedPtr timer_control_; std::shared_ptr vehicle_cmd_ptr_{nullptr}; diff --git a/control/vehicle_cmd_analyzer/package.xml b/control/vehicle_cmd_analyzer/package.xml index 32b4585f..dd9f7194 100644 --- a/control/vehicle_cmd_analyzer/package.xml +++ b/control/vehicle_cmd_analyzer/package.xml @@ -15,7 +15,7 @@ autoware_vehicle_info_utils rclcpp rclcpp_components - tier4_debug_msgs + autoware_internal_debug_msgs ament_lint_auto autoware_lint_common diff --git a/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp b/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp index e4d4a467..f9a58916 100644 --- a/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp +++ b/control/vehicle_cmd_analyzer/src/vehicle_cmd_analyzer.cpp @@ -33,7 +33,7 @@ VehicleCmdAnalyzer::VehicleCmdAnalyzer(const rclcpp::NodeOptions & options) sub_vehicle_cmd_ = this->create_subscription( "/control/command/control_cmd", rclcpp::QoS(10), std::bind(&VehicleCmdAnalyzer::callbackVehicleCommand, this, std::placeholders::_1)); - pub_debug_ = create_publisher( + pub_debug_ = create_publisher( "~/debug_values", rclcpp::QoS{1}); // Timer @@ -83,7 +83,7 @@ void VehicleCmdAnalyzer::publishDebugData() debug_values_.setValues(DebugValues::TYPE::CURRENT_TARGET_LATERAL_ACC, a_lat); // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = this->now(); for (const auto & v : debug_values_.getValues()) { debug_msg.data.push_back(v); From 705be500adcd38d7a50b85c2d6126849dcae6b12 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:19:32 +0900 Subject: [PATCH 06/10] feat(autoware_planning_data_analyzer)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- planning/autoware_planning_data_analyzer/README.md | 8 ++++---- planning/autoware_planning_data_analyzer/package.xml | 1 + .../autoware_planning_data_analyzer/src/type_alias.hpp | 4 ++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/autoware_planning_data_analyzer/README.md b/planning/autoware_planning_data_analyzer/README.md index a4f2cafd..3cebead9 100644 --- a/planning/autoware_planning_data_analyzer/README.md +++ b/planning/autoware_planning_data_analyzer/README.md @@ -14,7 +14,7 @@ ros2 launch autoware_planning_data_analyzer behavior_analyzer.launch.xml bag_pat | Name | Type | Description | | ------------------------- | ------------------------------------------------- | --------------------------------------------------------------- | -| `~/output/manual_metrics` | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the driver's driving trajectory. | -| `~/output/system_metrics` | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the autoware output. | -| `~/output/manual_score` | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the driver's driving trajectory. | -| `~/output/system_score` | `tier4_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the autoware output. | +| `~/output/manual_metrics` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the driver's driving trajectory. | +| `~/output/system_metrics` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the autoware output. | +| `~/output/manual_score` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the driver's driving trajectory. | +| `~/output/system_score` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the autoware output. | diff --git a/planning/autoware_planning_data_analyzer/package.xml b/planning/autoware_planning_data_analyzer/package.xml index ce4875c5..6f56b62f 100644 --- a/planning/autoware_planning_data_analyzer/package.xml +++ b/planning/autoware_planning_data_analyzer/package.xml @@ -15,6 +15,7 @@ eigen3_cmake_module autoware_frenet_planner + autoware_internal_debug_msgs autoware_map_msgs autoware_motion_utils autoware_path_sampler diff --git a/planning/autoware_planning_data_analyzer/src/type_alias.hpp b/planning/autoware_planning_data_analyzer/src/type_alias.hpp index 53b0e443..a86e1b41 100644 --- a/planning/autoware_planning_data_analyzer/src/type_alias.hpp +++ b/planning/autoware_planning_data_analyzer/src/type_alias.hpp @@ -26,7 +26,7 @@ #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -60,7 +60,7 @@ using nav_msgs::msg::Odometry; using std_srvs::srv::SetBool; using std_srvs::srv::Trigger; using tf2_msgs::msg::TFMessage; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; } // namespace autoware::behavior_analyzer From b605f45dce449a1f73586ff7db2487f03bfd7b26 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:20:20 +0900 Subject: [PATCH 07/10] feat(planning_debug_tools)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- .../include/planning_debug_tools/trajectory_analyzer.hpp | 2 +- planning/planning_debug_tools/package.xml | 2 +- .../planning_debug_tools/scripts/closest_velocity_checker.py | 4 ++-- .../planning_debug_tools/scripts/processing_time_checker.py | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index 9053cb2f..0c8c6fb3 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -24,7 +24,7 @@ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float64_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index dd9ad622..bb5f8417 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -25,7 +25,7 @@ nav_msgs rclcpp rclcpp_components - tier4_debug_msgs + autoware_internal_debug_msgs tier4_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/planning_debug_tools/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py index a9eca848..f7954cbb 100755 --- a/planning/planning_debug_tools/scripts/closest_velocity_checker.py +++ b/planning/planning_debug_tools/scripts/closest_velocity_checker.py @@ -29,8 +29,8 @@ from tf2_ros import LookupException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -from tier4_debug_msgs.msg import Float32MultiArrayStamped -from tier4_debug_msgs.msg import Float32Stamped +from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped +from autoware_internal_debug_msgs.msg import Float32Stamped from tier4_planning_msgs.msg import PathWithLaneId from tier4_planning_msgs.msg import VelocityLimit diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py index 39e94ebc..b6b0829e 100755 --- a/planning/planning_debug_tools/scripts/processing_time_checker.py +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -22,7 +22,7 @@ import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float64Stamped +from autoware_internal_debug_msgs.msg import Float64Stamped class ProcessingTimeSubscriber(Node): From 1acf2f5ea5a1fe3560e04ae3644e1e68a3be252f Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:20:59 +0900 Subject: [PATCH 08/10] feat(parameter_estimator)!: replace tier4_debug_msgs with tier4_internal_debug_msgs Signed-off-by: Ryohsuke Mitsudome --- .../include/parameter_estimator/debugger.hpp | 8 ++++---- vehicle/parameter_estimator/package.xml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp b/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp index b042293e..265ecf41 100644 --- a/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp +++ b/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include #include @@ -33,14 +33,14 @@ struct Debugger rclcpp::QoS durable_qos(queue_size); durable_qos.transient_local(); // option for latching - pub_debug_ = rclcpp::create_publisher( + pub_debug_ = rclcpp::create_publisher( node, "~/debug_values/" + name, durable_qos); debug_values_.data.resize(num_debug_values_, 0.0); } - rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr pub_debug_; void publishDebugValue() { pub_debug_->publish(debug_values_); } static constexpr std::uint8_t num_debug_values_ = 20; - mutable tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values_; + mutable autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_values_; }; #endif // PARAMETER_ESTIMATOR__DEBUGGER_HPP_ diff --git a/vehicle/parameter_estimator/package.xml b/vehicle/parameter_estimator/package.xml index 6f8023d2..c9db2230 100644 --- a/vehicle/parameter_estimator/package.xml +++ b/vehicle/parameter_estimator/package.xml @@ -17,7 +17,7 @@ rclcpp sensor_msgs tier4_calibration_msgs - tier4_debug_msgs + autoware_internal_debug_msgs plotjuggler plotjuggler_ros From 04047f207655c6846bb444106aa8f99233982d40 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Jan 2025 03:32:29 +0000 Subject: [PATCH 09/10] style(pre-commit): autofix --- common/tier4_debug_rviz_plugin/package.xml | 2 +- .../src/float32_multi_array_stamped_pie_chart.cpp | 12 +++++++----- .../tier4_debug_tools/lateral_error_publisher.hpp | 2 +- common/tier4_debug_tools/package.xml | 2 +- .../src/lateral_error_publisher.cpp | 7 ++++--- .../stop_accel_evaluator_node.hpp | 8 ++++---- control/stop_accel_evaluator/package.xml | 2 +- .../vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp | 3 ++- control/vehicle_cmd_analyzer/package.xml | 2 +- .../deviation_estimator/deviation_estimator.hpp | 2 +- .../include/deviation_estimator/utils.hpp | 14 +++++++++----- .../deviation_estimator/velocity_coef_module.hpp | 2 +- .../deviation_estimator/package.xml | 2 +- .../deviation_estimator/src/utils.cpp | 6 ++++-- .../deviation_evaluator/deviation_evaluator.hpp | 2 +- .../deviation_evaluator/package.xml | 2 +- planning/autoware_planning_data_analyzer/README.md | 4 ++-- .../src/type_alias.hpp | 4 ++-- .../planning_debug_tools/trajectory_analyzer.hpp | 2 +- planning/planning_debug_tools/package.xml | 2 +- .../scripts/closest_velocity_checker.py | 4 ++-- .../scripts/processing_time_checker.py | 2 +- .../include/parameter_estimator/debugger.hpp | 8 +++++--- vehicle/parameter_estimator/package.xml | 2 +- 24 files changed, 55 insertions(+), 43 deletions(-) diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml index bb1bf348..d63dd219 100644 --- a/common/tier4_debug_rviz_plugin/package.xml +++ b/common/tier4_debug_rviz_plugin/package.xml @@ -10,6 +10,7 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_internal_debug_msgs libqt5-core libqt5-gui @@ -19,7 +20,6 @@ rviz_common rviz_default_plugins rviz_rendering - autoware_internal_debug_msgs ament_lint_auto autoware_lint_common diff --git a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp index b7dc95c7..bb147f1e 100644 --- a/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp +++ b/common/tier4_debug_rviz_plugin/src/float32_multi_array_stamped_pie_chart.cpp @@ -60,7 +60,8 @@ Float32MultiArrayStampedPieChartDisplay::Float32MultiArrayStampedPieChartDisplay : rviz_common::Display(), update_required_(false), first_time_(true), data_(0.0) { update_topic_property_ = new rviz_common::properties::StringProperty( - "Topic", "", "autoware_internal_debug_msgs::msg::Float32MultiArrayStamped topic to subscribe to.", this, + "Topic", "", + "autoware_internal_debug_msgs::msg::Float32MultiArrayStamped topic to subscribe to.", this, SLOT(updateTopic()), this); data_index_property_ = new rviz_common::properties::IntProperty( "data index", 0, "data index in message to visualize", this, SLOT(updateDataIndex())); @@ -297,10 +298,11 @@ void Float32MultiArrayStampedPieChartDisplay::subscribe() if (topic_name.length() > 0 && topic_name != "/") { rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - sub_ = raw_node->create_subscription( - topic_name, 1, - std::bind( - &Float32MultiArrayStampedPieChartDisplay::processMessage, this, std::placeholders::_1)); + sub_ = + raw_node->create_subscription( + topic_name, 1, + std::bind( + &Float32MultiArrayStampedPieChartDisplay::processMessage, this, std::placeholders::_1)); } } diff --git a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp index afa0ae61..423b05f1 100644 --- a/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp +++ b/common/tier4_debug_tools/include/tier4_debug_tools/lateral_error_publisher.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include class LateralErrorPublisher : public rclcpp::Node { diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index 509660ae..978bf4dd 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs autoware_universe_utils @@ -17,7 +18,6 @@ rclcpp rclcpp_components tf2_ros - autoware_internal_debug_msgs launch_ros python3-rtree diff --git a/common/tier4_debug_tools/src/lateral_error_publisher.cpp b/common/tier4_debug_tools/src/lateral_error_publisher.cpp index 60c67adf..d6030e9c 100644 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -35,10 +35,11 @@ LateralErrorPublisher::LateralErrorPublisher(const rclcpp::NodeOptions & node_op sub_ground_truth_pose_ = create_subscription( "~/input/ground_truth_pose_with_covariance", rclcpp::QoS{1}, std::bind(&LateralErrorPublisher::onGroundTruthPose, this, _1)); - pub_control_lateral_error_ = - create_publisher("~/control_lateral_error", 1); + pub_control_lateral_error_ = create_publisher( + "~/control_lateral_error", 1); pub_localization_lateral_error_ = - create_publisher("~/localization_lateral_error", 1); + create_publisher( + "~/localization_lateral_error", 1); pub_lateral_error_ = create_publisher("~/lateral_error", 1); } diff --git a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp index 83f006de..243d7b7d 100644 --- a/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp +++ b/control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp @@ -19,11 +19,11 @@ #include "autoware/universe_utils/ros/self_pose_listener.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include #include @@ -31,11 +31,11 @@ namespace stop_accel_evaluator { using autoware::universe_utils::SelfPoseListener; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; -using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; -using autoware_internal_debug_msgs::msg::Float32Stamped; class StopAccelEvaluatorNode : public rclcpp::Node { diff --git a/control/stop_accel_evaluator/package.xml b/control/stop_accel_evaluator/package.xml index 9f978aa7..a0d20ee1 100644 --- a/control/stop_accel_evaluator/package.xml +++ b/control/stop_accel_evaluator/package.xml @@ -10,6 +10,7 @@ autoware_cmake + autoware_internal_debug_msgs autoware_signal_processing autoware_universe_utils geometry_msgs @@ -18,7 +19,6 @@ rclcpp_components sensor_msgs std_msgs - autoware_internal_debug_msgs ament_lint_auto autoware_lint_common diff --git a/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp index 7822c278..f02f6360 100644 --- a/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp +++ b/control/vehicle_cmd_analyzer/include/vehicle_cmd_analyzer/vehicle_cmd_analyzer.hpp @@ -33,7 +33,8 @@ class VehicleCmdAnalyzer : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr + pub_debug_; rclcpp::TimerBase::SharedPtr timer_control_; std::shared_ptr vehicle_cmd_ptr_{nullptr}; diff --git a/control/vehicle_cmd_analyzer/package.xml b/control/vehicle_cmd_analyzer/package.xml index dd9f7194..793d2b84 100644 --- a/control/vehicle_cmd_analyzer/package.xml +++ b/control/vehicle_cmd_analyzer/package.xml @@ -12,10 +12,10 @@ autoware_cmake autoware_control_msgs + autoware_internal_debug_msgs autoware_vehicle_info_utils rclcpp rclcpp_components - autoware_internal_debug_msgs ament_lint_auto autoware_lint_common 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 diff --git a/planning/autoware_planning_data_analyzer/README.md b/planning/autoware_planning_data_analyzer/README.md index 3cebead9..dedbd84e 100644 --- a/planning/autoware_planning_data_analyzer/README.md +++ b/planning/autoware_planning_data_analyzer/README.md @@ -12,8 +12,8 @@ ros2 launch autoware_planning_data_analyzer behavior_analyzer.launch.xml bag_pat ## Output -| Name | Type | Description | -| ------------------------- | ------------------------------------------------- | --------------------------------------------------------------- | +| Name | Type | Description | +| ------------------------- | ------------------------------------------------------------- | --------------------------------------------------------------- | | `~/output/manual_metrics` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the driver's driving trajectory. | | `~/output/system_metrics` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Metrics calculated from the autoware output. | | `~/output/manual_score` | `autoware_internal_debug_msgs::msg::Float32MultiArrayStamped` | Driving scores calculated from the driver's driving trajectory. | diff --git a/planning/autoware_planning_data_analyzer/src/type_alias.hpp b/planning/autoware_planning_data_analyzer/src/type_alias.hpp index a86e1b41..6435370e 100644 --- a/planning/autoware_planning_data_analyzer/src/type_alias.hpp +++ b/planning/autoware_planning_data_analyzer/src/type_alias.hpp @@ -18,6 +18,7 @@ #include #include +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/lanelet_route.hpp" @@ -26,7 +27,6 @@ #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -51,6 +51,7 @@ using autoware_vehicle_msgs::msg::SteeringReport; using route_handler::RouteHandler; // ros2 +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -60,7 +61,6 @@ using nav_msgs::msg::Odometry; using std_srvs::srv::SetBool; using std_srvs::srv::Trigger; using tf2_msgs::msg::TFMessage; -using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; } // namespace autoware::behavior_analyzer diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index 0c8c6fb3..ba5c3904 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -21,10 +21,10 @@ #include "planning_debug_tools/util.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index bb5f8417..a3b1104d 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -17,6 +17,7 @@ rosidl_default_generators + autoware_internal_debug_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs @@ -25,7 +26,6 @@ nav_msgs rclcpp rclcpp_components - autoware_internal_debug_msgs tier4_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/planning_debug_tools/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py index f7954cbb..2ed65eda 100755 --- a/planning/planning_debug_tools/scripts/closest_velocity_checker.py +++ b/planning/planning_debug_tools/scripts/closest_velocity_checker.py @@ -18,6 +18,8 @@ from autoware_adapi_v1_msgs.msg import OperationModeState from autoware_control_msgs.msg import Control as AckermannControlCommand +from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped +from autoware_internal_debug_msgs.msg import Float32Stamped from autoware_planning_msgs.msg import Path from autoware_planning_msgs.msg import Trajectory from autoware_vehicle_msgs.msg import VelocityReport @@ -29,8 +31,6 @@ from tf2_ros import LookupException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped -from autoware_internal_debug_msgs.msg import Float32Stamped from tier4_planning_msgs.msg import PathWithLaneId from tier4_planning_msgs.msg import VelocityLimit diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py index b6b0829e..c22c6d85 100755 --- a/planning/planning_debug_tools/scripts/processing_time_checker.py +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -20,9 +20,9 @@ import os import sys +from autoware_internal_debug_msgs.msg import Float64Stamped import rclpy from rclpy.node import Node -from autoware_internal_debug_msgs.msg import Float64Stamped class ProcessingTimeSubscriber(Node): diff --git a/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp b/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp index 265ecf41..5d26c3a5 100644 --- a/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp +++ b/vehicle/parameter_estimator/include/parameter_estimator/debugger.hpp @@ -33,11 +33,13 @@ struct Debugger rclcpp::QoS durable_qos(queue_size); durable_qos.transient_local(); // option for latching - pub_debug_ = rclcpp::create_publisher( - node, "~/debug_values/" + name, durable_qos); + pub_debug_ = + rclcpp::create_publisher( + node, "~/debug_values/" + name, durable_qos); debug_values_.data.resize(num_debug_values_, 0.0); } - rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr + pub_debug_; void publishDebugValue() { pub_debug_->publish(debug_values_); } static constexpr std::uint8_t num_debug_values_ = 20; mutable autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_values_; diff --git a/vehicle/parameter_estimator/package.xml b/vehicle/parameter_estimator/package.xml index c9db2230..d2ff997a 100644 --- a/vehicle/parameter_estimator/package.xml +++ b/vehicle/parameter_estimator/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto + autoware_internal_debug_msgs autoware_vehicle_info_utils autoware_vehicle_msgs estimator_utils @@ -17,7 +18,6 @@ rclcpp sensor_msgs tier4_calibration_msgs - autoware_internal_debug_msgs plotjuggler plotjuggler_ros From adeb9c045be28762bd921bf6231fb8c09be6fed7 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:30:02 +0900 Subject: [PATCH 10/10] fix: remove unnecessary dependency Signed-off-by: Ryohsuke Mitsudome --- common/tier4_debug_rviz_plugin/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/common/tier4_debug_rviz_plugin/package.xml b/common/tier4_debug_rviz_plugin/package.xml index d63dd219..788ad33d 100644 --- a/common/tier4_debug_rviz_plugin/package.xml +++ b/common/tier4_debug_rviz_plugin/package.xml @@ -10,7 +10,6 @@ ament_cmake autoware_cmake - autoware_internal_debug_msgs autoware_internal_debug_msgs libqt5-core libqt5-gui