diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 084b538f39ddb..17eafb7f5ce72 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/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 diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 914e15a997096..dfc2b2e6ddb44 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/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 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index ac9326ea7d16f..3c35abe7fa14c 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -25,18 +25,6 @@ #include #include -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) { diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 01d3fc979c2cc..b509192d34371 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -52,8 +52,6 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( boost::optional getClosestStopObstacle( const Trajectory & traj, const std::vector & target_obstacles); -std::string toHexString(const unique_identifier_msgs::msg::UUID & id); - template size_t getIndexWithLongitudinalOffset( const T & points, const double longitudinal_offset, boost::optional start_idx) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index d91c30a4854d6..8b344affd9c7f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -624,7 +624,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( std::vector 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; }); diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 19bf69bc2b235..56491004775f9 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/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( diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 51a02aa776a40..45cc0a2e6c89d 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -154,13 +154,4 @@ boost::optional 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