Skip to content
New issue

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

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

Already on GitHub? Sign in to your account

Feat/safety check for start goal planner #800

Closed
1 change: 1 addition & 0 deletions planning/behavior_path_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,10 @@ MarkerArray createObjectsMarkerArray(
MarkerArray createDrivableLanesMarkerArray(
const std::vector<DrivableLanes> & 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_
Original file line number Diff line number Diff line change
Expand Up @@ -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 <freespace_planning_algorithms/astar_search.hpp>
#include <freespace_planning_algorithms/rrtstar.hpp>
Expand Down Expand Up @@ -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,
Expand All @@ -65,7 +74,7 @@ enum class PathType {
FREESPACE,
};

struct PUllOverStatus
struct PullOverStatus
{
std::shared_ptr<PullOverPath> pull_over_path{};
std::shared_ptr<PullOverPath> lane_parking_pull_over_path{};
Expand All @@ -76,7 +85,8 @@ struct PUllOverStatus
lanelet::ConstLanelets pull_over_lanes{};
std::vector<DrivableLanes> 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};
Expand Down Expand Up @@ -105,7 +115,14 @@ class GoalPlannerModule : public SceneModuleInterface

void updateModuleParams(const std::shared_ptr<GoalPlannerParameters> & parameters)
{
parameters_ = parameters;
parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
if (parameters_->safety_check_params.enable_safety_check) {
ego_predicted_path_params_ =
std::make_shared<EgoPredictedPathParams>(parameters_->ego_predicted_path_params);
objects_filtering_params_ =
std::make_shared<ObjectsFilteringParams>(parameters_->objects_filtering_params);
safety_check_params_ = std::make_shared<SafetyCheckParams>(parameters_->safety_check_params);
}
}

BehaviorModuleOutput run() override;
Expand All @@ -127,10 +144,24 @@ 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_;

mutable StartGoalPlannerData goal_planner_data_;

std::shared_ptr<GoalPlannerParameters> parameters_;

mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
mutable std::shared_ptr<SafetyCheckParams> safety_check_params_;

vehicle_info_util::VehicleInfo vehicle_info_;

// planner
std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
std::unique_ptr<PullOverPlannerBase> freespace_planner_;
Expand Down Expand Up @@ -198,7 +229,6 @@ class GoalPlannerModule : public SceneModuleInterface
const Pose & search_start_offset_pose, PathWithLaneId & path) const;
PathWithLaneId generateStopPath();
PathWithLaneId generateFeasibleStopPath();
boost::optional<double> calcFeasibleDecelDistance(const double target_velocity) const;
void keepStoppedWithCurrentPath(PathWithLaneId & path);
double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;

Expand Down Expand Up @@ -267,6 +297,13 @@ class GoalPlannerModule : public SceneModuleInterface
// rtc
std::pair<double, double> calcDistanceToPathChange() const;

// safety check
SafetyCheckParams createSafetyCheckParams() const;
void updateSafetyCheckTargetObjectsData(
const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
bool isSafePath() const;

// debug
void setDebugData();
void printParkingPositionError() const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <lane_departure_checker/lane_departure_checker.hpp>
#include <lanelet2_extension/utility/message_conversion.hpp>
Expand All @@ -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;

Expand All @@ -53,7 +61,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{};

Expand Down Expand Up @@ -87,6 +96,13 @@ class StartPlannerModule : public SceneModuleInterface
void setParameters(const std::shared_ptr<StartPlannerParameters> & parameters)
{
parameters_ = parameters;
if (parameters->safety_check_params.enable_safety_check) {
ego_predicted_path_params_ =
std::make_shared<EgoPredictedPathParams>(parameters_->ego_predicted_path_params);
objects_filtering_params_ =
std::make_shared<ObjectsFilteringParams>(parameters_->objects_filtering_params);
safety_check_params_ = std::make_shared<SafetyCheckParams>(parameters_->safety_check_params);
}
}
void resetStatus();

Expand All @@ -101,10 +117,14 @@ class StartPlannerModule : public SceneModuleInterface

private:
std::shared_ptr<StartPlannerParameters> parameters_;
mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
mutable std::shared_ptr<SafetyCheckParams> safety_check_params_;
vehicle_info_util::VehicleInfo vehicle_info_;

std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
PullOutStatus status_;
mutable StartGoalPlannerData start_planner_data_;

std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;

Expand Down Expand Up @@ -146,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<PoseWithVelocityStamped> & 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.
Expand All @@ -154,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();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,10 @@ class GeometricParallelParking

std::vector<PathWithLaneId> getArcPaths() const { return arc_paths_; }
std::vector<PathWithLaneId> getPaths() const { return paths_; }
std::vector<std::pair<double, double>> getPairsTerminalVelocityAndAccel() const
{
return pairs_terminal_velocity_and_accel_;
}
PathWithLaneId getPathByIdx(size_t const idx) const;
PathWithLaneId getCurrentPath() const;
PathWithLaneId getFullPath() const;
Expand All @@ -112,6 +116,7 @@ class GeometricParallelParking

std::vector<PathWithLaneId> arc_paths_;
std::vector<PathWithLaneId> paths_;
std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel_;
size_t current_path_idx_ = 0;

void clearPaths();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <freespace_planning_algorithms/abstract_algorithm.hpp>
#include <freespace_planning_algorithms/astar_search.hpp>
Expand Down Expand Up @@ -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;
};
Expand Down
Loading
Loading