diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index 7bd4d37ba5797..8abff415e47df 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/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 & arg_collision_points) + const double arg_lat_velocity, const std::vector & 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 collision_points; // time-series collision points + bool is_yield_obstacle; }; struct SlowDownObstacle : public TargetObstacleInterface diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 629dba73e85dc..6a106776e48b5 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/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 #include #include @@ -180,6 +181,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node this, "~/input/pointcloud"}; autoware::universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface + objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"}; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_{nullptr}; diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index df760ee59de37..75405c3aded2a 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -23,6 +23,7 @@ autoware_lanelet2_extension autoware_motion_utils autoware_object_recognition_utils + autoware_objects_of_interest_marker_interface autoware_osqp_interface autoware_perception_msgs autoware_planning_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 8275f2a1cd7d4..6931ff58e8457 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/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("~/debug/processing_time_ms", 1); - debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); - debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); - debug_slow_down_wall_marker_pub_ = - create_publisher("~/debug/slow_down/virtual_wall", 1); + debug_cruise_wall_marker_pub_ = create_publisher("~/virtual_wall/cruise", 1); + debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall/stop", 1); + debug_slow_down_wall_marker_pub_ = create_publisher("~/virtual_wall/slow_down", 1); debug_marker_pub_ = create_publisher("~/debug/marker", 1); debug_stop_planning_info_pub_ = create_publisher("~/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 ObstacleCruisePlannerNode::createYieldCruiseObstac obstacle.pose, obstacle.longitudinal_velocity, obstacle.approach_velocity, - collision_points.value()}; + collision_points.value(), + true}; } std::optional> ObstacleCruisePlannerNode::findYieldCruiseObstacles( @@ -1456,6 +1457,9 @@ std::optional> 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); } } } diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index f8982ed3fec2a..544597f05ff75 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -320,8 +320,11 @@ std::vector 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 = diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index dd4373c115fe0..82f4e6978181f 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/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}; }