Skip to content

Commit

Permalink
Merge pull request #1127 from tier4/hotfix/v0.44.0/avoidance
Browse files Browse the repository at this point in the history
fix(avoidance): don't use polygon centroid in shiftable ratio calculation (autowarefoundation#6285)
  • Loading branch information
shmpwk authored Feb 5, 2024
2 parents c906189 + 24bc357 commit 50641ce
Showing 1 changed file with 20 additions and 16 deletions.
36 changes: 20 additions & 16 deletions planning/behavior_path_avoidance_module/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include "behavior_path_planner_common/utils/traffic_light_utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"

#include <lanelet2_extension/utility/message_conversion.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/ros/uuid_helper.hpp>

Expand Down Expand Up @@ -476,11 +477,11 @@ bool isObjectOnRoadShoulder(
ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
using boost::geometry::return_centroid;
using boost::geometry::within;
using lanelet::geometry::distance2d;
using lanelet::geometry::toArcCoordinates;
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;

// assume that there are no parked vehicles in intersection.
std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else");
Expand All @@ -498,13 +499,9 @@ bool isObjectOnRoadShoulder(
// +: object position
// o: nearest point on centerline

lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());

const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
const auto centerline_pose =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
lanelet::BasicPoint3d centerline_point(
centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z);
const auto centerline_pos =
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position).position;

bool is_left_side_parked_vehicle = false;
if (!isOnRight(object)) {
Expand All @@ -526,8 +523,9 @@ bool isObjectOnRoadShoulder(
}
}

const auto center_to_left_boundary =
distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point));
const auto center_to_left_boundary = distance2d(
to2D(most_left_lanelet.leftBound().basicLineString()),
to2D(toLaneletPoint(centerline_pos)).basicPoint());

return std::make_pair(
center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
Expand All @@ -538,7 +536,8 @@ bool isObjectOnRoadShoulder(
}

const auto arc_coordinates = toArcCoordinates(
to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid);
to2D(object.overhang_lanelet.centerline().basicLineString()),
to2D(toLaneletPoint(object_pose.position)).basicPoint());
object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;

is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio;
Expand All @@ -564,8 +563,9 @@ bool isObjectOnRoadShoulder(
}
}

const auto center_to_right_boundary =
distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point));
const auto center_to_right_boundary = distance2d(
to2D(most_right_lanelet.rightBound().basicLineString()),
to2D(toLaneletPoint(centerline_pos)).basicPoint());

return std::make_pair(
center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
Expand All @@ -576,7 +576,8 @@ bool isObjectOnRoadShoulder(
}

const auto arc_coordinates = toArcCoordinates(
to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid);
to2D(object.overhang_lanelet.centerline().basicLineString()),
to2D(toLaneletPoint(object_pose.position)).basicPoint());
object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;

is_right_side_parked_vehicle =
Expand Down Expand Up @@ -754,6 +755,8 @@ bool isSatisfiedWithVehicleCondition(
const std::shared_ptr<AvoidanceParameters> & parameters)
{
using boost::geometry::within;
using lanelet::utils::to2D;
using lanelet::utils::conversion::toLaneletPoint;

object.behavior = getObjectBehavior(object, parameters);
object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
Expand All @@ -764,9 +767,10 @@ bool isSatisfiedWithVehicleCondition(
}

// check vehicle shift ratio
lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y());
const auto on_ego_driving_lane =
within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon());
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
const auto on_ego_driving_lane = within(
to2D(toLaneletPoint(object_pos)).basicPoint(),
object.overhang_lanelet.polygon2d().basicPolygon());
if (on_ego_driving_lane) {
if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) {
return true;
Expand Down

0 comments on commit 50641ce

Please sign in to comment.