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..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 { @@ -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..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 - tier4_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..d6030e9c 100644 --- a/common/tier4_debug_tools/src/lateral_error_publisher.cpp +++ b/common/tier4_debug_tools/src/lateral_error_publisher.cpp @@ -35,12 +35,13 @@ 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); + create_publisher("~/lateral_error", 1); } void LateralErrorPublisher::onTrajectory( @@ -131,17 +132,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);