feat(behavior_velocity_planner): replace Polygon2d with common's one (t…

…ier4#1767)

* feat(behavior_velocity_planner): replace Polygon2d with common's one

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

* fix

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>

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp

@@ -18,7 +18,6 @@
 #include "tier4_autoware_utils/geometry/boost_geometry.hpp"
 
 #include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
-// #include "geometry_msgs/msg/point32.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
 #include <boost/geometry.hpp>
@@ -35,6 +34,7 @@ geometry_msgs::msg::Polygon rotatePolygon(
   const geometry_msgs::msg::Polygon & polygon, const double & angle);
 Polygon2d toPolygon2d(
   const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape);
+Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object);
 double getArea(const autoware_auto_perception_msgs::msg::Shape & shape);
 
 }  // namespace tier4_autoware_utils

common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp

@@ -167,6 +167,13 @@ Polygon2d toPolygon2d(
   return isClockwise(polygon) ? polygon : inverseClockwise(polygon);
 }
 
+tier4_autoware_utils::Polygon2d toPolygon2d(
+  const autoware_auto_perception_msgs::msg::PredictedObject & object)
+{
+  return tier4_autoware_utils::toPolygon2d(
+    object.kinematics.initial_pose_with_covariance.pose, object.shape);
+}
+
 double getArea(const autoware_auto_perception_msgs::msg::Shape & shape)
 {
   if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -239,24 +239,6 @@ class IntersectionModule : public SceneModuleInterface
   bool isTargetStuckVehicleType(
     const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
 
-  /**
-   * @brief convert object to footprint polygon
-   * @param object detected object
-   * @return 2d polygon of the object footprint
-   */
-  Polygon2d toFootprintPolygon(
-    const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
-
-  /**
-   * @brief convert predicted object to footprint polygon
-   * @param object detected object
-   * @param predicted_pose predicted object pose
-   * @return 2d polygon of the object footprint
-   */
-  Polygon2d toPredictedFootprintPolygon(
-    const autoware_auto_perception_msgs::msg::PredictedObject & object,
-    const geometry_msgs::msg::Pose & predicted_pose) const;
-
   /**
    * @brief Whether target tier4_api_msgs::Intersection::status is valid or not
    * @param target_status target tier4_api_msgs::Intersection::status

planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp

@@ -90,15 +90,15 @@ class RunOutModule : public SceneModuleInterface
     const float velocity_mps) const;
 
   bool checkCollisionWithShape(
-    const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
-    const Shape & shape, std::vector<geometry_msgs::msg::Point> & collision_points) const;
+    const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape,
+    std::vector<geometry_msgs::msg::Point> & collision_points) const;
 
   bool checkCollisionWithCylinder(
-    const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
-    const float radius, std::vector<geometry_msgs::msg::Point> & collision_points) const;
+    const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius,
+    std::vector<geometry_msgs::msg::Point> & collision_points) const;
 
   bool checkCollisionWithBoundingBox(
-    const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
+    const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range,
     const geometry_msgs::msg::Vector3 & dimension,
     std::vector<geometry_msgs::msg::Point> & collision_points) const;
 

planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp

@@ -15,6 +15,8 @@
 #ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
 #define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_
 
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+
 #include <autoware_auto_planning_msgs/msg/path_point.hpp>
 #include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
@@ -33,7 +35,6 @@
 #include <boost/geometry/geometries/point_xy.hpp>
 #include <boost/geometry/geometries/polygon.hpp>
 #include <boost/geometry/geometries/register/point.hpp>
-#include <boost/geometry/geometries/segment.hpp>
 
 #include <lanelet2_core/primitives/Polygon.h>
 #include <tf2/utils.h>
@@ -60,16 +61,16 @@ BOOST_GEOMETRY_REGISTER_POINT_3D(
 
 namespace behavior_velocity_planner
 {
-using Point2d = boost::geometry::model::d2::point_xy<double>;
-using Segment2d = boost::geometry::model::segment<Point2d>;
-using LineString2d = boost::geometry::model::linestring<Point2d>;
-using Polygon2d =
-  boost::geometry::model::polygon<Point2d, false, false>;  // counter-clockwise, open
+namespace bg = boost::geometry;
+
+using Point2d = tier4_autoware_utils::Point2d;
+using LineString2d = tier4_autoware_utils::LineString2d;
+using Polygon2d = tier4_autoware_utils::Polygon2d;
 
 template <class T>
 Point2d to_bg2d(const T & p)
 {
-  return Point2d(boost::geometry::get<0>(p), boost::geometry::get<1>(p));
+  return Point2d(bg::get<0>(p), bg::get<1>(p));
 }
 
 template <class T>
@@ -82,17 +83,6 @@ LineString2d to_bg2d(const std::vector<T> & vec)
   return ps;
 }
 
-inline Polygon2d linestring2polygon(const LineString2d & line_string)
-{
-  Polygon2d polygon;
-
-  for (const auto & p : line_string) {
-    polygon.outer().push_back(p);
-  }
-
-  return polygon;
-}
-
 inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line)
 {
   Polygon2d polygon;
@@ -107,78 +97,11 @@ inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2
     polygon.outer().push_back(*itr);
   }
 
+  bg::correct(polygon);
   return polygon;
 }
 
-inline Polygon2d obj2polygon(
-  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & shape)
-{
-  // rename
-  const double x = pose.position.x;
-  const double y = pose.position.y;
-  const double h = shape.x;
-  const double w = shape.y;
-  const double yaw = tf2::getYaw(pose.orientation);
-
-  // create base polygon
-  Polygon2d obj_poly;
-  boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of<Point2d>(h / 2.0, w / 2.0)(
-    -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0);
-
-  // rotate polygon(yaw)
-  boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double, 2, 2>
-    rotate(-yaw);  // anti-clockwise -> :clockwise rotation
-  Polygon2d rotate_obj_poly;
-  boost::geometry::transform(obj_poly, rotate_obj_poly, rotate);
-
-  // translate polygon(x, y)
-  boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translate(x, y);
-  Polygon2d translate_obj_poly;
-  boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate);
-  return translate_obj_poly;
-}
-
-inline double calcOverlapAreaRate(const Polygon2d & target, const Polygon2d & base)
-{
-  /* OverlapAreaRate: common area(target && base) / target area */
-
-  if (boost::geometry::within(target, base)) {
-    // target is within base, common area = target area
-    return 1.0;
-  }
-
-  if (!boost::geometry::intersects(target, base)) {
-    // target and base has not intersect area
-    return 0.0;
-  }
-
-  // calculate intersect polygon
-  std::vector<Polygon2d> intersects;
-  boost::geometry::intersection(target, base, intersects);
-
-  // calculate area of polygon
-  double intersect_area = 0.0;
-  for (const auto & intersect : intersects) {
-    intersect_area += boost::geometry::area(intersect);
-  }
-  const double target_area = boost::geometry::area(target);
-  // specification of boost1.65
-  // common area is not intersect area
-  const double common_area = target_area - intersect_area;
-
-  return common_area / target_area;
-}
-
-inline std::vector<Segment2d> makeSegments(const LineString2d & ls)
-{
-  std::vector<Segment2d> segments;
-  for (size_t i = 0; i < ls.size(); ++i) {
-    segments.emplace_back(ls.at(i), ls.at(i + 1));
-  }
-  return segments;
-}
-
-inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon)
+inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon)
 {
   geometry_msgs::msg::Polygon polygon_msg;
   geometry_msgs::msg::Point32 point_msg;
@@ -189,27 +112,6 @@ inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon)
   }
   return polygon_msg;
 }
-
-inline Polygon2d toBoostPoly(const geometry_msgs::msg::Polygon & polygon)
-{
-  Polygon2d boost_poly;
-  for (const auto & point : polygon.points) {
-    const Point2d point2d(point.x, point.y);
-    boost_poly.outer().push_back(point2d);
-  }
-  return boost_poly;
-}
-
-inline Polygon2d toBoostPoly(const lanelet::BasicPolygon2d & polygon)
-{
-  Polygon2d boost_poly;
-  for (const auto & vec : polygon) {
-    const Point2d point2d(vec.x(), vec.y());
-    boost_poly.outer().push_back(point2d);
-  }
-
-  return boost_poly;
-}
 }  // namespace behavior_velocity_planner
 
 #endif  // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -79,7 +79,6 @@ struct PointWithSearchRangeIndex
 using geometry_msgs::msg::Pose;
 using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 using Polygons2d = std::vector<Polygon2d>;
-using Point2d = boost::geometry::model::d2::point_xy<double>;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::Shape;
@@ -162,7 +161,6 @@ geometry_msgs::msg::Pose transformAbsCoordinate2D(
   const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin);
 SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id);
 
-Polygon2d toFootprintPolygon(const PredictedObject & object);
 bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
 geometry_msgs::msg::Pose getAheadPose(
   const size_t start_idx, const double ahead_dist,

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -293,7 +293,7 @@ bool IntersectionModule::checkCollision(
     ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point),
     &closest_lanelet);
 
-  debug_data_.ego_lane_polygon = toGeomMsg(ego_poly);
+  debug_data_.ego_lane_polygon = toGeomPoly(ego_poly);
 
   /* extract target objects */
   autoware_auto_perception_msgs::msg::PredictedObjects target_objects;
@@ -403,12 +403,14 @@ bool IntersectionModule::checkCollision(
 
         polygon.outer().emplace_back(polygon.outer().front());
 
-        debug_data_.candidate_collision_ego_lane_polygon = toGeomMsg(polygon);
+        bg::correct(polygon);
+
+        debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon);
 
         for (auto itr = first_itr; itr != last_itr.base(); ++itr) {
-          const auto footprint_polygon = toPredictedFootprintPolygon(object, *itr);
+          const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape);
           debug_data_.candidate_collision_object_polygons.emplace_back(
-            toGeomMsg(footprint_polygon));
+            toGeomPoly(footprint_polygon));
           if (bg::intersects(polygon, footprint_polygon)) {
             collision_detected = true;
             break;
@@ -464,6 +466,7 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon(
   }
 
   polygon.outer().emplace_back(polygon.outer().front());
+  bg::correct(polygon);
 
   return polygon;
 }
@@ -538,7 +541,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
   const Polygon2d & stuck_vehicle_detect_area) const
 {
-  debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area);
+  debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area);
 
   for (const auto & object : objects_ptr->objects) {
     if (!isTargetStuckVehicleType(object)) {
@@ -550,7 +553,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection(
     }
 
     // check if the footprint is in the stuck detect area
-    const Polygon2d obj_footprint = toFootprintPolygon(object);
+    const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object);
     const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area);
     if (is_in_stuck_area) {
       RCLCPP_DEBUG(logger_, "stuck vehicle found.");
@@ -561,27 +564,6 @@ bool IntersectionModule::checkStuckVehicleInIntersection(
   return false;
 }
 
-Polygon2d IntersectionModule::toFootprintPolygon(
-  const autoware_auto_perception_msgs::msg::PredictedObject & object) const
-{
-  Polygon2d obj_footprint;
-  if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
-    obj_footprint = toBoostPoly(object.shape.footprint);
-  } else {
-    // cylinder type is treated as square-polygon
-    obj_footprint =
-      obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions);
-  }
-  return obj_footprint;
-}
-
-Polygon2d IntersectionModule::toPredictedFootprintPolygon(
-  const autoware_auto_perception_msgs::msg::PredictedObject & object,
-  const geometry_msgs::msg::Pose & predicted_pose) const
-{
-  return obj2polygon(predicted_pose, object.shape.dimensions);
-}
-
 bool IntersectionModule::isTargetCollisionVehicleType(
   const autoware_auto_perception_msgs::msg::PredictedObject & object) const
 {
@@ -794,7 +776,7 @@ bool IntersectionModule::checkFrontVehicleDeceleration(
   predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point;
   predicted_object.kinematics.initial_pose_with_covariance.pose.orientation =
     tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw);
-  Polygon2d predicted_obj_footprint = toFootprintPolygon(predicted_object);
+  auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object);
   const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area);
   debug_data_.predicted_obj_pose.position = stopping_point;
   debug_data_.predicted_obj_pose.orientation =

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp

@@ -162,8 +162,8 @@ bool NoStoppingAreaModule::modifyPathVelocity(
     setSafe(true);
     return true;
   }
-  debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area);
-  debug_data_.stop_line_detect_area = toGeomMsg(stop_line_detect_area);
+  debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area);
+  debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area);
   // Find stuck vehicle in no stopping area
   const bool is_entry_prohibited_by_stuck_vehicle =
     checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr);
@@ -230,11 +230,11 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea(
       continue;  // not stop vehicle
     }
     // check if the footprint is in the stuck detect area
-    const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object);
+    const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object);
     const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly);
     if (is_in_stuck_area) {
       RCLCPP_DEBUG(logger_, "stuck vehicle found.");
-      for (const auto p : obj_footprint.outer()) {
+      for (const auto & p : obj_footprint.outer()) {
         geometry_msgs::msg::Point point;
         point.x = p.x();
         point.y = p.y();

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp

@@ -36,6 +36,8 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius)
   line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle));
   // std::cout << boost::geometry::wkt(line_poly) << std::endl;
   // std::cout << boost::geometry::wkt(line) << std::endl;
+
+  bg::correct(line_poly);
   return line_poly;
 }
 
@@ -173,6 +175,8 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons
   poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, 0, 0).position));
   poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position));
   poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position));
+
+  bg::correct(poly);
   return poly;
 }