feat(behavior_velocity_planner)!: remove stop_reason

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp

@@ -114,7 +114,7 @@ class BlindSpotModule : public SceneModuleInterface
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path
    * and object predicted path
    */
-  bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
+  bool modifyPathVelocity(PathWithLaneId * path) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   std::vector<autoware::motion_utils::VirtualWall> createVirtualWalls() override;
@@ -133,20 +133,18 @@ class BlindSpotModule : public SceneModuleInterface
   // Parameter
 
   void initializeRTCStatus();
-  BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason);
+  BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path);
   // setSafe(), setDistance()
   void setRTCStatus(
     const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path);
   template <typename Decision>
   void setRTCStatusByDecision(
     const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path);
   // stop/GO
-  void reactRTCApproval(
-    const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason);
+  void reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path);
   template <typename Decision>
   void reactRTCApprovalByDecision(
-    const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path,
-    StopReason * stop_reason);
+    const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path);
 
   std::optional<InterpolatedPathInfo> generateInterpolatedPathInfo(
     const tier4_planning_msgs::msg::PathWithLaneId & input_path) const;

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp

@@ -33,8 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision(
 template <typename T>
 void BlindSpotModule::reactRTCApprovalByDecision(
   [[maybe_unused]] const T & decision,
-  [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path,
-  [[maybe_unused]] StopReason * stop_reason)
+  [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path)
 {
   static_assert("Unsupported type passed to reactRTCApprovalByDecision");
 }
@@ -53,8 +52,7 @@ void BlindSpotModule::setRTCStatusByDecision(
 template <>
 void BlindSpotModule::reactRTCApprovalByDecision(
   [[maybe_unused]] const InternalError & decision,
-  [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path,
-  [[maybe_unused]] StopReason * stop_reason)
+  [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path)
 {
   return;
 }
@@ -73,8 +71,7 @@ void BlindSpotModule::setRTCStatusByDecision(
 template <>
 void BlindSpotModule::reactRTCApprovalByDecision(
   [[maybe_unused]] const OverPassJudge & decision,
-  [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path,
-  [[maybe_unused]] StopReason * stop_reason)
+  [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path)
 {
   return;
 }
@@ -95,20 +92,15 @@ void BlindSpotModule::setRTCStatusByDecision(
 
 template <>
 void BlindSpotModule::reactRTCApprovalByDecision(
-  const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path,
-  StopReason * stop_reason)
+  const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path)
 {
   if (!isActivated()) {
     constexpr double stop_vel = 0.0;
     planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
     debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
       decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
 
-    tier4_planning_msgs::msg::StopFactor stop_factor;
     const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
-    stop_factor.stop_pose = stop_pose;
-    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
-    planning_utils::appendStopReason(stop_factor, stop_reason);
     velocity_factor_.set(
       path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
   }
@@ -131,19 +123,15 @@ void BlindSpotModule::setRTCStatusByDecision(
 
 template <>
 void BlindSpotModule::reactRTCApprovalByDecision(
-  const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason)
+  const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path)
 {
   if (!isActivated()) {
     constexpr double stop_vel = 0.0;
     planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
     debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
       decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
 
-    tier4_planning_msgs::msg::StopFactor stop_factor;
     const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
-    stop_factor.stop_pose = stop_pose;
-    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
-    planning_utils::appendStopReason(stop_factor, stop_reason);
     velocity_factor_.set(
       path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
   }

planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp

@@ -64,8 +64,7 @@
   setDistance(std::numeric_limits<double>::lowest());
 }
 
-BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
-  PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
+BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * path)
 {
   if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) {
     return OverPassJudge{"already over the pass judge line. no plan needed."};
@@ -160,26 +159,23 @@
     decision);
 }
 
-void BlindSpotModule::reactRTCApproval(
-  const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason)
+void BlindSpotModule::reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path)
 {
   std::visit(
-    VisitorSwitch{[&](const auto & sub_decision) {
-      reactRTCApprovalByDecision(sub_decision, path, stop_reason);
-    }},
+    VisitorSwitch{
+      [&](const auto & sub_decision) { reactRTCApprovalByDecision(sub_decision, path); }},
     decision);
 }
 
-bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
+bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path)
 {
   debug_data_ = DebugData();
-  *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT);
 
   initializeRTCStatus();
-  const auto decision = modifyPathVelocityDetail(path, stop_reason);
+  const auto decision = modifyPathVelocityDetail(path);
   const auto & input_path = *path;
   setRTCStatus(decision, input_path);
-  reactRTCApproval(decision, path, stop_reason);
+  reactRTCApproval(decision, path);
 
   return true;
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -201,7 +201,7 @@
     node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
 }
 
-bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
+bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path)
 {
   if (path->points.size() < 2) {
     RCLCPP_DEBUG(logger_, "Do not interpolate because path size is less than 2.");
@@ -213,7 +213,6 @@
     logger_, planner_param_.show_processing_time,
     "=========== module_id: %ld ===========", module_id_);
 
-  *stop_reason = planning_utils::initializeStopReason(StopReason::CROSSWALK);
   const auto & ego_pos = planner_data_->current_odometry->pose.position;
   const auto objects_ptr = planner_data_->predicted_objects;
 
@@ -274,7 +273,7 @@
   if (isActivated()) {
     planGo(*path, nearest_stop_factor);
   } else {
-    planStop(*path, nearest_stop_factor, default_stop_pose, stop_reason);
+    planStop(*path, nearest_stop_factor, default_stop_pose);
   }
   recordTime(4);
 
@@ -1353,7 +1352,7 @@
 
 void CrosswalkModule::planStop(
   PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
-  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
+  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
 {
   // Calculate stop factor
   auto stop_factor = [&]() -> std::optional<StopFactor> {
@@ -1376,7 +1375,6 @@
 
   // Plan stop
   insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path);
-  planning_utils::appendStopReason(*stop_factor, stop_reason);
   velocity_factor_.set(
     ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose,
     VelocityFactor::UNKNOWN);

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -336,7 +336,7 @@ class CrosswalkModule : public SceneModuleInterface
     const PlannerParam & planner_param, const rclcpp::Logger & logger,
     const rclcpp::Clock::SharedPtr clock);
 
-  bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
+  bool modifyPathVelocity(PathWithLaneId * path) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   autoware::motion_utils::VirtualWalls createVirtualWalls() override;
@@ -399,7 +399,7 @@ class CrosswalkModule : public SceneModuleInterface
 
   void planStop(
     PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
-    const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason);
+    const std::optional<geometry_msgs::msg::Pose> & default_stop_pose);
 
   // minor functions
   std::pair<double, double> getAttentionRange(

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

@@ -47,165 +47,147 @@
   velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
 }
 
-bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
+bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
 {
   // Store original path
   const auto original_path = *path;
 
   // Reset data
   debug_data_ = DebugData();
   debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
-  *stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA);
 
   // Find obstacles in detection area
   const auto obstacle_points = detection_area::get_obstacle_points(
     detection_area_reg_elem_.detectionAreas(), *planner_data_->no_ground_pointcloud);
   debug_data_.obstacle_points = obstacle_points;
   if (!obstacle_points.empty()) {
     last_obstacle_found_time_ = std::make_shared<const rclcpp::Time>(clock_->now());
   }
 
   // Get stop line geometry
   const auto stop_line = detection_area::get_stop_line_geometry2d(
     detection_area_reg_elem_, planner_data_->stop_line_extend_length);
 
   // Get self pose
   const auto & self_pose = planner_data_->current_odometry->pose;
   const size_t current_seg_idx = findEgoSegmentIndex(path->points);
 
   // Get stop point
   const auto stop_point = arc_lane_utils::createTargetPoint(
     original_path, stop_line, planner_param_.stop_margin,
     planner_data_->vehicle_info_.max_longitudinal_offset_m);
   if (!stop_point) {
     return true;
   }
 
   const auto & stop_point_idx = stop_point->first;
   const auto & stop_pose = stop_point->second;
   const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
     path->points, stop_pose.position, stop_point_idx);
 
   auto modified_stop_pose = stop_pose;
   size_t modified_stop_line_seg_idx = stop_line_seg_idx;
 
   const auto is_stopped = planner_data_->isVehicleStopped(0.0);
   const auto stop_dist = calcSignedArcLength(
     path->points, self_pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx);
 
   // Don't re-approach when the ego stops closer to the stop point than hold_stop_margin_distance
   if (is_stopped && stop_dist < planner_param_.hold_stop_margin_distance) {
     const auto ego_pos_on_path =
       calcLongitudinalOffsetPose(original_path.points, self_pose.position, 0.0);
 
     if (!ego_pos_on_path) {
       return false;
     }
 
     modified_stop_pose = ego_pos_on_path.value();
     modified_stop_line_seg_idx = current_seg_idx;
   }
 
   setDistance(stop_dist);
 
   // Check state
   setSafe(detection_area::can_clear_stop_state(
     last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time));
   if (isActivated()) {
     last_obstacle_found_time_ = {};
     if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) {
       state_ = State::GO;
     }
     return true;
   }
 
   // Force ignore objects after dead_line
   if (planner_param_.use_dead_line) {
     // Use '-' for margin because it's the backward distance from stop line
     const auto dead_line_point = arc_lane_utils::createTargetPoint(
       original_path, stop_line, -planner_param_.dead_line_margin,
       planner_data_->vehicle_info_.max_longitudinal_offset_m);
 
     if (dead_line_point) {
       const size_t dead_line_point_idx = dead_line_point->first;
       const auto & dead_line_pose = dead_line_point->second;
 
       const size_t dead_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
         path->points, dead_line_pose.position, dead_line_point_idx);
 
       debug_data_.dead_line_poses.push_back(dead_line_pose);
 
       const double dist_from_ego_to_dead_line = calcSignedArcLength(
         original_path.points, self_pose.position, current_seg_idx, dead_line_pose.position,
         dead_line_seg_idx);
       if (dist_from_ego_to_dead_line < 0.0) {
         RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line");
         setSafe(true);
         return true;
       }
     }
   }
 
   // Ignore objects detected after stop_line if not in STOP state
   const double dist_from_ego_to_stop = calcSignedArcLength(
     original_path.points, self_pose.position, current_seg_idx, stop_pose.position,
     stop_line_seg_idx);
   if (
     state_ != State::STOP &&
     dist_from_ego_to_stop < -planner_param_.distance_to_judge_over_stop_line) {
     setSafe(true);
     return true;
   }
 
   // Ignore objects if braking distance is not enough
   if (planner_param_.use_pass_judge_line) {
     const auto current_velocity = planner_data_->current_velocity->twist.linear.x;
     const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithAccLimit(
       current_velocity, planner_data_->current_acceleration->accel.accel.linear.x,
       planner_data_->delay_response_time);
     if (
       state_ != State::STOP &&
       !detection_area::has_enough_braking_distance(
         self_pose, stop_point->second, pass_judge_line_distance, current_velocity)) {
       RCLCPP_WARN_THROTTLE(
         logger_, *clock_, std::chrono::milliseconds(1000).count(),
         "[detection_area] vehicle is over stop border");
       setSafe(true);
       return true;
     }
   }
 
   // Insert stop point
   state_ = State::STOP;
   planning_utils::insertStopPoint(modified_stop_pose.position, modified_stop_line_seg_idx, *path);
 
   // For virtual wall
   debug_data_.stop_poses.push_back(stop_point->second);
 
   // Create StopReason
   {
-    StopFactor stop_factor{};
-    stop_factor.stop_pose = stop_point->second;
-    stop_factor.stop_factor_points = obstacle_points;
-    planning_utils::appendStopReason(stop_factor, stop_reason);
     velocity_factor_.set(
       path->points, planner_data_->current_odometry->pose, stop_point->second,
       VelocityFactor::UNKNOWN);
   }
 
-  // Create legacy StopReason
-  {
-    const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength(
-      path->points, 0, stop_pose.position, stop_point->first);
-
-    if (
-      !first_stop_path_point_distance_ ||
-      stop_path_point_distance < first_stop_path_point_distance_.value()) {
-      debug_data_.first_stop_pose = stop_point->second;
-      first_stop_path_point_distance_ = stop_path_point_distance;
-    }
-  }
-
   return true;
 }
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp

@@ -70,7 +70,7 @@ class DetectionAreaModule : public SceneModuleInterface
     const PlannerParam & planner_param, const rclcpp::Logger & logger,
     const rclcpp::Clock::SharedPtr clock);
 
-  bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
+  bool modifyPathVelocity(PathWithLaneId * path) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   autoware::motion_utils::VirtualWalls createVirtualWalls() override;