diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index e3763720c2d8a..0c6656fda3247 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -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()); } diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index 0a56c17d89fd0..d5d4bc9627114 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -279,10 +279,104 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( - parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); + { + const std::string ns = "lane_change."; + updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); + updateParam(parameters, ns + "prepare_duration", p->lane_change_prepare_duration); + + updateParam( + parameters, ns + "backward_length_buffer_for_end_of_lane", + p->backward_length_buffer_for_end_of_lane); + updateParam( + parameters, ns + "backward_length_buffer_for_blocking_object", + p->backward_length_buffer_for_blocking_object); + updateParam( + parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); + + updateParam( + parameters, ns + "lane_changing_lateral_jerk", p->lane_changing_lateral_jerk); + + updateParam( + parameters, ns + "minimum_lane_changing_velocity", p->minimum_lane_changing_velocity); + updateParam( + parameters, ns + "prediction_time_resolution", p->prediction_time_resolution); + + int longitudinal_acc_sampling_num = 0; + updateParam( + 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; + } + + int lateral_acc_sampling_num = 0; + updateParam( + 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; + } + + updateParam( + parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + + // longitudinal acceleration + updateParam(parameters, ns + "min_longitudinal_acc", p->min_longitudinal_acc); + updateParam(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); + } + + { + const std::string ns = "lane_change.safety_check.lane_expansion."; + updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); + updateParam(parameters, ns + "right_offset", p->lane_expansion_right_offset); + } + + { + const std::string ns = "lane_change.target_object."; + updateParam(parameters, ns + "car", p->object_types_to_check.check_car); + updateParam(parameters, ns + "truck", p->object_types_to_check.check_truck); + updateParam(parameters, ns + "bus", p->object_types_to_check.check_bus); + updateParam(parameters, ns + "trailer", p->object_types_to_check.check_trailer); + updateParam(parameters, ns + "unknown", p->object_types_to_check.check_unknown); + updateParam(parameters, ns + "bicycle", p->object_types_to_check.check_bicycle); + updateParam(parameters, ns + "motorcycle", p->object_types_to_check.check_motorcycle); + updateParam(parameters, ns + "pedestrian", p->object_types_to_check.check_pedestrian); + } + + { + const std::string ns = "lane_change.regulation."; + updateParam(parameters, ns + "crosswalk", p->regulate_on_crosswalk); + updateParam(parameters, ns + "intersection", p->regulate_on_intersection); + updateParam(parameters, ns + "traffic_light", p->regulate_on_traffic_light); + } + + { + const std::string ns = "lane_change.stuck_detection."; + updateParam(parameters, ns + "velocity", p->stop_velocity_threshold); + updateParam(parameters, ns + "stop_time", p->stop_time_threshold); + } + { + const std::string ns = "lane_change.cancel."; + bool enable_on_prepare_phase = true; + updateParam(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; + } + + bool enable_on_lane_changing_phase = true; + updateParam( + 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; + } + + updateParam(parameters, ns + "delta_time", p->cancel.delta_time); + updateParam(parameters, ns + "duration", p->cancel.duration); + updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); + updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); + } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); });