diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index f4d3fbf6cc2c1..1170afc490a75 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -33,7 +33,6 @@ using BasicPolygons = std::vector; 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; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 22b93c2c0236c..f22cedbe67c8f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/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 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 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)}; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 47242b70f0cbd..7f495f042d7f8 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/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>; using BasicPolygons2d = std::vector;