diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index f31003da09e95..78abda6861593 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -58,7 +58,7 @@ option: steer_limit_constraint: true fix_points_around_ego: true - plan_from_ego: false + plan_from_ego: true max_plan_from_ego_length: 10.0 visualize_sampling_num: 1 enable_manual_warm_start: true 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 f31003da09e95..78abda6861593 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,7 @@ option: steer_limit_constraint: true fix_points_around_ego: true - plan_from_ego: false + plan_from_ego: true max_plan_from_ego_length: 10.0 visualize_sampling_num: 1 enable_manual_warm_start: true 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..fe9c91ff22c65 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 @@ -127,7 +127,7 @@ struct ReferencePoint // one is fixing points around ego for stability // 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; + bool plan_from_ego = true; Eigen::Vector2d optimized_kinematic_state; double optimized_input;