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