From a84dfaffc416f69fe8c19a89c41e6d6e67c9da65 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 22 Aug 2023 19:50:33 +0900 Subject: [PATCH 1/7] feat(behavior_path_planner): add status_is_safe_dynamic_objects (#4691) * add_status_is_safe_dynamic_objects Signed-off-by: kyoichi-sugahara * revert unnecessary changes Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.hpp | 3 ++- .../start_planner/start_planner_module.cpp | 20 +++++++++---------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 24fd27d9a7a78..9ecbe52fdb1be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -53,7 +53,8 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool back_finished{false}; Pose pull_out_start_pose{}; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 98796d52c6e73..60db93405c94f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -209,7 +209,7 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); @@ -314,7 +314,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); @@ -427,7 +427,7 @@ void StartPlannerModule::planWithPriority( // use current path if back is not needed if (status_.back_finished) { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -448,7 +448,7 @@ void StartPlannerModule::planWithPriority( // Update status variables with the next path information { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path_next; status_.pull_out_start_pose = pull_out_start_pose_next; status_.planner_type = planner->getPlannerType(); @@ -503,7 +503,7 @@ void StartPlannerModule::planWithPriority( // not found safe path if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe = false; + status_.is_safe_static_objects = false; status_.planner_type = PlannerType::NONE; } } @@ -713,7 +713,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe) { + if (!status_.back_finished || !status_.is_safe_static_objects) { return false; } @@ -800,7 +800,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1011,7 +1011,7 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.back_finished = true; return true; } @@ -1062,8 +1062,8 @@ void StartPlannerModule::setDebugData() const const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); From d5b07ec7a9aba1834e37778bee35385af10b90ee Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 23 Aug 2023 16:29:26 +0900 Subject: [PATCH 2/7] feat(behavior_path_planner): add PullOutPath member variables for ego predicted path generation (#4669) * add_pull_out_member_variables Signed-off-by: kyoichi-sugahara * update pull_out path struct Signed-off-by: kyoichi-sugahara * update pull_out path struct Signed-off-by: kyoichi-sugahara * minor update Signed-off-by: kyoichi-sugahara * fix calculation Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * fix code Signed-off-by: kyoichi-sugahara * fix code Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../utils/start_planner/pull_out_path.hpp | 3 ++ .../start_planner/start_planner_module.cpp | 1 + .../start_planner/geometric_pull_out.cpp | 35 ++++++++++++++++++- .../utils/start_planner/shift_pull_out.cpp | 5 +++ 4 files changed, 43 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index d68985ec27350..608b918e839fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp @@ -19,6 +19,7 @@ #include +#include #include namespace behavior_path_planner @@ -27,6 +28,8 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; }; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 60db93405c94f..20dd48eebb9fb 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -106,6 +106,7 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { + // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. // Execute when current pose is near route start pose const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 721ffd3064840..45d36d51d69b4 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -80,15 +80,48 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p parameters_.collision_check_margin)) { return {}; } + const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); + /* + Calculate the acceleration required to reach the forward parking velocity at the center of + the front path, assuming constant acceleration and deceleration. + v v + | | + | /\ | /\ + | / \ | / \ + | / \ | / \ + | / \ | / \ + |/________\_____ x |/________\______ t + 0 a_l/2 a_l 0 t_c 2*t_c + Notes: + a_l represents "arc_length_on_path_front" + t_c represents "time_to_center" + */ // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; + const double arc_length_on_first_arc_path = + motion_utils::calcArcLength(output.partial_paths.front().points); + const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); + const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); + const double average_acceleration = average_velocity / (time_to_center * 2); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(average_velocity, average_acceleration)); + const double arc_length_on_second_arc_path = + motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { - auto partial_paths = planner_.getPaths(); + const auto partial_paths = planner_.getPaths(); const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); + + // Calculate the acceleration required to reach the forward parking velocity at the center of + // the path, assuming constant acceleration and deceleration. + const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 08dcc0254deb1..7352821e140be 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -163,9 +163,12 @@ std::vector ShiftPullOut::calcPullOutPaths( // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { PullOutPath non_shifted_path{}; + // In non_shifted_path, to minimize safety checks, 0 is assigned to prevent the predicted_path + // of the ego vehicle from becoming too large. non_shifted_path.partial_paths.push_back(road_lane_reference_path); non_shifted_path.start_pose = start_pose; non_shifted_path.end_pose = start_pose; + non_shifted_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(0, 0)); return non_shifted_path; }); @@ -283,6 +286,8 @@ std::vector ShiftPullOut::calcPullOutPaths( candidate_path.partial_paths.push_back(shifted_path.path); candidate_path.start_pose = shift_line.start; candidate_path.end_pose = shift_line.end; + candidate_path.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(terminal_velocity, longitudinal_acc)); candidate_paths.push_back(candidate_path); } From d2115f57728cd520e83270b5b6b050503e13b372 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 25 Aug 2023 21:43:08 +0900 Subject: [PATCH 3/7] feat(behavior_path_planner): visualize ego predicted path (#4743) visualize ego predicted path Signed-off-by: kyoichi-sugahara --- .../marker_utils/utils.hpp | 4 ++ .../path_safety_checker/safety_check.hpp | 3 + .../src/marker_utils/utils.cpp | 55 +++++++++++++++++++ .../path_safety_checker/safety_check.cpp | 13 +++++ 4 files changed, 75 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 6894edaece828..1ed4413c50936 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -131,6 +131,10 @@ MarkerArray createObjectsMarkerArray( MarkerArray createDrivableLanesMarkerArray( const std::vector & drivable_lanes, std::string && ns); +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8606f2cd2397d..14b4419e4f058 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -63,6 +63,9 @@ Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, CollisionCheckDebug & debug); +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution); + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, const double front_object_deceleration, const double rear_object_deceleration, diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 80fea95b03caa..6e598a630f6bb 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -408,4 +408,59 @@ MarkerArray createDrivableLanesMarkerArray( return msg; } +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + if (predicted_path.path.empty()) { + return MarkerArray{}; + } + + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + const auto & path = predicted_path.path; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), + + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + for (size_t i = 0; i < path.size(); ++i) { + marker.id = i + id; + marker.points.clear(); + + const auto & predicted_path_pose = path.at(i); + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); + + marker_array.markers.push_back(marker); + } + return marker_array; + + marker.points.reserve(path.size()); + for (const auto & point : path) { + marker.points.push_back(point.position); + } + msg.markers.push_back(marker); + return msg; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 34ba607938385..cfa4f78793771 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -133,6 +133,19 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution) +{ + PredictedPath predicted_path; + predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); + predicted_path.path.resize(path.size()); + + for (size_t i = 0; i < path.size(); ++i) { + predicted_path.path.at(i) = path.at(i).pose; + } + return predicted_path; +} + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, const double front_object_deceleration, const double rear_object_deceleration, From f10bcf6599d66292fa1ac42afee2947bfe6258d0 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 25 Aug 2023 23:14:18 +0900 Subject: [PATCH 4/7] feat(behavior_path_planner): update filter objects by velocity (#4742) --- .../path_safety_checker/objects_filtering.hpp | 35 +++++++++++++------ .../path_safety_checker/objects_filtering.cpp | 16 ++++++--- 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index b4575eb4d3b7e..8a3b7094c69d1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -55,24 +55,37 @@ PredictedObjects filterObjects( const std::shared_ptr & params); /** - * @brief Filter objects based on their velocity. + * @brief Filters objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param lim_v Velocity limit for filtering. - * @return PredictedObjects The filtered objects. + * Depending on the remove_above_threshold parameter, this function either removes objects with + * velocities above the given threshold or only keeps those objects. It uses the helper function + * filterObjectsByVelocity() to do the actual filtering. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The velocity threshold for the filtering. + * @param remove_above_threshold If true, objects with velocities above the threshold are removed. + * If false, only objects with velocities above the threshold are + * kept. + * @return A new collection of objects that have been filtered according to the rules. */ -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold = true); /** - * @brief Filter objects based on a velocity range. + * @brief Helper function to filter objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param min_v Minimum velocity for filtering. - * @param max_v Maximum velocity for filtering. - * @return PredictedObjects The filtered objects. + * This function iterates over all objects and calculates their velocity norm. If the velocity norm + * is within the velocity_threshold and max_velocity range, the object is added to a new collection. + * This new collection is then returned. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The minimum velocity for an object to be included in the output. + * @param max_velocity The maximum velocity for an object to be included in the output. + * @return A new collection of objects that have been filtered according to the rules. */ PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); + const PredictedObjects & objects, double velocity_threshold, double max_velocity); /** * @brief Filter objects based on their position relative to a current_pose. diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 33afe0fe6642f..a775e7c16efed 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -37,7 +37,7 @@ PredictedObjects filterObjects( PredictedObjects filtered_objects; - filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); filterObjectsByClass(filtered_objects, target_object_types); @@ -51,13 +51,19 @@ PredictedObjects filterObjects( return filtered_objects; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold) { - return filterObjectsByVelocity(objects, -lim_v, lim_v); + if (remove_above_threshold) { + return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); + } else { + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); + } } PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) + const PredictedObjects & objects, double velocity_threshold, double max_velocity) { PredictedObjects filtered; filtered.header = objects.header; @@ -65,7 +71,7 @@ PredictedObjects filterObjectsByVelocity( const auto v_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - if (min_v < v_norm && v_norm < max_v) { + if (velocity_threshold < v_norm && v_norm < max_velocity) { filtered.objects.push_back(obj); } } From c6cc18610fb2459754e163249deaa5bad26f616e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sun, 3 Sep 2023 19:25:36 +0900 Subject: [PATCH 5/7] refactor(behavior_path_planner): consolidate common function which is used by start and goal planner (#4804) * refactor code * consolidate function * add utility * util to utils * define parameters * fix typo * fix arg name * commonize calcFeasibleDecelDistance * define modifyVelocityByDirection * add description --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../goal_planner/goal_planner_module.hpp | 14 +- .../geometric_parallel_parking.hpp | 5 + .../goal_planner/goal_planner_parameters.hpp | 13 ++ .../goal_planner/pull_over_planner_base.hpp | 3 + .../path_safety_checker_parameters.hpp | 2 +- .../common_module_data.hpp | 45 ++++++ .../utils/start_goal_planner_common/utils.hpp | 94 +++++++++++ .../start_planner_parameters.hpp | 13 ++ .../goal_planner/goal_planner_module.cpp | 95 +++++------ .../geometric_parallel_parking.cpp | 15 ++ .../goal_planner/freespace_pull_over.cpp | 15 +- .../goal_planner/geometric_pull_over.cpp | 1 + .../utils/goal_planner/shift_pull_over.cpp | 1 + .../path_safety_checker/objects_filtering.cpp | 6 +- .../src/utils/path_utils.cpp | 1 + .../utils/start_goal_planner_common/utils.cpp | 148 ++++++++++++++++++ .../start_planner/geometric_pull_out.cpp | 10 +- 18 files changed, 411 insertions(+), 71 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp create mode 100644 planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0352096d02b2b..f62e418371401 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp + src/utils/start_goal_planner_common/utils.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 5177feaaf61f4..3b6aecfb42d1e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -65,7 +65,7 @@ enum class PathType { FREESPACE, }; -struct PUllOverStatus +struct PullOverStatus { std::shared_ptr pull_over_path{}; std::shared_ptr lane_parking_pull_over_path{}; @@ -76,7 +76,8 @@ struct PUllOverStatus lanelet::ConstLanelets pull_over_lanes{}; std::vector lanes{}; // current + pull_over bool has_decided_path{false}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; @@ -127,7 +128,13 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; }; private: - PUllOverStatus status_; + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + + PullOverStatus status_; std::shared_ptr parameters_; @@ -198,7 +205,6 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - boost::optional calcFeasibleDecelDistance(const double target_velocity) const; void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index cb769a1f2884c..9cae5e18f1352 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -94,6 +94,10 @@ class GeometricParallelParking std::vector getArcPaths() const { return arc_paths_; } std::vector getPaths() const { return paths_; } + std::vector> getPairsTerminalVelocityAndAccel() const + { + return pairs_terminal_velocity_and_accel_; + } PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; @@ -112,6 +116,7 @@ class GeometricParallelParking std::vector arc_paths_; std::vector paths_; + std::vector> pairs_terminal_velocity_and_accel_; size_t current_path_idx_ = 0; void clearPaths(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..6b8a62d2caec9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -99,6 +100,18 @@ struct GoalPlannerParameters AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; + // debug bool print_debug_info; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4b800917d4aec..20788e53309ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -46,6 +47,8 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 519f589f561ee..0fe4d354ca8fb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -163,7 +163,7 @@ struct ObjectsFilteringParams struct SafetyCheckParams { bool enable_safety_check; ///< Enable safety checks. - double backward_lane_length; ///< Length of the backward lane for path generation. + double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp new file mode 100644 index 0000000000000..4ba2cb08b6341 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ + +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +/* + * Common data for start/goal_planner module + */ +struct StartGoalPlannerData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp new file mode 100644 index 0000000000000..60bbb3c78b401 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -0,0 +1,94 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity); + +/** + * @brief Update path velocities based on driving direction. + * + * This function updates the longitudinal velocity of each point in the provided paths, + * based on whether the vehicle is driving forward or backward. It also sets the terminal + * velocity and acceleration for each path. + * + * @param paths A vector of paths with lane IDs to be updated. + * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal + * velocity and acceleration for a corresponding path. + * @param target_velocity The target velocity for ego vehicle predicted path. + * @param acceleration The acceleration for ego vehicle predicted path. + */ +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel); + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx); + +} // namespace behavior_path_planner::utils::start_goal_planner_common + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 1e26bef0c85be..771fbd93f7196 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -76,6 +77,18 @@ struct StartPlannerParameters PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 97e047be749b5..fd8681f1d19d8 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -34,6 +35,7 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; @@ -117,7 +119,7 @@ GoalPlannerModule::GoalPlannerModule( void GoalPlannerModule::resetStatus() { - PUllOverStatus initial_status{}; + PullOverStatus initial_status{}; status_ = initial_status; pull_over_path_candidates_.clear(); closest_start_pose_.reset(); @@ -342,7 +344,8 @@ bool GoalPlannerModule::isExecutionReady() const double GoalPlannerModule::calcModuleRequestLength() const { - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return parameters_->minimum_request_length; } @@ -442,7 +445,7 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); status_.pull_over_path = std::make_shared(*freespace_path); status_.current_path_idx = 0; - status_.is_safe = true; + status_.is_safe_static_objects = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); debug_data_.freespace_planner.is_planning = false; @@ -479,7 +482,7 @@ void GoalPlannerModule::returnToLaneParking() } mutex_.lock(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.has_decided_path = false; status_.pull_over_path = status_.lane_parking_pull_over_path; status_.current_path_idx = 0; @@ -536,7 +539,7 @@ void GoalPlannerModule::selectSafePullOverPath() const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); - status_.is_safe = false; + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -555,7 +558,7 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe = true; + status_.is_safe_static_objects = true; mutex_.lock(); status_.pull_over_path = std::make_shared(pull_over_path); status_.current_path_idx = 0; @@ -567,7 +570,7 @@ void GoalPlannerModule::selectSafePullOverPath() } // decelerate before the search area start - if (status_.is_safe) { + if (status_.is_safe_static_objects) { const auto search_start_offset_pose = calcLongitudinalOffsetPose( status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - @@ -578,7 +581,9 @@ void GoalPlannerModule::selectSafePullOverPath() decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance(parameters_->pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -611,7 +616,7 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe) { + if (status_.is_safe_static_objects) { // clear stop pose when the path is safe and activated if (isActivated()) { resetWallPoses(); @@ -640,7 +645,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe; + status_.prev_is_safe = status_.is_safe_static_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -688,7 +693,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe) { + if (status_.is_safe_static_objects) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose; @@ -736,7 +741,7 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return false; } @@ -853,7 +858,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe + path_candidate_ = status_.is_safe_static_objects ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -947,17 +952,19 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { + if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() - : *search_start_offset_pose); + const Pose stop_pose = + status_.is_safe_static_objects + ? status_.pull_over_path->start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -974,7 +981,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); for (auto & p : reference_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1002,7 +1011,8 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return stop_path; } @@ -1087,7 +1097,7 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1202,7 +1212,8 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - const auto current_to_stop_distance = calcFeasibleDecelDistance(0.0); + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -1238,30 +1249,6 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional GoalPlannerModule::calcFeasibleDecelDistance( - const double target_velocity) const -{ - const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; - const auto a_now = planner_data_->self_acceleration->accel.accel.linear.x; - const auto a_lim = parameters_->maximum_deceleration; // positive value - const auto j_lim = parameters_->maximum_jerk; - - if (v_now < target_velocity) { - return 0.0; - } - - auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( - v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim); - - if (!min_stop_distance) { - return {}; - } - - min_stop_distance = std::max(*min_stop_distance, 0.0); - - return min_stop_distance; -} - double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { @@ -1286,7 +1273,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const float decel_vel = std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); - const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -1304,7 +1292,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { const auto stop_point = utils::insertStopPoint(stop_point_length, path); @@ -1318,7 +1307,9 @@ void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -1460,7 +1451,7 @@ void GoalPlannerModule::setDebugData() } // Visualize path and related pose - if (status_.is_safe) { + if (status_.is_safe_static_objects) { add(createPoseMarkerArray( status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -1480,8 +1471,8 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index a5ccfc9517771..27dbfb532c1fc 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -163,6 +163,7 @@ void GeometricParallelParking::clearPaths() current_path_idx_ = 0; arc_paths_.clear(); paths_.clear(); + pairs_terminal_velocity_and_accel_.clear(); } bool GeometricParallelParking::planPullOver( @@ -182,6 +183,7 @@ bool GeometricParallelParking::planPullOver( if (is_forward) { // When turning forward to the right, the front left goes out, // so reduce the steer angle at that time for seach no lane departure path. + // TODO(Sugahara): define in the config constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; @@ -481,6 +483,19 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + } else { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + } + // set pull_over start and end pose // todo: make start and end pose for pull_out start_pose_ = start_pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index f4aabb13b8916..9656094f5a478 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include #include @@ -55,8 +56,7 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - const bool found_path = planner_->makePlan(current_pose, end_pose); - if (!found_path) { + if (!planner_->makePlan(current_pose, end_pose)) { return {}; } @@ -113,18 +113,21 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) addPose(goal_pose); } - utils::correctDividedPathVelocity(partial_paths); + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::start_goal_planner_common::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); - if (!is_driving_forward) { - // path points is less than 2 + if (!motion_utils::isDrivingForward(path.points)) { return {}; } } PullOverPath pull_over_path{}; pull_over_path.partial_paths = partial_paths; + pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; pull_over_path.start_pose = current_pose; pull_over_path.end_pose = goal_pose; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 6a5ccc827db29..7760b9b57d8ab 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -75,6 +75,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 2f5c1d9b05b7f..c5ac14d4d9727 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -204,6 +204,7 @@ boost::optional ShiftPullOver::generatePullOverPath( PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths.push_back(shifted_path.path); + pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index a775e7c16efed..23016b01cb9ab 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -343,13 +343,13 @@ TargetObjectsOnLane createTargetObjectsOnLane( }; // TODO(Sugahara): Consider shoulder and other lane objects - if (object_lane_configuration.check_current_lane) { + if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } - if (object_lane_configuration.check_left_lane) { + if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); } - if (object_lane_configuration.check_right_lane) { + if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ec14a064bf51b..c4bfb62276d42 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -418,6 +418,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; } diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp new file mode 100644 index 0000000000000..f29711c82b3ce --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -0,0 +1,148 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity) +{ + const auto v_now = planner_data->self_odometry->twist.twist.linear.x; + const auto a_now = planner_data->self_acceleration->accel.accel.linear.x; + + if (v_now < target_velocity) { + return 0.0; + } + + auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( + v_now, target_velocity, a_now, -acc_lim, jerk_lim, -1.0 * jerk_lim); + + if (!min_stop_distance) { + return {}; + } + + min_stop_distance = std::max(*min_stop_distance, 0.0); + + return min_stop_distance; +} + +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration) +{ + assert(paths.size() == terminal_vel_acc_pairs.size()); + + auto path_itr = std::begin(paths); + auto pair_itr = std::begin(terminal_vel_acc_pairs); + + for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { + const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + + // If the number of points in the path is less than 2, don't insert stop velocity and + // set pairs_terminal_velocity_and_accel to 0 + if (!is_driving_forward) { + *pair_itr = std::make_pair(0.0, 0.0); + continue; + } + + if (*is_driving_forward) { + for (auto & point : path_itr->points) { + // TODO(Sugahara): velocity calculation can be improved by considering the acceleration + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + // TODO(Sugahara): Consider the calculation of the target velocity and acceleration for ego's + // predicted path when ego will stop at the end of the path + *pair_itr = std::make_pair(target_velocity, acceleration); + } else { + for (auto & point : path_itr->points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + *pair_itr = std::make_pair(-target_velocity, -acceleration); + } + path_itr->points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel) +{ + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; +} + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx) +{ + if (pairs_terminal_velocity_and_accel.size() <= current_path_idx) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel.at(current_path_idx); +} + +} // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 45d36d51d69b4..29abdb08c5c55 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -69,11 +69,10 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); + const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, @@ -123,6 +122,7 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; From e66620ad4dc0c6880b42abfd3f78a133a99a9ae8 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 4 Sep 2023 12:17:18 +0900 Subject: [PATCH 6/7] refactor(behavior_path_planner): add vehicle_info_ as member variable in goal_planner (#4859) add vehicle_info_ Signed-off-by: kyoichi-sugahara --- .../scene_module/goal_planner/goal_planner_module.hpp | 2 ++ .../src/scene_module/goal_planner/goal_planner_module.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 3b6aecfb42d1e..52ee72d307b01 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -138,6 +138,8 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr parameters_; + vehicle_info_util::VehicleInfo vehicle_info_; + // planner std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index fd8681f1d19d8..cddfbd841bc6b 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -53,10 +53,12 @@ GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + lane_departure_checker.setVehicleInfo(vehicle_info_); occupancy_grid_map_ = std::make_shared(); From eee21e9e0dbacaf28392fcbbcadad6a542bc3559 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 5 Sep 2023 20:08:50 +0900 Subject: [PATCH 7/7] feat(behavior_path_planner): add safety check against dynamic objects for start/goal planner (#4873) * add safety check for start/goal planner Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara * update comments Signed-off-by: kyoichi-sugahara * add const Signed-off-by: kosuke55 * fix empty check Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Signed-off-by: kosuke55 Co-authored-by: kosuke55 --- .../goal_planner/goal_planner.param.yaml | 60 +++++++ .../start_planner/start_planner.param.yaml | 60 +++++++ .../goal_planner/goal_planner_module.hpp | 31 +++- .../start_planner/start_planner_module.hpp | 24 +++ .../utils/start_goal_planner_common/utils.hpp | 6 +- .../utils/start_planner/util.hpp | 4 + .../goal_planner/goal_planner_module.cpp | 98 +++++++++++ .../src/scene_module/goal_planner/manager.cpp | 152 +++++++++++++++--- .../scene_module/start_planner/manager.cpp | 116 +++++++++++++ .../start_planner/start_planner_module.cpp | 94 +++++++++++ .../utils/start_goal_planner_common/utils.cpp | 9 +- 11 files changed, 631 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..433ac428fe73e 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -104,6 +104,66 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..b62262423fa72 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -72,3 +72,63 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 52ee72d307b01..af65de83c045c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -24,6 +24,9 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + enum class PathType { NONE = 0, SHIFT, @@ -106,7 +115,14 @@ class GoalPlannerModule : public SceneModuleInterface void updateModuleParams(const std::shared_ptr & parameters) { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); + if (parameters_->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } BehaviorModuleOutput run() override; @@ -136,8 +152,14 @@ class GoalPlannerModule : public SceneModuleInterface PullOverStatus status_; + mutable StartGoalPlannerData goal_planner_data_; + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + vehicle_info_util::VehicleInfo vehicle_info_; // planner @@ -275,6 +297,13 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 9ecbe52fdb1be..558ba04d3dff4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,12 +17,15 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -43,6 +46,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -88,6 +96,13 @@ class StartPlannerModule : public SceneModuleInterface void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -102,10 +117,14 @@ class StartPlannerModule : public SceneModuleInterface private: std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; @@ -147,6 +166,10 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -155,6 +178,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 60bbb3c78b401..bea671c266899 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -63,7 +63,7 @@ void updateEgoPredictedPathParams( void updateEgoPredictedPathParams( std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateSafetyCheckParams( std::shared_ptr & safety_check_params, @@ -71,7 +71,7 @@ void updateSafetyCheckParams( void updateSafetyCheckParams( std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, @@ -79,7 +79,7 @@ void updateObjectsFilteringParams( void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updatePathProperty( std::shared_ptr & ego_predicted_path_params, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..d85e6181764d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,9 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index cddfbd841bc6b..9fc40fa24384d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" @@ -341,6 +342,22 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. + if (status_.pull_over_path == nullptr) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -1419,12 +1436,82 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto & pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto & pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel: %f, %f", terminal_velocity_and_accel.first, + terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto & ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1468,6 +1555,17 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } } // Visualize planner type text diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 9e94a234a7f48..e44164ab0fb95 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -32,18 +32,18 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.minimum_request_length = node->declare_parameter(base_ns + "minimum_request_length"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search { - std::string ns = "goal_planner.goal_search."; + const std::string ns = base_ns + "goal_search."; p.search_priority = node->declare_parameter(ns + "search_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); @@ -72,7 +72,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { - std::string ns = "goal_planner.occupancy_grid."; + const std::string ns = base_ns + "occupancy_grid."; p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); p.use_occupancy_grid_for_longitudinal_margin = node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); @@ -84,7 +84,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); @@ -95,7 +95,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); @@ -106,7 +106,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -118,7 +118,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -134,7 +134,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -151,7 +151,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -174,7 +174,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -191,14 +191,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -208,7 +208,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -218,9 +218,125 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index ec18d55a31069..25f1f79affd4f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -148,6 +148,122 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 20dd48eebb9fb..6c76d3b521c77 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -169,6 +171,21 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { + if (status_.pull_out_path.partial_paths.empty()) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -922,6 +939,70 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +void StartPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + start_planner_data_.filtered_objects = filtered_objects; + start_planner_data_.target_objects_on_lane = target_objects_on_lane; + start_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto & pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto & ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1039,8 +1120,10 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1058,6 +1141,17 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index f29711c82b3ce..cff6e9f81f02e 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -131,8 +131,15 @@ void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) { + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; - ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; + ego_predicted_path_params->acceleration = acceleration; } std::pair getPairsTerminalVelocityAndAccel(