update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_cruise_planner/README.md

@@ -175,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_additional = v_pid > 0 ? v_pid * w_acc : v_pid
+v_target = v_target = max(v_ego + v_additional, v_min_cruise)
+$$
+
+| Variable       | Description                             |
+| -------------- | --------------------------------------- |
+| `d`            | actual distane to obstacle              |
+| `d_rss`        | ideal distance to obstacle based on RSS |
+| `lpf(val)`     | apply low-pass filter to `val`          |
+| `v_min_cruise` | `min_cruise_target_vel`                 |
+| `w_acc`        | `output_ratio_during_accel`             |
+| `pid(val)`     | apply pid to `val`                      |
 
 ## Implementation
 

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -3,15 +3,20 @@
     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]
       obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
       obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
@@ -23,7 +28,7 @@
         bus: true
         trailer: true
         motorcycle: true
-        bicycle: false
+        bicycle: true
         pedestrian: false
 
       stop_obstacle_type:
@@ -60,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
@@ -70,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

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -25,18 +25,6 @@
 #include <string>
 #include <vector>
 
-namespace
-{
-std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
-{
-  std::stringstream ss;
-  for (auto i = 0; i < 16; ++i) {
-    ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
-  }
-  return ss.str();
-}
-}  // namespace
-
 struct TargetObstacle
 {
   TargetObstacle(
@@ -50,7 +38,7 @@ struct TargetObstacle
     velocity_reliable = true;
     velocity = aligned_velocity;
     classification = object.classification.at(0);
-    uuid = toHexString(object.object_id);
+    uuid = tier4_autoware_utils::toHexString(object.object_id);
 
     predicted_paths.clear();
     for (const auto & path : object.kinematics.predicted_paths) {

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp

@@ -52,8 +52,6 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose(
 boost::optional<TargetObstacle> getClosestStopObstacle(
   const Trajectory & traj, const std::vector<TargetObstacle> & target_obstacles);
 
-std::string toHexString(const unique_identifier_msgs::msg::UUID & id);
-
 template <class T>
 size_t getIndexWithLongitudinalOffset(
   const T & points, const double longitudinal_offset, boost::optional<size_t> start_idx)

planning/obstacle_cruise_planner/src/node.cpp

@@ -624,7 +624,8 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
 
   std::vector<TargetObstacle> target_obstacles;
   for (const auto & predicted_object : predicted_objects.objects) {
-    const auto object_id = toHexString(predicted_object.object_id).substr(0, 4);
+    const auto object_id =
+      tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4);
 
     // filter object whose label is not cruised or stopped
     const bool is_target_obstacle = isStopObstacle(predicted_object.classification.front().label) ||
@@ -849,7 +850,7 @@ void ObstacleCruisePlannerNode::checkConsistency(
   const auto predicted_object_itr = std::find_if(
     predicted_objects.objects.begin(), predicted_objects.objects.end(),
     [&](PredictedObject predicted_object) {
-      return obstacle_cruise_utils::toHexString(predicted_object.object_id) ==
+      return tier4_autoware_utils::toHexString(predicted_object.object_id) ==
              prev_closest_obstacle_ptr_->uuid;
     });
 

planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -450,11 +450,6 @@ Trajectory PIDBasedPlanner::doCruiseWithTrajectory(
     return target_jerk_ratio;
   }();
 
-  /*
-  const double target_acc = vel_to_acc_weight_ * additional_vel;
-  const double target_acc_with_acc_limit =
-    std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel);
-  */
   cruise_planning_debug_info_.set(
     CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc);
   cruise_planning_debug_info_.set(

planning/obstacle_cruise_planner/src/utils.cpp

@@ -154,13 +154,4 @@ boost::optional<TargetObstacle> getClosestStopObstacle(
   }
   return closest_stop_obstacle;
 }
-
-std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
-{
-  std::stringstream ss;
-  for (auto i = 0; i < 16; ++i) {
-    ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
-  }
-  return ss.str();
-}
 }  // namespace obstacle_cruise_utils