From 54abb672f176df17d902f1179ad47090c83151ac Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Fri, 14 Jan 2022 02:25:51 +0900
Subject: [PATCH] add skip_optimization flag

---
 .../include/obstacle_avoidance_planner/node.hpp        |  1 +
 .../obstacle_avoidance_planner/src/mpt_optimizer.cpp   |  6 ++++++
 planning/obstacle_avoidance_planner/src/node.cpp       | 10 ++++++++++
 planning/obstacle_avoidance_planner/src/utils.cpp      |  4 ++++
 4 files changed, 21 insertions(+)

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
index 4bc1f1236831f..0676519487481 100644
--- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
+++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
@@ -100,6 +100,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   bool is_using_vehicle_config_;
   bool enable_avoidance_;
   bool enable_pre_smoothing_;
+  bool skip_optimization_;
   bool reset_prev_info_;
   bool use_footprint_for_drivability_;
 
diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
index a99863f24fdf5..b757202a6bd12 100644
--- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
+++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -1561,6 +1561,12 @@ void MPTOptimizer::calcVehicleBounds(
 {
   stop_watch_.tic(__func__);
 
+  if (ref_points.size() == 1) {
+    for ([[maybe_unused]] const double d : mpt_param_ptr_->avoiding_circle_offsets) {
+      ref_points.at(0).vehicle_bounds.push_back(ref_points.at(0).bounds);
+    }
+  }
+
   SplineInterpolation2d ref_points_spline_interpolation;
   ref_points_spline_interpolation.calcSplineCoefficients(ref_points);
 
diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp
index 080a7a30015f7..db8f540826c52 100644
--- a/planning/obstacle_avoidance_planner/src/node.cpp
+++ b/planning/obstacle_avoidance_planner/src/node.cpp
@@ -183,6 +183,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
     is_using_vehicle_config_ = declare_parameter<bool>("option.is_using_vehicle_config");
     enable_avoidance_ = declare_parameter<bool>("option.enable_avoidance");
     enable_pre_smoothing_ = declare_parameter<bool>("option.enable_pre_smoothing");
+    skip_optimization_ = declare_parameter<bool>("option.skip_optimization");
     visualize_sampling_num_ = declare_parameter<int>("option.visualize_sampling_num");
   }
 
@@ -435,6 +436,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback
 
     updateParam<bool>(parameters, "option.enable_avoidance", enable_avoidance_);
     updateParam<bool>(parameters, "option.enable_pre_smoothing", enable_pre_smoothing_);
+    updateParam<bool>(parameters, "option.skip_optimization_", skip_optimization_);
     updateParam<int>(parameters, "option.visualize_sampling_num", visualize_sampling_num_);
   }
 
@@ -882,6 +884,14 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory(
 {
   stop_watch_.tic(__func__);
 
+  if (skip_optimization_) {
+    const auto traj = points_utils::convertToTrajectoryPoints(path.points);
+    Trajectories trajs;
+    trajs.smoothed_trajectory = traj;
+    trajs.model_predictive_trajectory = traj;
+    return trajs;
+  }
+
   // EB: smooth trajectory if enable_pre_smoothing is true
   const auto eb_traj =
     [&]() -> boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>> {
diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 9723c21c37f9e..91eb85d836a94 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -571,6 +571,10 @@ void compensateLastPose(
   const TrajectoryParam & traj_param,
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points)
 {
+  if (traj_points.empty()) {
+    return;
+  }
+
   const geometry_msgs::msg::Pose last_pose = traj_points.back().pose;
   const auto extended_point_opt = getLastExtendedTrajPoint(
     last_path_point, last_pose, traj_param.delta_yaw_threshold_for_closest_point,