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(pull_over): minor change with drivable area expansion #3515

Merged
merged 3 commits into from
Apr 24, 2023
Merged

feat(pull_over): minor change with drivable area expansion #3515

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3872672
feat(pull_over): can use different offsets for right left drivable ar…
takayuki5168 Apr 24, 2023
cd0590f
fix
takayuki5168 Apr 24, 2023
85fa413
fix
takayuki5168 Apr 24, 2023
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
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,8 +216,8 @@ void generateDrivableArea(
const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);

void generateDrivableArea(
PathWithLaneId & path, const double vehicle_length, const double vehicle_width,
const double margin, const bool is_driving_forward = true);
PathWithLaneId & path, const double vehicle_length, const double right_offset,
const double left_offset, const bool is_driving_forward = true);

lanelet::ConstLineStrings3d getMaximumDrivableArea(
const std::shared_ptr<const PlannerData> & planner_data);
Expand Down
8 changes: 6 additions & 2 deletions planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,9 +109,13 @@ boost::optional<PullOverPath> FreespacePullOver::plan(const Pose & goal_pose)
// path points is less than 2
return {};
}

const double right_offset =
planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
const double left_offset = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
utils::generateDrivableArea(
path, planner_data_->parameters.vehicle_length, planner_data_->parameters.vehicle_width,
drivable_area_margin, *is_driving_forward);
path, planner_data_->parameters.vehicle_length, right_offset, left_offset,
*is_driving_forward);
}

PullOverPath pull_over_path{};
Expand Down
24 changes: 12 additions & 12 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -971,8 +971,8 @@ void generateDrivableArea(

// generate drivable area by expanding path for freespace
void generateDrivableArea(
PathWithLaneId & path, const double vehicle_length, const double vehicle_width,
const double margin, const bool is_driving_forward)
PathWithLaneId & path, const double vehicle_length, const double right_offset,
const double left_offset, const bool is_driving_forward)
{
using tier4_autoware_utils::calcOffsetPose;

Expand Down Expand Up @@ -1007,8 +1007,8 @@ void generateDrivableArea(
for (const auto & point : resampled_path.points) {
const auto & pose = point.point.pose;

const auto left_point = calcOffsetPose(pose, 0, vehicle_width / 2.0 + margin, 0);
const auto right_point = calcOffsetPose(pose, 0, -vehicle_width / 2.0 - margin, 0);
const auto left_point = calcOffsetPose(pose, 0, left_offset, 0);
const auto right_point = calcOffsetPose(pose, 0, -right_offset, 0);

left_bound.push_back(left_point.position);
right_bound.push_back(right_point.position);
Expand All @@ -1018,32 +1018,32 @@ void generateDrivableArea(
// add backward offset point to bound
const Pose first_point =
calcOffsetPose(resampled_path.points.front().point.pose, -vehicle_length, 0, 0);
const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0);
const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0);
const Pose left_first_point = calcOffsetPose(first_point, 0, left_offset, 0);
const Pose right_first_point = calcOffsetPose(first_point, 0, -right_offset, 0);
left_bound.insert(left_bound.begin(), left_first_point.position);
right_bound.insert(right_bound.begin(), right_first_point.position);

// add forward offset point to bound
const Pose last_point =
calcOffsetPose(resampled_path.points.back().point.pose, vehicle_length, 0, 0);
const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0);
const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0);
const Pose left_last_point = calcOffsetPose(last_point, 0, left_offset, 0);
const Pose right_last_point = calcOffsetPose(last_point, 0, -right_offset, 0);
left_bound.push_back(left_last_point.position);
right_bound.push_back(right_last_point.position);
} else {
// add forward offset point to bound
const Pose first_point =
calcOffsetPose(resampled_path.points.front().point.pose, vehicle_length, 0, 0);
const Pose left_first_point = calcOffsetPose(first_point, 0, vehicle_width / 2.0 + margin, 0);
const Pose right_first_point = calcOffsetPose(first_point, 0, -vehicle_width / 2.0 - margin, 0);
const Pose left_first_point = calcOffsetPose(first_point, 0, left_offset, 0);
const Pose right_first_point = calcOffsetPose(first_point, 0, -right_offset, 0);
left_bound.insert(left_bound.begin(), left_first_point.position);
right_bound.insert(right_bound.begin(), right_first_point.position);

// add backward offset point to bound
const Pose last_point =
calcOffsetPose(resampled_path.points.back().point.pose, -vehicle_length, 0, 0);
const Pose left_last_point = calcOffsetPose(last_point, 0, vehicle_width / 2.0 + margin, 0);
const Pose right_last_point = calcOffsetPose(last_point, 0, -vehicle_width / 2.0 - margin, 0);
const Pose left_last_point = calcOffsetPose(last_point, 0, left_offset, 0);
const Pose right_last_point = calcOffsetPose(last_point, 0, -right_offset, 0);
left_bound.push_back(left_last_point.position);
right_bound.push_back(right_last_point.position);
}
Expand Down