Skip to content

Commit 2f379a6

Browse files
fix(obstacle_avoidance_planner, interpolation): improve slerp and fix yaw (#1687)
* merge slerp of x y yaw Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp> * ci(pre-commit): autofix * move and rename new slerp Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp> * add include file Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp> Signed-off-by: Xinyu Wang <xinyu.wang@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent fd9677c commit 2f379a6

File tree

4 files changed

+75
-1
lines changed

4 files changed

+75
-1
lines changed

common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,10 @@
2121

2222
namespace interpolation
2323
{
24+
std::array<std::vector<double>, 3> slerp2dFromXY(
25+
const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
26+
const std::vector<double> & base_y_values, const std::vector<double> & query_keys);
27+
2428
template <typename T>
2529
std::vector<double> splineYawFromPoints(const std::vector<T> & points);
2630
} // namespace interpolation

common/interpolation/src/spline_interpolation_points_2d.cpp

+24
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,29 @@ std::array<std::vector<double>, 3> getBaseValues(
6868

6969
namespace interpolation
7070
{
71+
72+
std::array<std::vector<double>, 3> slerp2dFromXY(
73+
const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
74+
const std::vector<double> & base_y_values, const std::vector<double> & query_keys)
75+
{
76+
SplineInterpolation interpolator_x, interpolator_y;
77+
std::vector<double> yaw_vec;
78+
79+
// calculate spline coefficients
80+
interpolator_x.calcSplineCoefficients(base_keys, base_x_values);
81+
interpolator_y.calcSplineCoefficients(base_keys, base_y_values);
82+
const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys);
83+
const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys);
84+
for (size_t i = 0; i < diff_x.size(); i++) {
85+
double yaw = std::atan2(diff_y[i], diff_x[i]);
86+
yaw_vec.push_back(yaw);
87+
}
88+
// interpolate base_keys at query_keys
89+
return {
90+
interpolator_x.getSplineInterpolatedValues(query_keys),
91+
interpolator_y.getSplineInterpolatedValues(query_keys), yaw_vec};
92+
}
93+
7194
template <typename T>
7295
std::vector<double> splineYawFromPoints(const std::vector<T> & points)
7396
{
@@ -86,6 +109,7 @@ std::vector<double> splineYawFromPoints(const std::vector<T> & points)
86109
}
87110
template std::vector<double> splineYawFromPoints(
88111
const std::vector<geometry_msgs::msg::Point> & points);
112+
89113
} // namespace interpolation
90114

91115
geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint(

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include "eigen3/Eigen/Core"
1919
#include "interpolation/linear_interpolation.hpp"
2020
#include "interpolation/spline_interpolation.hpp"
21+
#include "interpolation/spline_interpolation_points_2d.hpp"
2122
#include "motion_utils/trajectory/trajectory.hpp"
2223
#include "obstacle_avoidance_planner/common_structs.hpp"
2324

@@ -115,6 +116,9 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
115116
const std::vector<double> & base_x, const std::vector<double> & base_y,
116117
const std::vector<double> & base_yaw, const double resolution);
117118

119+
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTrajectoryPoints(
120+
const std::vector<double> & base_x, const std::vector<double> & base_y, const double resolution);
121+
118122
template <typename T>
119123
std::vector<geometry_msgs::msg::Point> getInterpolatedPoints(
120124
const T & points, const double delta_arc_length, const double offset = 0)

planning/obstacle_avoidance_planner/src/utils.cpp

+43-1
Original file line numberDiff line numberDiff line change
@@ -366,13 +366,55 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
366366
return interpolated_points;
367367
}
368368

369+
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTrajectoryPoints(
370+
const std::vector<double> & base_x, const std::vector<double> & base_y, const double resolution)
371+
{
372+
if (base_x.empty() || base_y.empty()) {
373+
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
374+
}
375+
std::vector<double> base_s = calcEuclidDist(base_x, base_y);
376+
if (base_s.empty() || base_s.size() == 1) {
377+
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
378+
}
379+
std::vector<double> new_s;
380+
for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) {
381+
new_s.push_back(i);
382+
}
383+
384+
// spline interpolation
385+
// x = interpolated[0], y = interpolated[1], yaw = interpolated[2]
386+
std::array<std::vector<double>, 3> interpolated =
387+
interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s);
388+
const auto & interpolated_x = interpolated[0];
389+
const auto & interpolated_y = interpolated[1];
390+
const auto & interpolated_yaw = interpolated[2];
391+
392+
for (size_t i = 0; i < interpolated_x.size(); i++) {
393+
if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
394+
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
395+
}
396+
}
397+
398+
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
399+
for (size_t i = 0; i < interpolated_x.size(); i++) {
400+
autoware_auto_planning_msgs::msg::TrajectoryPoint point;
401+
point.pose.position.x = interpolated_x[i];
402+
point.pose.position.y = interpolated_y[i];
403+
point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
404+
tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
405+
interpolated_points.push_back(point);
406+
}
407+
408+
return interpolated_points;
409+
}
410+
369411
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getInterpolatedTrajectoryPoints(
370412
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
371413
const double delta_arc_length)
372414
{
373415
std::array<std::vector<double>, 3> validated_pose = validateTrajectoryPoints(points);
374416
return interpolation_utils::interpolate2DTrajectoryPoints(
375-
validated_pose.at(0), validated_pose.at(1), validated_pose.at(2), delta_arc_length);
417+
validated_pose.at(0), validated_pose.at(1), delta_arc_length);
376418
}
377419

378420
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getConnectedInterpolatedPoints(

0 commit comments

Comments
 (0)