feat(local_planner): Support SSSP with size < 4

Eigen's implementation of cubic spline interpolation is used to interpolate Lattice SSSP. This does not work with SSSP size < 4. Perform linear interpolation instead if SSSP size < 4.
Ashuh · Feb 8, 2022 · d719fc1 · d719fc1
1 parent 32e9793
commit d719fc1
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 2 deletions.
diff --git a/local_planner/include/local_planner/reference_trajectory_generator.h b/local_planner/include/local_planner/reference_trajectory_generator.h
@@ -35,6 +35,12 @@ class ReferenceTrajectoryGenerator
 
   std::vector<geometry_msgs::Point> cubicSplineInterpolate(const std::vector<geometry_msgs::Point>& path);
 
+  std::vector<geometry_msgs::Point> linearInterpolate(const geometry_msgs::Point& from, const geometry_msgs::Point& to,
+                                                      const double step_size);
+
+  std::vector<geometry_msgs::Point> linearInterpolate(const std::vector<geometry_msgs::Point>& points,
+                                                      const double step_size);
+
   Path pointsToPath(std::vector<geometry_msgs::Point> points);
 
   /**

diff --git a/local_planner/src/reference_trajectory_generator.cpp b/local_planner/src/reference_trajectory_generator.cpp
@@ -42,7 +42,7 @@ Path ReferenceTrajectoryGenerator::generateReferencePath(Lattice& lattice)
   lattice.computeShortestPaths();
 
   // Get SSSP for each vertex in the furthest possible layer
-  static constexpr int MIN_PATH_SIZE = 4;  // Cubic spline interpolation requires at least 4 points
+  static constexpr int MIN_PATH_SIZE = 2;
   int max_offset = (lattice.getNumLateralSamples() - 1) / 2;
 
   std::vector<std::pair<std::vector<geometry_msgs::Point>, double>> sssp_results;
@@ -72,7 +72,12 @@ Path ReferenceTrajectoryGenerator::generateReferencePath(Lattice& lattice)
 
   std::vector<geometry_msgs::Point> best_sssp = getBestSSSP(sssp_results);
   visualizeSSSP(best_sssp);
-  Path reference_path = pointsToPath(cubicSplineInterpolate(best_sssp));
+
+  static constexpr int MIN_CUBIC_INTERPOLATION_POINTS = 4;  // Cubic spline interpolation requires at least 4 points
+  std::vector<geometry_msgs::Point> interpolated_sssp = best_sssp.size() < MIN_CUBIC_INTERPOLATION_POINTS ?
+                                                            linearInterpolate(best_sssp, 0.1) :
+                                                            cubicSplineInterpolate(best_sssp);
+  Path reference_path = pointsToPath(interpolated_sssp);
   return reference_path;
 }
 
@@ -115,6 +120,50 @@ ReferenceTrajectoryGenerator::cubicSplineInterpolate(const std::vector<geometry_
   return spline_points;
 }
 
+std::vector<geometry_msgs::Point> ReferenceTrajectoryGenerator::linearInterpolate(const geometry_msgs::Point& from,
+                                                                                  const geometry_msgs::Point& to,
+                                                                                  const double step_size)
+{
+  double distance = calculateDistance(from.x, from.y, to.x, to.y);
+  double dx = to.x - from.x;
+  double dy = to.y - from.y;
+  double angle = atan2(dy, dx);
+  double step_x = step_size * cos(angle);
+  double step_y = step_size * sin(angle);
+
+  std::vector<geometry_msgs::Point> points;
+  points.push_back(from);
+
+  int num_via_points = std::ceil(distance / step_size) - 1;
+
+  for (int i = 1; i <= num_via_points; ++i)
+  {
+    geometry_msgs::Point point = from;
+    point.x += i * step_x;
+    point.y += i * step_y;
+    points.push_back(point);
+  }
+
+  points.push_back(to);
+  return points;
+}
+
+std::vector<geometry_msgs::Point>
+ReferenceTrajectoryGenerator::linearInterpolate(const std::vector<geometry_msgs::Point>& points, const double step_size)
+{
+  std::vector<geometry_msgs::Point> interpolated_points;
+
+  for (int i = 0; i < points.size() - 1; ++i)
+  {
+    geometry_msgs::Point from = points.at(i);
+    geometry_msgs::Point to = points.at(i + 1);
+    std::vector<geometry_msgs::Point> interpolated_section = linearInterpolate(from, to, step_size);
+    interpolated_points.insert(interpolated_points.end(), interpolated_section.begin(), interpolated_section.end());
+  }
+
+  return interpolated_points;
+}
+
 Path ReferenceTrajectoryGenerator::pointsToPath(std::vector<geometry_msgs::Point> points)
 {
   std::vector<double> distance_vec;