feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs (#9744)

* feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml

@@ -23,6 +23,7 @@
 
   <depend>autoware_behavior_velocity_planner_common</depend>
   <depend>autoware_grid_map_utils</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
@@ -41,7 +42,6 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
-  <depend>tier4_debug_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

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
 
 

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<std::tuple<std::string, CollisionPoint, CollisionState>> & 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<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
+  collision_info_pub_ = node.create_publisher<autoware_internal_debug_msgs::msg::StringStamped>(
+    "~/debug/collision_info", 1);
 }
 
 bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path)

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -23,9 +23,9 @@
 #include <autoware_lanelet2_extension/regulatory_elements/crosswalk.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <tier4_debug_msgs/msg/string_stamped.hpp>
 
 #include <boost/assert.hpp>
 #include <boost/assign/list_of.hpp>
@@ -456,7 +456,8 @@ class CrosswalkModule : public SceneModuleInterface
 
   const int64_t module_id_;
 
-  rclcpp::Publisher<tier4_debug_msgs::msg::StringStamped>::SharedPtr collision_info_pub_;
+  rclcpp::Publisher<autoware_internal_debug_msgs::msg::StringStamped>::SharedPtr
+    collision_info_pub_;
 
   lanelet::ConstLanelet crosswalk_;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

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")
 

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<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
+  ego_ttc_pub_ = node.create_publisher<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>(
     "~/debug/intersection/ego_ttc", 1);
-  object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
-    "~/debug/intersection/object_ttc", 1);
+  object_ttc_pub_ =
+    node.create_publisher<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>(
+      "~/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);

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -27,7 +27,7 @@
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <tier4_debug_msgs/msg/float64_multi_array_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -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<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr ego_ttc_pub_;
-  rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr object_ttc_pub_;
+  rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr
+    ego_ttc_pub_;
+  rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr
+    object_ttc_pub_;
 };
 
 }  // namespace autoware::behavior_velocity_planner

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<size_t> 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;

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -27,8 +27,8 @@
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_planning_msgs/msg/path.hpp>
-#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_rtc_msgs/msg/state.hpp>
 #include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
@@ -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;

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml

@@ -20,6 +20,7 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_map_msgs</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml

@@ -21,6 +21,7 @@
 
   <depend>autoware_behavior_velocity_crosswalk_module</depend>
   <depend>autoware_behavior_velocity_planner_common</depend>
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_object_recognition_utils</depend>
   <depend>autoware_perception_msgs</depend>

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);

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp

@@ -18,17 +18,17 @@
 
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 
-#include <tier4_debug_msgs/msg/float32_multi_array_stamped.hpp>
-#include <tier4_debug_msgs/msg/int32_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/int32_stamped.hpp>
 
 #include <memory>
 #include <string>
 #include <vector>
 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
 {

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<lanelet::BasicPolygon2d>;

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp

@@ -21,10 +21,10 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
+#include <autoware_internal_debug_msgs/msg/float32_stamped.hpp>
 #include <autoware_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_perception_msgs/msg/shape.hpp>
 #include <autoware_planning_msgs/msg/path_point.hpp>
-#include <tier4_debug_msgs/msg/float32_stamped.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <string>
@@ -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<tier4_planning_msgs::msg::PathPointWithLaneId>;
 struct CommonParam