Skip to content

Commit

Permalink
fix(avoidance): fix avoidance path chattering (autowarefoundation#2012)
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
satoshi-ota authored and tkimura4 committed Oct 13, 2022
1 parent 54da3fb commit 86e2ac5
Showing 1 changed file with 5 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ namespace behavior_path_planner
{
using motion_utils::calcSignedArcLength;
using motion_utils::findNearestIndex;
using motion_utils::findNearestSegmentIndex;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcLateralDeviation;
using tier4_autoware_utils::createPoint;
Expand Down Expand Up @@ -139,8 +140,11 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
// if the resampled path has only 1 point, use original path.
data.reference_path = center_path;
}

const size_t nearest_segment_index =
findNearestSegmentIndex(data.reference_path.points, data.reference_pose.position);
data.ego_closest_path_index =
findNearestIndex(data.reference_path.points, data.reference_pose.position);
std::min(nearest_segment_index + 1, data.reference_path.points.size() - 1);

// arclength from ego pose (used in many functions)
data.arclength_from_ego = util::calcPathArcLengthArray(
Expand Down

0 comments on commit 86e2ac5

Please sign in to comment.