Skip to content

Commit

Permalink
Properly handle max_arc_length = 0.0
Browse files Browse the repository at this point in the history
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
  • Loading branch information
maxime-clem committed Oct 18, 2023
1 parent 6bcdab6 commit 826b27c
Showing 1 changed file with 6 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,12 @@ void reuse_previous_poses(
if (cropped_poses.empty()) {
const auto resampled_path_points =
motion_utils::resamplePath(path, params.resample_interval, true, true, false).points;
const auto cropped_path = motion_utils::cropForwardPoints(
path.points, resampled_path_points.front().point.pose.position, 0,
params.max_path_arc_length);
const auto cropped_path =
params.max_path_arc_length <= 0.0
? resampled_path_points
: motion_utils::cropForwardPoints(
resampled_path_points, resampled_path_points.front().point.pose.position, 0,
params.max_path_arc_length);
for (const auto & p : cropped_path) cropped_poses.push_back(p.point.pose);
} else {
const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses);
Expand Down

0 comments on commit 826b27c

Please sign in to comment.