Skip to content

Commit

Permalink
Merge pull request #846 from tier4/feat/update_start_goal_planner_v0.…
Browse files Browse the repository at this point in the history 
…10.1

feat: update start goal planner v0.10.1
  • Loading branch information
@tkimura4
tkimura4 authored Sep 19, 2023
2 parents 38ea8bf + 586bd26 commit 5aca319
Show file tree
Hide file tree
Showing 26 changed files with 541 additions and 308 deletions.
12 changes: 7 additions & 5 deletions planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
ros__parameters:
goal_planner:
# general params
minimum_request_length: 100.0
th_arrived_distance: 1.0
th_stopped_velocity: 0.01
th_stopped_time: 2.0 # It must be greater than the state_machine's.
Expand All @@ -17,25 +16,28 @@
longitudinal_margin: 3.0
max_lateral_offset: 0.5
lateral_offset_interval: 0.25
ignore_distance_from_lane_start: 15.0
ignore_distance_from_lane_start: 0.0
margin_from_boundary: 0.5

# occupancy grid map
occupancy_grid:
use_occupancy_grid: true
use_occupancy_grid_for_longitudinal_margin: false
use_occupancy_grid_for_goal_search: true
use_occupancy_grid_for_goal_longitudinal_margin: false
use_occupancy_grid_for_path_collision_check: false
occupancy_grid_collision_check_margin: 0.0
theta_size: 360
obstacle_threshold: 60

# object recognition
object_recognition:
use_object_recognition: true
use_object_recognition: false
object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker
object_recognition_collision_check_max_extra_stopping_margin: 1.0
th_moving_object_velocity: 1.0

# pull over
pull_over:
minimum_request_length: 100.0
pull_over_velocity: 3.0
pull_over_minimum_velocity: 1.38
decide_path_distance: 10.0
Expand Down
53 changes: 29 additions & 24 deletions planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,16 +86,16 @@ Either one is activated when all conditions are met.

### fixed_goal_planner

- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
- Route is set with `allow_goal_modification=false` by default.
- ego-vehicle is in the same lane as the goal.

<img src="https://user-images.githubusercontent.com/39142679/237929955-c0adf01b-9e3c-45e3-848d-98cf11e52b65.png" width="600">

### rough_goal_planner

#### pull over on road lane

- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`.
- Route is set with `allow_goal_modification=true` .
- We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
- We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
Expand All @@ -105,7 +105,7 @@ Either one is activated when all conditions are met.

#### pull over on shoulder lane

- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`.
- Goal is set in the `road_shoulder`.

<img src="https://user-images.githubusercontent.com/39142679/237929941-2ce26ea5-c84d-4d17-8cdc-103f5246db90.png" width="600">
Expand All @@ -118,17 +118,11 @@ Either one is activated when all conditions are met.

## General parameters for goal_planner

| Name | Unit | Type | Description | Default value |
| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 |
| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 |
| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 |
| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 |
| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 |
| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 |
| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 |
| Name | Unit | Type | Description | Default value |
| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 |

## **collision check**

Expand All @@ -138,13 +132,14 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio

#### Parameters for occupancy grid based collision check

| Name | Unit | Type | Description | Default value |
| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ |
| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true |
| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false |
| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 |
| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 |
| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 |
| Name | Unit | Type | Description | Default value |
| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ |
| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true |
| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false |
| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false |
| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 |
| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 |
| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 |

### **object recognition based collision check**

Expand Down Expand Up @@ -174,12 +169,22 @@ searched for in certain range of the shoulder lane.
| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 |
| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 |
| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 |
| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 |
| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 |
| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 |
| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 |

## **Path Generation**
## **Pull Over**

There are three path generation methods.
The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane.
The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane.

| Name | Unit | Type | Description | Default value |
| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 |
| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 |
| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 |
| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 |
| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 |

### **shift parking**

Expand Down
1 change: 0 additions & 1 deletion ...behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
std::shared_ptr<GoalPlannerParameters> parameters_;

std::vector<std::shared_ptr<GoalPlannerModule>> registered_modules_;
bool left_side_parking_;
};

} // namespace behavior_path_planner
Expand Down
10 changes: 6 additions & 4 deletions ...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,8 @@ struct PullOutStatus
PathWithLaneId backward_path{};
lanelet::ConstLanelets pull_out_lanes{};
bool is_safe{false};
bool back_finished{false};
bool back_finished{false}; // if backward driving is not required, this is also set to true
// todo: rename to clear variable name.
Pose pull_out_start_pose{};

PullOutStatus() {}
Expand Down Expand Up @@ -122,7 +123,8 @@ class StartPlannerModule : public SceneModuleInterface
std::mutex mutex_;

PathWithLaneId getFullPath() const;
std::vector<Pose> searchPullOutStartPoses();
PathWithLaneId calcStartPoseCandidatesBackwardPath() const;
std::vector<Pose> searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const;

std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;

Expand All @@ -132,8 +134,8 @@ class StartPlannerModule : public SceneModuleInterface
void incrementPathIndex();
PathWithLaneId getCurrentPath() const;
void planWithPriority(
const std::vector<Pose> & start_pose_candidates, const Pose & goal_pose,
const std::string search_priority);
const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
const Pose & goal_pose, const std::string search_priority);
PathWithLaneId generateStopPath() const;
lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const;
std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
Expand Down
8 changes: 5 additions & 3 deletions ...path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,6 @@ enum class ParkingPolicy {
struct GoalPlannerParameters
{
// general params
double minimum_request_length;
double th_arrived_distance;
double th_stopped_velocity;
double th_stopped_time;
Expand All @@ -59,8 +58,9 @@ struct GoalPlannerParameters
double margin_from_boundary;

// occupancy grid map
bool use_occupancy_grid;
bool use_occupancy_grid_for_longitudinal_margin;
bool use_occupancy_grid_for_goal_search;
bool use_occupancy_grid_for_goal_longitudinal_margin;
bool use_occupancy_grid_for_path_collision_check;
double occupancy_grid_collision_check_margin;
int theta_size;
int obstacle_threshold;
Expand All @@ -69,8 +69,10 @@ struct GoalPlannerParameters
bool use_object_recognition;
double object_recognition_collision_check_margin;
double object_recognition_collision_check_max_extra_stopping_margin;
double th_moving_object_velocity;

// pull over general params
double pull_over_minimum_request_length;
double pull_over_velocity;
double pull_over_minimum_velocity;
double decide_path_distance;
Expand Down
10 changes: 5 additions & 5 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,15 +46,15 @@ using visualization_msgs::msg::MarkerArray;

// TODO(sugahara) move to util
PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side);
lanelet::ConstLanelets getPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance);
PredictedObjects filterObjectsByLateralDistance(
const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
const double distance_thresh, const bool filter_inside);

bool isAllowedGoalModification(
const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking);
bool checkOriginalGoalIsInShoulder(
const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking);
bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);

// debug
MarkerArray createPullOverAreaMarkerArray(
Expand Down
2 changes: 1 addition & 1 deletion ...ior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase

PlannerType getPlannerType() override { return PlannerType::FREESPACE; }

boost::optional<PullOutPath> plan(Pose start_pose, Pose end_pose) override;
boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & end_pose) override;

protected:
std::unique_ptr<AbstractPlanningAlgorithm> planner_;
Expand Down
2 changes: 1 addition & 1 deletion ...ior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase
explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);

PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) override;
boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;

GeometricParallelParking planner_;
ParallelParkingParameters parallel_parking_parameters_;
Expand Down
2 changes: 1 addition & 1 deletion ..._path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ class PullOutPlannerBase
}

virtual PlannerType getPlannerType() = 0;
virtual boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) = 0;
virtual boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) = 0;

protected:
std::shared_ptr<const PlannerData> planner_data_;
Expand Down
2 changes: 1 addition & 1 deletion ...ehavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase
std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);

PlannerType getPlannerType() override { return PlannerType::SHIFT; };
boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) override;
boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;

std::vector<PullOutPath> calcPullOutPaths(
const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
Expand Down
Loading

0 comments on commit 5aca319

Please sign in to comment.