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

fix(goal_planner): check lateral distance for previous output path shape change check #1326

Merged
merged 2 commits into from
Jun 17, 2024
Merged

fix(goal_planner): check lateral distance for previous output path shape change check #1326

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
06204da
fix(goal_planner): check lateral distance for previous output path sh…
kosuke55 Jun 5, 2024
bfc8828
Merge branch 'beta/v0.19.1' into fix/goal_planner_prev_shape_check_x2
mkuri Jun 6, 2024
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
38 changes: 32 additions & 6 deletions planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,26 +138,52 @@ void GoalPlannerModule::updateOccupancyGrid()

bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const
{
constexpr double LATERAL_DEVIATION_THRESH = 0.3;
if (!last_previous_module_output_) {
return true;
}

// Calculate the lateral distance between each point of the current path and the nearest point of
// the last path
const auto current_path = getPreviousModuleOutput().path;

// the terminal distance is far
return calcDistance2d(
last_previous_module_output_->path.points.back().point,
current_path.points.back().point) > 0.3;
for (const auto & p : current_path.points) {
const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(
last_previous_module_output_->path.points, p.point.pose.position);
const auto seg_front = last_previous_module_output_->path.points.at(nearest_seg_idx);
const auto seg_back = last_previous_module_output_->path.points.at(nearest_seg_idx + 1);
// Check if the target point is within the segment
const Eigen::Vector3d segment_vec{
seg_back.point.pose.position.x - seg_front.point.pose.position.x,
seg_back.point.pose.position.y - seg_front.point.pose.position.y, 0.0};
const Eigen::Vector3d target_vec{
p.point.pose.position.x - seg_front.point.pose.position.x,
p.point.pose.position.y - seg_front.point.pose.position.y, 0.0};
const double dot_product = segment_vec.x() * target_vec.x() + segment_vec.y() * target_vec.y();
const double segment_length_squared =
segment_vec.x() * segment_vec.x() + segment_vec.y() * segment_vec.y();
if (dot_product < 0 || dot_product > segment_length_squared) {
// p.point.pose.position is not within the segment, skip lateral distance check
continue;
}
const double lateral_distance = std::abs(motion_utils::calcLateralOffset(
last_previous_module_output_->path.points, p.point.pose.position, nearest_seg_idx));
if (lateral_distance > LATERAL_DEVIATION_THRESH) {
return true;
}
}
return false;
}

bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const
{
constexpr double LATERAL_DEVIATION_THRESH = 0.3;

if (!last_previous_module_output_) {
return true;
}
return std::abs(motion_utils::calcLateralOffset(
last_previous_module_output_->path.points,
planner_data_->self_odometry->pose.pose.position)) > 0.3;
planner_data_->self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH;
}

// generate pull over candidate paths
Expand Down
Loading