From cde889fa742d6bc7a5b002eda6fc379d92201a5a Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 29 Jun 2022 14:55:43 +0900 Subject: [PATCH 1/5] feat(obstacle_avoidance_planner): add options for plan_from_ego Signed-off-by: kosuke55 --- .../config/obstacle_avoidance_planner.param.yaml | 3 ++- .../obstacle_avoidance_planner/common_structs.hpp | 1 + .../obstacle_avoidance_planner/mpt_optimizer.hpp | 1 + .../src/mpt_optimizer.cpp | 14 ++++++++++++-- planning/obstacle_avoidance_planner/src/node.cpp | 9 ++++++--- 5 files changed, 22 insertions(+), 6 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 index 667d2644f16d7..87493df635d10 100644 --- 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 index da7f284085ef6..5805ce4753e0a 100644 --- 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/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index a06844d554c77..f14aff06f5f44 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -128,6 +128,7 @@ struct ReferencePoint // second is fixing current ego pose when no velocity for planning from ego pose boost::optional fix_kinematic_state = boost::none; bool plan_from_ego = false; + double max_plan_from_ego_length = 10.0; Eigen::Vector2d optimized_kinematic_state; double optimized_input; diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index fe4082027d6f1..fb7959edaf7b0 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -411,8 +411,18 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & 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 = [&]() { + double sum_length = 0; + for (size_t i = 1; i < ref_points.size(); ++i) { + sum_length += + tier4_autoware_utils::calcDistance2d(ref_points.at(i - 1).p, ref_points.at(i).p); + } + return sum_length; + }(); + + 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; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 0102265d534dc..5e7b6fef8db75 100644 --- 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("mpt.option.plan_from_ego"); + mpt_param_.plan_from_ego = declare_parameter("mpt.option.plan_from_ego"); + mpt_param_.max_plan_from_ego_length = + declare_parameter("mpt.option.max_plan_from_ego_length"); mpt_param_.steer_limit_constraint = declare_parameter("mpt.option.steer_limit_constraint"); mpt_param_.fix_points_around_ego = declare_parameter("mpt.option.fix_points_around_ego"); @@ -643,7 +644,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback { // mpt param // option - // updateParam(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego); + updateParam(parameters, "mpt.option.plan_from_ego", mpt_param_.plan_from_ego); + updateParam( + parameters, "mpt.option.max_plan_from_ego_length", mpt_param_.max_plan_from_ego_length); updateParam( parameters, "mpt.option.steer_limit_constraint", mpt_param_.steer_limit_constraint); updateParam( From 669f80356fb022df66e642ede2ddda07776f2183 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 20 Jun 2022 13:59:15 +0900 Subject: [PATCH 2/5] Remove adjust weight when planning from ego Signed-off-by: kosuke55 --- .../src/mpt_optimizer.cpp | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index fb7959edaf7b0..40f915a4b7f49 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -643,25 +643,8 @@ MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( Eigen::SparseMatrix Qex_sparse_mat(D_x * N_ref, D_x * N_ref); std::vector> 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(std::max(0, static_cast(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 { - 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) { From 7b8fb4febd4e73938894e2b0c494d6715ded9e68 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Jul 2022 13:11:28 +0900 Subject: [PATCH 3/5] Readd near_kinematic_state_while_planning_from_ego as comment Signed-off-by: kosuke55 --- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 40f915a4b7f49..0c2b97679dede 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -643,6 +643,18 @@ MPTOptimizer::ValueMatrix MPTOptimizer::generateValueMatrix( Eigen::SparseMatrix Qex_sparse_mat(D_x * N_ref, D_x * N_ref); std::vector> 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(std::max(0, static_cast(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 { if (ref_points.at(i).near_objects) { return { From 0c3e2400b0da91f5066f468aebaee3bfeb35989c Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Jul 2022 13:16:53 +0900 Subject: [PATCH 4/5] Use calcArcLength Signed-off-by: kosuke55 --- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 0c2b97679dede..a828373bcacf8 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -411,14 +411,7 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) { // if plan from ego constexpr double epsilon = 1e-04; - const double trajectory_length = [&]() { - double sum_length = 0; - for (size_t i = 1; i < ref_points.size(); ++i) { - sum_length += - tier4_autoware_utils::calcDistance2d(ref_points.at(i - 1).p, ref_points.at(i).p); - } - return sum_length; - }(); + 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 && From 8a03c11cfc83d9870d6ec8f3388ffa9fee5890e8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 4 Jul 2022 13:26:35 +0900 Subject: [PATCH 5/5] Remove max_plan_from_ego_length from ReferecncePoint Signed-off-by: kosuke55 --- .../include/obstacle_avoidance_planner/mpt_optimizer.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index f14aff06f5f44..a06844d554c77 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -128,7 +128,6 @@ struct ReferencePoint // second is fixing current ego pose when no velocity for planning from ego pose boost::optional fix_kinematic_state = boost::none; bool plan_from_ego = false; - double max_plan_from_ego_length = 10.0; Eigen::Vector2d optimized_kinematic_state; double optimized_input;