fix(intersection_module): remove decel state (#1178)

* fix(intersection_module): remove decel mode

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

* Update planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -103,7 +103,6 @@ class IntersectionModule : public SceneModuleInterface
   struct PlannerParam
   {
     double state_transit_margin_time;
-    double decel_velocity;    //! used when in straight and traffic_light lane
     double stop_line_margin;  //! distance from auto-generated stopline to detection_area boundary
     double stuck_vehicle_detect_dist;  //! distance from end point to finish stuck vehicle check
     double

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -32,7 +32,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   auto & ip = intersection_param_;
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
   ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0);
-  ip.decel_velocity = node.declare_parameter(ns + ".decel_velocity", 30.0 / 3.6);
   ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0);
   ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0);
   ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) +
@@ -60,7 +59,6 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node
   auto & mp = merge_from_private_area_param_;
   mp.stop_duration_sec =
     node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0);
-  mp.decel_velocity = node.get_parameter("intersection.decel_velocity").as_double();
   mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double();
   mp.stop_line_margin = node.get_parameter("intersection.stop_line_margin").as_double();
 }

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -181,11 +181,9 @@ bool IntersectionModule::modifyPathVelocity(
   state_machine_.setStateWithMarginTime(
     isActivated() ? State::GO : State::STOP, logger_.get_child("state_machine"), *clock_);
 
-  /* set stop speed : TODO behavior on straight lane should be improved*/
-  const bool is_stop_required = is_stuck || !has_traffic_light_ || turn_direction_ != "straight";
   const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
 
-  setSafe(!(is_stop_required && is_entry_prohibited));
+  setSafe(!is_entry_prohibited);
   setDistance(tier4_autoware_utils::calcSignedArcLength(
     input_path.points, planner_data_->current_pose.pose.position,
     input_path.points.at(stop_line_idx).point.pose.position));
@@ -213,15 +211,6 @@ bool IntersectionModule::modifyPathVelocity(
     return true;
   }
 
-  if (state_machine_.getState() == State::STOP && is_entry_prohibited) {
-    const double v = planner_param_.decel_velocity;
-    util::setVelocityFrom(stop_line_idx, v, path);
-    setSafe(true);
-
-    debug_data_.stop_required = false;
-    debug_data_.slow_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path);
-  }
-
   RCLCPP_DEBUG(logger_, "===== plan end =====");
   return true;
 }

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -110,18 +110,14 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
 
   /* set stop speed */
   if (state_machine_.getState() == State::STOP) {
-    constexpr double stop_vel = 0.0;
-    const double decel_vel = planner_param_.decel_velocity;
-    double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel;
+    constexpr double v = 0.0;
     util::setVelocityFrom(stop_line_idx, v, path);
 
     /* get stop point and stop factor */
-    if (v == stop_vel) {
-      tier4_planning_msgs::msg::StopFactor stop_factor;
-      stop_factor.stop_pose = debug_data_.stop_point_pose;
-      stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point);
-      planning_utils::appendStopReason(stop_factor, stop_reason);
-    }
+    tier4_planning_msgs::msg::StopFactor stop_factor;
+    stop_factor.stop_pose = debug_data_.stop_point_pose;
+    stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point);
+    planning_utils::appendStopReason(stop_factor, stop_reason);
 
     const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
       path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position);