fix(obstacle_avoidance_planner): use fisrt ref point to find nearest …

…bounds (autowarefoundation#2248)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -1389,7 +1389,7 @@ void MPTOptimizer::calcBounds(
         if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) {
           return prev_trajs->mpt_ref_points.front().p;
         }
-        return current_ego_pose_.position;
+        return ref_points.at(i).p;
       }();
 
       geometry_msgs::msg::Pose ref_pose;