refactor(obstacle_cruise_planner)!: refactor rviz and terminal info (#…

…9594)

Signed-off-by: yuki-takagi-66 <[email protected]>

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

@@ -169,12 +169,15 @@ struct CruiseObstacle : public TargetObstacleInterface
   CruiseObstacle(
     const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
     const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity,
-    const double arg_lat_velocity, const std::vector<PointWithStamp> & arg_collision_points)
+    const double arg_lat_velocity, const std::vector<PointWithStamp> & arg_collision_points,
+    bool arg_is_yield_obstacle = false)
   : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
-    collision_points(arg_collision_points)
+    collision_points(arg_collision_points),
+    is_yield_obstacle(arg_is_yield_obstacle)
   {
   }
   std::vector<PointWithStamp> collision_points;  // time-series collision points
+  bool is_yield_obstacle;
 };
 
 struct SlowDownObstacle : public TargetObstacleInterface

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp

@@ -24,6 +24,7 @@
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
 
+#include <autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -180,6 +181,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     this, "~/input/pointcloud"};
   autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
     this, "~/input/acceleration"};
+  autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface
+    objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"};
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};

planning/autoware_obstacle_cruise_planner/package.xml

@@ -23,6 +23,7 @@
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_object_recognition_utils</depend>
+  <depend>autoware_objects_of_interest_marker_interface</depend>
   <depend>autoware_osqp_interface</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>

planning/autoware_obstacle_cruise_planner/src/node.cpp

@@ -515,10 +515,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
 
   // debug publisher
   debug_calculation_time_pub_ = create_publisher<Float64Stamped>("~/debug/processing_time_ms", 1);
-  debug_cruise_wall_marker_pub_ = create_publisher<MarkerArray>("~/debug/cruise/virtual_wall", 1);
-  debug_stop_wall_marker_pub_ = create_publisher<MarkerArray>("~/virtual_wall", 1);
-  debug_slow_down_wall_marker_pub_ =
-    create_publisher<MarkerArray>("~/debug/slow_down/virtual_wall", 1);
+  debug_cruise_wall_marker_pub_ = create_publisher<MarkerArray>("~/virtual_wall/cruise", 1);
+  debug_stop_wall_marker_pub_ = create_publisher<MarkerArray>("~/virtual_wall/stop", 1);
+  debug_slow_down_wall_marker_pub_ = create_publisher<MarkerArray>("~/virtual_wall/slow_down", 1);
   debug_marker_pub_ = create_publisher<MarkerArray>("~/debug/marker", 1);
   debug_stop_planning_info_pub_ =
     create_publisher<Float32MultiArrayStamped>("~/debug/stop_planning_info", 1);
@@ -729,6 +728,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
   planner_ptr_->publishMetrics(now());
   publishDebugMarker();
   publishDebugInfo();
+  objects_of_interest_marker_interface_.publishMarkerArray();
 
   // 9. Publish and print calculation time
   const double calculation_time = stop_watch_.toc(__func__);
@@ -1385,7 +1385,8 @@ std::optional<CruiseObstacle> ObstacleCruisePlannerNode::createYieldCruiseObstac
     obstacle.pose,
     obstacle.longitudinal_velocity,
     obstacle.approach_velocity,
-    collision_points.value()};
+    collision_points.value(),
+    true};
 }
 
 std::optional<std::vector<CruiseObstacle>> ObstacleCruisePlannerNode::findYieldCruiseObstacles(
@@ -1456,6 +1457,9 @@ std::optional<std::vector<CruiseObstacle>> ObstacleCruisePlannerNode::findYieldC
         const auto yield_obstacle = createYieldCruiseObstacle(moving_obstacle, traj_points);
         if (yield_obstacle) {
           yield_obstacles.push_back(*yield_obstacle);
+          using autoware::objects_of_interest_marker_interface::ColorName;
+          objects_of_interest_marker_interface_.insertObjectData(
+            stopped_obstacle.pose, stopped_obstacle.shape, ColorName::RED);
         }
       }
     }

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -320,8 +320,11 @@ std::vector<TrajectoryPoint> PIDBasedPlanner::planCruise(
       const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset(
         stop_traj_points, dist_to_rss_wall, ego_idx);
 
+      const std::string wall_reason_string = cruise_obstacle_info->obstacle.is_yield_obstacle
+                                               ? "obstacle cruise (yield)"
+                                               : "obstacle cruise";
       auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
-        stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0);
+        stop_traj_points.at(wall_idx).pose, wall_reason_string, planner_data.current_time, 0);
       // NOTE: use a different color from slow down one to visualize cruise and slow down
       // separately.
       markers.markers.front().color =

planning/autoware_obstacle_cruise_planner/src/utils.cpp

@@ -92,7 +92,7 @@ PoseWithStamp getCurrentObjectPose(
     getCurrentObjectPoseFromPredictedPaths(predicted_paths, obj_base_time, current_time);
 
   if (!interpolated_pose) {
-    RCLCPP_WARN(
+    RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner"), "Failed to find the interpolated obstacle pose");
     return PoseWithStamp{obj_base_time, pose};
   }