Skip to content

Commit

Permalink
Merge pull request #841 from tier4/cherry-pick-lane-change
Browse files Browse the repository at this point in the history 
hotfix(lane_change): cherry-pick lane change object filtering
  • Loading branch information
@tkimura4
tkimura4 authored Sep 15, 2023
2 parents 9f63a50 + 50ecbd5 commit 5a8c980
Show file tree
Hide file tree
Showing 4 changed files with 112 additions and 91 deletions.
4 changes: 4 additions & 0 deletions ...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase
const utils::path_safety_checker::RSSparams & rss_params,
CollisionCheckDebugMap & debug_data) const;

LaneChangeTargetObjectIndices filterObject(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
const lanelet::ConstLanelets & target_backward_lanes) const;

rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
};
} // namespace behavior_path_planner
Expand Down
6 changes: 0 additions & 6 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,12 +183,6 @@ boost::optional<size_t> getLeadingStaticObjectIdx(
std::optional<lanelet::BasicPolygon2d> createPolygon(
const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);

LaneChangeTargetObjectIndices filterObject(
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes,
const Pose & current_pose, const RouteHandler & route_handler,
const LaneChangeParameters & lane_change_parameters);

ExtendedPredictedObject transform(
const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters);
Expand Down
111 changes: 108 additions & 3 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -609,9 +609,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
route_handler, target_lanes, current_pose, backward_length);

// filter objects to get target object index
const auto target_obj_index = utils::lane_change::filterObject(
objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler,
*lane_change_parameters_);
const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes);

LaneChangeTargetObjects target_objects;
target_objects.current_lane.reserve(target_obj_index.current_lane.size());
Expand Down Expand Up @@ -642,6 +640,113 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
return target_objects;
}

LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
const lanelet::ConstLanelets & target_backward_lanes) const
{
const auto current_pose = getEgoPose();
const auto & route_handler = *getRouteHandler();
const auto & common_parameters = planner_data_->parameters;
const auto & objects = *planner_data_->dynamic_object;

// Guard
if (objects.objects.empty()) {
return {};
}

// Get path
const auto path =
route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
const auto target_path =
route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits<double>::max());

const auto current_polygon =
utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
const auto target_polygon =
utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
const auto target_backward_polygon = utils::lane_change::createPolygon(
target_backward_lanes, 0.0, std::numeric_limits<double>::max());
const auto dist_ego_to_current_lanes_center =
lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);

LaneChangeTargetObjectIndices filtered_obj_indices;
for (size_t i = 0; i < objects.objects.size(); ++i) {
const auto & object = objects.objects.at(i);
const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x;
const auto extended_object =
utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);

// ignore specific object types
if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) {
continue;
}

const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);

// calc distance from the current ego position
double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
double min_dist_ego_to_obj = std::numeric_limits<double>::max();
for (const auto & polygon_p : obj_polygon.outer()) {
const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
const double dist_ego_to_obj =
motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p);
max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj);
}

const auto is_lateral_far = [&]() {
const auto dist_object_to_current_lanes_center =
lanelet::utils::getLateralDistanceToClosestLanelet(
current_lanes, object.kinematics.initial_pose_with_covariance.pose);
const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center;
return std::abs(lateral) > (common_parameters.vehicle_width / 2);
};

// ignore static object that are behind the ego vehicle
if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) {
continue;
}

// check if the object intersects with target lanes
if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
// TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target
// lanes

if (max_dist_ego_to_obj >= 0 || is_lateral_far()) {
filtered_obj_indices.target_lane.push_back(i);
continue;
}
}

// check if the object intersects with target backward lanes
if (
target_backward_polygon &&
boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
filtered_obj_indices.target_lane.push_back(i);
continue;
}

// check if the object intersects with current lanes
if (
current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) &&
max_dist_ego_to_obj >= 0.0) {
// check only the objects that are in front of the ego vehicle
filtered_obj_indices.current_lane.push_back(i);
continue;
}

// ignore all objects that are behind the ego vehicle and not on the current and target
// lanes
if (max_dist_ego_to_obj < 0.0) {
continue;
}

filtered_obj_indices.other_lane.push_back(i);
}

return filtered_obj_indices;
}

PathWithLaneId NormalLaneChange::getTargetSegment(
const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose,
const double target_lane_length, const double lane_changing_length,
Expand Down
82 changes: 0 additions & 82 deletions planning/behavior_path_planner/src/utils/lane_change/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1063,88 +1063,6 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
return lanelet::utils::to2D(polygon_3d).basicPolygon();
}

LaneChangeTargetObjectIndices filterObject(
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes,
const Pose & current_pose, const RouteHandler & route_handler,
const LaneChangeParameters & lane_change_parameter)
{
// Guard
if (objects.objects.empty()) {
return {};
}

// Get path
const auto path =
route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());

const auto current_polygon =
createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
const auto target_polygon = createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
const auto target_backward_polygon =
createPolygon(target_backward_lanes, 0.0, std::numeric_limits<double>::max());

LaneChangeTargetObjectIndices filtered_obj_indices;
for (size_t i = 0; i < objects.objects.size(); ++i) {
const auto & object = objects.objects.at(i);
const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x;

// ignore specific object types
if (!isTargetObjectType(object, lane_change_parameter)) {
continue;
}

const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);

// calc distance from the current ego position
double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
for (const auto & polygon_p : obj_polygon.outer()) {
const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
const double dist_ego_to_obj =
motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p);
max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
}

// ignore static object that are behind the ego vehicle
if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) {
continue;
}

// check if the object intersects with target lanes
if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
filtered_obj_indices.target_lane.push_back(i);
continue;
}

// check if the object intersects with target backward lanes
if (
target_backward_polygon &&
boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
filtered_obj_indices.target_lane.push_back(i);
continue;
}

// check if the object intersects with current lanes
if (
current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) &&
max_dist_ego_to_obj >= 0.0) {
// check only the objects that are in front of the ego vehicle
filtered_obj_indices.current_lane.push_back(i);
continue;
}

// ignore all objects that are behind the ego vehicle and not on the current and target
// lanes
if (max_dist_ego_to_obj < 0.0) {
continue;
}

filtered_obj_indices.other_lane.push_back(i);
}

return filtered_obj_indices;
}

ExtendedPredictedObject transform(
const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters)
Expand Down

0 comments on commit 5a8c980

Please sign in to comment.