feat(avoidance): improve avoidance stop & decel behavior #4141
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -343,8 +343,8 @@ ObjectData AvoidanceModule::createObjectData( | |
| const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; | ||
| const auto object_closest_index = findNearestIndex(path_points, object_pose.position); | ||
| const auto object_closest_pose = path_points.at(object_closest_index).point.pose; | ||
| const auto object_type = utils::getHighestProbLabel(object.classification); | ||
| const auto object_parameter = parameters_->object_parameters.at(object_type); | ||
|
|
||
| ObjectData object_data{}; | ||
|
|
||
|
|
@@ -571,8 +571,8 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat | |
| } | ||
|
|
||
| const auto o_front = data.stop_target_object.get(); | ||
| const auto object_type = utils::getHighestProbLabel(o_front.object.classification); | ||
| const auto object_parameter = parameters_->object_parameters.at(object_type); | ||
| const auto & base_link2front = planner_data_->parameters.base_link2front; | ||
| const auto & vehicle_width = planner_data_->parameters.vehicle_width; | ||
|
|
||
|
|
@@ -620,6 +620,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif | |
| return; | ||
| } | ||
|
|
||
| insertPrepareVelocity(path); | ||
|
|
||
| switch (data.state) { | ||
| case AvoidanceState::NOT_AVOID: { | ||
| break; | ||
|
|
@@ -631,7 +633,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif | |
| break; | ||
| } | ||
| case AvoidanceState::AVOID_PATH_NOT_READY: { | ||
| insertWaitPoint(parameters_->use_constraints_for_decel, path); | ||
| break; | ||
| } | ||
|
|
@@ -860,8 +861,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( | |
| const auto nominal_return_distance = helper_.getNominalAvoidanceDistance(return_shift); | ||
|
|
||
| // use each object param | ||
| const auto object_type = utils::getHighestProbLabel(o.object.classification); | ||
| const auto object_parameter = parameters_->object_parameters.at(object_type); | ||
|
|
||
| /** | ||
| * Is there enough distance from ego to object for avoidance? | ||
|
|
@@ -3231,15 +3232,15 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const | |
| // D4: o_front.longitudinal | ||
| // D5: base_link2front | ||
|
|
||
| const auto object_type = utils::getHighestProbLabel(object.object.classification); | ||
| const auto object_parameter = parameters_->object_parameters.at(object_type); | ||
|
|
||
| const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + | ||
| object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; | ||
| const auto variable = helper_.getMinimumAvoidanceDistance( | ||
| helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); | ||
| const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + | ||
| base_link2front + p->stop_buffer; | ||
|
|
||
| return object.longitudinal - | ||
| std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); | ||
|
|
@@ -3350,23 +3351,64 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const | |
| { | ||
| const auto & data = avoidance_data_; | ||
|
|
||
| // do nothing if there is no avoidance target. | ||
| if (data.target_objects.empty()) { | ||
| return; | ||
| } | ||
|
|
||
| // insert slow down speed only when the avoidance maneuver is not initiated. | ||
| if (data.avoiding_now) { | ||
| return; | ||
| } | ||
|
|
||
| // insert slow down speed only when no shift line is approved. | ||
| if (!path_shifter_.getShiftLines().empty()) { | ||
| return; | ||
| } | ||
|
|
||
| const auto object = data.target_objects.front(); | ||
|
|
||
| // calculate shift length for front object. | ||
| const auto & vehicle_width = planner_data_->parameters.vehicle_width; | ||
| const auto object_type = utils::getHighestProbLabel(object.object.classification); | ||
| const auto object_parameter = parameters_->object_parameters.at(object_type); | ||
| const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + | ||
| object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; | ||
| const auto shift_length = | ||
| helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); | ||
|
|
||
| // check slow down feasibility | ||
| const auto min_avoid_distance = helper_.getMinimumAvoidanceDistance(shift_length); | ||
| const auto distance_to_object = object.longitudinal; | ||
| const auto remaining_distance = distance_to_object - min_avoid_distance; | ||
| const auto decel_distance = | ||
| helper_.getFeasibleDecelDistance(parameters_->min_sharp_avoidance_speed); | ||
| if (remaining_distance < decel_distance) { | ||
| return; | ||
| } | ||
|
|
||
| // decide slow down lower limit. | ||
| const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; | ||
|
|
||
| // insert slow down speed. | ||
| const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); | ||
| for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { | ||
| const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); | ||
|
|
||
| // slow down speed is inserted only in front of the object. | ||
| const auto shift_longitudinal_distance = distance_to_object - distance_from_ego; | ||
| if (shift_longitudinal_distance < min_avoid_distance) { | ||
| break; | ||
| } | ||
|
|
||
| // target speed with nominal jerk limits. | ||
| const double v_target = PathShifter::calcFeasibleVelocityFromJerk( | ||
| shift_length, parameters_->nominal_lateral_jerk, shift_longitudinal_distance); | ||
| const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; | ||
| const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); | ||
|
There was a problem hiding this comment. @satoshi-ota There was a problem hiding this comment. I added parameter description in b0ff9f6. Please see that 👍 |
||
|
|
||
| shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); | ||
| } | ||
| } | ||
|
|
||
| std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
❤️