diff --git a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp index 16cb6471deb2c..f1ad63738df7c 100644 --- a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp @@ -42,11 +42,11 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include // To be replaced by std::optional in C++17 @@ -115,7 +115,8 @@ class PurePursuitLateralController : public LateralControllerBase // Debug Publisher rclcpp::Publisher::SharedPtr pub_debug_marker_; - rclcpp::Publisher::SharedPtr pub_debug_values_; + rclcpp::Publisher::SharedPtr + pub_debug_values_; // Predicted Trajectory publish rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml index 06500ddb32af0..5b61cc10af1f5 100644 --- a/control/autoware_pure_pursuit/package.xml +++ b/control/autoware_pure_pursuit/package.xml @@ -15,6 +15,7 @@ autoware_cmake autoware_control_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs visualization_msgs ament_lint_auto diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index f3a7bc3c1333d..d173cfe64d3e8 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -92,8 +92,9 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) // Debug Publishers pub_debug_marker_ = node.create_publisher("~/debug/markers", 0); - pub_debug_values_ = node.create_publisher( - "~/debug/ld_outputs", rclcpp::QoS{1}); + pub_debug_values_ = + node.create_publisher( + "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory pub_predicted_trajectory_ = node.create_publisher( @@ -118,7 +119,7 @@ double PurePursuitLateralController::calcLookaheadDistance( std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance); auto pubDebugValues = [&]() { - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.data.resize(TYPE::SIZE); debug_msg.data.at(TYPE::VEL_LD) = static_cast(vel_ld); debug_msg.data.at(TYPE::CURVATURE_LD) = static_cast(curvature_ld);