From 825d8d582b5b5c2f097dc74c151403d7bf8fffcd Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 14 May 2024 10:31:43 +0900 Subject: [PATCH 1/8] refactor(behavior_velocity_occlusion_spot_module): remove unnecessary declaration Signed-off-by: Ryuta Kambe --- planning/behavior_velocity_occlusion_spot_module/src/debug.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index a6e13d0dfdcdc..f1de8365824b5 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; From a74541230ec0e0dbe9b2f9842d81f78d7c45b2c5 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 14 May 2024 10:42:17 +0900 Subject: [PATCH 2/8] remove unnecessary declaration Signed-off-by: Ryuta Kambe --- .../src/occlusion_spot_utils.hpp | 1 - 1 file changed, 1 deletion(-) 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 ec58e38dec45a..c4cd3646156a2 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 @@ -64,7 +64,6 @@ 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; From f9ccf6f6aa5c83b7906419a326327e129a882adc Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 14 May 2024 10:48:39 +0900 Subject: [PATCH 3/8] unify lanelet::ArcCoordinates Signed-off-by: Ryuta Kambe --- .../src/occlusion_spot_utils.cpp | 18 +++++++++--------- .../src/occlusion_spot_utils.hpp | 1 - 2 files changed, 9 insertions(+), 10 deletions(-) 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 f38768ce26da4..14b19c047fd43 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 @@ -242,7 +242,7 @@ void categorizeVehicles( return; } -ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) +lanelet::ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) { const auto poly = tier4_autoware_utils::toPolygon2d(obj); std::deque arcs; @@ -257,14 +257,14 @@ ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineStr * Ego===============> path **/ // sort by arc length - std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) { + 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) { + 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. @@ -286,11 +286,11 @@ 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) { + auto calcPosition = [](const ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) { BasicPoint2d bp = fromArcCoordinates(ll, arc); Point position; position.x = bp[0]; @@ -338,8 +338,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, @@ -451,7 +451,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 c4cd3646156a2..77188c97cc891 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,7 +57,6 @@ 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; From 41864cd242dc04fab7ca24b3d0f854e251037c6e Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 14 May 2024 10:50:11 +0900 Subject: [PATCH 4/8] unify lanelet::geometry::fromArcCoordinates Signed-off-by: Ryuta Kambe --- .../src/occlusion_spot_utils.cpp | 2 +- .../src/occlusion_spot_utils.hpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) 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 14b19c047fd43..f92065da703fb 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 @@ -291,7 +291,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { auto calcPosition = [](const ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) { - BasicPoint2d bp = fromArcCoordinates(ll, arc); + BasicPoint2d bp = lanelet::geometry::fromArcCoordinates;(ll, arc); Point position; position.x = bp[0]; position.y = bp[1]; 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 77188c97cc891..d3172e0bc3ed7 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 @@ -62,7 +62,6 @@ using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; using lanelet::ConstLineString2d; using lanelet::LaneletMapPtr; -using lanelet::geometry::fromArcCoordinates; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; From 6edc7fd9dba46774179321d5d75a3e66b097b56b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 May 2024 01:50:43 +0000 Subject: [PATCH 5/8] style(pre-commit): autofix --- .../src/occlusion_spot_utils.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) 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 f92065da703fb..c3180050494b8 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 @@ -242,7 +242,8 @@ void categorizeVehicles( return; } -lanelet::ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) +lanelet::ArcCoordinates getOcclusionPoint( + const PredictedObject & obj, const ConstLineString2d & ll_string) { const auto poly = tier4_autoware_utils::toPolygon2d(obj); std::deque arcs; @@ -257,16 +258,18 @@ lanelet::ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const Con * Ego===============> path **/ // sort by arc length - std::sort(arcs.begin(), arcs.end(), [](lanelet::ArcCoordinates arc1, lanelet::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(), [](lanelet::ArcCoordinates arc1, lanelet::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); } From b6ad3c033fcc39d197105aca927b1d0d84db3a2b Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 14 May 2024 11:00:36 +0900 Subject: [PATCH 6/8] unify lanelet::ConstLineString2d Signed-off-by: Ryuta Kambe --- .../src/occlusion_spot_utils.cpp | 4 ++-- .../src/occlusion_spot_utils.hpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) 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 c3180050494b8..085178f757fc6 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 @@ -243,7 +243,7 @@ void categorizeVehicles( } lanelet::ArcCoordinates getOcclusionPoint( - const PredictedObject & obj, const ConstLineString2d & ll_string) + const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string) { const auto poly = tier4_autoware_utils::toPolygon2d(obj); std::deque arcs; @@ -293,7 +293,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const lanelet::ArcCoordinates & arc_coord_occlusion_with_offset, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - auto calcPosition = [](const ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) { + auto calcPosition = [](const lanelet::ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) { BasicPoint2d bp = lanelet::geometry::fromArcCoordinates;(ll, arc); Point position; position.x = bp[0]; 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 d3172e0bc3ed7..cbf781a5e3aca 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 @@ -60,7 +60,6 @@ using geometry_msgs::msg::Pose; using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; -using lanelet::ConstLineString2d; using lanelet::LaneletMapPtr; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; From 720a37c60f40eee836ce87324263ad80fd4e2b20 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 May 2024 02:02:44 +0000 Subject: [PATCH 7/8] style(pre-commit): autofix --- .../src/occlusion_spot_utils.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) 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 085178f757fc6..29284cb5ce5cb 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 @@ -293,14 +293,16 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const lanelet::ArcCoordinates & arc_coord_occlusion_with_offset, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - 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; - }; + 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. * ^ From e0992bbc8a4407b09d41e3e68cfec8245bb59764 Mon Sep 17 00:00:00 2001 From: veqcc Date: Tue, 14 May 2024 12:19:06 +0900 Subject: [PATCH 8/8] fix Signed-off-by: veqcc --- .../src/occlusion_spot_utils.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 29284cb5ce5cb..e8290acdfcd73 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 @@ -295,8 +295,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( { auto calcPosition = [](const lanelet::ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) { - BasicPoint2d bp = lanelet::geometry::fromArcCoordinates; - (ll, arc); + BasicPoint2d bp = lanelet::geometry::fromArcCoordinates(ll, arc); Point position; position.x = bp[0]; position.y = bp[1];