Skip to content

Commit

Permalink
feat(behavior_path_planner): use object recognition for pull_over (au…
Browse files Browse the repository at this point in the history 
…towarefoundation#1777)

* feat(behavior_path_planner): use object recognition for pull_over

Signed-off-by: kosuke55 <[email protected]>

* Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

Co-authored-by: Shumpei Wakabayashi <[email protected]>
Signed-off-by: kosuke55 <[email protected]>

* rename checkCollision

Signed-off-by: kosuke55 <[email protected]>

* update docs

Signed-off-by: kosuke55 <[email protected]>

* remove unnecessary lines

Signed-off-by: kosuke55 <[email protected]>

* update warn message

Signed-off-by: kosuke55 <[email protected]>

Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: Shumpei Wakabayashi <[email protected]>
  • Loading branch information
@kosuke55 @shmpwk @boyali
2 people authored and boyali committed Oct 19, 2022
1 parent 35216cb commit cf0236b
Show file tree
Hide file tree
Showing 6 changed files with 162 additions and 118 deletions.
6 changes: 5 additions & 1 deletion ...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,13 @@
goal_search_interval: 1.0
goal_to_obj_margin: 2.0
# occupancy grid map
collision_check_margin: 0.5
use_occupancy_grid: true
occupancy_grid_collision_check_margin: 0.0
theta_size: 360
obstacle_threshold: 60
# object recognition
use_object_recognition: true
object_recognition_collision_check_margin: 1.0
# shift path
enable_shift_parking: true
pull_over_sampling_num: 4
Expand Down
20 changes: 14 additions & 6 deletions planning/behavior_path_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -238,18 +238,26 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio

##### Parameters for occupancy grid based collision check

| Name | Unit | Type | Description | Default value |
| :--------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ |
| collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.5 |
| 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 | - | bool | flag whether to use occupancy grid for collision check | true |
| 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 |

##### Parameters for object recognition based collision check

| Name | Unit | Type | Description | Default value |
| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ |
| use_object_recognition | - | bool | flag whether to use object recognition for collision check | true |
| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 |

#### **Goal Search**

If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is
searched for in certain range of the shoulder lane.

[Video of how goal search works](https://user-images.githubusercontent.com/39142679/178033628-bec1bbc7-3b27-47b1-b50b-55f855e2e399.mp4)
[Video of how goal search works](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4)

##### Parameters for goal search

Expand Down
6 changes: 5 additions & 1 deletion planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,13 @@
goal_search_interval: 1.0
goal_to_obj_margin: 2.0
# occupancy grid map
collision_check_margin: 0.5
use_occupancy_grid: true
occupancy_grid_collision_check_margin: 0.0
theta_size: 360
obstacle_threshold: 60
# object recognition
use_object_recognition: true
object_recognition_collision_check_margin: 1.0
# shift path
enable_shift_parking: true
pull_over_sampling_num: 4
Expand Down
39 changes: 20 additions & 19 deletions ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,13 @@ struct PullOverParameters
double goal_search_interval;
double goal_to_obj_margin;
// occupancy grid map
double collision_check_margin;
bool use_occupancy_grid;
double occupancy_grid_collision_check_margin;
double theta_size;
double obstacle_threshold;
// object recognition
bool use_object_recognition;
double object_recognition_collision_check_margin;
// shift path
bool enable_shift_parking;
int pull_over_sampling_num;
Expand Down Expand Up @@ -107,11 +111,11 @@ enum PathType {

struct PUllOverStatus
{
PathWithLaneId path;
PathWithLaneId path{};
std::shared_ptr<PathWithLaneId> prev_stop_path = nullptr;
lanelet::ConstLanelets current_lanes;
lanelet::ConstLanelets pull_over_lanes;
lanelet::ConstLanelets lanes; // current + pull_over
lanelet::ConstLanelets current_lanes{};
lanelet::ConstLanelets pull_over_lanes{};
lanelet::ConstLanelets lanes{}; // current + pull_over
bool has_decided_path = false;
int path_type = PathType::NONE;
bool is_safe = false;
Expand All @@ -120,18 +124,12 @@ struct PUllOverStatus
bool has_requested_approval_ = false;
};

struct PullOverArea
{
Pose start_pose;
Pose end_pose;
};

struct GoalCandidate
{
Pose goal_pose;
double distance_from_original_goal;
Pose goal_pose{};
double distance_from_original_goal = 0.0;

bool operator<(const GoalCandidate & other) noexcept
bool operator<(const GoalCandidate & other) const noexcept
{
return distance_from_original_goal < other.distance_from_original_goal;
}
Expand Down Expand Up @@ -161,11 +159,12 @@ class PullOverModule : public SceneModuleInterface

private:
PullOverParameters parameters_;

ShiftParkingPath shift_parking_path_;
rclcpp::Node * node_;
vehicle_info_util::VehicleInfo vehicle_info_;

double pull_over_lane_length_ = 200.0;
double check_distance_ = 100.0;
const double pull_over_lane_length_ = 200.0;
const double check_distance_ = 100.0;

rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_sub_;
rclcpp::Publisher<PoseStamped>::SharedPtr Cr_pub_;
Expand All @@ -174,17 +173,17 @@ class PullOverModule : public SceneModuleInterface
rclcpp::Publisher<PoseStamped>::SharedPtr goal_pose_pub_;
rclcpp::Publisher<PoseArray>::SharedPtr path_pose_array_pub_;
rclcpp::Publisher<MarkerArray>::SharedPtr parking_area_pub_;
rclcpp::Clock::SharedPtr clock_;

PUllOverStatus status_;
OccupancyGridBasedCollisionDetector occupancy_grid_map_;
std::vector<PullOverArea> pull_over_areas_;
Pose modified_goal_pose_;
Pose refined_goal_pose_;
std::vector<GoalCandidate> goal_candidates_;
GeometricParallelParking parallel_parking_planner_;
ParallelParkingParameters parallel_parking_parameters_;
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
std::unique_ptr<LaneDepartureChecker> lane_departure_checker_;
tier4_autoware_utils::LinearRing2d vehicle_footprint_;
std::unique_ptr<rclcpp::Time> last_received_time_;
std::unique_ptr<rclcpp::Time> last_approved_time_;

Expand All @@ -209,6 +208,8 @@ class PullOverModule : public SceneModuleInterface
void updateOccupancyGrid();
void researchGoal();
void resetStatus();
bool checkCollisionWithPose(const Pose & pose) const;
bool checkCollisionWithPath(const PathWithLaneId & path) const;

// turn signal
std::pair<HazardLightsCommand, double> getHazardInfo() const;
Expand Down
7 changes: 6 additions & 1 deletion planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -379,9 +379,14 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam()
p.goal_search_interval = dp("goal_search_interval", 5.0);
p.goal_to_obj_margin = dp("goal_to_obj_margin", 2.0);
// occupancy grid map
p.collision_check_margin = dp("collision_check_margin", 0.5);
p.use_occupancy_grid = dp("use_occupancy_grid", true);
p.occupancy_grid_collision_check_margin = dp("occupancy_grid_collision_check_margin", 0.0);
p.theta_size = dp("theta_size", 360);
p.obstacle_threshold = dp("obstacle_threshold", 90);
// object recognition
p.use_object_recognition = dp("use_object_recognition", true);
p.object_recognition_collision_check_margin =
dp("object_recognition_collision_check_margin", 1.0);
// shift path
p.enable_shift_parking = dp("enable_shift_parking", true);
p.pull_over_sampling_num = dp("pull_over_sampling_num", 4);
Expand Down
Loading

0 comments on commit cf0236b

Please sign in to comment.