Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(strat_planner): add a prepare time for blinker before taking action for approval #6438

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
backward_path_update_duration: 3.0
ignore_distance_from_lane_end: 15.0
# turns signal
prepare_time_before_start: 0.0
th_turn_signal_on_lateral_offset: 1.0
intersection_search_length: 30.0
length_ratio_for_turn_signal_deactivation_near_intersection: 0.5
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ struct StartPlannerParameters
double th_arrived_distance{0.0};
double th_stopped_velocity{0.0};
double th_stopped_time{0.0};
double prepare_time_before_start{0.0};
double th_turn_signal_on_lateral_offset{0.0};
double th_distance_to_middle_of_the_road{0.0};
double intersection_search_length{0.0};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,8 @@ struct PullOutStatus
bool prev_is_safe_dynamic_objects{false};
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
std::optional<Pose> stop_pose{std::nullopt};
//! record the first time when the state changed from !isActivated() to isActivated()
std::optional<rclcpp::Time> first_approved_time{std::nullopt};

PullOutStatus() {}
};
Expand Down Expand Up @@ -261,6 +263,7 @@ class StartPlannerModule : public SceneModuleInterface
const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose,
const double velocity_threshold, const double object_check_backward_distance,
const double object_check_forward_distance) const;
bool needToPrepareBlinkerBeforeStart() const;
bool hasFinishedPullOut() const;
bool hasFinishedBackwardDriving() const;
bool hasCollisionWithDynamicObjects() const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");

Check warning on line 39 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

StartPlannerModuleManager::init increases from 266 to 267 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

Check warning on line 39 in planning/behavior_path_start_planner_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/manager.cpp#L39

Added line #L39 was not covered by tests
p.th_turn_signal_on_lateral_offset =
node->declare_parameter<double>(ns + "th_turn_signal_on_lateral_offset");
p.th_distance_to_middle_of_the_road =
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1302 to 1314, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -117,7 +117,7 @@
BehaviorModuleOutput StartPlannerModule::run()
{
updateData();
if (!isActivated()) {
if (!isActivated() || needToPrepareBlinkerBeforeStart()) {

Check warning on line 120 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L120

Added line #L120 was not covered by tests
return planWaitingApproval();
}

Expand Down Expand Up @@ -176,6 +176,10 @@
DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
}

if (!status_.first_approved_time && isActivated()) {
status_.first_approved_time = clock_->now();

Check warning on line 180 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L179-L180

Added lines #L179 - L180 were not covered by tests
}

if (hasFinishedBackwardDriving()) {
updateStatusAfterBackwardDriving();
DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
Expand Down Expand Up @@ -1078,6 +1082,16 @@
return has_finished;
}

bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const

Check warning on line 1085 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1085

Added line #L1085 was not covered by tests
{
if (!status_.first_approved_time) {

Check warning on line 1087 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1087

Added line #L1087 was not covered by tests
return true;
}
const auto first_approved_time = status_.first_approved_time.value();
const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds();
return elapsed < parameters_->prepare_time_before_start;
}

Check warning on line 1093 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_start_planner_module/src/start_planner_module.cpp#L1090-L1093

Added lines #L1090 - L1093 were not covered by tests

bool StartPlannerModule::isStuck()
{
if (!isStopped()) {
Expand Down
Loading