Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(obstacle_avoidance_planner): add options for plan_from_ego #1191

Merged
merged 5 commits into from
Jul 4, 2022
Merged

feat(obstacle_avoidance_planner): add options for plan_from_ego #1191

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
cde889f
feat(obstacle_avoidance_planner): add options for plan_from_ego
kosuke55 Jun 29, 2022
669f803
Remove adjust weight when planning from ego
kosuke55 Jun 20, 2022
7b8fb4f
Readd near_kinematic_state_while_planning_from_ego as comment
kosuke55 Jul 4, 2022
0c3e240
Use calcArcLength
kosuke55 Jul 4, 2022
8a03c11
Remove max_plan_from_ego_length from ReferecncePoint
kosuke55 Jul 4, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_
34 changes: 16 additions & 18 deletions planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down
9 changes: 6 additions & 3 deletions planning/obstacle_avoidance_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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>(
Expand Down