Merge pull request autowarefoundation#43 from tier4/sync-upstream

chore: sync upstream

.markdown-link-check.json

@@ -3,6 +3,9 @@
   "ignorePatterns": [
     {
       "pattern": "^http://localhost"
+    },
+    {
+      "pattern": "^https://github.com/.*/discussions/new"
     }
   ],
   "retryOn429": true,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -157,8 +157,7 @@ class AvoidanceModule : public SceneModuleInterface
   // debug
   mutable DebugData debug_data_;
   void setDebugData(const PathShifter & shifter, const DebugData & debug);
-  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_object;
-  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point;
+  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point_;
   // =====================================
   // ========= helper functions ==========
   // =====================================

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -129,7 +129,7 @@ AvoidanceDebugMsgArray BehaviorTreeManager::getAvoidanceDebugMsgArray()
   const auto avoidance_module = std::find_if(
     scene_modules_.begin(), scene_modules_.end(),
     [](const std::shared_ptr<SceneModuleInterface> & module_ptr) {
-      return module_ptr->current_state_ == BT::NodeStatus::SUCCESS;
+      return module_ptr->name() == "Avoidance";
     });
   if (avoidance_module != scene_modules_.end()) {
     const auto & ptr = avoidance_module->get()->getAvoidanceDebugMsgArray();

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -77,8 +77,6 @@ bool AvoidanceModule::isExecutionReady() const
   {
     DebugData debug;
     static_cast<void>(calcAvoidancePlanningData(debug));
-    debug_avoidance_msg_array_ptr_ =
-      std::make_shared<AvoidanceDebugMsgArray>(debug.avoidance_debug_msg_array);
   }
 
   if (current_state_ == BT::NodeStatus::RUNNING) {
@@ -95,8 +93,6 @@ BT::NodeStatus AvoidanceModule::updateState()
   DebugData debug;
   const auto avoid_data = calcAvoidancePlanningData(debug);
   const bool has_avoidance_target = !avoid_data.objects.empty();
-  debug_avoidance_msg_array_ptr_ =
-    std::make_shared<AvoidanceDebugMsgArray>(debug.avoidance_debug_msg_array);
   if (!is_plan_running && !has_avoidance_target) {
     current_state_ = BT::NodeStatus::SUCCESS;
   } else {
@@ -298,8 +294,13 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
 
   // debug
   {
-    debug_avoidance_initializer_for_object = avoidance_debug_msg_array;
-    debug.avoidance_debug_msg_array.avoidance_info = std::move(avoidance_debug_msg_array);
+    auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info;
+    debug_data_avoidance = avoidance_debug_msg_array;
+    debug_data_avoidance.insert(
+      debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point_.begin(),
+      debug_avoidance_initializer_for_shift_point_.end());
+    debug_avoidance_msg_array_ptr_ =
+      std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
     debug.farthest_linestring_from_overhang =
       std::make_shared<lanelet::ConstLineStrings3d>(debug_linestring);
     debug.current_lanelets = std::make_shared<lanelet::ConstLanelets>(current_lanes);
@@ -595,14 +596,7 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
     avoidance_debug_msg_array.push_back(avoidance_debug_msg);
   }
 
-  {
-    debug_avoidance_initializer_for_shift_point = std::move(avoidance_debug_msg_array);
-    auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info;
-    debug_data_avoidance = debug_avoidance_initializer_for_object;
-    debug_data_avoidance.insert(
-      debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point.begin(),
-      debug_avoidance_initializer_for_shift_point.end());
-  }
+  debug_avoidance_initializer_for_shift_point_ = std::move(avoidance_debug_msg_array);
   fillAdditionalInfoFromLongitudinal(avoid_points);
 
   return avoid_points;
@@ -2177,8 +2171,6 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval()
   // we can execute the plan() since it handles the approval appropriately.
   BehaviorModuleOutput out = plan();
   out.path_candidate = std::make_shared<PathWithLaneId>(planCandidate());
-  debug_avoidance_msg_array_ptr_ =
-    std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
   return out;
 }
 
@@ -2401,9 +2393,6 @@ void AvoidanceModule::updateData()
 {
   debug_data_ = DebugData();
   avoidance_data_ = calcAvoidancePlanningData(debug_data_);
-  const auto avoidance_debug_msgs = debug_data_.avoidance_debug_msg_array.avoidance_info;
-  debug_avoidance_msg_array_ptr_ =
-    std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
 
   // TODO(Horibe): this is not tested yet, disable now.
   updateRegisteredObject(avoidance_data_.objects);
@@ -2551,6 +2540,7 @@ void AvoidanceModule::initVariables()
   path_shifter_ = PathShifter{};
 
   debug_avoidance_msg_array_ptr_.reset();
+  debug_avoidance_initializer_for_shift_point_.clear();
   debug_data_ = DebugData();
 
   registered_raw_shift_points_ = {};

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -235,7 +235,8 @@ class MPTOptimizer
 
   void calcVelocity(
     std::vector<ReferencePoint> & ref_points,
-    const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points) const;
+    const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
+    const double yaw_thresh) const;
 
   void calcCurvature(std::vector<ReferencePoint> & ref_points) const;
 
@@ -299,6 +300,10 @@ class MPTOptimizer
     const std::vector<ReferencePoint> & ref_points,
     std::shared_ptr<DebugData> debug_data_ptr) const;
 
+  size_t findNearestIndexWithSoftYawConstraints(
+    const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
+    const double yaw_threshold) const;
+
 public:
   MPTOptimizer(
     const bool is_showing_debug_info, const TrajectoryParam & traj_param,

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp

@@ -27,6 +27,7 @@
 #include "boost/optional/optional_fwd.hpp"
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 #include <vector>
 
@@ -188,14 +189,19 @@ T clipBackwardPoints(
 
 template <typename T>
 T clipBackwardPoints(
-  const T & points, const geometry_msgs::msg::Point pos, const double backward_length,
-  const double delta_length)
+  const T & points, const geometry_msgs::msg::Pose pose, const double backward_length,
+  const double delta_length, const double delta_yaw)
 {
   if (points.empty()) {
     return T{};
   }
 
-  const size_t target_idx = tier4_autoware_utils::findNearestIndex(points, pos);
+  const auto target_idx_optional = tier4_autoware_utils::findNearestIndex(
+    points, pose, std::numeric_limits<double>::max(), delta_yaw);
+
+  const size_t target_idx = target_idx_optional
+                              ? *target_idx_optional
+                              : tier4_autoware_utils::findNearestIndex(points, pose.position);
 
   const int begin_idx =
     std::max(0, static_cast<int>(target_idx) - static_cast<int>(backward_length / delta_length));

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -286,16 +286,32 @@ std::vector<ReferencePoint> MPTOptimizer::getReferencePoints(
         // interpolate and crop backward
         const auto interpolated_points = interpolation_utils::getInterpolatedPoints(
           smoothed_points, mpt_param_.delta_arc_length_for_mpt_points);
-        const auto cropped_interpolated_points = points_utils::clipBackwardPoints(
-          interpolated_points, current_ego_pose_.position, traj_param_.backward_fixing_distance,
-          mpt_param_.delta_arc_length_for_mpt_points);
+        const auto interpolated_points_with_yaw =
+          points_utils::convertToPosesWithYawEstimation(interpolated_points);
+        const auto cropped_interpolated_points_with_yaw = points_utils::clipBackwardPoints(
+          interpolated_points_with_yaw, current_ego_pose_, traj_param_.backward_fixing_distance,
+          mpt_param_.delta_arc_length_for_mpt_points,
+          traj_param_.delta_yaw_threshold_for_closest_point);
+        const auto cropped_interpolated_points =
+          points_utils::convertToPoints(cropped_interpolated_points_with_yaw);
 
         return points_utils::convertToReferencePoints(cropped_interpolated_points);
       }
 
       // calc non fixed traj points
-      const size_t seg_idx =
-        tier4_autoware_utils::findNearestSegmentIndex(smoothed_points, fixed_ref_points.back().p);
+      const auto fixed_ref_points_with_yaw = points_utils::convertToPosesWithYawEstimation(
+        points_utils::convertToPoints(fixed_ref_points));
+      const auto seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex(
+        smoothed_points, fixed_ref_points_with_yaw.back(), std::numeric_limits<double>::max(),
+        traj_param_.delta_yaw_threshold_for_closest_point);
+
+      if (!seg_idx_optional) {
+        RCLCPP_INFO_EXPRESSION(
+          rclcpp::get_logger("mpt_optimizer"), is_showing_debug_info_,
+          "cannot find nearest segment index in getReferencePoints");
+        return std::vector<ReferencePoint>{};
+      }
+      const auto seg_idx = *seg_idx_optional;
       const auto non_fixed_traj_points =
         std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{
           smoothed_points.begin() + seg_idx, smoothed_points.end()};
@@ -322,7 +338,7 @@ 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);
+    calcVelocity(ref_points, smoothed_points, traj_param_.delta_yaw_threshold_for_closest_point);
     calcCurvature(ref_points);
     calcArcLength(ref_points);
     calcPlanningFromEgo(
@@ -385,8 +401,9 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector<ReferencePoint> & ref_points)
     */
 
     // assign fix kinematics
-    const size_t nearest_ref_idx =
-      tier4_autoware_utils::findNearestIndex(ref_points, current_ego_pose_.position);
+    const size_t nearest_ref_idx = findNearestIndexWithSoftYawConstraints(
+      points_utils::convertToPoints(ref_points), current_ego_pose_,
+      traj_param_.delta_yaw_threshold_for_closest_point);
 
     // calculate cropped_ref_points.at(nearest_ref_idx) with yaw
     const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose {
@@ -422,8 +439,9 @@ std::vector<ReferencePoint> MPTOptimizer::getFixedReferencePoints(
   }
 
   const auto & prev_ref_points = prev_trajs->mpt_ref_points;
-  const int nearest_prev_ref_idx =
-    tier4_autoware_utils::findNearestIndex(prev_ref_points, current_ego_pose_.position);
+  const int nearest_prev_ref_idx = static_cast<int>(findNearestIndexWithSoftYawConstraints(
+    points_utils::convertToPoints(prev_ref_points), current_ego_pose_,
+    traj_param_.delta_yaw_threshold_for_closest_point));
 
   // calculate begin_prev_ref_idx
   const int begin_prev_ref_idx = [&]() {
@@ -1174,6 +1192,19 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> MPTOptimizer::get
   return traj_points;
 }
 
+size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints(
+  const std::vector<geometry_msgs::msg::Point> & points, const geometry_msgs::msg::Pose & pose,
+  const double yaw_threshold) const
+{
+  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);
+}
+
 void MPTOptimizer::calcOrientation(std::vector<ReferencePoint> & ref_points) const
 {
   const auto yaw_angles = slerpYawFromReferencePoints(ref_points);
@@ -1188,11 +1219,16 @@ 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
+  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[tier4_autoware_utils::findNearestIndex(points, ref_points.at(i).p)]
-                           .longitudinal_velocity_mps;
+    ref_points.at(i).v =
+      points[findNearestIndexWithSoftYawConstraints(
+               points_utils::convertToPoints(points), ref_points_with_yaw.at(i), yaw_thresh)]
+        .longitudinal_velocity_mps;
   }
 }
 
@@ -1277,8 +1313,11 @@ void MPTOptimizer::calcExtraPoints(
     if (prev_trajs && !prev_trajs->mpt_ref_points.empty()) {
       const auto & prev_ref_points = prev_trajs->mpt_ref_points;
 
-      const size_t prev_idx =
-        tier4_autoware_utils::findNearestIndex(prev_ref_points, ref_points.at(i).p);
+      const auto ref_points_with_yaw =
+        points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points));
+      const size_t prev_idx = findNearestIndexWithSoftYawConstraints(
+        points_utils::convertToPoints(prev_ref_points), ref_points_with_yaw.at(i),
+        traj_param_.delta_yaw_threshold_for_closest_point);
       const double dist_to_nearest_prev_ref =
         tier4_autoware_utils::calcDistance2d(prev_ref_points.at(prev_idx), ref_points.at(i));
       if (dist_to_nearest_prev_ref < 1.0 && prev_ref_points.at(prev_idx).near_objects) {
@@ -1337,7 +1376,7 @@ void MPTOptimizer::calcBounds(
       BoundsCandidates filtered_bounds_candidates;
       for (const auto & bounds_candidate : bounds_candidates) {
         // Step 1. Bounds is continuous to the previous one,
-        //         and the overlapped signed length is longer than vehice width
+        //         and the overlapped signed length is longer than vehicle width
         //         overlapped_signed_length already considers vehicle width.
         const double overlapped_signed_length =
           calcOverlappedBoundsSignedLength(prev_continuous_bounds, bounds_candidate);

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1387,16 +1387,22 @@ ObstacleAvoidancePlanner::alignVelocity(
   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);
 
-    const auto & target_pos = fine_traj_points_with_vel[i].pose.position;
-    const size_t closest_seg_idx =
-      tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pos);
+    const auto & target_pose = fine_traj_points_with_vel[i].pose;
+    const auto closest_seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex(
+      truncated_points, target_pose, std::numeric_limits<double>::max(),
+      traj_param_.delta_yaw_threshold_for_closest_point);
+
+    const auto closest_seg_idx =
+      closest_seg_idx_optional
+        ? *closest_seg_idx_optional
+        : tier4_autoware_utils::findNearestSegmentIndex(truncated_points, target_pose.position);
 
     // lerp z
     fine_traj_points_with_vel[i].pose.position.z =
-      lerpPoseZ(truncated_points, target_pos, closest_seg_idx);
+      lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx);
 
     // lerp vx
-    const double target_vel = lerpTwistX(truncated_points, target_pos, closest_seg_idx);
+    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