Skip to content

Commit

Permalink
refactor(avoidance): use common safety checker (#4388)
Browse files Browse the repository at this point in the history 
* refactor(avoidance): use common safety checker

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* docs(avoidance): rename params

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* chore(avoidance): rename variables

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Jul 27, 2023
1 parent f72b71b commit a567bb2
Show file tree
Hide file tree
Showing 8 changed files with 295 additions and 463 deletions.
21 changes: 11 additions & 10 deletions planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
enable_bound_clipping: false
enable_update_path_when_object_is_gone: false
enable_force_avoidance_for_stopped_vehicle: false
enable_safety_check: true
enable_yield_maneuver: true
enable_yield_maneuver_during_shifting: false
disable_path_update: false
Expand Down Expand Up @@ -133,10 +132,18 @@

# For safety check
safety_check:
# safety check configuration
enable: true # [-]
check_current_lane: false # [-]
check_shift_side_lane: true # [-]
check_other_side_lane: false # [-]
check_unavoidable_object: false # [-]
check_other_object: true # [-]
# collision check parameters
check_all_predicted_path: false # [-]
time_resolution: 0.5 # [s]
time_horizon: 10.0 # [s]
safety_check_backward_distance: 100.0 # [m]
safety_check_time_horizon: 10.0 # [s]
safety_check_idling_time: 1.5 # [s]
safety_check_accel_for_rss: 2.5 # [m/ss]
safety_check_hysteresis_factor: 2.0 # [-]
safety_check_ego_offset: 1.0 # [m]

Expand Down Expand Up @@ -186,12 +193,6 @@
max_jerk: 1.0 # [m/sss]
max_acceleration: 1.0 # [m/ss]

target_velocity_matrix:
col_size: 2
matrix:
[2.78, 13.9, # velocity [m/s]
0.50, 1.00] # margin [m]

shift_line_pipeline:
trim:
quantize_filter_threshold: 0.2
Expand Down
44 changes: 27 additions & 17 deletions planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -485,15 +485,22 @@ The hatched road marking is defined on Lanelet map. See [here](https://github.co

### Safety check

The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable_safety_check` as `true`.
The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable` as `true`.

```yaml
enable_safety_check: false
# For safety check
# safety check configuration
enable: true # [-]
check_current_lane: false # [-]
check_shift_side_lane: true # [-]
check_other_side_lane: false # [-]
check_unavoidable_object: false # [-]
check_other_object: true # [-]
# collision check parameters
check_all_predicted_path: false # [-]
time_horizon: 10.0 # [s]
idling_time: 1.5 # [s]
safety_check_backward_distance: 50.0 # [m]
safety_check_time_horizon: 10.0 # [s]
safety_check_idling_time: 1.5 # [s]
safety_check_accel_for_rss: 2.5 # [m/ss]
```

Expand Down Expand Up @@ -621,12 +628,9 @@ namespace: `avoidance.`
| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 |
| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 |
| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false |
| enable_safety_check | [-] | bool | Flag to enable safety check. | false |
| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false |
| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false |

**NOTE:** It has to set both `enable_safety_check` and `enable_yield_maneuver` to enable yield maneuver.

| Name | Unit | Type | Description | Default value |
| :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- |
| enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false |
Expand Down Expand Up @@ -695,14 +699,20 @@ namespace: `avoidance.target_filtering.`

namespace: `avoidance.safety_check.`

| Name | Unit | Type | Description | Default value |
| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- |
| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 |
| safety_check_time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 |
| safety_check_idling_time | [t] | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5 |
| safety_check_accel_for_rss | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS. | 2.5 |
| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 |
| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 |
| Name | Unit | Type | Description | Default value |
| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- |
| enable | [-] | bool | Enable to use safety check feature. | true |
| check_current_lane | [-] | bool | Check objects on current driving lane. | false |
| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true |
| check_other_side_lane | [-] | bool | Check objects on other side lane. | false |
| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false |
| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false |
| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false |
| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 |
| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 |
| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 |
| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 |
| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 |

### Avoidance maneuver parameters

Expand Down
61 changes: 2 additions & 59 deletions ...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
#include "behavior_path_planner/utils/avoidance/helper.hpp"
#include "behavior_path_planner/utils/safety_check.hpp"

#include <rclcpp/rclcpp.hpp>

Expand Down Expand Up @@ -256,12 +257,6 @@ class AvoidanceModule : public SceneModuleInterface
ObjectData createObjectData(
const AvoidancePlanningData & data, const PredictedObject & object) const;

/**
* @brief get objects that are driving on adjacent lanes.
* @param left or right lanelets.
*/
ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const;

/**
* @brief fill additional data so that the module judges target objects.
* @param avoidance data.
Expand Down Expand Up @@ -481,65 +476,13 @@ class AvoidanceModule : public SceneModuleInterface

// safety check

/**
* @brief get shift side adjacent lanes.
* @param path shifter.
* @param forward distance to get.
* @param backward distance to get.
* @return adjacent lanes.
*/
lanelet::ConstLanelets getAdjacentLane(
const PathShifter & path_shifter, const double forward_distance,
const double backward_distance) const;

/**
* @brief calculate lateral margin from avoidance velocity for safety check.
* @param target velocity.
*/
double getLateralMarginFromVelocity(const double velocity) const;

/**
* @brief calculate rss longitudinal distance for safety check.
* @param ego velocity.
* @param object velocity.
* @param option for rss calculation.
* @return rss longitudinal distance.
*/
double getRSSLongitudinalDistance(
const double v_ego, const double v_obj, const bool is_front_object) const;

/**
* @brief check avoidance path safety for surround moving objects.
* @param path shifter.
* @param avoidance path.
* @param debug data.
* @return result.
*/
bool isSafePath(
const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const;

/**
* @brief check avoidance path safety for surround moving objects.
* @param avoidance path.
* @param shift side lanes.
* @param debug data.
* @return result.
*/
bool isSafePath(
const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes,
DebugData & debug) const;

/**
* @brief check predicted points safety.
* @param predicted points.
* @param future time.
* @param object data.
* @param margin data for debug.
* @return result.
*/
bool isEnoughMargin(
const PathPointWithLaneId & p_ego, const double t, const ObjectData & object,
MarginData & margin_data) const;
bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const;

// post process

Expand Down
37 changes: 17 additions & 20 deletions ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_

#include "behavior_path_planner/marker_utils/utils.hpp"
#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"

#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -46,6 +47,8 @@ using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::TransformStamped;

using marker_utils::CollisionCheckDebug;

struct ObjectParameter
{
bool is_target{false};
Expand Down Expand Up @@ -95,9 +98,6 @@ struct AvoidanceParameters
// enable avoidance for all parking vehicle
bool enable_force_avoidance_for_stopped_vehicle{false};

// enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver
bool enable_safety_check{false};

// enable yield maneuver.
bool enable_yield_maneuver{false};

Expand Down Expand Up @@ -177,20 +177,23 @@ struct AvoidanceParameters
// for longitudinal direction
double longitudinal_collision_margin_time;

// find adjacent lane vehicles
double safety_check_backward_distance;

// minimum longitudinal margin for vehicles in adjacent lane
double safety_check_min_longitudinal_margin;
// parameters for safety check area
bool enable_safety_check{false};
bool check_current_lane{false};
bool check_shift_side_lane{false};
bool check_other_side_lane{false};

// safety check time horizon
double safety_check_time_horizon;
// parameters for safety check target.
bool check_unavoidable_object{false};
bool check_other_object{false};

// use in RSS calculation
double safety_check_idling_time;
// parameters for collision check.
bool check_all_predicted_path{false};
double safety_check_time_horizon{0.0};
double safety_check_time_resolution{0.0};

// use in RSS calculation
double safety_check_accel_for_rss;
// find adjacent lane vehicles
double safety_check_backward_distance;

// transit hysteresis (unsafe to safe)
double safety_check_hysteresis_factor;
Expand Down Expand Up @@ -280,12 +283,6 @@ struct AvoidanceParameters
// Maximum lateral acceleration limitation map.
std::vector<double> lateral_max_accel_map;

// target velocity matrix
std::vector<double> target_velocity_matrix;

// matrix col size
size_t col_size;

// parameters depend on object class
std::unordered_map<uint8_t, ObjectParameter> object_parameters;

Expand Down
21 changes: 21 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,20 @@

#include "behavior_path_planner/data_manager.hpp"
#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
#include "behavior_path_planner/utils/safety_check.hpp"

#include <algorithm>
#include <memory>
#include <string>
#include <vector>

namespace behavior_path_planner::utils::avoidance
{
using behavior_path_planner::PlannerData;
using behavior_path_planner::utils::safety_check::ExtendedPredictedObject;
using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped;
using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped;
using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon;

bool isOnRight(const ObjectData & obj);

Expand Down Expand Up @@ -79,11 +85,19 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
const double vehicle_width);

std::vector<PoseWithVelocityStamped> convertToPredictedPath(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);

double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);

bool isCentroidWithinLanelets(
const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets);

lanelet::ConstLanelets getAdjacentLane(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);

lanelet::ConstLanelets getTargetLanelets(
const std::shared_ptr<const PlannerData> & planner_data, lanelet::ConstLanelets & route_lanelets,
const double left_offset, const double right_offset);
Expand Down Expand Up @@ -139,6 +153,13 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine

AvoidLineArray combineRawShiftLinesWithUniqueCheck(
const AvoidLineArray & base_lines, const AvoidLineArray & added_lines);

ExtendedPredictedObject transform(
const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);

std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
} // namespace behavior_path_planner::utils::avoidance

#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_
Loading

0 comments on commit a567bb2

Please sign in to comment.