Skip to content

Commit

Permalink
Merge branch 'beta/v0.29.0-1' into fix/overwrite_invalid_detection_re…
Browse files Browse the repository at this point in the history
…sult/v0.29.0-1
  • Loading branch information
saka1-s authored Dec 19, 2024
2 parents 47e2931 + e89815f commit f8c8bb1
Show file tree
Hide file tree
Showing 8 changed files with 20,176 additions and 93 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,16 @@ target_link_libraries(${PROJECT_NAME}

ament_auto_package(INSTALL_TO_SHARE config)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
${TEST_SOURCES}
)
target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
endif()

install(PROGRAMS
scripts/ttc.py
DESTINATION lib/${PROJECT_NAME}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
<depend>autoware_planning_msgs</depend>
<depend>autoware_route_handler</depend>
<depend>autoware_rtc_interface</depend>
<depend>autoware_test_utils</depend>
<depend>autoware_universe_utils</depend>
<depend>autoware_vehicle_info_utils</depend>
<depend>fmt</depend>
Expand All @@ -39,6 +40,7 @@
<depend>tier4_planning_msgs</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -590,7 +590,8 @@ class IntersectionModule : public SceneModuleInterface
* @brief generate discretized detection lane linestring.
*/
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
lanelet::ConstLanelets detection_lanelets,
const lanelet::ConstLanelets & occlusion_detection_lanelets,
const lanelet::ConstLanelets & conflicting_detection_lanelets,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
/** @} */

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -266,8 +266,8 @@ Result<IntersectionModule::BasicData, InternalError> IntersectionModule::prepare

if (!occlusion_attention_divisions_) {
occlusion_attention_divisions_ = generateDetectionLaneDivisions(
intersection_lanelets.occlusion_attention(), routing_graph_ptr,
planner_data_->occupancy_grid->info.resolution);
intersection_lanelets.occlusion_attention(), intersection_lanelets.attention_non_preceding(),
routing_graph_ptr, planner_data_->occupancy_grid->info.resolution);
}

if (has_traffic_light_) {
Expand Down Expand Up @@ -577,6 +577,73 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
return intersection_stoplines;
}

static std::vector<std::deque<lanelet::ConstLanelet>> getPrecedingLaneletsUptoIntersectionRecursive(
const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
const double length, const lanelet::ConstLanelets & exclude_lanelets)
{
std::vector<std::deque<lanelet::ConstLanelet>> preceding_lanelet_sequences;

const auto prev_lanelets = graph->previous(lanelet);
const double lanelet_length = lanelet::utils::getLaneletLength3d(lanelet);

// end condition of the recursive function
if (prev_lanelets.empty() || lanelet_length >= length) {
preceding_lanelet_sequences.push_back({lanelet});
return preceding_lanelet_sequences;
}

for (const auto & prev_lanelet : prev_lanelets) {
if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
// if prev_lanelet is included in exclude_lanelets,
// remove prev_lanelet from preceding_lanelet_sequences
continue;
}
if (const std::string turn_direction = prev_lanelet.attributeOr("turn_direction", "else");
turn_direction == "left" || turn_direction == "right") {
continue;
}

// get lanelet sequence after prev_lanelet
auto tmp_lanelet_sequences = getPrecedingLaneletsUptoIntersectionRecursive(
graph, prev_lanelet, length - lanelet_length, exclude_lanelets);
for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
tmp_lanelet_sequence.push_back(lanelet);
preceding_lanelet_sequences.push_back(tmp_lanelet_sequence);
}
}

if (preceding_lanelet_sequences.empty()) {
preceding_lanelet_sequences.push_back({lanelet});
}
return preceding_lanelet_sequences;
}

static std::vector<lanelet::ConstLanelets> getPrecedingLaneletsUptoIntersection(
const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
const double length, const lanelet::ConstLanelets & exclude_lanelets)
{
std::vector<lanelet::ConstLanelets> lanelet_sequences_vec;
const auto prev_lanelets = graph->previous(lanelet);
for (const auto & prev_lanelet : prev_lanelets) {
if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
// if prev_lanelet is included in exclude_lanelets,
// remove prev_lanelet from preceding_lanelet_sequences
continue;
}
if (const std::string turn_direction = prev_lanelet.attributeOr("turn_direction", "else");
turn_direction == "left" || turn_direction == "right") {
continue;
}
// convert deque into vector
const auto lanelet_sequences_deq =
getPrecedingLaneletsUptoIntersectionRecursive(graph, prev_lanelet, length, exclude_lanelets);
for (const auto & lanelet_sequence : lanelet_sequences_deq) {
lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end());
}
}
return lanelet_sequences_vec;
}

IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const lanelet::ConstLanelet assigned_lanelet) const
Expand Down Expand Up @@ -706,8 +773,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll);
// get preceding lanelets without ego_lanelets
// to prevent the detection area from including the ego lanes and its' preceding lanes.
const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences(
routing_graph_ptr, ll, length, ego_lanelets);
const auto lanelet_sequences =
getPrecedingLaneletsUptoIntersection(routing_graph_ptr, ll, length, ego_lanelets);
for (const auto & ls : lanelet_sequences) {
for (const auto & l : ls) {
const auto & inserted = detection_ids.insert(l.id());
Expand All @@ -716,17 +783,10 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
}
}
}
lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction;
for (const auto & ll : occlusion_detection_and_preceding_lanelets) {
const std::string turn_direction = ll.attributeOr("turn_direction", "else");
if (turn_direction == "left" || turn_direction == "right") {
continue;
}
occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll);
}

auto [attention_lanelets, original_attention_lanelet_sequences] =
util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr);
util::mergeLaneletsByTopologicalSort(
detection_and_preceding_lanelets, detection_lanelets, routing_graph_ptr);

IntersectionLanelets result;
result.attention_ = std::move(attention_lanelets);
Expand Down Expand Up @@ -764,8 +824,7 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
// NOTE: occlusion_attention is not inverted here
// TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and
// then trim part of them based on curvature threshold
result.occlusion_attention_ =
std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction);
result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets);

// NOTE: to properly update(), each element in conflicting_/conflicting_area_,
// attention_non_preceding_/attention_non_preceding_area_ need to be matched
Expand Down Expand Up @@ -851,7 +910,8 @@ std::optional<PathLanelets> IntersectionModule::generatePathLanelets(
}

std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLaneDivisions(
lanelet::ConstLanelets detection_lanelets_all,
const lanelet::ConstLanelets & occlusion_detection_lanelets,
const lanelet::ConstLanelets & conflicting_detection_lanelets,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const
{
const double curvature_threshold =
Expand All @@ -861,9 +921,9 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan

using lanelet::utils::getCenterlineWithOffset;

// (0) remove left/right lanelet
// (0) remove curved
lanelet::ConstLanelets detection_lanelets;
for (const auto & detection_lanelet : detection_lanelets_all) {
for (const auto & detection_lanelet : occlusion_detection_lanelets) {
// TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet
const auto fine_centerline =
lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds);
Expand All @@ -874,9 +934,17 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
detection_lanelets.push_back(detection_lanelet);
}

std::vector<lanelet::ConstLineString3d> detection_divisions;
if (detection_lanelets.empty()) {
// NOTE(soblin): due to the above filtering detection_lanelets may be empty or do not contain
// conflicting_detection_lanelets
// OK to return empty detction_divsions
return detection_divisions;
}

// (1) tsort detection_lanelets
const auto [merged_detection_lanelets, originals] =
util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr);
const auto [merged_detection_lanelets, originals] = util::mergeLaneletsByTopologicalSort(
detection_lanelets, conflicting_detection_lanelets, routing_graph_ptr);

// (2) merge each branch to one lanelet
// NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here
Expand All @@ -892,7 +960,6 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
}

// (3) discretize each merged lanelet
std::vector<lanelet::ConstLineString3d> detection_divisions;
for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) {
const double length = bg::length(merged_lanelet.centerline());
const double width = area / length;
Expand Down
Loading

0 comments on commit f8c8bb1

Please sign in to comment.