Skip to content

Commit 4f6e857

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 882494d commit 4f6e857

File tree

4 files changed

+93
-28
lines changed

4 files changed

+93
-28
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
@@ -166,7 +166,7 @@ double calcDistanceToAvoidStartLine(
166166
const std::shared_ptr<const PlannerData> & planner_data,
167167
const std::shared_ptr<AvoidanceParameters> & parameters);
168168

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

planning/behavior_path_avoidance_module/src/scene.cpp

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

882882
BehaviorModuleOutput output;
883883

884+
const auto is_ignore_signal = [this](const UUID & uuid) {
885+
if (!ignore_signal_.has_value()) {
886+
return false;
887+
}
888+
889+
return ignore_signal_.value() == uuid;
890+
};
891+
892+
const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
893+
ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
894+
};
895+
884896
// turn signal info
885897
if (path_shifter_.getShiftLines().empty()) {
886898
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
899+
} else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) {
900+
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
887901
} else {
888902
const auto original_signal = getPreviousModuleOutput().turn_signal_info;
889-
const auto new_signal = utils::avoidance::calcTurnSignalInfo(
903+
const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo(
890904
linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
891905
planner_data_);
892906
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
893907
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
894908
spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
895909
planner_data_->parameters.ego_nearest_dist_threshold,
896910
planner_data_->parameters.ego_nearest_yaw_threshold);
911+
update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore);
897912
}
898913

899914
// sparse resampling for computational cost

planning/behavior_path_avoidance_module/src/utils.cpp

+74-26
Original file line numberDiff line numberDiff line change
@@ -248,16 +248,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
248248
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
249249
}
250250

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

258-
const auto avoid_shift =
259-
std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD;
260-
if (avoid_shift) {
281+
if (isAvoidShift(start_shift_length, end_shift_length)) {
261282
// Left avoid. But there is no adjacent lane. No need blinker.
262283
if (relative_shift_length > 0.0 && no_left_lanes) {
263284
return false;
@@ -269,9 +290,7 @@ bool existShiftSideLane(
269290
}
270291
}
271292

272-
const auto return_shift =
273-
std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD;
274-
if (return_shift) {
293+
if (isReturnShift(start_shift_length, end_shift_length)) {
275294
// Right return. But there is no adjacent lane. No need blinker.
276295
if (relative_shift_length > 0.0 && no_right_lanes) {
277296
return false;
@@ -283,8 +302,7 @@ bool existShiftSideLane(
283302
}
284303
}
285304

286-
const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD;
287-
if (left_middle_shift) {
305+
if (isLeftMiddleShift(start_shift_length, end_shift_length)) {
288306
// Left avoid. But there is no adjacent lane. No need blinker.
289307
if (relative_shift_length > 0.0 && no_left_lanes) {
290308
return false;
@@ -296,8 +314,7 @@ bool existShiftSideLane(
296314
}
297315
}
298316

299-
const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD;
300-
if (right_middle_shift) {
317+
if (isRightMiddleShift(start_shift_length, end_shift_length)) {
301318
// Right avoid. But there is no adjacent lane. No need blinker.
302319
if (relative_shift_length < 0.0 && no_right_lanes) {
303320
return false;
@@ -312,6 +329,27 @@ bool existShiftSideLane(
312329
return true;
313330
}
314331

332+
bool isNearEndOfShift(
333+
const double start_shift_length, const double end_shift_length, const Point & ego_pos,
334+
const lanelet::ConstLanelets & original_lanes)
335+
{
336+
using boost::geometry::within;
337+
using lanelet::utils::to2D;
338+
using lanelet::utils::conversion::toLaneletPoint;
339+
340+
if (!isReturnShift(start_shift_length, end_shift_length)) {
341+
return false;
342+
}
343+
344+
for (const auto & lane : original_lanes) {
345+
if (within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon())) {
346+
return true;
347+
}
348+
}
349+
350+
return false;
351+
}
352+
315353
bool straddleRoadBound(
316354
const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes,
317355
const vehicle_info_util::VehicleInfo & vehicle_info)
@@ -1260,11 +1298,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
12601298
throw std::logic_error("empty path.");
12611299
}
12621300

1263-
if (path.points.front().lane_ids.empty()) {
1301+
const auto idx = planner_data->findEgoIndex(path.points);
1302+
1303+
if (path.points.at(idx).lane_ids.empty()) {
12641304
throw std::logic_error("empty lane ids.");
12651305
}
12661306

1267-
const auto start_id = path.points.front().lane_ids.front();
1307+
const auto start_id = path.points.at(idx).lane_ids.front();
12681308
const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id);
12691309
const auto & p = planner_data->parameters;
12701310

@@ -2224,7 +2264,7 @@ double calcDistanceToReturnDeadLine(
22242264
return distance_to_return_dead_line;
22252265
}
22262266

2227-
TurnSignalInfo calcTurnSignalInfo(
2267+
std::pair<TurnSignalInfo, bool> calcTurnSignalInfo(
22282268
const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length,
22292269
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data)
22302270
{
@@ -2236,22 +2276,22 @@ TurnSignalInfo calcTurnSignalInfo(
22362276

22372277
if (shift_line.start_idx + 1 > path.shift_length.size()) {
22382278
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2239-
return {};
2279+
return std::make_pair(TurnSignalInfo{}, true);
22402280
}
22412281

22422282
if (shift_line.start_idx + 1 > path.path.points.size()) {
22432283
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2244-
return {};
2284+
return std::make_pair(TurnSignalInfo{}, true);
22452285
}
22462286

22472287
if (shift_line.end_idx + 1 > path.shift_length.size()) {
22482288
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2249-
return {};
2289+
return std::make_pair(TurnSignalInfo{}, true);
22502290
}
22512291

22522292
if (shift_line.end_idx + 1 > path.path.points.size()) {
22532293
RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency.");
2254-
return {};
2294+
return std::make_pair(TurnSignalInfo{}, true);
22552295
}
22562296

22572297
const auto start_shift_length = path.shift_length.at(shift_line.start_idx);
@@ -2260,12 +2300,12 @@ TurnSignalInfo calcTurnSignalInfo(
22602300

22612301
// If shift length is shorter than the threshold, it does not need to turn on blinkers
22622302
if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
2263-
return {};
2303+
return std::make_pair(TurnSignalInfo{}, true);
22642304
}
22652305

22662306
// If the vehicle does not shift anymore, we turn off the blinker
22672307
if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) {
2268-
return {};
2308+
return std::make_pair(TurnSignalInfo{}, true);
22692309
}
22702310

22712311
const auto get_command = [](const auto & shift_length) {
@@ -2280,7 +2320,7 @@ TurnSignalInfo calcTurnSignalInfo(
22802320
p.vehicle_info.max_longitudinal_offset_m;
22812321

22822322
if (signal_prepare_distance < ego_front_to_shift_start) {
2283-
return {};
2323+
return std::make_pair(TurnSignalInfo{}, false);
22842324
}
22852325

22862326
const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
@@ -2297,12 +2337,12 @@ TurnSignalInfo calcTurnSignalInfo(
22972337
turn_signal_info.turn_signal.command = get_command(relative_shift_length);
22982338

22992339
if (!p.turn_signal_on_swerving) {
2300-
return turn_signal_info;
2340+
return std::make_pair(turn_signal_info, false);
23012341
}
23022342

23032343
lanelet::ConstLanelet lanelet;
23042344
if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) {
2305-
return {};
2345+
return std::make_pair(TurnSignalInfo{}, true);
23062346
}
23072347

23082348
const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
@@ -2314,13 +2354,21 @@ TurnSignalInfo calcTurnSignalInfo(
23142354
right_same_direction_lane.has_value() || !right_opposite_lanes.empty();
23152355

23162356
if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) {
2317-
return {};
2357+
return std::make_pair(TurnSignalInfo{}, true);
23182358
}
23192359

23202360
if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) {
2321-
return {};
2361+
return std::make_pair(TurnSignalInfo{}, true);
2362+
}
2363+
2364+
constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
2365+
if (ego_speed < STOPPED_THRESHOLD) {
2366+
if (isNearEndOfShift(
2367+
start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets)) {
2368+
return std::make_pair(TurnSignalInfo{}, true);
2369+
}
23222370
}
23232371

2324-
return turn_signal_info;
2372+
return std::make_pair(turn_signal_info, false);
23252373
}
23262374
} // namespace behavior_path_planner::utils::avoidance

0 commit comments

Comments
 (0)