diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index a2daafa395cd5..3fab5b7090ca3 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -26,7 +26,7 @@ common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takay
common/polar_grid/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
-common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
+common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners
common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp @autowarefoundation/autoware-global-codeowners
common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners
@@ -118,13 +118,28 @@ perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura
perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners
+perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/traffic_light_selector/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners
perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_speed_bump_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
index 0e09ca4482275..3d7479f54ed11 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -346,13 +346,6 @@ class AvoidanceModule : public SceneModuleInterface
AvoidLineArray applyPreProcessToRawShiftLines(
AvoidLineArray & current_raw_shift_points, DebugData & debug) const;
- /*
- * @brief fill gap between two shift lines.
- * @param original shift lines.
- * @return processed shift lines.
- */
- AvoidLineArray fillShiftLineGap(const AvoidLineArray & shift_lines) const;
-
/*
* @brief merge negative & positive shift lines.
* @param original shift lines.
@@ -392,15 +385,19 @@ class AvoidanceModule : public SceneModuleInterface
/*
* @brief add return shift line from ego position.
* @param shift lines which the return shift is added.
- * @param current shift lines.
* Pick up the last shift point, which is the most farthest from ego, from the current candidate
* avoidance points and registered points in the shifter. If the last shift length of the point is
* non-zero, add a return-shift to center line from the point. If there is no shift point in
* candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to
* center line from ego.
*/
- void addReturnShiftLineFromEgo(
- AvoidLineArray & sl_candidates, AvoidLineArray & current_raw_shift_lines) const;
+ void addReturnShiftLineFromEgo(AvoidLineArray & shift_lines) const;
+
+ /*
+ * @brief fill gap between two shift lines.
+ * @param original shift lines.
+ */
+ void fillShiftLineGap(AvoidLineArray & shift_lines) const;
/*
* @brief generate total shift line. total shift line has shift length and gradient array.
@@ -432,19 +429,6 @@ class AvoidanceModule : public SceneModuleInterface
*/
void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const;
- /**
- * @brief remove short "return to center" shift point. ¯¯\_/¯¯ -> ¯¯¯¯¯¯
- * @param input shift lines.
- * @details
- * Is the shift point for "return to center"?
- * - no : Do not trim anything.
- * - yes: Is it short distance enough to be removed?
- * - no : Do not trim anything.
- * - yes: Remove the "return" shift point.
- * Recalculate longitudinal distance and modify the shift point.
- */
- void trimMomentaryReturn(AvoidLineArray & shift_lines) const;
-
/*
* @brief trim invalid shift lines whose gradient it too large to follow.
* @param target shift lines.
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 2856a20e26a10..a702f4480a7dd 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -732,7 +732,7 @@ void AvoidanceModule::updateRegisteredRawShiftLines()
}
AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines(
- AvoidLineArray & current_raw_shift_lines, DebugData & debug) const
+ AvoidLineArray & raw_shift_lines, DebugData & debug) const
{
/**
* Use all registered points. For the current points, if the similar one of the current
@@ -741,12 +741,11 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines(
* shift, because it cannot handle the case like "we don't have to avoid
* the object anymore".
*/
- auto total_raw_shift_lines = utils::avoidance::combineRawShiftLinesWithUniqueCheck(
- registered_raw_shift_lines_, current_raw_shift_lines);
+ raw_shift_lines = utils::avoidance::combineRawShiftLinesWithUniqueCheck(
+ registered_raw_shift_lines_, raw_shift_lines);
- printShiftLines(current_raw_shift_lines, "current_raw_shift_lines");
+ printShiftLines(raw_shift_lines, "raw_shift_lines");
printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines");
- printShiftLines(total_raw_shift_lines, "total_raw_shift_lines");
/*
* Add return-to-center shift point from the last shift point, if needed.
@@ -758,11 +757,13 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines(
// It is temporally solved by changing the threshold of trimSimilarGrad, but it needs to be
// fixed in a proper way.
// Maybe after merge, all shift points before the prepare distance can be deleted.
- addReturnShiftLineFromEgo(total_raw_shift_lines, current_raw_shift_lines);
- printShiftLines(total_raw_shift_lines, "total_raw_shift_lines_with_extra_return_shift");
+ addReturnShiftLineFromEgo(raw_shift_lines);
- auto gap_filled = fillShiftLineGap(total_raw_shift_lines);
- debug.gap_filled = gap_filled;
+ /*
+ * Add gap filled shift lines so that merged shift lines connect smoothly.
+ */
+ fillShiftLineGap(raw_shift_lines);
+ debug.gap_filled = raw_shift_lines;
/**
* On each path point, compute shift length with considering the raw shift points.
@@ -773,7 +774,7 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines(
* Note: Because this function just foolishly extracts points, it includes
* insignificant small (useless) shift points, which should be removed in post-process.
*/
- auto merged_shift_lines = mergeShiftLines(gap_filled, debug);
+ auto merged_shift_lines = mergeShiftLines(raw_shift_lines, debug);
debug.merged = merged_shift_lines;
/*
@@ -815,28 +816,31 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future)
future_with_info.at(i).start_shift_length = future_with_info.at(i - 1).end_shift_length;
}
- const auto isAlreadyRegistered = [this](const auto id) {
+ const auto is_registered = [this](const auto id) {
const auto & r = registered_raw_shift_lines_;
- return std::any_of(r.begin(), r.end(), [id](const auto & r_sl) { return r_sl.id == id; });
+ return std::any_of(r.begin(), r.end(), [id](const auto & s) { return s.id == id; });
};
- const auto getAvoidLineByID = [this](const auto id) {
- for (const auto & sl : current_raw_shift_lines_) {
- if (sl.id == id) {
- return sl;
- }
+ const auto same_id_shift_line = [this](const auto id) {
+ const auto & r = current_raw_shift_lines_;
+ const auto itr = std::find_if(r.begin(), r.end(), [id](const auto & s) { return s.id == id; });
+ if (itr != r.end()) {
+ return *itr;
}
- return AvoidLine{};
+ throw std::logic_error("not found same id current raw shift line.");
};
- for (const auto & al : future_with_info) {
- if (al.parent_ids.empty()) {
+ for (const auto & s : future_with_info) {
+ if (s.parent_ids.empty()) {
RCLCPP_ERROR(getLogger(), "avoid line for path_shifter must have parent_id.");
}
- for (const auto parent_id : al.parent_ids) {
- if (!isAlreadyRegistered(parent_id)) {
- registered_raw_shift_lines_.push_back(getAvoidLineByID(parent_id));
+
+ for (const auto id : s.parent_ids) {
+ if (is_registered(id)) {
+ continue;
}
+
+ registered_raw_shift_lines_.push_back(same_id_shift_line(id));
}
}
@@ -1196,23 +1200,19 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_
return merged_avoid_lines;
}
-AvoidLineArray AvoidanceModule::fillShiftLineGap(const AvoidLineArray & input) const
+void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const
{
using utils::avoidance::setEndData;
- if (input.empty()) {
- return input;
+ if (shift_lines.empty()) {
+ return;
}
const auto & data = avoidance_data_;
- AvoidLineArray sorted_input = input;
-
- helper_.alignShiftLinesOrder(sorted_input, false);
+ helper_.alignShiftLinesOrder(shift_lines, false);
- AvoidLineArray output = sorted_input;
-
- const auto fill_gap = [&output, this](const auto & front_line, const auto & back_line) {
+ const auto fill_gap = [&shift_lines, this](const auto & front_line, const auto & back_line) {
const auto has_gap = back_line.start_longitudinal - front_line.end_longitudinal > 0.0;
if (!has_gap) {
return;
@@ -1225,7 +1225,7 @@ AvoidLineArray AvoidanceModule::fillShiftLineGap(const AvoidLineArray & input) c
new_line.end_longitudinal = back_line.start_longitudinal;
new_line.id = getOriginalShiftLineUniqueId();
- output.push_back(new_line);
+ shift_lines.push_back(new_line);
};
// fill gap between ego and nearest shift line.
@@ -1234,19 +1234,17 @@ AvoidLineArray AvoidanceModule::fillShiftLineGap(const AvoidLineArray & input) c
setEndData(
ego_line, helper_.getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0);
- fill_gap(ego_line, sorted_input.front());
+ fill_gap(ego_line, shift_lines.front());
}
// fill gap among shift lines.
- for (size_t i = 0; i < sorted_input.size() - 1; ++i) {
- fill_gap(sorted_input.at(i), sorted_input.at(i + 1));
+ for (size_t i = 0; i < shift_lines.size() - 1; ++i) {
+ fill_gap(shift_lines.at(i), shift_lines.at(i + 1));
}
- utils::avoidance::fillAdditionalInfoFromLongitudinal(data, output);
-
- helper_.alignShiftLinesOrder(output, false);
+ utils::avoidance::fillAdditionalInfoFromLongitudinal(data, shift_lines);
- return output;
+ helper_.alignShiftLinesOrder(shift_lines, false);
}
AvoidLineArray AvoidanceModule::mergeShiftLines(
@@ -1476,147 +1474,6 @@ void AvoidanceModule::trimSimilarGradShiftLine(
DEBUG_PRINT("size %lu -> %lu", input.size(), avoid_lines.size());
}
-void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const
-{
- const auto isZero = [](double v) { return std::abs(v) < 1.0e-5; };
-
- AvoidLineArray shift_lines_orig = shift_lines;
- shift_lines.clear();
-
- const double DISTANCE_AFTER_RETURN_THR = 5.0 * helper_.getNominalAvoidanceEgoSpeed();
-
- const auto & arclength = avoidance_data_.arclength_from_ego;
-
- const auto check_reduce_shift = [](const double now_length, const double prev_length) {
- const auto abs_shift_diff = std::abs(now_length) - std::abs(prev_length);
- const auto has_same_sign = (now_length * prev_length >= 0.0);
- const bool is_reduce_shift = (abs_shift_diff < 0.0 && has_same_sign);
- return is_reduce_shift;
- };
-
- for (size_t i = 0; i < shift_lines_orig.size(); ++i) {
- const auto sl_now = shift_lines_orig.at(i);
- const auto sl_prev_length =
- shift_lines.empty() ? helper_.getEgoLinearShift() : shift_lines.back().end_shift_length;
- const auto abs_shift_diff = std::abs(sl_now.end_shift_length) - std::abs(sl_prev_length);
- const bool is_reduce_shift = check_reduce_shift(sl_now.end_shift_length, sl_prev_length);
-
- // Do nothing for non-reduce shift point
- if (!is_reduce_shift) {
- shift_lines.push_back(sl_now);
- DEBUG_PRINT(
- "i = %lu, not reduce shift. take this one.abs_shift_diff = %f, sl_now.length = %f, "
- "sl_prev_length = %f, sl_now.length * sl_prev_length = %f",
- i, abs_shift_diff, sl_now.end_shift_length, sl_prev_length,
- sl_now.end_shift_length * sl_prev_length);
- continue;
- }
-
- // The last point is out of target of this function.
- const bool is_last_sl = (i == shift_lines_orig.size() - 1);
- if (is_last_sl) {
- shift_lines.push_back(sl_now);
- DEBUG_PRINT("i = %lu, last shift. take this one.", i);
- continue;
- }
-
- // --- From here, the shift point is "return to center" or "straight". ---
- // -----------------------------------------------------------------------
-
- const auto sl_next = shift_lines_orig.at(i + 1);
-
- // there is no straight interval, combine them. ¯¯\/¯¯ -> ¯¯¯¯¯¯
- if (!isZero(sl_next.getRelativeLength())) {
- DEBUG_PRINT(
- "i = %lu, return-shift is detected, next shift_diff (%f) is nonzero. combine them. (skip "
- "next shift).",
- i, sl_next.getRelativeLength());
- auto sl_modified = sl_next;
- utils::avoidance::setStartData(
- sl_modified, sl_now.end_shift_length, sl_now.start, sl_now.start_idx,
- sl_now.start_longitudinal);
- sl_modified.parent_ids =
- utils::avoidance::concatParentIds(sl_modified.parent_ids, sl_now.parent_ids);
- shift_lines.push_back(sl_modified);
- ++i; // skip next shift point
- continue;
- }
-
- // Find next shifting point, i.e. ¯¯\____"/"¯¯
- // now ↑ ↑ target
- const auto next_avoid_idx = [&]() {
- for (size_t j = i + 1; j < shift_lines_orig.size(); ++j) {
- if (!isZero(shift_lines_orig.at(j).getRelativeLength())) {
- return j;
- }
- }
- return shift_lines_orig.size();
- }();
-
- // The straight distance lasts until end. take this one.
- // ¯¯\______
- if (next_avoid_idx == shift_lines_orig.size()) {
- shift_lines.push_back(sl_now);
- DEBUG_PRINT("i = %lu, back -> straight lasts until end. take this one.", i);
- continue;
- }
-
- const auto sl_next_avoid = shift_lines_orig.at(next_avoid_idx);
- const auto straight_distance = sl_next_avoid.start_longitudinal - sl_now.end_longitudinal;
-
- // The straight distance after "return to center" is long enough. take this one.
- // ¯¯\______/¯¯ (enough long straight line!)
- if (straight_distance > DISTANCE_AFTER_RETURN_THR) {
- shift_lines.push_back(sl_now);
- DEBUG_PRINT("i = %lu, back -> straight: distance is long. take this one", i);
- continue;
- }
-
- // From here, back to center and go straight, straight distance is too short.
- // ¯¯\______/¯¯ (short straight line!)
-
- const auto relative_shift = sl_next_avoid.end_shift_length - sl_now.end_shift_length;
- const auto avoid_distance = helper_.getNominalAvoidanceDistance(relative_shift);
-
- // Calculate start point from end point and avoidance distance.
- auto sl_next_modified = sl_next_avoid;
- sl_next_modified.start_shift_length = sl_prev_length;
- sl_next_modified.start_longitudinal =
- std::max(sl_next_avoid.end_longitudinal - avoid_distance, sl_now.start_longitudinal);
- sl_next_modified.start_idx =
- utils::avoidance::findPathIndexFromArclength(arclength, sl_next_modified.start_longitudinal);
- sl_next_modified.start =
- avoidance_data_.reference_path.points.at(sl_next_modified.start_idx).point.pose;
- sl_next_modified.parent_ids =
- utils::avoidance::calcParentIds(current_raw_shift_lines_, sl_next_modified);
-
- // Straight shift point
- if (sl_next_modified.start_idx > sl_now.start_idx) { // the case where a straight route exists.
- auto sl_now_modified = sl_now;
- sl_now_modified.start_shift_length = sl_prev_length;
- utils::avoidance::setEndData(
- sl_now_modified, sl_prev_length, sl_next_modified.start, sl_next_modified.start_idx,
- sl_next_modified.start_longitudinal);
- sl_now_modified.parent_ids =
- utils::avoidance::calcParentIds(current_raw_shift_lines_, sl_now_modified);
- shift_lines.push_back(sl_now_modified);
- }
- shift_lines.push_back(sl_next_modified);
-
- DEBUG_PRINT(
- "i = %lu, find remove target!: next_avoid_idx = %lu, shift length = (now: %f, prev: %f, "
- "next_avoid: %f, next_mod: %f).",
- i, next_avoid_idx, sl_now.end_shift_length, sl_prev_length, sl_next_avoid.end_shift_length,
- sl_next_modified.end_shift_length);
-
- i = next_avoid_idx; // skip shifting until next_avoid_idx.
- }
-
- helper_.alignShiftLinesOrder(shift_lines);
-
- DEBUG_PRINT("trimMomentaryReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size());
-}
-
void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const
{
AvoidLineArray shift_lines_orig = shift_lines;
@@ -1803,8 +1660,7 @@ void AvoidanceModule::trimTooSharpShift(AvoidLineArray & avoid_lines) const
DEBUG_PRINT("size %lu -> %lu", avoid_lines_orig.size(), avoid_lines.size());
}
-void AvoidanceModule::addReturnShiftLineFromEgo(
- AvoidLineArray & sl_candidates, AvoidLineArray & current_raw_shift_lines) const
+void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const
{
constexpr double ep = 1.0e-3;
const auto & data = avoidance_data_;
@@ -1974,9 +1830,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo(
sl_candidates.push_back(al);
printShiftLines(AvoidLineArray{al}, "prepare for return");
debug_data_.extra_return_shift.push_back(al);
-
- // TODO(Horibe) think how to store the current object
- current_raw_shift_lines.push_back(al);
}
// shift point for return to center line
@@ -1996,9 +1849,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo(
sl_candidates.push_back(al);
printShiftLines(AvoidLineArray{al}, "return point");
debug_data_.extra_return_shift.push_back(al);
-
- // TODO(Horibe) think how to store the current object
- current_raw_shift_lines.push_back(al);
}
DEBUG_PRINT("Return Shift is added.");
diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
index e9579b8cd9c7a..d478fca57f4b4 100644
--- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -796,7 +796,14 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
// pull out path does not overlap
const double distance_from_end =
motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
- const double lateral_offset = inverseTransformPoint(end_pose.position, start_pose).y;
+
+ if (path.points.empty()) {
+ return {};
+ }
+ const auto closest_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
+ const auto lane_id = path.points.at(closest_idx).lane_ids.front();
+ const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id);
+ const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose);
if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) {
turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml
index 2f10b111c043e..3dd16f2fd792a 100644
--- a/planning/behavior_velocity_speed_bump_module/package.xml
+++ b/planning/behavior_velocity_speed_bump_module/package.xml
@@ -7,6 +7,7 @@
Tomoya Kimura
Shumpei Wakabayashi
+ Mehmet Dogru
Apache License 2.0
diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp
index 771f6ba014bba..62e8cd31ec062 100644
--- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp
+++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp
@@ -101,7 +101,7 @@ motion_utils::VirtualWalls SpeedBumpModule::createVirtualWalls()
wall.ns = std::to_string(module_id_) + "_";
wall.style = motion_utils::VirtualWallType::slowdown;
for (const auto & p : debug_data_.slow_start_poses) {
- wall.pose = p;
+ wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
virtual_walls.push_back(wall);
}
return virtual_walls;