From aba16ba6c30fea7986a14673ef54397c0bf0a347 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Wed, 22 Jan 2025 12:16:43 +0900 Subject: [PATCH 1/2] 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 6089aff7d..afa0ae61b 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 53454235c..509660ae6 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 bd8ab750b..60c67adfc 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 c77efdaa81e29cf3543c932b0f6685c7b4a200cd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 Jan 2025 01:03:44 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../include/tier4_debug_tools/lateral_error_publisher.hpp | 2 +- common/tier4_debug_tools/package.xml | 2 +- common/tier4_debug_tools/src/lateral_error_publisher.cpp | 7 ++++--- 3 files changed, 6 insertions(+), 5 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 afa0ae61b..423b05f1f 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 509660ae6..978bf4dd9 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 60c67adfc..d6030e9cf 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); }