@@ -687,7 +687,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
687
687
break ;
688
688
}
689
689
case AvoidanceState::YIELD: {
690
- insertYieldVelocity (path);
691
690
insertWaitPoint (isBestEffort (parameters_->policy_deceleration ), path);
692
691
break ;
693
692
}
@@ -1503,6 +1502,11 @@ void AvoidanceModule::insertWaitPoint(
1503
1502
{
1504
1503
const auto & data = avoid_data_;
1505
1504
1505
+ // If avoidance path is NOT valid, don't insert any stop points.
1506
+ if (!data.valid ) {
1507
+ return ;
1508
+ }
1509
+
1506
1510
if (!data.stop_target_object ) {
1507
1511
return ;
1508
1512
}
@@ -1589,28 +1593,20 @@ void AvoidanceModule::insertStopPoint(
1589
1593
getEgoPosition (), stop_distance - MARGIN, 0.0 , shifted_path.path , stop_pose_);
1590
1594
}
1591
1595
1592
- void AvoidanceModule::insertYieldVelocity (ShiftedPath & shifted_path) const
1596
+ void AvoidanceModule::insertPrepareVelocity (ShiftedPath & shifted_path) const
1593
1597
{
1594
- const auto & p = parameters_;
1595
1598
const auto & data = avoid_data_;
1596
1599
1597
- if (data.target_objects .empty ()) {
1600
+ // If avoidance path is NOT safe, don't insert any slow down sections.
1601
+ if (!data.safe && !data.stop_target_object ) {
1598
1602
return ;
1599
1603
}
1600
1604
1601
- if (helper_->isShifted ()) {
1605
+ // If avoidance path is NOT safe, don't insert any slow down sections.
1606
+ if (!data.valid ) {
1602
1607
return ;
1603
1608
}
1604
1609
1605
- const auto decel_distance = helper_->getFeasibleDecelDistance (p->yield_velocity , false );
1606
- utils::avoidance::insertDecelPoint (
1607
- getEgoPosition (), decel_distance, p->yield_velocity , shifted_path.path , slow_pose_);
1608
- }
1609
-
1610
- void AvoidanceModule::insertPrepareVelocity (ShiftedPath & shifted_path) const
1611
- {
1612
- const auto & data = avoid_data_;
1613
-
1614
1610
// do nothing if there is no avoidance target.
1615
1611
if (data.target_objects .empty ()) {
1616
1612
return ;
@@ -1626,34 +1622,53 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
1626
1622
return ;
1627
1623
}
1628
1624
1629
- const auto object = data.target_objects .front ();
1625
+ const auto itr = std::find_if (
1626
+ data.target_objects .begin (), data.target_objects .end (),
1627
+ [](const auto & o) { return o.avoid_required ; });
1628
+
1629
+ const auto object = [&]() -> std::optional<ObjectData> {
1630
+ if (!data.yield_required ) {
1631
+ return data.target_objects .front ();
1632
+ }
1633
+
1634
+ if (itr == data.target_objects .end ()) {
1635
+ return std::nullopt;
1636
+ }
1637
+
1638
+ return *itr;
1639
+ }();
1640
+
1641
+ // do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary.
1642
+ if (!object.has_value ()) {
1643
+ return ;
1644
+ }
1630
1645
1631
1646
const auto enough_space =
1632
- object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
1647
+ object.value (). is_avoidable || object. value () .reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
1633
1648
if (!enough_space) {
1634
1649
return ;
1635
1650
}
1636
1651
1637
1652
// calculate shift length for front object.
1638
1653
const auto & vehicle_width = planner_data_->parameters .vehicle_width ;
1639
- const auto object_type = utils::getHighestProbLabel (object.object .classification );
1654
+ const auto object_type = utils::getHighestProbLabel (object.value (). object .classification );
1640
1655
const auto object_parameter = parameters_->object_parameters .at (object_type);
1641
- const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
1656
+ const auto avoid_margin = object_parameter.lateral_hard_margin * object.value (). distance_factor +
1642
1657
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
1643
- const auto shift_length =
1644
- helper_-> getShiftLength ( object, utils::avoidance::isOnRight (object), avoid_margin);
1658
+ const auto shift_length = helper_-> getShiftLength (
1659
+ object. value () , utils::avoidance::isOnRight (object. value () ), avoid_margin);
1645
1660
1646
1661
// check slow down feasibility
1647
1662
const auto min_avoid_distance = helper_->getMinAvoidanceDistance (shift_length);
1648
- const auto distance_to_object = object.longitudinal ;
1663
+ const auto distance_to_object = object.value (). longitudinal ;
1649
1664
const auto remaining_distance = distance_to_object - min_avoid_distance;
1650
1665
const auto decel_distance = helper_->getFeasibleDecelDistance (parameters_->velocity_map .front ());
1651
1666
if (remaining_distance < decel_distance) {
1652
1667
return ;
1653
1668
}
1654
1669
1655
1670
// decide slow down lower limit.
1656
- const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed ;
1671
+ const auto lower_speed = object.value (). avoid_required ? 0.0 : parameters_->min_slow_down_speed ;
1657
1672
1658
1673
// insert slow down speed.
1659
1674
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk (
0 commit comments