feat(local_planner): Stop at end of global path

local_planner/include/local_planner/acceleration_regulator.h

@@ -1,13 +1,13 @@
 #ifndef LOCAL_PLANNER_ACCELERATION_REGULATOR_H
 #define LOCAL_PLANNER_ACCELERATION_REGULATOR_H
 
-#include <utility>
-#include <vector>
-
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Pose.h>
 #include <nav_msgs/Path.h>
 
+#include <utility>
+#include <vector>
+
 #include "local_planner/path.h"
 
 class AccelerationRegulator
@@ -26,11 +26,14 @@ class AccelerationRegulator
 
   void setMaxLongitudinalDeceleration(const double deceleration);
 
+  void setZeroFinalVelocity(const bool has_zero_final_velocity);
+
 private:
   double max_speed_;
   double max_lat_acc_;
   double max_lon_acc_;
   double max_lon_dec_;
+  bool has_zero_final_velocity_;
 
   bool isValidProfile(const Path& path, const std::vector<double>& velocity_profile) const;
 

local_planner/include/local_planner/lattice.h

@@ -1,19 +1,19 @@
 #ifndef LOCAL_PLANNER_LATTICE_H
 #define LOCAL_PLANNER_LATTICE_H
 
-#include <utility>
-#include <string>
-#include <vector>
-
-#include <boost/graph/adjacency_list.hpp>
-#include <boost/optional.hpp>
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Pose.h>
 #include <grid_map_msgs/GridMap.h>
-#include <grid_map_ros/grid_map_ros.hpp>
 #include <nav_msgs/Path.h>
 #include <visualization_msgs/MarkerArray.h>
 
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/optional.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <string>
+#include <utility>
+#include <vector>
+
 #include "costmap_generator/collision_checker.h"
 
 class Lattice
@@ -67,6 +67,7 @@ class Lattice
 
   int num_layers_;
   int num_lateral_samples_;
+  bool is_end_of_path_;
 
   Graph graph_;
   PositionMap position_map_;
@@ -75,7 +76,7 @@ class Lattice
   std::vector<double> distances_;
 
   Lattice(const Graph& graph, const PositionMap& position_map, const VertexDescriptor source_id, const int num_layers,
-          const int num_lateral_samples);
+          const int num_lateral_samples, const bool is_end_of_path);
 
 public:
   class Generator
@@ -137,6 +138,8 @@ class Lattice
 
   int getNumLateralSamples() const;
 
+  bool isEndOfPath() const;
+
   visualization_msgs::Marker generateVertexMarker(const int marker_id, const std::string& ns, const double scale,
                                                   const double r, const double g, const double b,
                                                   const double a = 1.0) const;

local_planner/include/local_planner/reference_trajectory_generator.h

@@ -1,14 +1,14 @@
 #ifndef LOCAL_PLANNER_REFERENCE_TRAJECTORY_GENERATOR_H
 #define LOCAL_PLANNER_REFERENCE_TRAJECTORY_GENERATOR_H
 
-#include <utility>
-#include <vector>
-
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Pose.h>
 #include <nav_msgs/Path.h>
 #include <visualization_msgs/MarkerArray.h>
 
+#include <utility>
+#include <vector>
+
 #include "local_planner/acceleration_regulator.h"
 #include "local_planner/lattice.h"
 #include "local_planner/path.h"
@@ -28,7 +28,7 @@ class ReferenceTrajectoryGenerator
   std::shared_ptr<AccelerationRegulator> acc_regulator_ptr_;
   std::shared_ptr<visualization_msgs::MarkerArray> viz_ptr_;
 
-  Path generateReferencePath(const geometry_msgs::Pose& current_pose);
+  Path generateReferencePath(Lattice& lattice);
 
   std::vector<geometry_msgs::Point>
   getBestSSSP(std::vector<std::pair<std::vector<geometry_msgs::Point>, double>>& sssp_results);

local_planner/src/acceleration_regulator.cpp

@@ -1,15 +1,16 @@
+#include "local_planner/acceleration_regulator.h"
+
 #include <utility>
 #include <vector>
 
-#include "local_planner/acceleration_regulator.h"
-
 AccelerationRegulator::AccelerationRegulator(const double max_speed, const double max_lat_acc, const double max_lon_acc,
                                              const double max_lon_dec)
 {
   setMaxSpeed(max_speed);
   setMaxLateralAcceleration(max_lat_acc);
   setMaxLongitudinalAcceleration(max_lon_acc);
   setMaxLongitudinalDeceleration(max_lon_dec);
+  has_zero_final_velocity_ = false;
 }
 
 std::vector<double> AccelerationRegulator::generateVelocityProfile(const Path& path) const
@@ -22,6 +23,11 @@ std::vector<double> AccelerationRegulator::generateVelocityProfile(const Path& p
     velocity_profile.push_back(std::min(max_speed_, curvature_speed_limit));
   }
 
+  if (has_zero_final_velocity_)
+  {
+    velocity_profile.back() = 0;
+  }
+
   do
   {
     std::vector<std::pair<int, int>> regions = identifyRegions(velocity_profile);
@@ -136,6 +142,11 @@ AccelerationRegulator::identifyRegions(const std::vector<double>& velocity_profi
   return regions;
 }
 
+void AccelerationRegulator::setZeroFinalVelocity(const bool has_zero_final_velocity)
+{
+  has_zero_final_velocity_ = has_zero_final_velocity;
+}
+
 bool AccelerationRegulator::isValidProfile(const Path& path, const std::vector<double>& velocity_profile) const
 {
   for (int i = 0; i < path.size() - 1; ++i)

local_planner/src/lattice.cpp

@@ -1,22 +1,23 @@
-#include <limits>
-#include <string>
-#include <utility>
-#include <vector>
+#include "local_planner/lattice.h"
 
-#include <boost/graph/adjacency_list.hpp>
-#include <boost/graph/dijkstra_shortest_paths.hpp>
-#include <boost/optional.hpp>
 #include <geometry_msgs/Point.h>
 #include <grid_map_msgs/GridMap.h>
-#include <grid_map_ros/grid_map_ros.hpp>
 #include <nav_msgs/Path.h>
 #include <visualization_msgs/MarkerArray.h>
 
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include <boost/optional.hpp>
+#include <grid_map_ros/grid_map_ros.hpp>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
 #include "costmap_generator/collision_checker.h"
 #include "costmap_generator/costmap_value.h"
 #include "f1tenth_utils/math.h"
 #include "f1tenth_utils/tf2_wrapper.h"
-#include "local_planner/lattice.h"
 
 /* -------------------------------------------------------------------------- */
 /*                              Lattice Position                              */
@@ -182,7 +183,8 @@ Lattice Lattice::Generator::generate(const geometry_msgs::Pose& source_pose) con
     }
   }
 
-  return Lattice(graph, map, source_id, reference_poses.size(), 2 * num_lateral_samples_per_side_ + 1);
+  bool is_end_of_path = reference_poses.back() == global_path_->poses.back().pose;
+  return Lattice(graph, map, source_id, reference_poses.size(), 2 * num_lateral_samples_per_side_ + 1, is_end_of_path);
 }
 
 int Lattice::Generator::getNearestWaypointId(const geometry_msgs::Pose& current_pose) const
@@ -352,13 +354,14 @@ void Lattice::Generator::setMovementWeight(const double weight)
 /* -------------------------------------------------------------------------- */
 
 Lattice::Lattice(const Graph& graph, const PositionMap& position_map, const VertexDescriptor source_id,
-                 const int num_layers, const int num_lateral_samples)
+                 const int num_layers, const int num_lateral_samples, const bool is_end_of_path)
 {
   graph_ = graph;
   position_map_ = position_map;
   source_id_ = source_id;
   num_layers_ = num_layers;
   num_lateral_samples_ = num_lateral_samples;
+  is_end_of_path_ = is_end_of_path;
 }
 
 void Lattice::computeShortestPaths()
@@ -446,6 +449,11 @@ int Lattice::getNumLateralSamples() const
   return num_lateral_samples_;
 }
 
+bool Lattice::isEndOfPath() const
+{
+  return is_end_of_path_;
+}
+
 visualization_msgs::Marker Lattice::generateVertexMarker(const int marker_id, const std::string& ns, const double scale,
                                                          const double r, const double g, const double b,
                                                          const double a) const

local_planner/src/reference_trajectory_generator.cpp

@@ -1,16 +1,18 @@
-#include <algorithm>
-#include <utility>
-#include <vector>
+#include "local_planner/reference_trajectory_generator.h"
 
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Pose.h>
 #include <nav_msgs/Path.h>
+
+#include <algorithm>
 #include <unsupported/Eigen/Splines>
+#include <utility>
+#include <vector>
 
+#include "f1tenth_utils/math.h"
 #include "local_planner/acceleration_regulator.h"
 #include "local_planner/lattice.h"
 #include "local_planner/path.h"
-#include "local_planner/reference_trajectory_generator.h"
 #include "local_planner/trajectory.h"
 
 ReferenceTrajectoryGenerator::ReferenceTrajectoryGenerator(
@@ -25,18 +27,18 @@ ReferenceTrajectoryGenerator::ReferenceTrajectoryGenerator(
 
 Trajectory ReferenceTrajectoryGenerator::generateReferenceTrajectory(const geometry_msgs::Pose& current_pose)
 {
-  Path reference_path = generateReferencePath(current_pose);
+  Lattice lattice = lat_gen_ptr_->generate(current_pose);
+  visualizeLattice(lattice);
+  Path reference_path = generateReferencePath(lattice);
+  acc_regulator_ptr_->setZeroFinalVelocity(lattice.isEndOfPath());
   Trajectory reference_trajectory(reference_path, *acc_regulator_ptr_);
   visualizeReferenceTrajectory(reference_trajectory);
 
   return reference_trajectory;
 }
 
-Path ReferenceTrajectoryGenerator::generateReferencePath(const geometry_msgs::Pose& current_pose)
+Path ReferenceTrajectoryGenerator::generateReferencePath(Lattice& lattice)
 {
-  // Generate lattice
-  Lattice lattice = lat_gen_ptr_->generate(current_pose);
-  visualizeLattice(lattice);
   lattice.computeShortestPaths();
 
   // Get SSSP for each vertex in the furthest possible layer