Skip to content

Commit 0992f1d

Browse files
mkqudaawf-autoware-bot[bot]github-actionsestevepre-commit-ci[bot]
authored
feat(lane_change): reduce prepare duration when blinker has been activated (#9185)
* add minimum prepare duration parameter Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * reduce prepare duration according to signal activation time Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * chore: update CODEOWNERS (#9203) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions <github-actions@github.com> * refactor(time_utils): prefix package and namespace with autoware (#9173) * refactor(time_utils): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp> * refactor(time_utils): prefix package and namespace with autoware Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(rtc_interface): add requested field (#9202) * add requested feature Signed-off-by: Go Sakayori <gsakayori@gmail.com> * Update planning/autoware_rtc_interface/test/test_rtc_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Go Sakayori <gsakayori@gmail.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix(mpc_lateral_controller): correctly resample the MPC trajectory yaws (#9199) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * fix(bpp): prevent accessing nullopt (#9204) fix(bpp): calcDistanceToRedTrafficLight null Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp> * refactor(autoware_map_based_prediction): split pedestrian and bicycle predictor (#9201) * refactor: grouping functions Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: grouping parameters Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: rename member road_users_history to road_users_history_ Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: separate util functions Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: Add predictor_vru.cpp and utils.cpp to map_based_prediction_node Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: Add explicit template instantiation for removeOldObjectsHistory function Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: Add tf2_geometry_msgs to data_structure Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: Remove unused variables and functions in map_based_prediction_node.cpp Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Update perception/autoware_map_based_prediction/include/map_based_prediction/predictor_vru.hpp * Apply suggestions from code review * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: Mamoru Sobue <hilo.soblin@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * refactor(ndt_scan_matcher, ndt_omp): move ndt_omp into ndt_scan_matcher (#8912) * Moved ndt_omp into ndt_scan_matcher Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added Copyright Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed cast style Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed include Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed honorific title Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed honorific title Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include hierarchy Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed hierarchy Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed NVTP to NVTL Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added cspell:ignore Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed miss spell Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Renamed applyFilter Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Moved ***_impl.hpp from include/ to src/ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed variable scope Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to pass by reference Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(autoware_test_utils): add traffic light msgs parser (#9177) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * modify implementation to compute and keep actual prepare duration in transient data Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * if LC path is approved, set prepare duration in transient data from approved path prepare duration Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * change default value of LC param min_prepare_duration Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * add function to set signal activation time, add docstring for function calc_actual_prepare_duration Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * check for zero value max_acc to avoid division by zero Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * chore: rename codeowners file Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * chore: rename codeowners file Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * chore: rename codeowners file Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix spelling Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix units Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * run pre-commit check Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix spelling Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix format Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * run pre-commit check Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix spelling Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix format Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> --------- Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Signed-off-by: Esteve Fernandez <esteve.fernandez@tier4.jp> Signed-off-by: Go Sakayori <gsakayori@gmail.com> Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp> Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: awf-autoware-bot[bot] <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Co-authored-by: github-actions <github-actions@github.com> Co-authored-by: Esteve Fernandez <33620+esteve@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Go Sakayori <go-sakayori@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Taekjin LEE <taekjin.lee@tier4.jp> Co-authored-by: Mamoru Sobue <hilo.soblin@gmail.com> Co-authored-by: SakodaShintaro <shintaro.sakoda@tier4.jp> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>
1 parent 87e95bf commit 0992f1d

File tree

13 files changed

+104
-26
lines changed

13 files changed

+104
-26
lines changed

‎.github/CODEOWNERS ‎.github/_CODEOWNERS

File renamed without changes.

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

+2-1
Original file line numberDiff line numberDiff line change
@@ -886,7 +886,8 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
886886
| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 |
887887
| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 |
888888
| `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 |
889-
| `trajectory.prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 |
889+
| `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 |
890+
| `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 |
890891
| `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 |
891892
| `trajectory.minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 |
892893
| `trajectory.lon_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 |

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

+2-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,8 @@
1111

1212
# trajectory generation
1313
trajectory:
14-
prepare_duration: 4.0
14+
max_prepare_duration: 4.0
15+
min_prepare_duration: 2.0
1516
lateral_jerk: 0.5
1617
min_longitudinal_acc: -1.0
1718
max_longitudinal_acc: 1.0

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ class NormalLaneChange : public LaneChangeBase
5454

5555
void update_lanes(const bool is_approved) final;
5656

57-
void update_transient_data() final;
57+
void update_transient_data(const bool is_approved) final;
5858

5959
void update_filtered_objects() final;
6060

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ class LaneChangeBase
6868

6969
virtual void update_lanes(const bool is_approved) = 0;
7070

71-
virtual void update_transient_data() = 0;
71+
virtual void update_transient_data(const bool is_approved) = 0;
7272

7373
virtual void update_filtered_objects() = 0;
7474

@@ -267,6 +267,15 @@ class LaneChangeBase
267267
return turn_signal;
268268
}
269269

270+
void set_signal_activation_time(const bool reset = false) const
271+
{
272+
if (reset) {
273+
signal_activation_time_ = std::nullopt;
274+
} else if (!signal_activation_time_) {
275+
signal_activation_time_ = clock_.now();
276+
}
277+
}
278+
270279
LaneChangeStatus status_{};
271280
PathShifter path_shifter_{};
272281

@@ -292,6 +301,7 @@ class LaneChangeBase
292301

293302
mutable StopWatch<std::chrono::milliseconds> stop_watch_;
294303
mutable lane_change::Debug lane_change_debug_;
304+
mutable std::optional<rclcpp::Time> signal_activation_time_{std::nullopt};
295305

296306
rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr());
297307
mutable rclcpp::Clock clock_{RCL_ROS_TIME};

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp

+19-1
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ double calc_dist_to_last_fit_width(
7575
* of the maximum lane change preparation duration and the maximum velocity of the ego vehicle.
7676
*
7777
* @param common_data_ptr Shared pointer to a CommonData structure, which should include:
78-
* - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change
78+
* - `lc_param_ptr->maximum_prepare_duration`: The maximum duration allowed for lane change
7979
* preparation.
8080
* - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle.
8181
*
@@ -131,6 +131,24 @@ std::vector<double> calc_lon_acceleration_samples(
131131
const CommonDataPtr & common_data_ptr, const double max_path_velocity,
132132
const double prepare_duration);
133133

134+
/**
135+
* @brief calculates the actual prepare duration that will be used for path generation
136+
*
137+
* @details computes the required prepare duration to be used for candidate path
138+
* generation based on the elapsed time of turn signal activation, the minimum
139+
* and maximum parameterized values for the prepare duration,
140+
* and the minimum lane changing velocity.
141+
*
142+
* @param common_data_ptr Shared pointer to a CommonData structure, which includes
143+
* lane change parameters and bpp common parameters.
144+
* @param current_velocity current ego vehicle velocity.
145+
* @param active_signal_duration elapsed time since turn signal activation.
146+
* @return The calculated prepare duration value in seconds (s)
147+
*/
148+
double calc_actual_prepare_duration(
149+
const CommonDataPtr & common_data_ptr, const double current_velocity,
150+
const double active_signal_duration);
151+
134152
std::vector<PhaseMetrics> calc_prepare_phase_metrics(
135153
const CommonDataPtr & common_data_ptr, const double current_velocity,
136154
const double max_path_velocity, const double min_length_threshold = 0.0,

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -222,6 +222,8 @@ struct TransientData
222222
size_t current_path_seg_idx; // index of nearest segment to ego along current path
223223
double current_path_velocity; // velocity of the current path at the ego position along the path
224224

225+
double lane_change_prepare_duration{0.0};
226+
225227
bool is_ego_near_current_terminal_start{false};
226228
bool is_ego_stuck{false};
227229

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,8 @@ struct SafetyParameters
115115

116116
struct TrajectoryParameters
117117
{
118-
double prepare_duration{4.0};
118+
double max_prepare_duration{4.0};
119+
double min_prepare_duration{1.0};
119120
double lateral_jerk{0.5};
120121
double min_longitudinal_acc{-1.0};
121122
double max_longitudinal_acc{1.0};

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ void LaneChangeInterface::updateData()
8080
module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
8181
module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
8282
module_type_->update_filtered_objects();
83-
module_type_->update_transient_data();
83+
module_type_->update_transient_data(getCurrentStatus() == ModuleStatus::RUNNING);
8484
module_type_->updateSpecialData();
8585

8686
if (isWaitingApproval() || module_type_->isAbortState()) {
@@ -155,10 +155,10 @@ BehaviorModuleOutput LaneChangeInterface::plan()
155155

156156
BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
157157
{
158-
*prev_approved_path_ = getPreviousModuleOutput().path;
159-
160158
BehaviorModuleOutput out = getPreviousModuleOutput();
161159

160+
*prev_approved_path_ = out.path;
161+
162162
module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path);
163163
out.turn_signal_info = module_type_->get_current_turn_signal_info();
164164

@@ -168,7 +168,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
168168
setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
169169
}
170170

171-
path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
171+
path_reference_ = std::make_shared<PathWithLaneId>(out.reference_path);
172172
stop_pose_ = module_type_->getStopPose();
173173

174174
if (!module_type_->isValidPath()) {

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

+10-5
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,10 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
4545

4646
// trajectory generation
4747
{
48-
p.trajectory.prepare_duration =
49-
getOrDeclareParameter<double>(*node, parameter("trajectory.prepare_duration"));
48+
p.trajectory.max_prepare_duration =
49+
getOrDeclareParameter<double>(*node, parameter("trajectory.max_prepare_duration"));
50+
p.trajectory.min_prepare_duration =
51+
getOrDeclareParameter<double>(*node, parameter("trajectory.min_prepare_duration"));
5052
p.trajectory.lateral_jerk =
5153
getOrDeclareParameter<double>(*node, parameter("trajectory.lateral_jerk"));
5254
p.trajectory.min_longitudinal_acc =
@@ -67,8 +69,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
6769
getOrDeclareParameter<int>(*node, parameter("trajectory.lat_acc_sampling_num"));
6870

6971
const auto max_acc = getOrDeclareParameter<double>(*node, "normal.max_acc");
70-
p.trajectory.min_lane_changing_velocity =
71-
std::min(p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.prepare_duration);
72+
p.trajectory.min_lane_changing_velocity = std::min(
73+
p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration);
7274

7375
// validation of trajectory parameters
7476
if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) {
@@ -311,7 +313,10 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
311313

312314
{
313315
const std::string ns = "lane_change.trajectory.";
314-
updateParam<double>(parameters, ns + "prepare_duration", p->trajectory.prepare_duration);
316+
updateParam<double>(
317+
parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration);
318+
updateParam<double>(
319+
parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration);
315320
updateParam<double>(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk);
316321
updateParam<double>(
317322
parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity);

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

+22-5
Original file line numberDiff line numberDiff line change
@@ -131,7 +131,7 @@ void NormalLaneChange::update_lanes(const bool is_approved)
131131
*common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_);
132132
}
133133

134-
void NormalLaneChange::update_transient_data()
134+
void NormalLaneChange::update_transient_data(const bool is_approved)
135135
{
136136
if (
137137
!common_data_ptr_ || !common_data_ptr_->is_data_available() ||
@@ -150,6 +150,13 @@ void NormalLaneChange::update_transient_data()
150150
prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps;
151151
transient_data.current_path_seg_idx = nearest_seg_idx;
152152

153+
const auto active_signal_duration =
154+
signal_activation_time_ ? (clock_.now() - signal_activation_time_.value()).seconds() : 0.0;
155+
transient_data.lane_change_prepare_duration =
156+
is_approved ? status_.lane_change_path.info.duration.prepare
157+
: calculation::calc_actual_prepare_duration(
158+
common_data_ptr_, common_data_ptr_->get_ego_speed(), active_signal_duration);
159+
153160
std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) =
154161
calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes());
155162

@@ -328,6 +335,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const
328335
return get_terminal_turn_signal_info();
329336
}
330337

338+
set_signal_activation_time();
339+
331340
return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end);
332341
}
333342

@@ -356,9 +365,14 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const
356365
const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
357366
const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx;
358367

359-
return getTurnSignalDecider().overwrite_turn_signal(
368+
const auto turn_signal_info = getTurnSignalDecider().overwrite_turn_signal(
360369
path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
361370
terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
371+
372+
set_signal_activation_time(
373+
turn_signal_info.turn_signal.command != terminal_turn_signal_info.turn_signal.command);
374+
375+
return turn_signal_info;
362376
}
363377

364378
LaneChangePath NormalLaneChange::getLaneChangePath() const
@@ -429,8 +443,6 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
429443
output.path = utils::combinePath(output.path, *found_extended_path);
430444
}
431445
output.reference_path = getReferencePath();
432-
output.turn_signal_info =
433-
get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end);
434446

435447
if (isStopState()) {
436448
const auto current_velocity = getEgoVelocity();
@@ -446,12 +458,17 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
446458

447459
extendOutputDrivableArea(output);
448460

461+
const auto turn_signal_info =
462+
get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end);
449463
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
450464
output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
451465
output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info,
452-
output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
466+
turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
453467
planner_data_->parameters.ego_nearest_yaw_threshold);
454468

469+
set_signal_activation_time(
470+
output.turn_signal_info.turn_signal.command != turn_signal_info.turn_signal.command);
471+
455472
return output;
456473
}
457474

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp

+27-4
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ double calc_dist_to_last_fit_width(
104104

105105
double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr)
106106
{
107-
const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.prepare_duration;
107+
const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration;
108108
const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel;
109109

110110
return max_prepare_duration * ego_max_speed;
@@ -197,7 +197,7 @@ std::vector<double> calc_max_lane_change_lengths(
197197

198198
const auto & params = common_data_ptr->lc_param_ptr->trajectory;
199199
const auto lat_jerk = params.lateral_jerk;
200-
const auto t_prepare = params.prepare_duration;
200+
const auto t_prepare = params.max_prepare_duration;
201201
const auto current_velocity = common_data_ptr->get_ego_speed();
202202
const auto path_velocity = common_data_ptr->transient_data.current_path_velocity;
203203

@@ -400,18 +400,41 @@ double calc_lane_changing_acceleration(
400400
prepare_longitudinal_acc);
401401
}
402402

403+
double calc_actual_prepare_duration(
404+
const CommonDataPtr & common_data_ptr, const double current_velocity,
405+
const double active_signal_duration)
406+
{
407+
const auto & params = common_data_ptr->lc_param_ptr->trajectory;
408+
const auto min_lc_velocity = params.min_lane_changing_velocity;
409+
410+
// need to ensure min prep duration is sufficient to reach minimum lane changing velocity
411+
const auto min_prepare_duration = std::invoke([&]() -> double {
412+
if (current_velocity >= min_lc_velocity) {
413+
return params.min_prepare_duration;
414+
}
415+
const auto max_acc =
416+
std::min(common_data_ptr->bpp_param_ptr->max_acc, params.max_longitudinal_acc);
417+
if (max_acc < eps) {
418+
return params.max_prepare_duration;
419+
}
420+
return (min_lc_velocity - current_velocity) / max_acc;
421+
});
422+
423+
return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration);
424+
}
425+
403426
std::vector<double> calc_prepare_durations(const CommonDataPtr & common_data_ptr)
404427
{
405428
const auto & lc_param_ptr = common_data_ptr->lc_param_ptr;
406429
const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front +
407430
lc_param_ptr->min_length_for_turn_signal_activation;
408-
const auto max_prepare_duration = lc_param_ptr->trajectory.prepare_duration;
409431

410432
// TODO(Azu) this check seems to cause scenario failures.
411433
if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) {
412-
return {max_prepare_duration};
434+
return {common_data_ptr->transient_data.lane_change_prepare_duration};
413435
}
414436

437+
const auto max_prepare_duration = lc_param_ptr->trajectory.max_prepare_duration;
415438
std::vector<double> prepare_durations;
416439
constexpr double step = 0.5;
417440

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -223,7 +223,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid)
223223
constexpr auto is_approved = true;
224224
normal_lane_change_->update_lanes(!is_approved);
225225
normal_lane_change_->update_filtered_objects();
226-
normal_lane_change_->update_transient_data();
226+
normal_lane_change_->update_transient_data(!is_approved);
227227
normal_lane_change_->updateLaneChangeStatus();
228228
const auto & lc_status = normal_lane_change_->getLaneChangeStatus();
229229

@@ -261,7 +261,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid)
261261
set_previous_approved_path();
262262
normal_lane_change_->update_lanes(!is_approved);
263263
normal_lane_change_->update_filtered_objects();
264-
normal_lane_change_->update_transient_data();
264+
normal_lane_change_->update_transient_data(!is_approved);
265265
const auto lane_change_required = normal_lane_change_->isLaneChangeRequired();
266266

267267
ASSERT_TRUE(lane_change_required);

0 commit comments

Comments
 (0)