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.

Sign up for GitHub

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

Jump to bottom

feat(behavior_path_planner): use object recognition for pull_over #1777

Merged
merged 6 commits into from
Sep 5, 2022
Merged

feat(behavior_path_planner): use object recognition for pull_over #1777

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
32436db
feat(behavior_path_planner): use object recognition for pull_over
kosuke55 Aug 8, 2022
ada4bb3
Update planning/behavior_path_planner/src/scene_module/pull_over/pull…
kosuke55 Sep 5, 2022
ad49941
rename checkCollision
kosuke55 Sep 5, 2022
ac7b882
update docs
kosuke55 Sep 5, 2022
d2c3016
remove unnecessary lines
kosuke55 Sep 5, 2022
1dcd4c3
update warn message
kosuke55 Sep 5, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion ...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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