feat(obstacle_avoidance_planner): make MPT's output not std::optional

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -109,8 +109,9 @@ class MPTOptimizer
     const std::shared_ptr<DebugData> debug_data_ptr,
     const std::shared_ptr<TimeKeeper> time_keeper_ptr);
 
-  std::optional<std::vector<TrajectoryPoint>> getModelPredictiveTrajectory(
+  std::vector<TrajectoryPoint> optimizeTrajectory(
     const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points);
+  std::optional<std::vector<TrajectoryPoint>> getPrevOptimizedTrajectoryPoints() const;
 
   void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param);
   void resetPreviousData();
@@ -237,6 +238,7 @@ class MPTOptimizer
   int prev_mat_n_ = 0;
   int prev_mat_m_ = 0;
   std::shared_ptr<std::vector<ReferencePoint>> prev_ref_points_ptr_{nullptr};
+  std::shared_ptr<std::vector<TrajectoryPoint>> prev_optimized_traj_points_ptr_{nullptr};
 
   void updateVehicleCircles();
 

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -78,9 +78,6 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   // variables for subscribers
   Odometry::SharedPtr ego_state_ptr_;
 
-  // variables for previous information
-  std::shared_ptr<std::vector<TrajectoryPoint>> prev_optimized_traj_points_ptr_;
-
   // interface publisher
   rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_pub_;

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -474,6 +474,7 @@ void MPTOptimizer::initialize(const bool enable_debug_info, const TrajectoryPara
 void MPTOptimizer::resetPreviousData()
 {
   prev_ref_points_ptr_ = nullptr;
+  prev_optimized_traj_points_ptr_ = nullptr;
 }
 
 void MPTOptimizer::onParam(const std::vector<rclcpp::Parameter> & parameters)
@@ -483,20 +484,27 @@ void MPTOptimizer::onParam(const std::vector<rclcpp::Parameter> & parameters)
   debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num;
 }
 
-std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::getModelPredictiveTrajectory(
+std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
   const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points)
 {
   time_keeper_ptr_->tic(__func__);
 
   const auto & p = planner_data;
   const auto & traj_points = p.traj_points;
 
+  const auto get_prev_optimized_traj_points = [&]() {
+    if (prev_optimized_traj_points_ptr_) {
+      return *prev_optimized_traj_points_ptr_;
+    }
+    return smoothed_points;
+  };
+
   // 1. calculate reference points
   auto ref_points = calcReferencePoints(planner_data, smoothed_points);
   if (ref_points.size() < 2) {
     RCLCPP_INFO_EXPRESSION(
       logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2.");
-    return std::nullopt;
+    return get_prev_optimized_traj_points();
   }
 
   // 2. calculate B and W matrices where x = B u + W
@@ -516,14 +524,14 @@ std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::getModelPredictiveTraj
   if (!optimized_steer_angles) {
     RCLCPP_INFO_EXPRESSION(
       logger_, enable_debug_info_, "return std::nullopt since could not solve qp");
-    return std::nullopt;
+    return get_prev_optimized_traj_points();
   }
 
   // 7. convert to points with validation
   const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_steer_angles, mpt_mat);
   if (!mpt_traj_points) {
     RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large.");
-    return std::nullopt;
+    return get_prev_optimized_traj_points();
   }
 
   // 8. publish trajectories for debug
@@ -533,10 +541,20 @@ std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::getModelPredictiveTraj
 
   debug_data_ptr_->ref_points = ref_points;
   prev_ref_points_ptr_ = std::make_shared<std::vector<ReferencePoint>>(ref_points);
+  prev_optimized_traj_points_ptr_ =
+    std::make_shared<std::vector<TrajectoryPoint>>(*mpt_traj_points);
 
   return *mpt_traj_points;
 }
 
+std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::getPrevOptimizedTrajectoryPoints() const
+{
+  if (prev_optimized_traj_points_ptr_) {
+    return *prev_optimized_traj_points_ptr_;
+  }
+  return std::nullopt;
+}
+
 std::vector<ReferencePoint> MPTOptimizer::calcReferencePoints(
   const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points) const
 {

planning/obstacle_avoidance_planner/src/node.cpp

@@ -187,8 +187,6 @@ void ObstacleAvoidancePlanner::initializePlanning()
 void ObstacleAvoidancePlanner::resetPreviousData()
 {
   mpt_optimizer_ptr_->resetPreviousData();
-
-  prev_optimized_traj_points_ptr_ = nullptr;
 }
 
 void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr)
@@ -335,26 +333,19 @@ std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::optimizeTrajectory(
 
   // 2. make trajectory kinematically-feasible and collision-free (= inside the drivable area)
   //    with model predictive trajectory
-  const auto mpt_traj =
-    mpt_optimizer_ptr_->getModelPredictiveTrajectory(planner_data, p.traj_points);
-  if (!mpt_traj) {
-    return getPrevOptimizedTrajectory(p.traj_points);
-  }
-
-  // 3. make prev trajectories
-  prev_optimized_traj_points_ptr_ = std::make_shared<std::vector<TrajectoryPoint>>(*mpt_traj);
+  const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, p.traj_points);
 
   time_keeper_ptr_->toc(__func__, "    ");
-  return *mpt_traj;
+  return mpt_traj;
 }
 
 std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::getPrevOptimizedTrajectory(
   const std::vector<TrajectoryPoint> & traj_points) const
 {
-  if (prev_optimized_traj_points_ptr_) {
-    return *prev_optimized_traj_points_ptr_;
+  const auto prev_optimized_traj_points = mpt_optimizer_ptr_->getPrevOptimizedTrajectoryPoints();
+  if (prev_optimized_traj_points) {
+    return *prev_optimized_traj_points;
   }
-
   return traj_points;
 }