feat(obstacle_avoidance_planner): add options for plan_from_ego (tier4#1191)

* feat(obstacle_avoidance_planner): add options for plan_from_ego

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Remove adjust weight when planning from ego

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Readd near_kinematic_state_while_planning_from_ego as comment

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Use calcArcLength

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Remove max_plan_from_ego_length from ReferecncePoint

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml

@@ -58,7 +58,8 @@
       option:
         steer_limit_constraint: true
         fix_points_around_ego: true
-        # plan_from_ego: false
+        plan_from_ego: false
+        max_plan_from_ego_length: 10.0
         visualize_sampling_num: 1
         enable_manual_warm_start: true
         enable_warm_start: true # false

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -254,6 +254,7 @@ struct MPTParam
   bool l_inf_norm;
   bool two_step_soft_constraint;
   bool plan_from_ego;
+  double max_plan_from_ego_length;
 };
 
 #endif  // OBSTACLE_AVOIDANCE_PLANNER__COMMON_STRUCTS_HPP_

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -411,8 +411,11 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector<ReferencePoint> & ref_points)
 {
   // if plan from ego
   constexpr double epsilon = 1e-04;
-  const bool plan_from_ego =
-    mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon && ref_points.size() > 1;
+  const double trajectory_length = tier4_autoware_utils::calcArcLength(ref_points);
+
+  const bool plan_from_ego = mpt_param_.plan_from_ego && std::abs(current_ego_vel_) < epsilon &&
+                             ref_points.size() > 1 &&
+                             trajectory_length < mpt_param_.max_plan_from_ego_length;
   if (plan_from_ego) {
     for (auto & ref_point : ref_points) {
       ref_point.fix_kinematic_state = boost::none;
@@ -634,24 +637,19 @@ MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix(
   std::vector<Eigen::Triplet<double>> Qex_triplet_vec;
   for (size_t i = 0; i < N_ref; ++i) {
     // this is for plan_from_ego
-    const bool near_kinematic_state_while_planning_from_ego = [&]() {
-      const size_t min_idx = static_cast<size_t>(std::max(0, static_cast<int>(i) - 20));
-      const size_t max_idx = std::min(ref_points.size() - 1, i + 20);
-      for (size_t j = min_idx; j <= max_idx; ++j) {
-        if (ref_points.at(j).plan_from_ego && ref_points.at(j).fix_kinematic_state) {
-          return true;
-        }
-      }
-      return false;
-    }();
+    // const bool near_kinematic_state_while_planning_from_ego = [&]() {
+    //   const size_t min_idx = static_cast<size_t>(std::max(0, static_cast<int>(i) - 20));
+    //   const size_t max_idx = std::min(ref_points.size() - 1, i + 20);
+    //   for (size_t j = min_idx; j <= max_idx; ++j) {
+    //     if (ref_points.at(j).plan_from_ego && ref_points.at(j).fix_kinematic_state) {
+    //       return true;
+    //     }
+    //   }
+    //   return false;
+    // }();
 
     const auto adaptive_error_weight = [&]() -> std::array<double, 2> {
-      if (near_kinematic_state_while_planning_from_ego) {
-        constexpr double obstacle_avoid_error_weight_ratio = 1 / 100.0;
-        return {
-          mpt_param_.obstacle_avoid_lat_error_weight * obstacle_avoid_error_weight_ratio,
-          mpt_param_.obstacle_avoid_yaw_error_weight * obstacle_avoid_error_weight_ratio};
-      } else if (ref_points.at(i).near_objects) {
+      if (ref_points.at(i).near_objects) {
         return {
           mpt_param_.obstacle_avoid_lat_error_weight, mpt_param_.obstacle_avoid_yaw_error_weight};
       } else if (i == N_ref - 1 && is_containing_path_terminal_point) {

planning/obstacle_avoidance_planner/src/node.cpp

@@ -383,8 +383,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
 
     // option
     // TODO(murooka) implement plan_from_ego
-    mpt_param_.plan_from_ego = false;
-    // mpt_param_.plan_from_ego = declare_parameter<bool>("mpt.option.plan_from_ego");
+    mpt_param_.plan_from_ego = declare_parameter<bool>("mpt.option.plan_from_ego");
+    mpt_param_.max_plan_from_ego_length =
+      declare_parameter<double>("mpt.option.max_plan_from_ego_length");
     mpt_param_.steer_limit_constraint =
       declare_parameter<bool>("mpt.option.steer_limit_constraint");
     mpt_param_.fix_points_around_ego = declare_parameter<bool>("mpt.option.fix_points_around_ego");
@@ -643,7 +644,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback
 
   {  // mpt param
     // option
-    // updateParam<bool>(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego);
+    updateParam<bool>(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego);
+    updateParam<double>(
+      parameters, "mpt.option.max_plan_from_ego_length", mpt_param_.max_plan_from_ego_length);
     updateParam<bool>(
       parameters, "mpt.option.steer_limit_constraint", mpt_param_.steer_limit_constraint);
     updateParam<bool>(