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

fix(lane_change): fix dynamic reconfigure and add parameters #6276

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
3 changes: 1 addition & 2 deletions planning/behavior_path_lane_change_module/src/interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -299,11 +299,10 @@ bool LaneChangeInterface::canTransitFailureState()

void LaneChangeInterface::updateDebugMarker() const
{
debug_marker_.markers.clear();
if (!parameters_->publish_debug_marker) {
return;
}

debug_marker_.markers.clear();
using marker_utils::lane_change_markers::createDebugMarkerArray;
debug_marker_ = createDebugMarkerArray(module_type_->getDebugData());
}
Expand Down
100 changes: 97 additions & 3 deletions planning/behavior_path_lane_change_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -279,10 +279,104 @@

auto p = parameters_;

const std::string ns = name_ + ".";
updateParam<double>(
parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold);
{
const std::string ns = "lane_change.";
updateParam<double>(parameters, ns + "backward_lane_length", p->backward_lane_length);
updateParam<double>(parameters, ns + "prepare_duration", p->lane_change_prepare_duration);

updateParam<double>(
parameters, ns + "backward_length_buffer_for_end_of_lane",
p->backward_length_buffer_for_end_of_lane);
updateParam<double>(
parameters, ns + "backward_length_buffer_for_blocking_object",
p->backward_length_buffer_for_blocking_object);
updateParam<double>(
parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer);

updateParam<double>(
parameters, ns + "lane_changing_lateral_jerk", p->lane_changing_lateral_jerk);

updateParam<double>(
parameters, ns + "minimum_lane_changing_velocity", p->minimum_lane_changing_velocity);
updateParam<double>(
parameters, ns + "prediction_time_resolution", p->prediction_time_resolution);

int longitudinal_acc_sampling_num = 0;
updateParam<int>(
parameters, ns + "longitudinal_acceleration_sampling_num", longitudinal_acc_sampling_num);
if (longitudinal_acc_sampling_num > 0) {
p->longitudinal_acc_sampling_num = longitudinal_acc_sampling_num;

Check warning on line 308 in planning/behavior_path_lane_change_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/manager.cpp#L308

Added line #L308 was not covered by tests
}

int lateral_acc_sampling_num = 0;
updateParam<int>(
parameters, ns + "lateral_acceleration_sampling_num", lateral_acc_sampling_num);
if (lateral_acc_sampling_num > 0) {
p->lateral_acc_sampling_num = lateral_acc_sampling_num;

Check warning on line 315 in planning/behavior_path_lane_change_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/manager.cpp#L315

Added line #L315 was not covered by tests
}

updateParam<double>(
parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold);
updateParam<bool>(parameters, ns + "publish_debug_marker", p->publish_debug_marker);

// longitudinal acceleration
updateParam<double>(parameters, ns + "min_longitudinal_acc", p->min_longitudinal_acc);
updateParam<double>(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc);
}

{
const std::string ns = "lane_change.safety_check.lane_expansion.";
updateParam<double>(parameters, ns + "left_offset", p->lane_expansion_left_offset);
updateParam<double>(parameters, ns + "right_offset", p->lane_expansion_right_offset);
}

{
const std::string ns = "lane_change.target_object.";
updateParam<bool>(parameters, ns + "car", p->object_types_to_check.check_car);
updateParam<bool>(parameters, ns + "truck", p->object_types_to_check.check_truck);
updateParam<bool>(parameters, ns + "bus", p->object_types_to_check.check_bus);
updateParam<bool>(parameters, ns + "trailer", p->object_types_to_check.check_trailer);
updateParam<bool>(parameters, ns + "unknown", p->object_types_to_check.check_unknown);
updateParam<bool>(parameters, ns + "bicycle", p->object_types_to_check.check_bicycle);
updateParam<bool>(parameters, ns + "motorcycle", p->object_types_to_check.check_motorcycle);
updateParam<bool>(parameters, ns + "pedestrian", p->object_types_to_check.check_pedestrian);
}

{
const std::string ns = "lane_change.regulation.";
updateParam<bool>(parameters, ns + "crosswalk", p->regulate_on_crosswalk);
updateParam<bool>(parameters, ns + "intersection", p->regulate_on_intersection);
updateParam<bool>(parameters, ns + "traffic_light", p->regulate_on_traffic_light);
}

{
const std::string ns = "lane_change.stuck_detection.";
updateParam<double>(parameters, ns + "velocity", p->stop_velocity_threshold);
updateParam<double>(parameters, ns + "stop_time", p->stop_time_threshold);
}

{
const std::string ns = "lane_change.cancel.";
bool enable_on_prepare_phase = true;
updateParam<bool>(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase);
if (!enable_on_prepare_phase) {
RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled.");
p->cancel.enable_on_prepare_phase = enable_on_prepare_phase;

Check warning on line 364 in planning/behavior_path_lane_change_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/manager.cpp#L364

Added line #L364 was not covered by tests
}

bool enable_on_lane_changing_phase = true;
updateParam<bool>(
parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase);
if (!enable_on_lane_changing_phase) {
RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled.");
p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase;

Check warning on line 372 in planning/behavior_path_lane_change_module/src/manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_lane_change_module/src/manager.cpp#L372

Added line #L372 was not covered by tests
}

updateParam<double>(parameters, ns + "delta_time", p->cancel.delta_time);
updateParam<double>(parameters, ns + "duration", p->cancel.duration);
updateParam<double>(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk);
updateParam<double>(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance);
}

Check warning on line 379 in planning/behavior_path_lane_change_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method

LaneChangeModuleManager::updateModuleParams has 91 lines, 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.
std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {
if (!observer.expired()) observer.lock()->updateModuleParams(p);
});
Expand Down
Loading