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(interpolation): add option to enable akima spline for xy #2802

Merged
merged 1 commit into from
Feb 2, 2023
Merged

feat(interpolation): add option to enable akima spline for xy #2802

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
40 changes: 20 additions & 20 deletions common/motion_utils/include/motion_utils/resample/resample.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,15 +45,15 @@ namespace motion_utils
* @param input_path input path(point) to resample
* @param resampled_arclength arclength that contains length of each resampling points from initial
* point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
* @return resampled path(poses)
*/
std::vector<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & points,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true);

/**
Expand All @@ -63,15 +63,15 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
* based on the interpolated position x and y.
* @param input_path input path(position) to resample
* @param resample_interval resampling interval
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
* @return resampled path(poses)
*/
std::vector<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & points, const double resample_interval,
const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true);
const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true);

/**
* @brief A resampling function for a path(poses). Note that in a default setting, position xy are
Expand All @@ -81,15 +81,15 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
* @param input_path input path(poses) to resample
* @param resampled_arclength arclength that contains length of each resampling points from initial
* point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
* @return resampled path(poses)
*/
std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true);

/**
Expand All @@ -99,15 +99,15 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
* based on the interpolated position x and y.
* @param input_path input path(poses) to resample
* @param resample_interval resampling interval
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
* @return resampled path(poses)
*/
std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points, const double resample_interval,
const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true);
const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true);

/**
* @brief A resampling function for a path with lane id. Note that in a default setting, position xy
Expand All @@ -119,7 +119,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
* @param input_path input path to resample
* @param resampled_arclength arclength that contains length of each resampling points from initial
* point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
Expand All @@ -129,7 +129,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
*/
autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);

/**
Expand All @@ -141,7 +141,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
* and is_final are also interpolated by zero order hold
* @param input_path input path to resample
* @param resampled_interval resampling interval point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
Expand All @@ -152,7 +152,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
*/
autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
const double resample_interval, const bool use_lerp_for_xy = false,
const double resample_interval, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true,
const bool resample_input_path_stop_point = true);

Expand All @@ -165,7 +165,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
* @param input_path input path to resample
* @param resampled_arclength arclength that contains length of each resampling points from initial
* point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
Expand All @@ -175,7 +175,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
*/
autoware_auto_planning_msgs::msg::Path resamplePath(
const autoware_auto_planning_msgs::msg::Path & input_path,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);

/**
Expand All @@ -186,7 +186,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
* forward difference method based on the interpolated position x and y.
* @param input_path input path to resample
* @param resampled_interval resampling interval point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
Expand All @@ -197,7 +197,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
*/
autoware_auto_planning_msgs::msg::Path resamplePath(
const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval,
const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true,
const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true);

/**
Expand All @@ -210,7 +210,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
* @param input_trajectory input trajectory to resample
* @param resampled_arclength arclength that contains length of each resampling points from initial
* point
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
Expand All @@ -220,7 +220,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
*/
autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true);

/**
Expand All @@ -233,7 +233,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
* method based on the interpolated position x and y.
* @param input_trajectory input trajectory to resample
* @param resampled_interval resampling interval
* @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
* @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
* y. Otherwise, it uses spline interpolation
* @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
* Otherwise, it uses spline interpolation
Expand All @@ -245,7 +245,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
*/
autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
const double resample_interval, const bool use_lerp_for_xy = false,
const double resample_interval, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true,
const bool resample_input_trajectory_stop_point = true);
} // namespace motion_utils
Expand Down
48 changes: 26 additions & 22 deletions common/motion_utils/src/resample/resample.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ namespace motion_utils
{
std::vector<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & points,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
const bool use_lerp_for_z)
{
// validate arguments
Expand Down Expand Up @@ -60,9 +60,12 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
const auto spline = [&](const auto & input) {
return interpolation::spline(input_arclength, input, resampled_arclength);
};
const auto spline_by_akima = [&](const auto & input) {
return interpolation::splineByAkima(input_arclength, input, resampled_arclength);
};

const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x);
const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y);
const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x);
const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y);
const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z);

std::vector<geometry_msgs::msg::Point> resampled_points;
Expand All @@ -82,7 +85,7 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(

std::vector<geometry_msgs::msg::Point> resamplePointVector(
const std::vector<geometry_msgs::msg::Point> & points, const double resample_interval,
const bool use_lerp_for_xy, const bool use_lerp_for_z)
const bool use_akima_spline_for_xy, const bool use_lerp_for_z)
{
const double input_length = motion_utils::calcArcLength(points);

Expand All @@ -102,12 +105,12 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
resampling_arclength.push_back(input_length);
}

return resamplePointVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z);
return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z);
}

std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
const bool use_lerp_for_z)
{
// validate arguments
Expand All @@ -120,7 +123,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
position.at(i) = points.at(i).position;
}
const auto resampled_position =
resamplePointVector(position, resampled_arclength, use_lerp_for_xy, use_lerp_for_z);
resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);

std::vector<geometry_msgs::msg::Pose> resampled_points(resampled_position.size());

Expand Down Expand Up @@ -148,7 +151,7 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(

std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
const std::vector<geometry_msgs::msg::Pose> & points, const double resample_interval,
const bool use_lerp_for_xy, const bool use_lerp_for_z)
const bool use_akima_spline_for_xy, const bool use_lerp_for_z)
{
const double input_length = motion_utils::calcArcLength(points);

Expand All @@ -168,12 +171,12 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
resampling_arclength.push_back(input_length);
}

return resamplePoseVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z);
return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z);
}

autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
const bool use_lerp_for_z, const bool use_zero_order_hold_for_v)
{
// validate arguments
Expand Down Expand Up @@ -245,7 +248,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
};

const auto interpolated_pose =
resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z);
resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);
const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon);
const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat);
const auto interpolated_heading_rate = lerp(heading_rate);
Expand Down Expand Up @@ -279,7 +282,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(

autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z,
const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z,
const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point)
{
// validate arguments
Expand Down Expand Up @@ -339,12 +342,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
}

return resamplePath(
input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, use_zero_order_hold_for_v);
input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z,
use_zero_order_hold_for_v);
}

autoware_auto_planning_msgs::msg::Path resamplePath(
const autoware_auto_planning_msgs::msg::Path & input_path,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
const bool use_lerp_for_z, const bool use_zero_order_hold_for_v)
{
// validate arguments
Expand Down Expand Up @@ -390,7 +394,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
};

const auto interpolated_pose =
resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z);
resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);
const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon);
const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat);
const auto interpolated_heading_rate = lerp(heading_rate);
Expand Down Expand Up @@ -420,8 +424,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath(

autoware_auto_planning_msgs::msg::Path resamplePath(
const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval,
const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist,
const bool resample_input_path_stop_point)
const bool use_akima_spline_for_xy, const bool use_lerp_for_z,
const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point)
{
// validate arguments
if (!resample_utils::validate_arguments(input_path.points, resample_interval)) {
Expand Down Expand Up @@ -473,13 +477,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
}

return resamplePath(
input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z,
input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z,
use_zero_order_hold_for_twist);
}

autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist)
{
// validate arguments
Expand Down Expand Up @@ -542,7 +546,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
};

const auto interpolated_pose =
resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z);
resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);
const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon);
const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat);
const auto interpolated_heading_rate = lerp(heading_rate);
Expand Down Expand Up @@ -579,7 +583,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(

autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z,
const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z,
const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point)
{
// validate arguments
Expand Down Expand Up @@ -632,7 +636,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
}

return resampleTrajectory(
input_trajectory, resampling_arclength, use_lerp_for_xy, use_lerp_for_z,
input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z,
use_zero_order_hold_for_twist);
}

Expand Down