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 a18e531806171..fbe6b5d756bb2 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -148,6 +148,9 @@ num: 3 radius_ratio: 0.8 + fitting_uniform_circle: + num: 3 # must be greater than 1 + rear_drive: num_for_calculation: 3 front_radius_ratio: 1.0 diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index acc5ab823ca59..46165fc05c0be 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1401,7 +1401,8 @@ void MPTOptimizer::calcBounds( mpt_param_.soft_clearance_from_road + mpt_param_.extra_desired_clearance_from_road; */ - const double min_soft_road_clearance = vehicle_param_.width / 2.0; + const double min_soft_road_clearance = + vehicle_param_.width / 2.0 + mpt_param_.soft_clearance_from_road; // search bounds candidate for each ref points debug_data.bounds_candidates.clear(); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 2775089335128..8508a71013479 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -85,6 +85,25 @@ std::tuple, std::vector> calcVehicleCirclesInfo( return {radiuses, longitudinal_offsets}; } +std::tuple, std::vector> calcVehicleCirclesInfo( + const VehicleParam & vehicle_param, size_t circle_num) +{ + circle_num = std::max(circle_num, static_cast(2)); + + std::vector longitudinal_offsets; + std::vector radiuses; + + const double radius = vehicle_param.width / 2.0; + const double unit_lon_length = vehicle_param.length / static_cast(circle_num - 1); + + for (size_t i = 0; i < circle_num; ++i) { + longitudinal_offsets.push_back(unit_lon_length * i - vehicle_param.rear_overhang); + radiuses.push_back(radius); + } + + return {radiuses, longitudinal_offsets}; +} + std::tuple, std::vector> calcVehicleCirclesInfoByBicycleModel( const VehicleParam & vehicle_param, const size_t circle_num, const double rear_radius_ratio, const double front_radius_ratio) @@ -443,6 +462,13 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n calcVehicleCirclesInfo( vehicle_param_, vehicle_circle_num_for_calculation_, vehicle_circle_radius_ratios_.front()); + } else if (vehicle_circle_method_ == "fitting_uniform_circle") { + vehicle_circle_num_for_calculation_ = declare_parameter( + "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num"); + + std::tie( + mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); } else if (vehicle_circle_method_ == "rear_drive") { vehicle_circle_num_for_calculation_ = declare_parameter( "advanced.mpt.collision_free_constraints.vehicle_circles.rear_drive.num_for_calculation"); @@ -720,6 +746,15 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( calcVehicleCirclesInfo( vehicle_param_, vehicle_circle_num_for_calculation_, vehicle_circle_radius_ratios_.front()); + } else if (vehicle_circle_method_ == "fitting_uniform_circle") { + updateParam( + parameters, + "advanced.mpt.collision_free_constraints.vehicle_circles.fitting_uniform_circle.num", + vehicle_circle_num_for_calculation_); + + std::tie( + mpt_param_.vehicle_circle_radiuses, mpt_param_.vehicle_circle_longitudinal_offsets) = + calcVehicleCirclesInfo(vehicle_param_, vehicle_circle_num_for_calculation_); } else if (vehicle_circle_method_ == "rear_drive") { updateParam( parameters,