refactor(behavior_velocity_occlusion_spot_module): remove unnecessary…

… declaration (#7000)

* refactor(behavior_velocity_occlusion_spot_module): remove unnecessary declaration

Signed-off-by: Ryuta Kambe <[email protected]>

* remove unnecessary declaration

Signed-off-by: Ryuta Kambe <[email protected]>

* unify lanelet::ArcCoordinates

Signed-off-by: Ryuta Kambe <[email protected]>

* unify lanelet::geometry::fromArcCoordinates

Signed-off-by: Ryuta Kambe <[email protected]>

* style(pre-commit): autofix

* unify lanelet::ConstLineString2d

Signed-off-by: Ryuta Kambe <[email protected]>

* style(pre-commit): autofix

* fix

Signed-off-by: veqcc <[email protected]>

---------

Signed-off-by: Ryuta Kambe <[email protected]>
Signed-off-by: veqcc <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_velocity_occlusion_spot_module/src/debug.cpp

@@ -33,7 +33,6 @@ using BasicPolygons = std::vector<lanelet::BasicPolygon2d>;
 using occlusion_spot_utils::PossibleCollisionInfo;
 using tier4_autoware_utils::appendMarkerArray;
 using tier4_autoware_utils::calcOffsetPose;
-using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerOrientation;
 using tier4_autoware_utils::createMarkerPosition;

planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp

@@ -245,7 +245,8 @@ void categorizeVehicles(
   return;
 }
 
-ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string)
+lanelet::ArcCoordinates getOcclusionPoint(
+  const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string)
 {
   const auto poly = tier4_autoware_utils::toPolygon2d(obj);
   std::deque<lanelet::ArcCoordinates> arcs;
@@ -260,16 +261,18 @@ ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineStr
    * Ego===============> path
    **/
   // sort by arc length
-  std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) {
-    return arc1.length < arc2.length;
-  });
+  std::sort(
+    arcs.begin(), arcs.end(), [](lanelet::ArcCoordinates arc1, lanelet::ArcCoordinates arc2) {
+      return arc1.length < arc2.length;
+    });
   // remove closest 2 polygon point
   arcs.pop_front();
   arcs.pop_front();
   // sort by arc distance
-  std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) {
-    return std::abs(arc1.distance) < std::abs(arc2.distance);
-  });
+  std::sort(
+    arcs.begin(), arcs.end(), [](lanelet::ArcCoordinates arc1, lanelet::ArcCoordinates arc2) {
+      return std::abs(arc1.distance) < std::abs(arc2.distance);
+    });
   // return closest to path point which is choosen by farthest 2 points.
   return arcs.at(0);
 }
@@ -289,18 +292,19 @@ double calcSignedLateralDistanceWithOffset(
 }
 
 PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
-  const ArcCoordinates & arc_coord_occlusion,
-  const ArcCoordinates & arc_coord_occlusion_with_offset,
+  const lanelet::ArcCoordinates & arc_coord_occlusion,
+  const lanelet::ArcCoordinates & arc_coord_occlusion_with_offset,
   const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param)
 {
-  auto calcPosition = [](const ConstLineString2d & ll, const ArcCoordinates & arc) {
-    BasicPoint2d bp = fromArcCoordinates(ll, arc);
-    Point position;
-    position.x = bp[0];
-    position.y = bp[1];
-    position.z = 0.0;
-    return position;
-  };
+  auto calcPosition =
+    [](const lanelet::ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) {
+      BasicPoint2d bp = lanelet::geometry::fromArcCoordinates(ll, arc);
+      Point position;
+      position.x = bp[0];
+      position.y = bp[1];
+      position.z = 0.0;
+      return position;
+    };
   /**
    * @brief calculate obstacle collision intersection from arc coordinate info.
    *                                      ^
@@ -341,8 +345,8 @@ bool generatePossibleCollisionsFromObjects(
   lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
   auto ll = path_lanelet.centerline2d();
   for (const auto & dyn : dyn_objects) {
-    ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll);
-    ArcCoordinates arc_coord_occlusion_with_offset = {
+    lanelet::ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll);
+    lanelet::ArcCoordinates arc_coord_occlusion_with_offset = {
       arc_coord_occlusion.length - param.baselink_to_front,
       calcSignedLateralDistanceWithOffset(
         arc_coord_occlusion.distance, param.right_overhang, param.left_overhang,
@@ -455,7 +459,7 @@ std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpo
     if (length_to_col < offset_from_start_to_ego) {
       continue;
     }
-    ArcCoordinates arc_coord_collision_point = {
+    lanelet::ArcCoordinates arc_coord_collision_point = {
       length_to_col,
       calcSignedLateralDistanceWithOffset(
         arc_coord_occlusion_point.distance, right_overhang, left_overhang, wheel_tread)};

planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp

@@ -57,14 +57,10 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
-using lanelet::ArcCoordinates;
 using lanelet::BasicLineString2d;
 using lanelet::BasicPoint2d;
 using lanelet::BasicPolygon2d;
-using lanelet::ConstLineString2d;
 using lanelet::LaneletMapPtr;
-using lanelet::geometry::fromArcCoordinates;
-using lanelet::geometry::toArcCoordinates;
 using DetectionAreaIdx = std::optional<std::pair<double, double>>;
 using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;