fix(motion_velocity_smoother): use common ego nearest search

[takayuki5168]

Signed-off-by: Takayuki Murooka takayuki5168@gmail.com

Description

use the new nearest index search algorithm in motion velocity smoother to deal with edge cases shown in the issue.

Related links

issue
#1581

depends on
#1582
tier4/autoware_launch#437

Tests performed

Notes for reviewers

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[takayuki5168]

I confirmed this PR works well

[rej55]

@takayuki5168
Please fix the following function.

autoware.universe/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp

Lines 129 to 171 in 7748688

	inline bool smoothPath(
	const PathWithLaneId & in_path, PathWithLaneId & out_path,
	const std::shared_ptr<const PlannerData> & planner_data)
	{
	using motion_utils::findNearestIndex;
	const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose;
	const double v0 = planner_data->current_velocity->twist.linear.x;
	const double a0 = planner_data->current_accel.get();
	const auto & external_v_limit = planner_data->external_velocity_limit;
	const auto & smoother = planner_data->velocity_smoother_;
	const double max = std::numeric_limits<double>::max();
	auto trajectory = convertPathToTrajectoryPoints(in_path);
	if (external_v_limit) {
	motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
	0, trajectory.size(), external_v_limit->max_velocity, trajectory);
	}
	const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
	// Resample trajectory with ego-velocity based interval distances
	auto traj_resampled = smoother->resampleTrajectory(
	*traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits<double>::max());
	const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4);
	if (!traj_resampled_closest) {
	return false;
	}
	std::vector<TrajectoryPoints> debug_trajectories;
	// Clip trajectory from closest point
	TrajectoryPoints clipped;
	TrajectoryPoints traj_smoothed;
	clipped.insert(
	clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end());
	if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
	std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
	return false;
	}
	traj_smoothed.insert(
	traj_smoothed.begin(), traj_resampled->begin(),
	traj_resampled->begin() + *traj_resampled_closest);
	out_path = convertTrajectoryPointsToPath(traj_smoothed);
	return true;
	}

[codecov]

Codecov Report

Merging #1584 (edb5e2d) into main (1d10ff3) will decrease coverage by 0.11%.
The diff coverage is 10.82%.

@@            Coverage Diff             @@
##             main    #1584      +/-   ##
==========================================
- Coverage   10.25%   10.14%   -0.12%     
==========================================
  Files        1204     1204              
  Lines       86787    88388    +1601     
  Branches    20063    20959     +896     
==========================================
+ Hits         8904     8969      +65     
- Misses      68613    70015    +1402     
- Partials     9270     9404     +134     

Flag	Coverage Δ		*Carryforward flag
differential	3.85% <11.87%> (?)
total	10.24% <6.57%> (ø)		Carriedforward from 1d10ff3

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...ils/test/src/trajectory/test_path_with_lane_id.cpp	43.28% <0.00%> (ø)
...lanner/scene_module/avoidance/avoidance_module.hpp	0.00% <ø> (ø)
...th_planner/scene_module/scene_module_interface.hpp	0.00% <ø> (ø)
...r_path_planner/scene_module/utils/path_shifter.hpp	0.00% <ø> (ø)
...lude/behavior_path_planner/turn_signal_decider.hpp	0.00% <ø> (ø)
...lanner/include/behavior_path_planner/utilities.hpp	0.00% <ø> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.22% <0.00%> (ø)
...nning/behavior_path_planner/src/path_utilities.cpp	0.00% <0.00%> (ø)
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)
...rc/scene_module/lane_change/lane_change_module.cpp	0.00% <0.00%> (ø)
... and 85 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

[takayuki5168]

@rej55 The function you pointed out is in behavior_velocity_planner.
Now I understood that that resulted in building error. I will fix.

[takayuki5168]

I'm thinking to create another PR for behavior velocity planner.
So I will wait for it to merge this PR.

[rej55]

LGTM