fix: hotfix to beta v0.7.0

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -717,9 +717,9 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman
 
 void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path)
 {
-  constexpr double scale_x = 0.1;
-  constexpr double scale_y = 0.1;
-  constexpr double scale_z = 0.1;
+  constexpr double scale_x = 0.2;
+  constexpr double scale_y = 0.2;
+  constexpr double scale_z = 0.2;
   constexpr double color_r = 0.0 / 256.0;
   constexpr double color_g = 148.0 / 256.0;
   constexpr double color_b = 205.0 / 256.0;

planning/behavior_velocity_planner/package.xml

@@ -7,6 +7,7 @@
 
   <!-- intersection module -->
   <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <!-- crosswalk module -->
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <!-- detection area module -->

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

@@ -150,6 +150,33 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   }
   const size_t closest_idx = closest_idx_opt.get();
 
+  if (stop_lines_idx_opt.has_value()) {
+    const auto stop_line_idx = stop_lines_idx_opt.value().stop_line;
+    const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line;
+    const auto keep_detection_line_idx = stop_lines_idx_opt.value().keep_detection_line;
+
+    const bool is_over_pass_judge_line =
+      util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx);
+    const bool is_before_keep_detection_line =
+      stop_lines_idx_opt.has_value()
+        ? util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx)
+        : false;
+    const bool keep_detection = is_before_keep_detection_line &&
+                                std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
+
+    if (is_over_pass_judge_line && keep_detection) {
+      // in case ego could not stop exactly before the stop line, but with some overshoot,
+      // keep detection within some margin under low velocity threshold
+    } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
+      RCLCPP_INFO(logger_, "over the keep_detection line and not low speed. no plan needed.");
+      RCLCPP_DEBUG(logger_, "===== plan end =====");
+      setSafe(true);
+      setDistance(motion_utils::calcSignedArcLength(
+        path->points, planner_data_->current_pose.pose.position,
+        path->points.at(stop_line_idx).point.pose.position));
+      return true;
+    }
+  }
   /* collision checking */
   bool is_entry_prohibited = false;
 
@@ -171,8 +198,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     closest_idx, stuck_vehicle_detect_area);
 
   /* calculate final stop lines */
-  int stop_line_idx_final = -1;
-  int pass_judge_line_idx_final = -1;
+  int stop_line_idx_final =
+    stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1;
+  int pass_judge_line_idx_final =
+    stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().pass_judge_line : -1;
   if (external_go) {
     is_entry_prohibited = false;
   } else if (external_stop) {
@@ -204,29 +233,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     return false;
   }
 
-  const bool is_over_pass_judge_line =
-    util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final);
-  const bool is_before_keep_detection_line =
-    stop_lines_idx_opt.has_value()
-      ? util::isBeforeTargetIndex(
-          *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line)
-      : false;
-  const bool keep_detection =
-    is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
-
-  if (is_over_pass_judge_line && keep_detection) {
-    // in case ego could not stop exactly before the stop line, but with some overshoot,
-    // keep detection within some margin under low velocity threshold
-  } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
-    RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
-    RCLCPP_DEBUG(logger_, "===== plan end =====");
-    setSafe(true);
-    setDistance(motion_utils::calcSignedArcLength(
-      path->points, planner_data_->current_pose.pose.position,
-      path->points.at(stop_line_idx_final).point.pose.position));
-    return true;
-  }
-
   state_machine_.setStateWithMarginTime(
     is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("state_machine"), *clock_);

planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp

@@ -154,6 +154,9 @@ EBPathOptimizer::getOptimizedTrajectory(
       interpolated_points = std::vector<geometry_msgs::msg::Point>(
         interpolated_points.begin(),
         interpolated_points.begin() + interpolated_points_end_seg_idx.get());
+      if (interpolated_points.empty()) {
+        return boost::none;
+      }
     }
   }
 

planning/obstacle_cruise_planner/README.md

@@ -4,30 +4,29 @@
 
 The `obstacle_cruise_planner` package has following modules.
 
-- obstacle stop planning
-  - inserting a stop point in the trajectory when there is a static obstacle on the trajectory.
-- adaptive cruise planning
-  - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory
+- Stop planning
+  - stop when there is a static obstacle near the trajectory.
+- Cruise planning
+  - slow down when there is a dynamic obstacle to cruise near the trajectory
 
 ## Interfaces
 
 ### Input topics
 
-| Name                          | Type                                            | Description                       |
-| ----------------------------- | ----------------------------------------------- | --------------------------------- |
-| `~/input/trajectory`          | autoware_auto_planning_msgs::Trajectory         | input trajectory                  |
-| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory         | trajectory with smoothed velocity |
-| `~/input/objects`             | autoware_auto_perception_msgs::PredictedObjects | dynamic objects                   |
-| `~/input/odometry`            | nav_msgs::msg::Odometry                         | ego odometry                      |
+| Name                 | Type                                            | Description      |
+| -------------------- | ----------------------------------------------- | ---------------- |
+| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory         | input trajectory |
+| `~/input/objects`    | autoware_auto_perception_msgs::PredictedObjects | dynamic objects  |
+| `~/input/odometry`   | nav_msgs::msg::Odometry                         | ego odometry     |
 
 ### Output topics
 
-| Name                           | Type                                           | Description                           |
-| ------------------------------ | ---------------------------------------------- | ------------------------------------- |
-| `~output/trajectory`           | autoware_auto_planning_msgs::Trajectory        | output trajectory                     |
-| `~output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
-| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
-| `~output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
+| Name                            | Type                                           | Description                           |
+| ------------------------------- | ---------------------------------------------- | ------------------------------------- |
+| `~/output/trajectory`           | autoware_auto_planning_msgs::Trajectory        | output trajectory                     |
+| `~/output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
+| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
+| `~/output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
 
 ## Design
 
@@ -176,16 +175,37 @@ This includes not only cruising a front vehicle, but also reacting a cut-in and
 The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.
 
 $$
-d = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
+d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
 $$
 
-assuming that $d$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
+assuming that $d_rss$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
 These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`.
 
 | Parameter                         | Type   | Description                                                                   |
 | --------------------------------- | ------ | ----------------------------------------------------------------------------- |
 | `common.idling_time`              | double | idling time for the ego to detect the front vehicle starting deceleration [s] |
-| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss]                                          |
+| `common.min_ego_accel_for_rss`    | double | ego's acceleration for RSS [m/ss]                                             |
+| `common.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss]                                  |
+
+The detailed formulation is as follows.
+
+$$
+d_{error} = d - d_{rss} \\
+d_{normalized} = lpf(d_{error} / d_{obstacle}) \\
+d_{quad, normalized} = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\
+v_{pid} = pid(d_{quad, normalized}) \\
+v_{add} = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\
+v_{target} = max(v_{ego} + v_{add}, v_{min, cruise})
+$$
+
+| Variable          | Description                             |
+| ----------------- | --------------------------------------- |
+| `d`               | actual distane to obstacle              |
+| `d_{rss}`         | ideal distance to obstacle based on RSS |
+| `v_{min, cruise}` | `min_cruise_target_vel`                 |
+| `w_{acc}`         | `output_ratio_during_accel`             |
+| `lpf(val)`        | apply low-pass filter to `val`          |
+| `pid(val)`        | apply pid to `val`                      |
 
 ## Implementation
 

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -3,15 +3,18 @@
     common:
       planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"
 
-      is_showing_debug_info: true
+      is_showing_debug_info: false
+      disable_stop_planning: false # true
 
       # longitudinal info
-      idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s]
+      idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
       terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
 
+      lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
+
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
@@ -25,7 +28,7 @@
         bus: true
         trailer: true
         motorcycle: true
-        bicycle: false
+        bicycle: true
         pedestrian: false
 
       stop_obstacle_type:
@@ -62,7 +65,7 @@
       stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle
 
       ignored_outside_obstacle_type:
-        unknown: false
+        unknown: true
         car: false
         truck: false
         bus: false
@@ -72,21 +75,41 @@
         pedestrian: true
 
     pid_based_planner:
-      # use_predicted_obstacle_pose: false
+      use_velocity_limit_based_planner: true
+      error_function_type: quadratic # choose from linear, quadratic
 
-      # PID gains to keep safe distance with the front vehicle
-      kp: 2.5
-      ki: 0.0
-      kd: 2.3
+      velocity_limit_based_planner:
+          # PID gains to keep safe distance with the front vehicle
+          kp: 10.0
+          ki: 0.0
+          kd: 2.0
 
-      min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
+          output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+          vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
+
+          enable_jerk_limit_to_output_acc: false
 
-      output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
-      vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
+          disable_target_acceleration: true
 
+      velocity_insertion_based_planner:
+          kp_acc: 6.0
+          ki_acc: 0.0
+          kd_acc: 2.0
+
+          kp_jerk: 20.0
+          ki_jerk: 0.0
+          kd_jerk: 0.0
+
+          output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+          output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+
+          enable_jerk_limit_to_output_acc: true
+
+      min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
       min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
+      time_to_evaluate_rss: 0.0
 
-      lpf_cruise_gain: 0.2
+      lpf_normalized_error_cruise_dist_gain: 0.2
 
     optimization_based_planner:
       dense_resampling_time_interval: 0.2