fix(obstacle_avoidance_planner): smooth path connection after optimiz…

…ation (autowarefoundation#3754)

* fix(obstacle_avoidance_planner): smooth path connection after optimization

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp

@@ -54,7 +54,7 @@ namespace trajectory_utils
 template <typename T>
 std::optional<size_t> getPointIndexAfter(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx,
-  const double offset)
+  const double max_offset, const double min_offset)
 {
   if (points.empty()) {
     return std::nullopt;
@@ -63,28 +63,35 @@ std::optional<size_t> getPointIndexAfter(
   double sum_length =
     -motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos);
 
+  std::optional<size_t> output_idx{std::nullopt};
+
   // search forward
-  if (sum_length < offset) {
+  if (sum_length < min_offset) {
     for (size_t i = target_seg_idx + 1; i < points.size(); ++i) {
       sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
-      if (offset < sum_length) {
-        return i - 1;
+      if (min_offset < sum_length) {
+        output_idx = i - 1;
+      }
+      if (max_offset < sum_length) {
+        break;
       }
     }
-
-    return std::nullopt;
+    return output_idx;
   }
 
   // search backward
   for (size_t i = target_seg_idx; 0 < i;
        --i) {  // NOTE: use size_t since i is always positive value
     sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
-    if (sum_length < offset) {
-      return i - 1;
+    if (sum_length < min_offset) {
+      output_idx = i - 1;
+    }
+    if (sum_length < max_offset) {
+      break;
     }
   }
 
-  return 0;
+  return output_idx;
 }
 
 template <typename T>

planning/obstacle_avoidance_planner/src/node.cpp

@@ -512,26 +512,37 @@ std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::extendTrajectory(
     trajectory_utils::findEgoSegmentIndex(traj_points, joint_start_pose, ego_nearest_param_);
 
   // crop trajectory for extension
-  constexpr double joint_traj_length_for_smoothing = 5.0;
+  constexpr double joint_traj_max_length_for_smoothing = 15.0;
+  constexpr double joint_traj_min_length_for_smoothing = 5.0;
   const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter(
     traj_points, joint_start_pose.position, joint_start_traj_seg_idx,
-    joint_traj_length_for_smoothing);
+    joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing);
 
   // calculate full trajectory points
   const auto full_traj_points = [&]() {
     if (!joint_end_traj_point_idx) {
       return optimized_traj_points;
     }
+
     const auto extended_traj_points = std::vector<TrajectoryPoint>{
       traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()};
-    return concatVectors(optimized_traj_points, extended_traj_points);
+
+    // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is
+    // zero velocity, the zero velocity will be inserted in the whole joint trajectory.
+    auto modified_optimized_traj_points = optimized_traj_points;
+    if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) {
+      modified_optimized_traj_points.back().longitudinal_velocity_mps =
+        extended_traj_points.front().longitudinal_velocity_mps;
+    }
+
+    return concatVectors(modified_optimized_traj_points, extended_traj_points);
   }();
 
   // resample trajectory points
   auto resampled_traj_points = trajectory_utils::resampleTrajectoryPoints(
     full_traj_points, traj_param_.output_delta_arc_length);
 
-  // update velocity on joint
+  // update stop velocity on joint
   for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) {
     if (hasZeroVelocity(traj_points.at(i))) {
       if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) {