feat(obstacle_avoidance_planner): add options for plan_from_ego (tier…

…4#1191) * feat(obstacle_avoidance_planner): add options for plan_from_ego Signed-off-by: kosuke55 <[email protected]> * Remove adjust weight when planning from ego Signed-off-by: kosuke55 <[email protected]> * Readd near_kinematic_state_while_planning_from_ego as comment Signed-off-by: kosuke55 <[email protected]> * Use calcArcLength Signed-off-by: kosuke55 <[email protected]> * Remove max_plan_from_ego_length from ReferecncePoint Signed-off-by: kosuke55 <[email protected]>
boyali · Sep 28, 2022 · 9afe592 · 9afe592
1 parent 7a9eb41
commit 9afe592
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 22 deletions.
diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/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

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/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_
diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/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) {

diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/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>(