add skip_optimization flag

autowarefoundation · Jan 20, 2022 · 54abb67 · 54abb67
1 parent 089dc3d
commit 54abb67
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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,