Merge pull request #1401 from tier4/hotfix/v0.26.1/static-obstacle-av…

…oidance

fix(static_obstacle_avoidance): cherry pick PRs to fix some issues

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -145,8 +145,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
       *planner_data_->dynamic_object, data.current_lanelets,
-      [](const auto & obj, const auto & lane) {
-        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      [](const auto & obj, const auto & lane, const auto yaw_threshold) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane, yaw_threshold);
       });
 
   // Assume that the maximum allocation for data.other object is the sum of

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -357,6 +357,10 @@ struct ObjectData  // avoidance target
   {
   }
 
+  Pose getPose() const { return object.kinematics.initial_pose_with_covariance.pose; }
+
+  Point getPosition() const { return object.kinematics.initial_pose_with_covariance.pose.position; }
+
   PredictedObject object;
 
   // object behavior.

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -64,7 +64,7 @@ MarkerArray createObjectsCubeMarkerArray(
     }
 
     marker.id = uuidToInt32(object.object.object_id);
-    marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
+    marker.pose = object.getPose();
     msg.markers.push_back(marker);
   }
 
@@ -80,10 +80,8 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std:
       "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
       createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
 
-    const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-
     for (const auto & p : object.envelope_poly.outer()) {
-      marker.points.push_back(createPoint(p.x(), p.y(), pos.z));
+      marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z));
     }
 
     marker.points.push_back(marker.points.front());
@@ -142,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray(
   for (const auto & object : objects) {
     if (verbose) {
       marker.id = uuidToInt32(object.object.object_id);
-      marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
+      marker.pose = object.getPose();
       std::ostringstream string_stream;
       string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
       string_stream << "ratio:" << object.shiftable_ratio << " [-]\n"
@@ -161,7 +159,7 @@ MarkerArray createObjectInfoMarkerArray(
 
     {
       marker.id = uuidToInt32(object.object.object_id);
-      marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
+      marker.pose = object.getPose();
       marker.pose.position.z += 2.0;
       std::ostringstream string_stream;
       string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : "");

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -165,9 +165,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     }
 
     // the avoidance path is already approved
-    const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-    const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) ||
-                             (helper_->getShift(object_pos) < 0.0 && !is_object_on_right);
+    const auto is_approved =
+      (helper_->getShift(object.getPosition()) > 0.0 && is_object_on_right) ||
+      (helper_->getShift(object.getPosition()) < 0.0 && !is_object_on_right);
     if (is_approved) {
       return std::make_pair(desire_shift_length, avoidance_distance);
     }
@@ -362,9 +362,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       if (is_return_shift_to_goal) {
         return true;
       }
-      const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position;
       const bool has_object_near_goal =
-        tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) <
+        tier4_autoware_utils::calcDistance2d(goal_pose.position, o.getPosition()) <
         parameters_->object_check_goal_distance;
       return has_object_near_goal;
     }();
@@ -1022,8 +1021,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     const auto has_object_near_goal =
       std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
         return tier4_autoware_utils::calcDistance2d(
-                 data_->route_handler->getGoalPose().position,
-                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+                 data_->route_handler->getGoalPose().position, o.getPosition()) <
                parameters_->object_check_goal_distance;
       });
     if (has_object_near_goal) {
@@ -1090,9 +1088,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   if (utils::isAllowedGoalModification(data_->route_handler)) {
     const bool has_last_shift_near_goal =
       std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
-        return tier4_autoware_utils::calcDistance2d(
-                 last_sl.end.position,
-                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+        return tier4_autoware_utils::calcDistance2d(last_sl.end.position, o.getPosition()) <
                parameters_->object_check_goal_distance;
       });
     if (has_last_shift_near_goal) {