From 72d52f226d0666036c496911bbaab6ce77ef57aa Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 21 Jul 2022 11:28:21 +0900 Subject: [PATCH] Refactoring and renaming Remove some of the unused variables Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 2 +- .../config/lane_change/lane_change.param.yaml | 11 --- .../behavior_path_planner/parameters.hpp | 2 +- .../lane_change/lane_change_module.hpp | 7 +- .../scene_module/lane_change/util.hpp | 6 ++ .../behavior_path_planner/utilities.hpp | 6 +- .../src/behavior_path_planner_node.cpp | 9 +-- .../lane_change/lane_change_module.cpp | 16 +--- .../src/scene_module/lane_change/util.cpp | 74 +++++++++++-------- .../behavior_path_planner/src/utilities.cpp | 16 ++-- 10 files changed, 66 insertions(+), 83 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 03c04b93844e5..82a65ab83a2f2 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -23,4 +23,4 @@ expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 rear_vehicle_reaction_time: 1.0 - safety_time_margin_for_control: 2.0 + rear_vehicle_safety_time_margin: 2.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 810fa2561e14d..0cd8fa4f7d374 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -1,26 +1,15 @@ /**: ros__parameters: lane_change: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 lane_change_prepare_duration: 4.0 lane_changing_duration: 8.0 lane_change_finish_judge_buffer: 3.0 minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 maximum_deceleration: 1.0 lane_change_sampling_num: 10 enable_abort_lane_change: false enable_collision_check_at_prepare_phase: false use_predicted_path_outside_lanelet: false use_all_predicted_path: false - enable_blocked_by_obstacle: false lane_change_search_distance: 30.0 - safety_time_margin_for_control: 2.0 - rear_vehicle_reaction_time: 1.0 - lateral_distance_threshold: 5.0 - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 742577cd00a65..645ec423aa112 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -57,7 +57,7 @@ struct BehaviorPathPlannerParameters double expected_front_deceleration; double expected_rear_deceleration; double rear_vehicle_reaction_time; - double safety_time_margin_for_control; + double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 3178291c72583..1099e62128f6b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -40,16 +40,12 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct LaneChangeParameters { - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; double lane_change_prepare_duration; double lane_changing_duration; + double minimum_lane_change_prepare_distance; double lane_change_finish_judge_buffer; double minimum_lane_change_velocity; - double prediction_duration; double prediction_time_resolution; - double static_obstacle_velocity_thresh; double maximum_deceleration; int lane_change_sampling_num; double abort_lane_change_velocity_thresh; @@ -59,7 +55,6 @@ struct LaneChangeParameters bool enable_collision_check_at_prepare_phase; bool use_predicted_path_outside_lanelet; bool use_all_predicted_path; - bool enable_blocked_by_obstacle; double lane_change_search_distance; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index d2e19d61c948d..251fad73f077f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -45,6 +45,12 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); +double getExpectedVelocityWhenDecelerate( + const double & current_velocity, const double & expected_acceleration, + const double & lane_change_prepare_duration); +double getDistanceWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration, + const double & minimum_distance); std::vector getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 7578417dc283a..02b72401034ae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -332,17 +332,13 @@ bool isObjectFront(const Pose & projected_ego_pose); double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel); -double stoppingDistance( - const double & rear_vehicle_velocity, const double & rear_vehicle_accel, - const double & rear_vehicle_reaction_time); - double frontVehicleStopDistance( const double & front_vehicle_velocity, const double & front_vehicle_accel, const double & distance_to_collision); double rearVehicleStopDistance( const double & rear_vehicle_velocity, const double & rear_vehicle_accel, - const double & rear_vehicle_reaction_time, const double & safety_time_margin_for_control); + const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin); bool isLongitudinalDistanceEnough( const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index ccdab1f6bc583..4fea20a94ca22 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -213,7 +213,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0); p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0); p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 1.0); - p.safety_time_margin_for_control = declare_parameter("safety_time_margin_for_control", 2.0); + p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0); return p; } @@ -318,16 +318,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() }; LaneChangeParameters p{}; - p.min_stop_distance = dp("min_stop_distance", 5.0); - p.stop_time = dp("stop_time", 2.0); - p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0); p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); p.lane_changing_duration = dp("lane_changing_duration", 4.0); + p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3); - p.prediction_duration = dp("prediction_duration", 8.0); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); - p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); p.enable_abort_lane_change = dp("enable_abort_lane_change", true); @@ -338,7 +334,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.abort_lane_change_angle_thresh = dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); - p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false); p.lane_change_search_distance = dp("lane_change_search_distance", 30.0); // validation of parameters diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index cbcef504c7339..d99abfc58e4a9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -93,9 +93,8 @@ bool LaneChangeModule::isExecutionRequested() const const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); return found_valid_path; @@ -111,10 +110,8 @@ bool LaneChangeModule::isExecutionReady() const const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); return found_safe_path; @@ -182,12 +179,8 @@ CandidateOutput LaneChangeModule::planCandidate() const const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - // Find lane change path - bool found_valid_path = false; - bool found_safe_path = false; - LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); @@ -233,9 +226,8 @@ void LaneChangeModule::updateLaneChangeStatus() status_.lane_change_lanes = lane_change_lanes; // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); // Update status diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index bc9b9754d253f..d5c6137fe24a6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -66,7 +66,19 @@ bool isPathInLanelets( } return true; } +double getExpectedVelocityWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration) +{ + return velocity + expected_acceleration * duration; +} +double getDistanceWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration, + const double & minimum_distance) +{ + const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2); + return std::max(distance, minimum_distance); +} std::vector getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, @@ -79,17 +91,19 @@ std::vector getLaneChangePaths( } // rename parameter - const double backward_path_length = common_parameter.backward_path_length; - const double forward_path_length = common_parameter.forward_path_length; - const double lane_change_prepare_duration = parameter.lane_change_prepare_duration; - const double lane_changing_duration = parameter.lane_changing_duration; - const double minimum_lane_change_length = common_parameter.minimum_lane_change_length; - const double minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; - const double maximum_deceleration = parameter.maximum_deceleration; - const int lane_change_sampling_num = parameter.lane_change_sampling_num; + const auto & backward_path_length = common_parameter.backward_path_length; + const auto & forward_path_length = common_parameter.forward_path_length; + const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; + const auto & lane_changing_duration = parameter.lane_changing_duration; + const auto & minimum_lane_change_prepare_distance = + parameter.minimum_lane_change_prepare_distance; + const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length; + const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; + const auto & maximum_deceleration = parameter.maximum_deceleration; + const auto & lane_change_sampling_num = parameter.lane_change_sampling_num; // get velocity - const double v0 = util::l2Norm(twist.linear); + const double current_velocity = util::l2Norm(twist.linear); constexpr double buffer = 1.0; // buffer for min_lane_change_length const double acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; @@ -99,23 +113,25 @@ std::vector getLaneChangePaths( for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - PathWithLaneId reference_path{}; - const double v1 = v0 + acceleration * lane_change_prepare_duration; - const double v2 = v1 + acceleration * lane_changing_duration; + const double lane_change_prepare_velocity = getExpectedVelocityWhenDecelerate( + current_velocity, acceleration, lane_change_prepare_duration); + const double lane_changing_velocity = getExpectedVelocityWhenDecelerate( + lane_change_prepare_velocity, acceleration, lane_changing_duration); // skip if velocity becomes less than zero before finishing lane change - if (v2 < 0.0) { + if (lane_changing_velocity < 0.0) { continue; } // get path on original lanes - const double straight_distance = v0 * lane_change_prepare_duration + - 0.5 * acceleration * std::pow(lane_change_prepare_duration, 2); - double lane_change_distance = - v1 * lane_changing_duration + 0.5 * acceleration * std::pow(lane_changing_duration, 2); - lane_change_distance = std::max(lane_change_distance, minimum_lane_change_length); - - if (straight_distance < target_distance) { + const double prepare_distance = getDistanceWhenDecelerate( + current_velocity, acceleration, lane_change_prepare_duration, + minimum_lane_change_prepare_distance); + const double lane_changing_distance = getDistanceWhenDecelerate( + lane_change_prepare_velocity, acceleration, lane_changing_duration, + minimum_lane_change_length + buffer); + + if (prepare_distance < target_distance) { continue; } @@ -123,21 +139,21 @@ std::vector getLaneChangePaths( { const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose); const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position.length + straight_distance; + const double s_end = arc_position.length + prepare_distance; reference_path1 = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); } reference_path1.points.back().point.longitudinal_velocity_mps = std::min( reference_path1.points.back().point.longitudinal_velocity_mps, static_cast( - std::max(straight_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); + std::max(prepare_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); PathWithLaneId reference_path2{}; { const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); const auto arc_position = lanelet::utils::getArcCoordinates(target_lanelets, pose); const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); - double s_start = arc_position.length + straight_distance + lane_change_distance; + double s_start = arc_position.length + prepare_distance + lane_changing_distance; s_start = std::min(s_start, lane_length - num * minimum_lane_change_length); double s_end = s_start + forward_path_length; s_end = std::min(s_end, lane_length - num * (minimum_lane_change_length + buffer)); @@ -149,7 +165,7 @@ std::vector getLaneChangePaths( point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + std::max(lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); } if (reference_path1.points.empty() || reference_path2.points.empty()) { @@ -161,8 +177,8 @@ std::vector getLaneChangePaths( LaneChangePath candidate_path; candidate_path.acceleration = acceleration; - candidate_path.preparation_length = straight_distance; - candidate_path.lane_change_length = lane_change_distance; + candidate_path.preparation_length = prepare_distance; + candidate_path.lane_change_length = lane_changing_distance; PathWithLaneId target_lane_reference_path; { @@ -170,7 +186,7 @@ std::vector getLaneChangePaths( lanelet::utils::getArcCoordinates( target_lanelets, reference_path1.points.back().point.pose); double s_start = lane_change_start_arc_position.length; - double s_end = s_start + straight_distance + lane_change_distance + forward_path_length; + double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); } @@ -220,8 +236,8 @@ std::vector getLaneChangePaths( } point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + static_cast(std::max( + lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); const auto nearest_idx = tier4_autoware_utils::findNearestIndex(reference_path2.points, point.point.pose); point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index f3bb6c36d415a..7900bc2573fc2 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2075,13 +2075,6 @@ double stoppingDistance(const double & vehicle_velocity, const double & vehicle_ return -std::pow(vehicle_velocity, 2) / (2.0 * deceleration); } -double stoppingDistance( - const double & rear_vehicle_velocity, const double & rear_vehicle_accel, - const double & rear_vehicle_reaction_time) -{ - return rear_vehicle_velocity * rear_vehicle_reaction_time + - stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel); -} double frontVehicleStopDistance( const double & front_vehicle_velocity, const double & front_vehicle_accel, const double & distance_to_collision) @@ -2091,10 +2084,11 @@ double frontVehicleStopDistance( double rearVehicleStopDistance( const double & rear_vehicle_velocity, const double & rear_vehicle_accel, - const double & rear_vehicle_reaction_time, const double & safety_time_margin_for_control) + const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin) { - return stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel, rear_vehicle_reaction_time) + - rear_vehicle_velocity * safety_time_margin_for_control; + return rear_vehicle_velocity * rear_vehicle_reaction_time + + stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel) + + rear_vehicle_velocity * rear_vehicle_safety_time_margin; } bool isLongitudinalDistanceEnough( @@ -2133,7 +2127,7 @@ bool hasEnoughDistance( const auto rear_vehicle_stop_threshold = rearVehicleStopDistance( util::l2Norm(rear_vehicle_velocity), rear_vehicle_accel, param.rear_vehicle_reaction_time, - param.safety_time_margin_for_control); + param.rear_vehicle_safety_time_margin); return isLongitudinalDistanceEnough(rear_vehicle_stop_threshold, front_vehicle_stop_threshold); }