Skip to content

Commit 355d677

Browse files
veqccpre-commit-ci[bot]
authored andcommitted
refactor(behavior_velocity_occlusion_spot_module): remove unnecessary declaration (autowarefoundation#7000)
* refactor(behavior_velocity_occlusion_spot_module): remove unnecessary declaration Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * remove unnecessary declaration Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * unify lanelet::ArcCoordinates Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * unify lanelet::geometry::fromArcCoordinates Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * style(pre-commit): autofix * unify lanelet::ConstLineString2d Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * style(pre-commit): autofix * fix Signed-off-by: veqcc <ryuta.kambe@tier4.jp> --------- Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> Signed-off-by: veqcc <ryuta.kambe@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: vividf <yihsiang.fang@tier4.jp>
1 parent f5b1b94 commit 355d677

File tree

3 files changed

+24
-25
lines changed

3 files changed

+24
-25
lines changed

planning/behavior_velocity_occlusion_spot_module/src/debug.cpp

-1
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,6 @@ using BasicPolygons = std::vector<lanelet::BasicPolygon2d>;
3333
using occlusion_spot_utils::PossibleCollisionInfo;
3434
using tier4_autoware_utils::appendMarkerArray;
3535
using tier4_autoware_utils::calcOffsetPose;
36-
using tier4_autoware_utils::createDefaultMarker;
3736
using tier4_autoware_utils::createMarkerColor;
3837
using tier4_autoware_utils::createMarkerOrientation;
3938
using tier4_autoware_utils::createMarkerPosition;

planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp

+24-20
Original file line numberDiff line numberDiff line change
@@ -245,7 +245,8 @@ void categorizeVehicles(
245245
return;
246246
}
247247

248-
ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string)
248+
lanelet::ArcCoordinates getOcclusionPoint(
249+
const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string)
249250
{
250251
const auto poly = tier4_autoware_utils::toPolygon2d(obj);
251252
std::deque<lanelet::ArcCoordinates> arcs;
@@ -260,16 +261,18 @@ ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineStr
260261
* Ego===============> path
261262
**/
262263
// sort by arc length
263-
std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) {
264-
return arc1.length < arc2.length;
265-
});
264+
std::sort(
265+
arcs.begin(), arcs.end(), [](lanelet::ArcCoordinates arc1, lanelet::ArcCoordinates arc2) {
266+
return arc1.length < arc2.length;
267+
});
266268
// remove closest 2 polygon point
267269
arcs.pop_front();
268270
arcs.pop_front();
269271
// sort by arc distance
270-
std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) {
271-
return std::abs(arc1.distance) < std::abs(arc2.distance);
272-
});
272+
std::sort(
273+
arcs.begin(), arcs.end(), [](lanelet::ArcCoordinates arc1, lanelet::ArcCoordinates arc2) {
274+
return std::abs(arc1.distance) < std::abs(arc2.distance);
275+
});
273276
// return closest to path point which is choosen by farthest 2 points.
274277
return arcs.at(0);
275278
}
@@ -289,18 +292,19 @@ double calcSignedLateralDistanceWithOffset(
289292
}
290293

291294
PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
292-
const ArcCoordinates & arc_coord_occlusion,
293-
const ArcCoordinates & arc_coord_occlusion_with_offset,
295+
const lanelet::ArcCoordinates & arc_coord_occlusion,
296+
const lanelet::ArcCoordinates & arc_coord_occlusion_with_offset,
294297
const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param)
295298
{
296-
auto calcPosition = [](const ConstLineString2d & ll, const ArcCoordinates & arc) {
297-
BasicPoint2d bp = fromArcCoordinates(ll, arc);
298-
Point position;
299-
position.x = bp[0];
300-
position.y = bp[1];
301-
position.z = 0.0;
302-
return position;
303-
};
299+
auto calcPosition =
300+
[](const lanelet::ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) {
301+
BasicPoint2d bp = lanelet::geometry::fromArcCoordinates(ll, arc);
302+
Point position;
303+
position.x = bp[0];
304+
position.y = bp[1];
305+
position.z = 0.0;
306+
return position;
307+
};
304308
/**
305309
* @brief calculate obstacle collision intersection from arc coordinate info.
306310
* ^
@@ -341,8 +345,8 @@ bool generatePossibleCollisionsFromObjects(
341345
lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
342346
auto ll = path_lanelet.centerline2d();
343347
for (const auto & dyn : dyn_objects) {
344-
ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll);
345-
ArcCoordinates arc_coord_occlusion_with_offset = {
348+
lanelet::ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll);
349+
lanelet::ArcCoordinates arc_coord_occlusion_with_offset = {
346350
arc_coord_occlusion.length - param.baselink_to_front,
347351
calcSignedLateralDistanceWithOffset(
348352
arc_coord_occlusion.distance, param.right_overhang, param.left_overhang,
@@ -455,7 +459,7 @@ std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpo
455459
if (length_to_col < offset_from_start_to_ego) {
456460
continue;
457461
}
458-
ArcCoordinates arc_coord_collision_point = {
462+
lanelet::ArcCoordinates arc_coord_collision_point = {
459463
length_to_col,
460464
calcSignedLateralDistanceWithOffset(
461465
arc_coord_occlusion_point.distance, right_overhang, left_overhang, wheel_tread)};

planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp

-4
Original file line numberDiff line numberDiff line change
@@ -57,14 +57,10 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
5757
using autoware_auto_planning_msgs::msg::PathWithLaneId;
5858
using geometry_msgs::msg::Point;
5959
using geometry_msgs::msg::Pose;
60-
using lanelet::ArcCoordinates;
6160
using lanelet::BasicLineString2d;
6261
using lanelet::BasicPoint2d;
6362
using lanelet::BasicPolygon2d;
64-
using lanelet::ConstLineString2d;
6563
using lanelet::LaneletMapPtr;
66-
using lanelet::geometry::fromArcCoordinates;
67-
using lanelet::geometry::toArcCoordinates;
6864
using DetectionAreaIdx = std::optional<std::pair<double, double>>;
6965
using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
7066

0 commit comments

Comments
 (0)