Skip to content

Commit 47f7d3e

Browse files
committed
fix(avoidance): turn off blinker when the ego return original lane
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 4adc7d4 commit 47f7d3e

File tree

4 files changed

+89
-26
lines changed

4 files changed

+89
-26
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -404,6 +404,8 @@ class AvoidanceModule : public SceneModuleInterface
404404

405405
bool safe_{true};
406406

407+
std::optional<UUID> ignore_signal_{std::nullopt};
408+
407409
std::shared_ptr<AvoidanceHelper> helper_;
408410

409411
std::shared_ptr<AvoidanceParameters> parameters_;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -171,7 +171,7 @@ double calcDistanceToAvoidStartLine(
171171
const std::shared_ptr<const PlannerData> & planner_data,
172172
const std::shared_ptr<AvoidanceParameters> & parameters);
173173

174-
TurnSignalInfo calcTurnSignalInfo(
174+
std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
175175
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
176176
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data);
177177
} // namespace behavior_path_planner::utils::avoidance

planning/behavior_path_avoidance_module/src/scene.cpp

+16-1
Original file line numberDiff line numberDiff line change
@@ -892,19 +892,34 @@ BehaviorModuleOutput AvoidanceModule::plan()
892892

893893
BehaviorModuleOutput output;
894894

895+
const auto is_ignore_signal = [this](const UUID & uuid) {
896+
if (!ignore_signal_.has_value()) {
897+
return false;
898+
}
899+
900+
return ignore_signal_.value() == uuid;
901+
};
902+
903+
const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
904+
ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
905+
};
906+
895907
// turn signal info
896908
if (path_shifter_.getShiftLines().empty()) {
897909
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
910+
} else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) {
911+
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
898912
} else {
899913
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
900-
const auto new_signal = utils::avoidance::calcTurnSignalInfo(
914+
const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
901915
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
902916
planner_data_);
903917
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
904918
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
905919
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
906920
planner_data_->parameters.ego_nearest_dist_threshold,
907921
planner_data_->parameters.ego_nearest_yaw_threshold);
922+
update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore);
908923
}
909924

910925
// sparse resampling for computational cost

planning/behavior_path_avoidance_module/src/utils.cpp

+70-24
Original file line numberDiff line numberDiff line change
@@ -250,16 +250,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
250250
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
251251
}
252252

253+
bool isAvoidShift(const double start_shift_length, const double end_shift_length)
254+
{
255+
constexpr double THRESHOLD = 0.1;
256+
return std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
257+
}
258+
259+
bool isReturnShift(const double start_shift_length, const double end_shift_length)
260+
{
261+
constexpr double THRESHOLD = 0.1;
262+
return std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
263+
}
264+
265+
bool isLeftMiddleShift(const double start_shift_length, const double end_shift_length)
266+
{
267+
constexpr double THRESHOLD = 0.1;
268+
return start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
269+
}
270+
271+
bool isRightMiddleShift(const double start_shift_length, const double end_shift_length)
272+
{
273+
constexpr double THRESHOLD = 0.1;
274+
return start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
275+
}
276+
253277
bool existShiftSideLane(
254278
const double start_shift_length, const double end_shift_length, const bool no_left_lanes,
255279
const bool no_right_lanes)
256280
{
257-
constexpr double THRESHOLD = 0.1;
258281
const auto relative_shift_length = end_shift_length - start_shift_length;
259282

260-
const auto avoid_shift =
261-
std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
262-
if (avoid_shift) {
283+
if (isAvoidShift(start_shift_length, end_shift_length)) {
263284
// Left avoid. But there is no adjacent lane. No need blinker.
264285
if (relative_shift_length > 0.0 && no_left_lanes) {
265286
return false;
@@ -271,9 +292,7 @@ bool existShiftSideLane(
271292
}
272293
}
273294

274-
const auto return_shift =
275-
std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
276-
if (return_shift) {
295+
if (isReturnShift(start_shift_length, end_shift_length)) {
277296
// Right return. But there is no adjacent lane. No need blinker.
278297
if (relative_shift_length > 0.0 && no_right_lanes) {
279298
return false;
@@ -285,8 +304,7 @@ bool existShiftSideLane(
285304
}
286305
}
287306

288-
const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
289-
if (left_middle_shift) {
307+
if (isLeftMiddleShift(start_shift_length, end_shift_length)) {
290308
// Left avoid. But there is no adjacent lane. No need blinker.
291309
if (relative_shift_length > 0.0 && no_left_lanes) {
292310
return false;
@@ -298,8 +316,7 @@ bool existShiftSideLane(
298316
}
299317
}
300318

301-
const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
302-
if (right_middle_shift) {
319+
if (isRightMiddleShift(start_shift_length, end_shift_length)) {
303320
// Right avoid. But there is no adjacent lane. No need blinker.
304321
if (relative_shift_length < 0.0 && no_right_lanes) {
305322
return false;
@@ -314,6 +331,27 @@ bool existShiftSideLane(
314331
return true;
315332
}
316333

334+
bool isNearEndOfShift(
335+
const double start_shift_length, const double end_shift_length, const Point & ego_pos,
336+
const lanelet::ConstLanelets & original_lanes)
337+
{
338+
using boost::geometry::within;
339+
using lanelet::utils::to2D;
340+
using lanelet::utils::conversion::toLaneletPoint;
341+
342+
if (!isReturnShift(start_shift_length, end_shift_length)) {
343+
return false;
344+
}
345+
346+
for (const auto & lane : original_lanes) {
347+
if (within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon())) {
348+
return true;
349+
}
350+
}
351+
352+
return false;
353+
}
354+
317355
bool straddleRoadBound(
318356
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
319357
const vehicle_info_util::VehicleInfo & vehicle_info)
@@ -2286,7 +2324,7 @@ double calcDistanceToReturnDeadLine(
22862324
return distance_to_return_dead_line;
22872325
}
22882326

2289-
TurnSignalInfo calcTurnSignalInfo(
2327+
std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
22902328
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
22912329
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
22922330
{
@@ -2298,22 +2336,22 @@ TurnSignalInfo calcTurnSignalInfo(
22982336

22992337
if (shift_line.start_idx + 1 > path.shift_length.size()) {
23002338
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2301-
return {};
2339+
return std::make_pair(TurnSignalInfo{}, true);
23022340
}
23032341

23042342
if (shift_line.start_idx + 1 > path.path.points.size()) {
23052343
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2306-
return {};
2344+
return std::make_pair(TurnSignalInfo{}, true);
23072345
}
23082346

23092347
if (shift_line.end_idx + 1 > path.shift_length.size()) {
23102348
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2311-
return {};
2349+
return std::make_pair(TurnSignalInfo{}, true);
23122350
}
23132351

23142352
if (shift_line.end_idx + 1 > path.path.points.size()) {
23152353
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2316-
return {};
2354+
return std::make_pair(TurnSignalInfo{}, true);
23172355
}
23182356

23192357
const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
@@ -2322,12 +2360,12 @@ TurnSignalInfo calcTurnSignalInfo(
23222360

23232361
// If shift length is shorter than the threshold, it does not need to turn on blinkers
23242362
if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
2325-
return {};
2363+
return std::make_pair(TurnSignalInfo{}, true);
23262364
}
23272365

23282366
// If the vehicle does not shift anymore, we turn off the blinker
23292367
if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) {
2330-
return {};
2368+
return std::make_pair(TurnSignalInfo{}, true);
23312369
}
23322370

23332371
const auto get_command = [](const auto & shift_length) {
@@ -2342,7 +2380,7 @@ TurnSignalInfo calcTurnSignalInfo(
23422380
p.vehicle_info.max_longitudinal_offset_m;
23432381

23442382
if (signal_prepare_distance < ego_front_to_shift_start) {
2345-
return {};
2383+
return std::make_pair(TurnSignalInfo{}, false);
23462384
}
23472385

23482386
const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
@@ -2359,12 +2397,12 @@ TurnSignalInfo calcTurnSignalInfo(
23592397
turn_signal_info.turn_signal.command = get_command(relative_shift_length);
23602398

23612399
if (!p.turn_signal_on_swerving) {
2362-
return turn_signal_info;
2400+
return std::make_pair(turn_signal_info, false);
23632401
}
23642402

23652403
lanelet::ConstLanelet lanelet;
23662404
if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
2367-
return {};
2405+
return std::make_pair(TurnSignalInfo{}, true);
23682406
}
23692407

23702408
const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
@@ -2376,13 +2414,21 @@ TurnSignalInfo calcTurnSignalInfo(
23762414
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
23772415

23782416
if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) {
2379-
return {};
2417+
return std::make_pair(TurnSignalInfo{}, true);
23802418
}
23812419

23822420
if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
2383-
return {};
2421+
return std::make_pair(TurnSignalInfo{}, true);
2422+
}
2423+
2424+
constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
2425+
if (ego_speed < STOPPED_THRESHOLD) {
2426+
if (isNearEndOfShift(
2427+
start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets)) {
2428+
return std::make_pair(TurnSignalInfo{}, true);
2429+
}
23842430
}
23852431

2386-
return turn_signal_info;
2432+
return std::make_pair(turn_signal_info, false);
23872433
}
23882434
} // namespace behavior_path_planner::utils::avoidance

0 commit comments

Comments
 (0)