feat(obstacle_avoidance_planner): explicitly insert zero velocity (au…

…towarefoundation#906)

* feat(obstacle_avoidance_planner) fix bug of stop line unalignment

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

* fix bug of unsorted output points

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

* move calcVelocity in node.cpp

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

* fix build error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp

@@ -46,7 +46,6 @@ class EBPathOptimizer
   struct Anchor
   {
     geometry_msgs::msg::Pose pose;
-    double velocity;
   };
 
   struct ConstrainRectangles

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -231,13 +231,6 @@ class MPTOptimizer
 
   void calcOrientation(std::vector<ReferencePoint> & ref_points) const;
 
-  void calcVelocity(std::vector<ReferencePoint> & ref_points) const;
-
-  void calcVelocity(
-    std::vector<ReferencePoint> & ref_points,
-    const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
-    const double yaw_thresh) const;
-
   void calcCurvature(std::vector<ReferencePoint> & ref_points) const;
 
   void calcArcLength(std::vector<ReferencePoint> & ref_points) const;

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -42,11 +42,53 @@
 
 #include "boost/optional.hpp"
 
+#include <algorithm>
 #include <memory>
 #include <vector>
 
 namespace
 {
+template <typename T>
+boost::optional<geometry_msgs::msg::Pose> lerpPose(
+  const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
+{
+  constexpr double epsilon = 1e-6;
+
+  const double closest_to_target_dist =
+    tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, target_pos);
+  const double seg_dist =
+    tier4_autoware_utils::calcSignedArcLength(points, closest_seg_idx, closest_seg_idx + 1);
+
+  const auto & closest_pose = points[closest_seg_idx].pose;
+  const auto & next_pose = points[closest_seg_idx + 1].pose;
+
+  geometry_msgs::msg::Pose interpolated_pose;
+  if (std::abs(seg_dist) < epsilon) {
+    interpolated_pose.position.x = next_pose.position.x;
+    interpolated_pose.position.y = next_pose.position.y;
+    interpolated_pose.position.z = next_pose.position.z;
+    interpolated_pose.orientation = next_pose.orientation;
+  } else {
+    const double ratio = closest_to_target_dist / seg_dist;
+    if (ratio < 0 || 1 < ratio) {
+      return {};
+    }
+
+    interpolated_pose.position.x =
+      interpolation::lerp(closest_pose.position.x, next_pose.position.x, ratio);
+    interpolated_pose.position.y =
+      interpolation::lerp(closest_pose.position.y, next_pose.position.y, ratio);
+    interpolated_pose.position.z =
+      interpolation::lerp(closest_pose.position.z, next_pose.position.z, ratio);
+
+    const double closest_yaw = tf2::getYaw(closest_pose.orientation);
+    const double next_yaw = tf2::getYaw(next_pose.orientation);
+    const double interpolated_yaw = interpolation::lerp(closest_yaw, next_yaw, ratio);
+    interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(interpolated_yaw);
+  }
+  return interpolated_pose;
+}
+
 template <typename T>
 double lerpTwistX(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
@@ -65,9 +107,12 @@ double lerpTwistX(
   const double closest_vel = points[closest_seg_idx].longitudinal_velocity_mps;
   const double next_vel = points[closest_seg_idx + 1].longitudinal_velocity_mps;
 
-  return std::abs(seg_dist) < epsilon
-           ? next_vel
-           : interpolation::lerp(closest_vel, next_vel, closest_to_target_dist / seg_dist);
+  if (std::abs(seg_dist) < epsilon) {
+    return next_vel;
+  }
+
+  const double ratio = std::min(1.0, std::max(0.0, closest_to_target_dist / seg_dist));
+  return interpolation::lerp(closest_vel, next_vel, ratio);
 }
 
 template <typename T>
@@ -212,6 +257,10 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   Trajectories getPrevTrajs(
     const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points) const;
 
+  void calcVelocity(
+    const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
+    std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points) const;
+
   void insertZeroVelocityOutsideDrivableArea(
     std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points,
     const CVMaps & cv_maps);

planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp

@@ -354,15 +354,14 @@ int EBPathOptimizer::getNumFixedPoints(
 
 std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
 EBPathOptimizer::convertOptimizedPointsToTrajectory(
-  const std::vector<double> optimized_points, const std::vector<ConstrainRectangle> & constraints,
-  const int farthest_idx)
+  const std::vector<double> optimized_points,
+  [[maybe_unused]] const std::vector<ConstrainRectangle> & constraints, const int farthest_idx)
 {
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> traj_points;
   for (int i = 0; i <= farthest_idx; i++) {
     autoware_auto_planning_msgs::msg::TrajectoryPoint tmp_point;
     tmp_point.pose.position.x = optimized_points[i];
     tmp_point.pose.position.y = optimized_points[i + eb_param_.num_sampling_points_for_eb];
-    tmp_point.longitudinal_velocity_mps = constraints[i].velocity;
     traj_points.push_back(tmp_point);
   }
   for (size_t i = 0; i < traj_points.size(); i++) {
@@ -462,7 +461,6 @@ EBPathOptimizer::Anchor EBPathOptimizer::getAnchor(
   Anchor anchor;
   anchor.pose.position = interpolated_points[interpolated_idx];
   anchor.pose.orientation = nearest_q;
-  anchor.velocity = path_points[nearest_idx].longitudinal_velocity_mps;
   return anchor;
 }
 
@@ -595,7 +593,6 @@ ConstrainRectangle EBPathOptimizer::getConstrainRectangle(
   bottom_right.y = -1 * clearance;
   constrain_range.bottom_right =
     geometry_utils::transformToAbsoluteCoordinate2D(bottom_right, anchor.pose);
-  constrain_range.velocity = anchor.velocity;
   return constrain_range;
 }
 
@@ -608,6 +605,5 @@ ConstrainRectangle EBPathOptimizer::getConstrainRectangle(
   rect.top_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, max_x, min_y, 0.0).position;
   rect.bottom_left = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, max_y, 0.0).position;
   rect.bottom_right = tier4_autoware_utils::calcOffsetPose(anchor.pose, min_x, min_y, 0.0).position;
-  rect.velocity = anchor.velocity;
   return rect;
 }

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -338,7 +338,6 @@ std::vector<ReferencePoint> MPTOptimizer::getReferencePoints(
     // set some information to reference points considering fix kinematics
     trimPoints(ref_points);
     calcOrientation(ref_points);
-    calcVelocity(ref_points, smoothed_points, traj_param_.delta_yaw_threshold_for_closest_point);
     calcCurvature(ref_points);
     calcArcLength(ref_points);
     calcPlanningFromEgo(
@@ -1146,7 +1145,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> MPTOptimizer::get
     autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point;
     traj_point.pose = calcVehiclePose(ref_point, lat_error, yaw_error, 0.0);
 
-    traj_point.longitudinal_velocity_mps = ref_point.v;
     traj_points.push_back(traj_point);
 
     {  // for debug visualization
@@ -1217,21 +1215,6 @@ void MPTOptimizer::calcOrientation(std::vector<ReferencePoint> & ref_points) con
   }
 }
 
-void MPTOptimizer::calcVelocity(
-  std::vector<ReferencePoint> & ref_points,
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
-  const double yaw_thresh) const
-{
-  const auto ref_points_with_yaw =
-    points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points));
-  for (size_t i = 0; i < ref_points.size(); i++) {
-    ref_points.at(i).v =
-      points[findNearestIndexWithSoftYawConstraints(
-               points_utils::convertToPoints(points), ref_points_with_yaw.at(i), yaw_thresh)]
-        .longitudinal_velocity_mps;
-  }
-}
-
 void MPTOptimizer::calcCurvature(std::vector<ReferencePoint> & ref_points) const
 {
   const size_t num_points = static_cast<int>(ref_points.size());

planning/obstacle_avoidance_planner/src/node.cpp

@@ -175,6 +175,18 @@ std::tuple<double, std::vector<double>> calcVehicleCirclesInfo(
   }
 }
 
+size_t findNearestIndexWithSoftYawConstraints(
+  const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
+  const double yaw_threshold)
+{
+  const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points);
+
+  const auto nearest_idx_optional = tier4_autoware_utils::findNearestIndex(
+    points_with_yaw, pose, std::numeric_limits<double>::max(), yaw_threshold);
+  return nearest_idx_optional
+           ? *nearest_idx_optional
+           : tier4_autoware_utils::findNearestIndex(points_with_yaw, pose.position);
+}
 }  // namespace
 
 ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options)
@@ -927,6 +939,10 @@ ObstacleAvoidancePlanner::generateOptimizedTrajectory(
   // calculate trajectory with EB and MPT
   auto optimal_trajs = optimizeTrajectory(path, cv_maps);
 
+  // calculate velocity
+  // NOTE: Velocity is not considered in optimization.
+  calcVelocity(path.points, optimal_trajs.model_predictive_trajectory);
+
   // insert 0 velocity when trajectory is over drivable area
   if (is_stopping_if_outside_drivable_area_) {
     insertZeroVelocityOutsideDrivableArea(optimal_trajs.model_predictive_trajectory, cv_maps);
@@ -1076,6 +1092,40 @@ Trajectories ObstacleAvoidancePlanner::getPrevTrajs(
   return trajs;
 }
 
+void ObstacleAvoidancePlanner::calcVelocity(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
+  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points) const
+{
+  for (size_t i = 0; i < traj_points.size(); i++) {
+    const size_t nearest_path_idx = findNearestIndexWithSoftYawConstraints(
+      points_utils::convertToPoints(path_points), traj_points.at(i).pose,
+      traj_param_.delta_yaw_threshold_for_closest_point);
+    const size_t second_nearest_path_idx = [&]() -> size_t {
+      if (nearest_path_idx == 0) {
+        return 1;
+      } else if (nearest_path_idx == path_points.size() - 1) {
+        return path_points.size() - 2;
+      }
+
+      const double prev_dist = tier4_autoware_utils::calcDistance2d(
+        traj_points.at(i), path_points.at(nearest_path_idx - 1));
+      const double next_dist = tier4_autoware_utils::calcDistance2d(
+        traj_points.at(i), path_points.at(nearest_path_idx + 1));
+      if (prev_dist < next_dist) {
+        return nearest_path_idx - 1;
+      }
+      return nearest_path_idx + 1;
+    }();
+
+    // NOTE: std::max, not std::min, is used here since traj_points' sampling width may be longer
+    // than path_points' sampling width. A zero velocity point is guaranteed to be inserted in an
+    // output trajectory in the alignVelocity function
+    traj_points.at(i).longitudinal_velocity_mps = std::max(
+      path_points.at(nearest_path_idx).longitudinal_velocity_mps,
+      path_points.at(second_nearest_path_idx).longitudinal_velocity_mps);
+  }
+}
+
 void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points,
   const CVMaps & cv_maps)
@@ -1373,16 +1423,65 @@ ObstacleAvoidancePlanner::alignVelocity(
 {
   stop_watch_.tic(__func__);
 
+  // insert zero velocity path index, and get optional zero_vel_path_idx
+  const auto path_zero_vel_info = [&]()
+    -> std::pair<
+      std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>, boost::optional<size_t>> {
+    const auto opt_path_zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(path_points);
+    if (opt_path_zero_vel_idx) {
+      const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get());
+      const auto opt_traj_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(
+        fine_traj_points, zero_vel_path_point.pose, std::numeric_limits<double>::max(),
+        traj_param_.delta_yaw_threshold_for_closest_point);
+      if (opt_traj_seg_idx) {
+        const auto interpolated_pose =
+          lerpPose(fine_traj_points, zero_vel_path_point.pose.position, opt_traj_seg_idx.get());
+        if (interpolated_pose) {
+          autoware_auto_planning_msgs::msg::TrajectoryPoint zero_vel_traj_point;
+          zero_vel_traj_point.pose = interpolated_pose.get();
+          zero_vel_traj_point.longitudinal_velocity_mps =
+            zero_vel_path_point.longitudinal_velocity_mps;
+
+          if (
+            tier4_autoware_utils::calcDistance2d(
+              fine_traj_points.at(opt_traj_seg_idx.get()).pose, zero_vel_traj_point.pose) < 1e-3) {
+            return {fine_traj_points, opt_traj_seg_idx.get()};
+          } else if (
+            tier4_autoware_utils::calcDistance2d(
+              fine_traj_points.at(opt_traj_seg_idx.get() + 1).pose, zero_vel_traj_point.pose) <
+            1e-3) {
+            return {fine_traj_points, opt_traj_seg_idx.get() + 1};
+          }
+
+          auto fine_traj_points_with_zero_vel = fine_traj_points;
+          fine_traj_points_with_zero_vel.insert(
+            fine_traj_points_with_zero_vel.begin() + opt_traj_seg_idx.get() + 1,
+            zero_vel_traj_point);
+          return {fine_traj_points_with_zero_vel, opt_traj_seg_idx.get() + 1};
+        }
+      }
+    }
+
+    return {fine_traj_points, {}};
+  }();
+  const auto fine_traj_points_with_path_zero_vel = path_zero_vel_info.first;
+  const auto opt_zero_vel_path_idx = path_zero_vel_info.second;
+
   // search zero velocity index of fine_traj_points
   const size_t zero_vel_fine_traj_idx = [&]() {
-    const size_t zero_vel_path_idx = searchExtendedZeroVelocityIndex(fine_traj_points, path_points);
+    // zero velocity for being outside drivable area
     const size_t zero_vel_traj_idx =
-      searchExtendedZeroVelocityIndex(fine_traj_points, traj_points);  // for outside drivable area
+      searchExtendedZeroVelocityIndex(fine_traj_points_with_path_zero_vel, traj_points);
 
-    return std::min(zero_vel_path_idx, zero_vel_traj_idx);
+    // zero velocity in path points
+    if (opt_zero_vel_path_idx) {
+      return std::min(opt_zero_vel_path_idx.get(), zero_vel_traj_idx);
+    }
+    return zero_vel_traj_idx;
   }();
 
-  auto fine_traj_points_with_vel = fine_traj_points;
+  // interpolate z and velocity
+  auto fine_traj_points_with_vel = fine_traj_points_with_path_zero_vel;
   size_t prev_begin_idx = 0;
   for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) {
     const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0);
@@ -1403,12 +1502,9 @@ ObstacleAvoidancePlanner::alignVelocity(
 
     // lerp vx
     const double target_vel = lerpTwistX(truncated_points, target_pose.position, closest_seg_idx);
+
     if (i >= zero_vel_fine_traj_idx) {
       fine_traj_points_with_vel[i].longitudinal_velocity_mps = 0.0;
-    } else if (target_vel < 1e-6) {  // NOTE: velocity may be negative due to linear interpolation
-      const auto prev_idx = std::max(static_cast<int>(i) - 1, 0);
-      fine_traj_points_with_vel[i].longitudinal_velocity_mps =
-        fine_traj_points_with_vel[prev_idx].longitudinal_velocity_mps;
     } else {
       fine_traj_points_with_vel[i].longitudinal_velocity_mps = target_vel;
     }