adds the polygon to polygon distance measurement

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -139,14 +139,31 @@ using geometry_msgs::msg::Vector3;
 using nav_msgs::msg::OccupancyGrid;
 using route_handler::RouteHandler;
 using tier4_autoware_utils::LineString2d;
+using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
+namespace bg = boost::geometry;
+using geometry_msgs::msg::Pose;
 
 struct FrenetCoordinate3d
 {
   double length{0.0};    // longitudinal
   double distance{0.0};  // lateral
 };
 
+struct ProjectedDistancePoint
+{
+  Point2d projected_point;
+  double distance{0.0};
+};
+
+template <typename Pythagoras = bg::strategy::distance::pythagoras<> >
+ProjectedDistancePoint pointToSegment(
+  const Point2d & reference_point, const Point2d & point_from_ego,
+  const Point2d & point_from_object);
+
+void getProjectedDistancePointFromPolygons(
+  const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego,
+  Pose & point_on_object);
 // data conversions
 
 Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id);

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -182,14 +182,16 @@ CandidateOutput LaneChangeModule::planCandidate() const
   const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_);
 
   // Find lane change path
-  bool found_valid_path, found_safe_path;
+  bool found_valid_path = false;
+  bool found_safe_path = false;
+
   LaneChangePath selected_path;
   std::tie(found_valid_path, found_safe_path) =
     getSafePath(lane_change_lanes, check_distance_, selected_path);
   selected_path.path.header = planner_data_->route_handler->getRouteHeader();
 
   if (selected_path.path.points.empty()) {
-    return CandidateOutput{};
+    return output;
   }
 
   const auto start_idx = selected_path.shift_point.start_idx;

planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

@@ -281,12 +281,13 @@ bool selectSafePath(
       return true;
     }
 
-    if (isLaneChangePathSafe(
-          paths.front().path, current_lanes, target_lanes, dynamic_objects, current_pose,
-          current_twist, vehicle_width, vehicle_length, ros_parameters, true, path.acceleration)) {
-      *selected_path = path;
-      return true;
-    }
+    // if (isLaneChangePathSafe(
+    //       paths.front().path, current_lanes, target_lanes, dynamic_objects, current_pose,
+    //       current_twist, vehicle_width, vehicle_length, ros_parameters, true, path.acceleration))
+    //       {
+    //   *selected_path = path;
+    //   return true;
+    // }
   }
 
   return false;
@@ -330,10 +331,10 @@ bool isLaneChangePathSafe(
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const Twist & current_twist, const double & vehicle_width, const double & vehicle_length,
-  const LaneChangeParameters & ros_parameters, const bool use_buffer, const double acceleration)
+  const LaneChangeParameters & ros_parameters, const bool use_buffer,
+  [[maybe_unused]] const double acceleration)
 {
   if (dynamic_objects == nullptr) {
-    [[maybe_unused]] const auto a = acceleration;
     return true;
   }
 
@@ -427,6 +428,8 @@ bool isLaneChangePathSafe(
           return false;
         }
 
+        util::getProjectedDistancePointFromPolygons(
+          ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose);
         const auto & object_twist = obj.kinematics.initial_twist_with_covariance.twist;
         if (!hasEnoughDistance(
               expected_ego_pose, current_twist, expected_obj_pose, object_twist, ros_parameters)) {
@@ -476,6 +479,9 @@ bool isLaneChangePathSafe(
             return false;
           }
 
+          util::getProjectedDistancePointFromPolygons(
+            ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose);
+
           const auto & object_twist = obj.kinematics.initial_twist_with_covariance.twist;
           if (!hasEnoughDistance(
                 expected_ego_pose, current_twist, expected_obj_pose, object_twist,
@@ -502,6 +508,11 @@ bool isLaneChangePathSafe(
         if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
           return false;
         }
+
+        Pose expected_obj_pose = obj.kinematics.initial_pose_with_covariance.pose;
+        util::getProjectedDistancePointFromPolygons(
+          ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose);
+
         const auto object_twist = obj.kinematics.initial_twist_with_covariance.twist;
         if (!hasEnoughDistance(
               expected_ego_pose, current_twist, obj.kinematics.initial_pose_with_covariance.pose,

planning/behavior_path_planner/src/utilities.cpp

@@ -1844,5 +1844,75 @@ Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const
 
   return object_polygon;
 }
+
+template <typename Pythagoras>
+ProjectedDistancePoint pointToSegment(
+  const Point2d & reference_point, const Point2d & point_from_ego,
+  const Point2d & point_from_object)
+{
+  auto copied_point_from_object = point_from_object;
+  auto copied_point_from_reference = reference_point;
+  bg::subtract_point(copied_point_from_object, point_from_ego);
+  bg::subtract_point(copied_point_from_reference, point_from_ego);
+
+  const auto c1 = bg::dot_product(copied_point_from_reference, copied_point_from_object);
+  if (!(c1 > 0)) {
+    return {point_from_ego, Pythagoras::apply(reference_point, point_from_ego)};
+  }
+
+  const auto c2 = bg::dot_product(copied_point_from_object, copied_point_from_object);
+  if (!(c2 > c1)) {
+    return {point_from_object, Pythagoras::apply(reference_point, point_from_object)};
+  }
+
+  Point2d projected = point_from_ego;
+  bg::multiply_value(copied_point_from_object, c1 / c2);
+  bg::add_point(projected, copied_point_from_object);
+
+  return {projected, Pythagoras::apply(reference_point, projected)};
+}
+
+void getProjectedDistancePointFromPolygons(
+  const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego,
+  Pose & point_on_object)
+{
+  ProjectedDistancePoint nearest;
+  std::unique_ptr<Point2d> current_point;
+
+  std::size_t count{0};
+
+  for (const auto & [ego_to_polygon, polygon_to_ego] :
+       {std::tie(ego_polygon, object_polygon), std::tie(object_polygon, ego_polygon)}) {
+    const auto segments = boost::make_iterator_range(
+      bg::segments_begin(ego_to_polygon), bg::segments_end(ego_to_polygon));
+    const auto points =
+      boost::make_iterator_range(bg::points_begin(polygon_to_ego), bg::points_end(polygon_to_ego));
+
+    for (auto && segment : segments) {
+      count = 0;
+      for (auto && point : points) {
+        const auto projected = pointToSegment(point, *segment.first, *segment.second);
+        if (!current_point || projected.distance < nearest.distance) {
+          current_point = std::make_unique<Point2d>(point);
+          nearest = projected;
+        }
+        ++count;
+      }
+    }
+  }
+
+  if (count < object_polygon.outer().size()) {
+    point_on_object.position.x = current_point->x();
+    point_on_object.position.y = current_point->y();
+    point_on_ego.position.x = nearest.projected_point.x();
+    point_on_ego.position.y = nearest.projected_point.y();
+  } else {
+    point_on_ego.position.x = current_point->x();
+    point_on_ego.position.y = current_point->y();
+    point_on_object.position.x = nearest.projected_point.x();
+    point_on_object.position.y = nearest.projected_point.y();
+  }
+}
+
 }  // namespace util
 }  // namespace behavior_path_planner