Skip to content

Commit

Permalink
feat(avoidance): suppress unnecessary avoidance path
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Oct 24, 2023
1 parent 7c3bde1 commit 806fcd3
Show file tree
Hide file tree
Showing 4 changed files with 214 additions and 131 deletions.
4 changes: 4 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,10 @@ bool isWithinCrosswalk(
bool isTargetObjectType(
const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters);

bool isParkedOnRoadShoulder(
ObjectData & object, std::shared_ptr<RouteHandler> & route_handler,
std::shared_ptr<AvoidanceParameters> & parameters);

double calcShiftLength(
const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);

Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -330,6 +330,8 @@ std::optional<double> getSignedDistanceFromBoundary(

Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet);

Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon);

std::vector<Polygon2d> getTargetLaneletPolygons(
const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length,
const std::string & target_type);
Expand Down
328 changes: 197 additions & 131 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <lanelet2_extension/utility/utilities.hpp>
#include <motion_utils/trajectory/interpolation.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/math/unit_conversion.hpp>
#include <tier4_autoware_utils/ros/uuid_helper.hpp>

#include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
Expand Down Expand Up @@ -292,6 +293,177 @@ bool isWithinCrosswalk(
return false;
}

bool isWithinIntersection(
const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
{
const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else");
if (id == "else") {
return false;
}

const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);

const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));

return boost::geometry::within(
object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
}

bool isParkedOnRoadShoulder(
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;

// ============================================ <- most_left_lanelet.leftBound()
// y road shoulder
// ^ ------------------------------------------
// | x +
// +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline()
//
// --------------------------------------------
// +: 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);

bool is_left_side_parked_vehicle = false;
if (!isOnRight(object)) {
auto [object_shiftable_distance, sub_type] = [&]() {
const auto most_left_road_lanelet =
route_handler->getMostLeftLanelet(object.overhang_lanelet);
const auto most_left_lanelet_candidates =
route_handler->getLaneletMapPtr()->laneletLayer.findUsages(
most_left_road_lanelet.leftBound());

lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet;
const lanelet::Attribute sub_type =
most_left_lanelet.attribute(lanelet::AttributeName::Subtype);

for (const auto & ll : most_left_lanelet_candidates) {
const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
if (sub_type.value() == "road_shoulder") {
most_left_lanelet = ll;
}
}

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

return std::make_pair(
center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
}();

if (sub_type.value() != "road_shoulder") {
object_shiftable_distance += parameters->object_check_min_road_shoulder_width;
}

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

is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio;
}

bool is_right_side_parked_vehicle = false;
if (isOnRight(object)) {
auto [object_shiftable_distance, sub_type] = [&]() {
const auto most_right_road_lanelet =
route_handler->getMostRightLanelet(object.overhang_lanelet);
const auto most_right_lanelet_candidates =
route_handler->getLaneletMapPtr()->laneletLayer.findUsages(
most_right_road_lanelet.rightBound());

lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet;
const lanelet::Attribute sub_type =
most_right_lanelet.attribute(lanelet::AttributeName::Subtype);

for (const auto & ll : most_right_lanelet_candidates) {
const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
if (sub_type.value() == "road_shoulder") {
most_right_lanelet = ll;
}
}

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

return std::make_pair(
center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type);
}();

if (sub_type.value() != "road_shoulder") {
object_shiftable_distance += parameters->object_check_min_road_shoulder_width;
}

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

is_right_side_parked_vehicle =
object.shiftable_ratio > parameters->object_check_shiftable_ratio;
}

return !is_left_side_parked_vehicle && !is_right_side_parked_vehicle;
}

bool isForceAvoidanceTarget(
ObjectData & object, const lanelet::ConstLanelets & extend_lanelets,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
if (!parameters->enable_force_avoidance_for_stopped_vehicle) {
return false;
}

const auto stop_time_longer_than_threshold =
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;

if (!stop_time_longer_than_threshold) {
return false;
}

const auto & ego_pose = planner_data->self_odometry->pose.pose;
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;

// force avoidance for stopped vehicle
bool not_parked_object = true;

// check traffic light
const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
{
not_parked_object =
to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
}

const auto route_graph_ptr = planner_data->route_handler->getOverallGraphPtr();

// check crosswalk
const auto to_crosswalk =
utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *route_graph_ptr) -
object.longitudinal;
{
const auto stop_for_crosswalk =
to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance &&
to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance;
not_parked_object = not_parked_object || stop_for_crosswalk;
}

object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk);

return !not_parked_object;
}

double calcShiftLength(
const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin)
{
Expand Down Expand Up @@ -1115,41 +1287,11 @@ void filterTargetObjects(
}

// from here condition check for vehicle type objects.

const auto stop_time_longer_than_threshold =
o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;

if (stop_time_longer_than_threshold && parameters->enable_force_avoidance_for_stopped_vehicle) {
// force avoidance for stopped vehicle
bool not_parked_object = true;

// check traffic light
const auto to_traffic_light =
utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets);
{
not_parked_object =
to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
}

// check crosswalk
const auto to_crosswalk =
utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) -
o.longitudinal;
{
const auto stop_for_crosswalk =
to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance &&
to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance;
not_parked_object = not_parked_object || stop_for_crosswalk;
}

o.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk);

if (!not_parked_object) {
o.last_seen = now;
o.avoid_margin = avoid_margin;
data.target_objects.push_back(o);
continue;
}
if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) {
o.last_seen = now;
o.avoid_margin = avoid_margin;
data.target_objects.push_back(o);
continue;
}

// Object is on center line -> ignore.
Expand All @@ -1159,118 +1301,42 @@ void filterTargetObjects(
continue;
}

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

/**
* Is not object in adjacent lane?
* - Yes -> Is parking object?
* - Yes -> the object is avoidance target.
* - No -> ignore this object.
* - No -> the object is avoidance target no matter whether it is parking object or not.
*/
lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y());
const auto is_in_ego_lane =
within(object_centroid, overhang_lanelet.polygon2d().basicPolygon());
if (is_in_ego_lane) {
/**
* TODO(Satoshi Ota) use intersection area
* under the assumption that there is no parking vehicle inside intersection,
* ignore all objects that is in the ego lane as not parking objects.
*/
std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else");
if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
if (isParkedOnRoadShoulder(o, rh, parameters)) {
o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
data.other_objects.push_back(o);
continue;
} else {
o.last_seen = now;
o.avoid_margin = avoid_margin;
data.target_objects.push_back(o);
continue;
}
}

const auto centerline_pose =
lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position);
lanelet::BasicPoint3d centerline_point(
centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z);

// ============================================ <- most_left_lanelet.leftBound()
// y road shoulder
// ^ ------------------------------------------
// | x +
// +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline()
//
// --------------------------------------------
// +: object position
// o: nearest point on centerline

bool is_left_side_parked_vehicle = false;
if (!isOnRight(o)) {
auto [object_shiftable_distance, sub_type] = [&]() {
const auto most_left_road_lanelet = rh->getMostLeftLanelet(overhang_lanelet);
const auto most_left_lanelet_candidates =
rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound());

lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet;
const lanelet::Attribute sub_type =
most_left_lanelet.attribute(lanelet::AttributeName::Subtype);

for (const auto & ll : most_left_lanelet_candidates) {
const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
if (sub_type.value() == "road_shoulder") {
most_left_lanelet = ll;
}
}

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

return std::make_pair(
center_to_left_boundary - 0.5 * o.object.shape.dimensions.y, sub_type);
}();

if (sub_type.value() != "road_shoulder") {
object_shiftable_distance += parameters->object_check_min_road_shoulder_width;
}

const auto arc_coordinates =
toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid);
o.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;

is_left_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio;
}

bool is_right_side_parked_vehicle = false;
if (isOnRight(o)) {
auto [object_shiftable_distance, sub_type] = [&]() {
const auto most_right_road_lanelet = rh->getMostRightLanelet(overhang_lanelet);
const auto most_right_lanelet_candidates =
rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound());

lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet;
const lanelet::Attribute sub_type =
most_right_lanelet.attribute(lanelet::AttributeName::Subtype);

for (const auto & ll : most_right_lanelet_candidates) {
const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype);
if (sub_type.value() == "road_shoulder") {
most_right_lanelet = ll;
}
}

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

return std::make_pair(
center_to_right_boundary - 0.5 * o.object.shape.dimensions.y, sub_type);
}();

if (sub_type.value() != "road_shoulder") {
object_shiftable_distance += parameters->object_check_min_road_shoulder_width;
}

const auto arc_coordinates =
toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid);
o.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;

is_right_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio;
std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else");
if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
if (!isWithinIntersection(o, rh)) {
o.last_seen = now;
o.avoid_margin = avoid_margin;
data.target_objects.push_back(o);
continue;
}

if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) {
constexpr double THRESHOLD = M_PI_2 / 9.0;
const auto closest_pose =
lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position);
const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose));
if (yaw_deviation < THRESHOLD || yaw_deviation > M_PI - THRESHOLD) {
o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
data.other_objects.push_back(o);
continue;
Expand Down
Loading

0 comments on commit 806fcd3

Please sign in to comment.