Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(behavior_velocity_occlusion_spot_module): remove unnecessary declaration #7000

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -242,7 +242,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;
Expand All @@ -257,16 +258,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);
}
Expand All @@ -286,18 +289,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.
* ^
Expand Down Expand Up @@ -338,8 +342,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,
Expand Down Expand Up @@ -451,7 +455,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)};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>;

Expand Down
Loading