diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 699f80ec8356e..08bd8f91c7d71 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_velocity_planner_common autoware_grid_map_utils + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -41,7 +42,6 @@ pluginlib rclcpp sensor_msgs - tier4_debug_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py index 8a9de1a563249..5b13b6f5752d3 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -19,10 +19,10 @@ from copy import deepcopy from ament_index_python.packages import get_package_share_directory +from autoware_internal_debug_msgs.msg import StringStamped import matplotlib.pyplot as plt import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import StringStamped import yaml diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 1bea0626b0b2f..07b2f1deff0c7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -153,11 +153,11 @@ StopFactor createStopFactor( return stop_factor; } -tier4_debug_msgs::msg::StringStamped createStringStampedMessage( +autoware_internal_debug_msgs::msg::StringStamped createStringStampedMessage( const rclcpp::Time & now, const int64_t module_id_, const std::vector> & collision_points) { - tier4_debug_msgs::msg::StringStamped msg; + autoware_internal_debug_msgs::msg::StringStamped msg; msg.stamp = now; for (const auto & collision_point : collision_points) { std::stringstream ss; @@ -199,8 +199,8 @@ CrosswalkModule::CrosswalkModule( road_ = lanelet_map_ptr->laneletLayer.get(lane_id); - collision_info_pub_ = - node.create_publisher("~/debug/collision_info", 1); + collision_info_pub_ = node.create_publisher( + "~/debug/collision_info", 1); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 9b771c0d503a4..b3fbc2f6cfaba 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -23,9 +23,9 @@ #include #include +#include #include #include -#include #include #include @@ -456,7 +456,8 @@ class CrosswalkModule : public SceneModuleInterface const int64_t module_id_; - rclcpp::Publisher::SharedPtr collision_info_pub_; + rclcpp::Publisher::SharedPtr + collision_info_pub_; lanelet::ConstLanelet crosswalk_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index b056f71614da3..7a64d1d6638ff 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py index 1eb6ae1ffafc1..c1bed003ea3a9 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py @@ -22,13 +22,13 @@ import time from PIL import Image +from autoware_internal_debug_msgs.msg import Float64MultiArrayStamped import imageio import matplotlib import matplotlib.pyplot as plt import numpy as np import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float64MultiArrayStamped matplotlib.use("TKAgg") diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 6f7841ebb0bbb..7c492a9dd42bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -88,10 +88,11 @@ IntersectionModule::IntersectionModule( static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); } - ego_ttc_pub_ = node.create_publisher( + ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); - object_ttc_pub_ = node.create_publisher( - "~/debug/intersection/object_ttc", 1); + object_ttc_pub_ = + node.create_publisher( + "~/debug/intersection/object_ttc", 1); } bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path) @@ -231,7 +232,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the // exit of intersection // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); @@ -240,7 +241,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // passed each pass judge line for the first time, save current collision status for late // diagnosis // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line, &object_ttc_time_array); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b718dd84d33af..164df7588bd68 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -748,7 +748,7 @@ class IntersectionModule : public SceneModuleInterface const PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, const TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, - tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, @@ -809,13 +809,15 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ mutable DebugData debug_data_; mutable InternalDebugData internal_debug_data_{}; - rclcpp::Publisher::SharedPtr ego_ttc_pub_; - rclcpp::Publisher::SharedPtr object_ttc_pub_; + rclcpp::Publisher::SharedPtr + ego_ttc_pub_; + rclcpp::Publisher::SharedPtr + object_ttc_pub_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 5a66bec15bab1..5103cd6cc46e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -152,7 +152,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( const IntersectionModule::TimeDistanceArray & time_distance_array, const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, - tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) { const auto & intersection_lanelets = intersection_lanelets_.value(); @@ -815,7 +815,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = planner_param_.collision_detection.velocity_profile.default_velocity; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index fadda66f12562..4e898d9d28715 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -27,8 +27,8 @@ #include #include +#include #include -#include #include #include #include @@ -57,8 +57,8 @@ using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInt using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; +using autoware_internal_debug_msgs::msg::Float64Stamped; using builtin_interfaces::msg::Time; -using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_rtc_msgs::msg::Module; using tier4_rtc_msgs::msg::State; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 6d94e20cfdb1b..a0b54cb879cab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_map_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index 90e1d32198854..b3ced8b2e9b9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -21,6 +21,7 @@ autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_motion_utils autoware_object_recognition_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 2823648e184d3..3f4b9f330e2d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -316,7 +316,7 @@ void RunOutDebug::setAccelReason(const AccelReason & accel_reason) void RunOutDebug::publishDebugValue() { // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = node_.now(); for (const auto & v : debug_values_.getValues()) { debug_msg.data.push_back(v); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3e913f57f30c0..1ffa826c4d1d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -18,17 +18,17 @@ #include -#include -#include +#include +#include #include #include #include namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Int32Stamped; using sensor_msgs::msg::PointCloud2; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Int32Stamped; class DebugValues { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 3673a215bb749..83123c71f461e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -32,10 +32,10 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 1f948cc7faaa1..c48e4f867fac0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -21,10 +21,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -39,11 +39,11 @@ using autoware::universe_utils::LineString2d; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; struct CommonParam