Skip to content

Commit

Permalink
fix(intersection): modify ego polygon calculation (autowarefoundation…
Browse files Browse the repository at this point in the history 
…#4694)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Aug 23, 2023
1 parent df6bfb4 commit b56a7fb
Show file tree
Hide file tree
Showing 5 changed files with 165 additions and 124 deletions.
185 changes: 90 additions & 95 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -719,9 +719,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
}

const auto & current_pose = planner_data_->current_odometry->pose;
const auto lanelets_on_path =
planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose);
if (!intersection_lanelets_) {
const auto lanelets_on_path =
planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose);
intersection_lanelets_ = util::getObjectiveLanelets(
lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_,
planner_param_.common.attention_area_length,
Expand Down Expand Up @@ -756,8 +756,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
[closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx,
pass_judge_line_idx] = intersection_stop_lines;

const auto ego_lane_with_next_lane = util::getEgoLaneWithNextLane(
*path, associative_ids_, planner_data_->vehicle_info_.vehicle_width_m);
const auto path_lanelets_opt = util::generatePathLanelets(
lanelets_on_path, *path, associative_ids_, closest_idx,
planner_data_->vehicle_info_.vehicle_width_m);
if (!path_lanelets_opt.has_value()) {
RCLCPP_DEBUG(logger_, "failed to generate PathLanelets");
return IntersectionModule::Indecisive{};
}
const auto path_lanelets = path_lanelets_opt.value();

const auto ego_lane_with_next_lane =
path_lanelets.next.has_value()
? std::vector<
lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()}
: std::vector<lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit};
const bool stuck_detected =
checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines);

Expand Down Expand Up @@ -846,8 +858,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
collision_state_machine_.getDuration());
const auto target_objects =
filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area);

const bool has_collision = checkCollision(
*path, target_objects, ego_lane_with_next_lane, closest_idx, time_delay, tl_arrow_solid_on);
*path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on);
collision_state_machine_.setStateWithMarginTime(
has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
logger_.get_child("collision state_machine"), *clock_);
Expand Down Expand Up @@ -1026,8 +1039,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT
bool IntersectionModule::checkCollision(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx,
const double time_delay, const bool tl_arrow_solid_on)
const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay,
const bool tl_arrow_solid_on)
{
using lanelet::utils::getArcCoordinates;
using lanelet::utils::getPolygonFromArcLength;
Expand All @@ -1042,12 +1055,10 @@ bool IntersectionModule::checkCollision(
auto target_objects = objects;
util::cutPredictPathWithDuration(&target_objects, clock_, passing_time);

const auto & concat_lanelets = path_lanelets.all;
const auto closest_arc_coords = getArcCoordinates(
ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point));
const auto & ego_lane = ego_lane_with_next_lane.front();
const double distance_until_intersection =
util::calcDistanceUntilIntersectionLanelet(ego_lane, path, closest_idx);
const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point));
const auto & ego_lane = path_lanelets.ego_or_entry2exit;

const auto ego_poly = ego_lane.polygon2d().basicPolygon();
// check collision between predicted_path and ego_area
Expand All @@ -1059,7 +1070,6 @@ bool IntersectionModule::checkCollision(
: planner_param_.collision_detection.normal.collision_end_margin_time;
bool collision_detected = false;
for (const auto & object : target_objects.objects) {
bool has_collision = false;
for (const auto & predicted_path : object.kinematics.predicted_paths) {
if (
predicted_path.confidence <
Expand All @@ -1068,97 +1078,82 @@ bool IntersectionModule::checkCollision(
continue;
}

std::vector<geometry_msgs::msg::Pose> predicted_poses;
for (const auto & pose : predicted_path.path) {
predicted_poses.push_back(pose);
}
has_collision = bg::intersects(ego_poly, to_bg2d(predicted_poses));
if (has_collision) {
const auto first_itr = std::adjacent_find(
predicted_path.path.cbegin(), predicted_path.path.cend(),
[&ego_poly](const auto & a, const auto & b) {
return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
});
const auto last_itr = std::adjacent_find(
predicted_path.path.crbegin(), predicted_path.path.crend(),
[&ego_poly](const auto & a, const auto & b) {
return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
});
const double ref_object_enter_time =
static_cast<double>(first_itr - predicted_path.path.begin()) *
rclcpp::Duration(predicted_path.time_step).seconds();
auto start_time_distance_itr = time_distance_array.begin();
if (ref_object_enter_time - collision_start_margin_time > 0) {
start_time_distance_itr = std::lower_bound(
time_distance_array.begin(), time_distance_array.end(),
ref_object_enter_time - collision_start_margin_time,
[](const auto & a, const double b) { return a.first < b; });
if (start_time_distance_itr == time_distance_array.end()) {
continue;
}
}
const double ref_object_exit_time =
static_cast<double>(last_itr.base() - predicted_path.path.begin()) *
rclcpp::Duration(predicted_path.time_step).seconds();
auto end_time_distance_itr = std::lower_bound(
// collision point
const auto first_itr = std::adjacent_find(
predicted_path.path.cbegin(), predicted_path.path.cend(),
[&ego_poly](const auto & a, const auto & b) {
return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
});
if (first_itr == predicted_path.path.cend()) continue;
const auto last_itr = std::adjacent_find(
predicted_path.path.crbegin(), predicted_path.path.crend(),
[&ego_poly](const auto & a, const auto & b) {
return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)});
});
if (last_itr == predicted_path.path.crend()) continue;

// possible collision time interval
const double ref_object_enter_time =
static_cast<double>(first_itr - predicted_path.path.begin()) *
rclcpp::Duration(predicted_path.time_step).seconds();
auto start_time_distance_itr = time_distance_array.begin();
if (ref_object_enter_time - collision_start_margin_time > 0) {
// start of possible ego position in the intersection
start_time_distance_itr = std::lower_bound(
time_distance_array.begin(), time_distance_array.end(),
ref_object_exit_time + collision_end_margin_time,
ref_object_enter_time - collision_start_margin_time,
[](const auto & a, const double b) { return a.first < b; });
if (end_time_distance_itr == time_distance_array.end()) {
end_time_distance_itr = time_distance_array.end() - 1;
}
const double start_arc_length = std::max(
0.0, closest_arc_coords.length + (*start_time_distance_itr).second -
distance_until_intersection);
const double end_arc_length = std::max(
0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front -
distance_until_intersection);

long double lanes_length = 0.0;
std::vector<lanelet::ConstLanelet> ego_lane_with_next_lanes;

const auto lanelets_on_path = planning_utils::getLaneletsOnPath(
path, planner_data_->route_handler_->getLaneletMapPtr(),
planner_data_->current_odometry->pose);
for (const auto & lane : lanelets_on_path) {
lanes_length += bg::length(lane.centerline());
ego_lane_with_next_lanes.push_back(lane);
if (lanes_length > start_arc_length && lanes_length < end_arc_length) {
break;
}
}
const auto trimmed_ego_polygon =
getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length);

if (trimmed_ego_polygon.empty()) {
if (start_time_distance_itr == time_distance_array.end()) {
// ego is already at the exit of intersection when npc is at collision point even if npc
// accelerates so ego's position interval is empty
continue;
}
}
const double ref_object_exit_time =
static_cast<double>(last_itr.base() - predicted_path.path.begin()) *
rclcpp::Duration(predicted_path.time_step).seconds();
auto end_time_distance_itr = std::lower_bound(
time_distance_array.begin(), time_distance_array.end(),
ref_object_exit_time + collision_end_margin_time,
[](const auto & a, const double b) { return a.first < b; });
if (end_time_distance_itr == time_distance_array.end()) {
// ego is already passing the intersection, when npc is is at collision point
// so ego's position interval is up to the end of intersection lane
end_time_distance_itr = time_distance_array.end() - 1;
}
const double start_arc_length = std::max(
0.0, closest_arc_coords.length + (*start_time_distance_itr).second -
planner_data_->vehicle_info_.rear_overhang_m);
const double end_arc_length = std::min(
closest_arc_coords.length + (*end_time_distance_itr).second +
planner_data_->vehicle_info_.max_longitudinal_offset_m,
lanelet::utils::getLaneletLength2d(concat_lanelets));

const auto trimmed_ego_polygon =
getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length);

if (trimmed_ego_polygon.empty()) {
continue;
}

Polygon2d polygon{};
for (const auto & p : trimmed_ego_polygon) {
polygon.outer().emplace_back(p.x(), p.y());
}

polygon.outer().emplace_back(polygon.outer().front());

bg::correct(polygon);

debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon);
Polygon2d polygon{};
for (const auto & p : trimmed_ego_polygon) {
polygon.outer().emplace_back(p.x(), p.y());
}
bg::correct(polygon);
debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon);

for (auto itr = first_itr; itr != last_itr.base(); ++itr) {
const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape);
debug_data_.candidate_collision_object_polygons.emplace_back(
toGeomPoly(footprint_polygon));
if (bg::intersects(polygon, footprint_polygon)) {
collision_detected = true;
break;
}
}
if (collision_detected) {
debug_data_.conflicting_targets.objects.push_back(object);
for (auto itr = first_itr; itr != last_itr.base(); ++itr) {
const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape);
if (bg::intersects(polygon, footprint_polygon)) {
collision_detected = true;
break;
}
}
if (collision_detected) {
debug_data_.conflicting_targets.objects.push_back(object);
break;
}
}
}

Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,8 +258,8 @@ class IntersectionModule : public SceneModuleInterface
bool checkCollision(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects,
const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx,
const double time_delay, const bool tl_arrow_solid_on);
const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay,
const bool tl_arrow_solid_on);

bool isOcclusionCleared(
const nav_msgs::msg::OccupancyGrid & occ_grid,
Expand Down
79 changes: 56 additions & 23 deletions planning/behavior_velocity_intersection_module/src/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -872,29 +872,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
return obj_pose;
}

lanelet::ConstLanelets getEgoLaneWithNextLane(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const std::set<int> & associative_ids, const double width)
{
// NOTE: findLaneIdsInterval returns (start, end) of associative_ids
const auto ego_lane_interval_opt = findLaneIdsInterval(path, associative_ids);
if (!ego_lane_interval_opt) {
return lanelet::ConstLanelets({});
}
const auto [ego_start, ego_end] = ego_lane_interval_opt.value();
if (ego_end < path.points.size() - 1) {
const int next_id = path.points.at(ego_end).lane_ids.at(0);
const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id});
if (next_lane_interval_opt) {
const auto [next_start, next_end] = next_lane_interval_opt.value();
return {
planning_utils::generatePathLanelet(path, ego_start, next_start + 1, width),
planning_utils::generatePathLanelet(path, next_start + 1, next_end, width)};
}
}
return {planning_utils::generatePathLanelet(path, ego_start, ego_end, width)};
}

static bool isTargetStuckVehicleType(
const autoware_auto_perception_msgs::msg::PredictedObject & object)
{
Expand Down Expand Up @@ -1138,5 +1115,61 @@ void IntersectionLanelets::update(
}
}

static lanelet::ConstLanelets getPrevLanelets(
const lanelet::ConstLanelets & lanelets_on_path, const std::set<int> & associative_ids)
{
lanelet::ConstLanelets prevs;
for (const auto & ll : lanelets_on_path) {
if (associative_ids.find(ll.id()) != associative_ids.end()) {
return prevs;
}
prevs.push_back(ll);
}
return prevs;
}

std::optional<PathLanelets> generatePathLanelets(
const lanelet::ConstLanelets & lanelets_on_path,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const std::set<int> & associative_ids, const size_t closest_idx, const double width)
{
const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids);
if (!assigned_lane_interval_opt) {
return std::nullopt;
}

PathLanelets path_lanelets;
// prev
path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids);
path_lanelets.all = path_lanelets.prev;

// entry2ego if exist
const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value();
if (closest_idx > assigned_lane_start) {
path_lanelets.all.push_back(
planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width));
}

// ego_or_entry2exit
const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start);
path_lanelets.ego_or_entry2exit =
planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width);
path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit);

// next
if (assigned_lane_end < path.points.size() - 1) {
const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0);
const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id});
if (next_lane_interval_opt) {
const auto [next_start, next_end] = next_lane_interval_opt.value();
path_lanelets.next =
planning_utils::generatePathLanelet(path, next_start + 1, next_end, width);
path_lanelets.all.push_back(path_lanelets.next.value());
}
}

return path_lanelets;
}

} // namespace util
} // namespace behavior_velocity_planner
9 changes: 5 additions & 4 deletions planning/behavior_velocity_intersection_module/src/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,10 +126,6 @@ std::optional<InterpolatedPathInfo> generateInterpolatedPath(
geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state);

lanelet::ConstLanelets getEgoLaneWithNextLane(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const std::set<int> & associative_ids, const double width);

bool checkStuckVehicleInIntersection(
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr,
Expand Down Expand Up @@ -160,6 +156,11 @@ double calcDistanceUntilIntersectionLanelet(
const lanelet::ConstLanelet & assigned_lanelet,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx);

std::optional<PathLanelets> generatePathLanelets(
const lanelet::ConstLanelets & lanelets_on_path,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const std::set<int> & associative_ids, const size_t closest_idx, const double width);

} // namespace util
} // namespace behavior_velocity_planner

Expand Down
12 changes: 12 additions & 0 deletions planning/behavior_velocity_intersection_module/src/util_type.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,18 @@ struct IntersectionStopLines
size_t pass_judge_line;
};

struct PathLanelets
{
lanelet::ConstLanelets prev;
// lanelet::Constlanelet entry2ego; this is included in `all` if exists
lanelet::ConstLanelet
ego_or_entry2exit; // this is `assigned lane` part of the path(not from
// ego) if ego is before the intersection, otherwise from ego to exit
std::optional<lanelet::ConstLanelet> next =
std::nullopt; // this is nullopt is the goal is inside intersection
lanelet::ConstLanelets all;
};

} // namespace behavior_velocity_planner::util

#endif // UTIL_TYPE_HPP_

0 comments on commit b56a7fb

Please sign in to comment.