Skip to content

Commit a0b620d

Browse files
committed
fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 9b8d9b3 commit a0b620d

File tree

4 files changed

+29
-44
lines changed

4 files changed

+29
-44
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -213,9 +213,6 @@ struct AvoidanceParameters
213213
size_t hysteresis_factor_safe_count;
214214
double hysteresis_factor_expand_rate{0.0};
215215

216-
// keep target velocity in yield maneuver
217-
double yield_velocity{0.0};
218-
219216
// maximum stop distance
220217
double stop_max_distance{0.0};
221218

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

-6
Original file line numberDiff line numberDiff line change
@@ -282,12 +282,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
282282
getOrDeclareParameter<double>(*node, ns + "traffic_light.buffer");
283283
}
284284

285-
// yield
286-
{
287-
const std::string ns = "avoidance.yield.";
288-
p.yield_velocity = getOrDeclareParameter<double>(*node, ns + "yield_velocity");
289-
}
290-
291285
// stop
292286
{
293287
const std::string ns = "avoidance.stop.";

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

-7
Original file line numberDiff line numberDiff line change
@@ -233,13 +233,6 @@ class AvoidanceModule : public SceneModuleInterface
233233
*/
234234
void insertPrepareVelocity(ShiftedPath & shifted_path) const;
235235

236-
/**
237-
* @brief insert decel point in output path in order to yield. the ego decelerates within
238-
* accel/jerk constraints.
239-
* @param target path.
240-
*/
241-
void insertYieldVelocity(ShiftedPath & shifted_path) const;
242-
243236
/**
244237
* @brief calculate stop distance based on object's overhang.
245238
* @param stop distance.

planning/behavior_path_avoidance_module/src/scene.cpp

+29-28
Original file line numberDiff line numberDiff line change
@@ -660,7 +660,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
660660
break;
661661
}
662662
case AvoidanceState::YIELD: {
663-
insertYieldVelocity(path);
664663
insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
665664
break;
666665
}
@@ -1549,24 +1548,6 @@ void AvoidanceModule::insertStopPoint(
15491548
getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_);
15501549
}
15511550

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-
15701551
void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
15711552
{
15721553
const auto & data = avoid_data_;
@@ -1586,34 +1567,54 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
15861567
return;
15871568
}
15881569

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+
}
15901590

15911591
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;
15931593
if (!enough_space) {
15941594
return;
15951595
}
15961596

15971597
// calculate shift length for front object.
15981598
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);
16001600
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);
16051606

16061607
// check slow down feasibility
16071608
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;
16091610
const auto remaining_distance = distance_to_object - min_avoid_distance;
16101611
const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front());
16111612
if (remaining_distance < decel_distance) {
16121613
return;
16131614
}
16141615

16151616
// 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;
16171618

16181619
// insert slow down speed.
16191620
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(

0 commit comments

Comments
 (0)