feat(obstacle_avoidance_planner): parameterize bounds search widths

launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -123,6 +123,8 @@
           eps_rel: 1.0e-10 # eps rel when solving OSQP
 
       mpt:
+        bounds_search_widths: [0.45, 0.15, 0.05, 0.01]
+
         clearance:  # clearance(distance) between vehicle and roads/objects when generating trajectory
           hard_clearance_from_road: 0.0 # clearance from road boundary[m]
           soft_clearance_from_road: 0.1 # clearance from road boundary[m]

planning/obstacle_avoidance_planner/README.md

@@ -706,6 +706,15 @@ $N_{circle}$ is the number of circles to check collision.
 - max steering wheel degree
   - `mpt.kinematics.max_steer_deg`
 
+### Boundary search
+
+- `advanced.mpt.bounds_search_widths`
+  - In order to efficiently search precise lateral boundaries on each trajectory point, different resolutions of search widths are defined.
+  - By default, [0.45, 0.15, 0.05, 0.01] is used. In this case, the goal is to get the boundaries' length on each trajectory point with 0.01 [m] resolution.
+  - Firstly, lateral boundaries are searhed with a rough resolution (= 0.45 [m]).
+  - Then, within its 0.45 [m] resolution which boundaries are inside, they are searched again with a bit precise resolution (= 0.15 [m]).
+  - Following this rule, finally boundaries with 0.01 [m] will be found.
+
 ### Assumptions
 
 - EB optimized trajectory length should be longer than MPT optimized trajectory length

planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml

@@ -123,6 +123,8 @@
           eps_rel: 1.0e-10 # eps rel when solving OSQP
 
       mpt:
+        bounds_search_widths: [0.45, 0.15, 0.05, 0.01]
+
         clearance:  # clearance(distance) between vehicle and roads/objects when generating trajectory
           hard_clearance_from_road: 0.0 # clearance from road boundary[m]
           soft_clearance_from_road: 0.1 # clearance from road boundary[m]

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -245,6 +245,8 @@ struct MPTParam
   double optimization_center_offset;
   double max_steer_rad;
 
+  std::vector<double> bounds_search_widths;
+
   bool soft_constraint;
   bool hard_constraint;
   bool l_inf_norm;

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -1481,7 +1481,7 @@ BoundsCandidates MPTOptimizer::getBoundsCandidates(
   BoundsCandidates bounds_candidate;
 
   constexpr double max_search_lane_width = 5.0;
-  const std::vector<double> ds_vec{0.45, 0.15, 0.05, 0.01};
+  const auto search_widths = mpt_param_.bounds_search_widths;
 
   // search right to left
   const double bound_angle =
@@ -1501,16 +1501,16 @@ BoundsCandidates MPTOptimizer::getBoundsCandidates(
   CollisionType latest_right_bound_collision_type = previous_collision_type;
 
   while (traversed_dist < max_search_lane_width) {
-    for (size_t ds_idx = 0; ds_idx < ds_vec.size(); ++ds_idx) {
-      const double ds = ds_vec.at(ds_idx);
+    for (size_t search_idx = 0; search_idx < search_widths.size(); ++search_idx) {
+      const double ds = search_widths.at(search_idx);
       while (true) {
         const CollisionType current_collision_type =
           getCollisionType(maps, enable_avoidance, avoiding_point, traversed_dist, bound_angle);
 
         if (has_collision(current_collision_type)) {  // currently collision
           if (!has_collision(previous_collision_type)) {
             // if target_position becomes collision from no collision or out_of_sight
-            if (ds_idx == ds_vec.size() - 1) {
+            if (search_idx == search_widths.size() - 1) {
               const double left_bound = traversed_dist - ds / 2.0;
               bounds_candidate.push_back(Bounds{
                 current_right_bound, left_bound, latest_right_bound_collision_type,
@@ -1523,7 +1523,7 @@ BoundsCandidates MPTOptimizer::getBoundsCandidates(
                                                                              // out_of_sight
           if (previous_collision_type == CollisionType::NO_COLLISION) {
             // if target_position becomes out_of_sight from no collision
-            if (ds_idx == ds_vec.size() - 1) {
+            if (search_idx == search_widths.size() - 1) {
               const double left_bound = max_search_lane_width;
               bounds_candidate.push_back(Bounds{
                 current_right_bound, left_bound, latest_right_bound_collision_type,
@@ -1535,7 +1535,7 @@ BoundsCandidates MPTOptimizer::getBoundsCandidates(
         } else {  // currently no collision
           if (has_collision(previous_collision_type)) {
             // if target_position becomes no collision from collision
-            if (ds_idx == ds_vec.size() - 1) {
+            if (search_idx == search_widths.size() - 1) {
               current_right_bound = traversed_dist - ds / 2.0;
               latest_right_bound_collision_type = previous_collision_type;
               previous_collision_type = current_collision_type;
@@ -1547,7 +1547,7 @@ BoundsCandidates MPTOptimizer::getBoundsCandidates(
         // if target_position is longer than max_search_lane_width
         if (traversed_dist >= max_search_lane_width) {
           if (!has_collision(previous_collision_type)) {
-            if (ds_idx == ds_vec.size() - 1) {
+            if (search_idx == search_widths.size() - 1) {
               const double left_bound = traversed_dist - ds / 2.0;
               bounds_candidate.push_back(Bounds{
                 current_right_bound, left_bound, latest_right_bound_collision_type,
@@ -1562,9 +1562,9 @@ BoundsCandidates MPTOptimizer::getBoundsCandidates(
         previous_collision_type = current_collision_type;
       }
 
-      if (ds_idx != ds_vec.size() - 1) {
+      if (search_idx != search_widths.size() - 1) {
         // go back with ds since target_position became empty or road/object
-        // NOTE: if ds is the last of ds_vec, don't have to go back
+        // NOTE: if ds is the last of search_widths, don't have to go back
         traversed_dist -= ds;
       }
     }

planning/obstacle_avoidance_planner/src/node.cpp

@@ -408,6 +408,10 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
       "mpt.kinematics.optimization_center_offset",
       vehicle_param_.wheelbase * default_wheelbase_ratio);
 
+    // bounds search
+    mpt_param_.bounds_search_widths =
+      declare_parameter<std::vector<double>>("advanced.mpt.bounds_search_widths");
+
     // collision free constraints
     mpt_param_.l_inf_norm =
       declare_parameter<bool>("advanced.mpt.collision_free_constraints.option.l_inf_norm");