Skip to content

Commit

Permalink
feat: REVERT "keep stop policy v0.11.2" (#1185)
Browse files Browse the repository at this point in the history
Revert "feat: keep stop policy v0.11.2 (#1125)"

This reverts commit 23d651f.
  • Loading branch information
saka1-s authored Mar 11, 2024
1 parent 23d651f commit 1535305
Show file tree
Hide file tree
Showing 4 changed files with 2 additions and 22 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -192,10 +192,6 @@
stop_buffer: 1.0 # [m]

policy:
# policy for rtc request. select "shift_line" or "maneuver".
# "shift_line": request approval for each shift line.
# "maneuver": request approval for avoidance maneuver (avoid + return).
approval: "shift_line"
# policy for vehicle slow down behavior. select "best_effort" or "reliable".
# "best_effort": slow down deceleration & jerk are limited by constraints.
# but there is a possibility that the vehicle can't stop in front of the vehicle.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -280,9 +280,6 @@ struct AvoidanceParameters
// policy
bool use_shorten_margin_immediately{false};

// policy
std::string policy_approval{"shift_line"};

// policy
std::string policy_deceleration{"best_effort"};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -85,11 +85,6 @@ bool isBestEffort(const std::string & policy)
{
return policy == "best_effort";
}

bool perManeuver(const std::string & policy)
{
return policy == "maneuver";
}
} // namespace

AvoidanceModule::AvoidanceModule(
Expand Down Expand Up @@ -2321,16 +2316,9 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat
break;
}

if (is_ignore_shift(candidate)) {
continue;
if (!is_ignore_shift(candidate)) {
return get_subsequent_shift(i);
}

if (perManeuver(parameters_->policy_approval)) {
return candidates;
}

const auto new_shift_lines = get_subsequent_shift(i);
return new_shift_lines;
}

DEBUG_PRINT("No new shift point exists.");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -218,7 +218,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
// policy
{
std::string ns = "avoidance.policy.";
p.policy_approval = getOrDeclareParameter<std::string>(*node, ns + "approval");
p.policy_deceleration = getOrDeclareParameter<std::string>(*node, ns + "deceleration");
p.policy_lateral_margin = getOrDeclareParameter<std::string>(*node, ns + "lateral_margin");
p.use_shorten_margin_immediately =
Expand Down

0 comments on commit 1535305

Please sign in to comment.