@@ -660,7 +660,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
660
660
break ;
661
661
}
662
662
case AvoidanceState::YIELD: {
663
- insertYieldVelocity (path);
664
663
insertWaitPoint (isBestEffort (parameters_->policy_deceleration ), path);
665
664
break ;
666
665
}
@@ -1549,24 +1548,6 @@ void AvoidanceModule::insertStopPoint(
1549
1548
getEgoPosition (), stop_distance - MARGIN, 0.0 , shifted_path.path , stop_pose_);
1550
1549
}
1551
1550
1552
- void AvoidanceModule::insertYieldVelocity (ShiftedPath & shifted_path) const
1553
- {
1554
- const auto & p = parameters_;
1555
- const auto & data = avoid_data_;
1556
-
1557
- if (data.target_objects .empty ()) {
1558
- return ;
1559
- }
1560
-
1561
- if (helper_->isShifted ()) {
1562
- return ;
1563
- }
1564
-
1565
- const auto decel_distance = helper_->getFeasibleDecelDistance (p->yield_velocity , false );
1566
- utils::avoidance::insertDecelPoint (
1567
- getEgoPosition (), decel_distance, p->yield_velocity , shifted_path.path , slow_pose_);
1568
- }
1569
-
1570
1551
void AvoidanceModule::insertPrepareVelocity (ShiftedPath & shifted_path) const
1571
1552
{
1572
1553
const auto & data = avoid_data_;
@@ -1586,34 +1567,54 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
1586
1567
return ;
1587
1568
}
1588
1569
1589
- const auto object = data.target_objects .front ();
1570
+ const auto itr = std::find_if (
1571
+ data.target_objects .begin (), data.target_objects .end (),
1572
+ [](const auto & o) { return o.avoid_required ; });
1573
+
1574
+ const auto object = [&]() -> std::optional<ObjectData> {
1575
+ if (!data.yield_required ) {
1576
+ return data.target_objects .front ();
1577
+ }
1578
+
1579
+ if (itr == data.target_objects .end ()) {
1580
+ return std::nullopt;
1581
+ }
1582
+
1583
+ return *itr;
1584
+ }();
1585
+
1586
+ // do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary.
1587
+ if (!object.has_value ()) {
1588
+ return ;
1589
+ }
1590
1590
1591
1591
const auto enough_space =
1592
- object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
1592
+ object.value (). is_avoidable || object. value () .reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
1593
1593
if (!enough_space) {
1594
1594
return ;
1595
1595
}
1596
1596
1597
1597
// calculate shift length for front object.
1598
1598
const auto & vehicle_width = planner_data_->parameters .vehicle_width ;
1599
- const auto object_type = utils::getHighestProbLabel (object.object .classification );
1599
+ const auto object_type = utils::getHighestProbLabel (object.value (). object .classification );
1600
1600
const auto object_parameter = parameters_->object_parameters .at (object_type);
1601
- const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
1602
- object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
1603
- const auto shift_length =
1604
- helper_->getShiftLength (object, utils::avoidance::isOnRight (object), avoid_margin);
1601
+ const auto avoid_margin =
1602
+ object_parameter.safety_buffer_lateral * object.value ().distance_factor +
1603
+ object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
1604
+ const auto shift_length = helper_->getShiftLength (
1605
+ object.value (), utils::avoidance::isOnRight (object.value ()), avoid_margin);
1605
1606
1606
1607
// check slow down feasibility
1607
1608
const auto min_avoid_distance = helper_->getMinAvoidanceDistance (shift_length);
1608
- const auto distance_to_object = object.longitudinal ;
1609
+ const auto distance_to_object = object.value (). longitudinal ;
1609
1610
const auto remaining_distance = distance_to_object - min_avoid_distance;
1610
1611
const auto decel_distance = helper_->getFeasibleDecelDistance (parameters_->velocity_map .front ());
1611
1612
if (remaining_distance < decel_distance) {
1612
1613
return ;
1613
1614
}
1614
1615
1615
1616
// decide slow down lower limit.
1616
- const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed ;
1617
+ const auto lower_speed = object.value (). avoid_required ? 0.0 : parameters_->min_slow_down_speed ;
1617
1618
1618
1619
// insert slow down speed.
1619
1620
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk (
0 commit comments