Skip to content

Commit

Permalink
return detection_lanelets and conflicting_lanelets in getObjectiveLan…
Browse files Browse the repository at this point in the history
…elets

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
soblin committed Oct 14, 2022
1 parent d68b012 commit 41bb84b
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 34 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <memory>
#include <optional>
#include <string>
#include <tuple>
#include <vector>

namespace behavior_velocity_planner
Expand All @@ -48,10 +49,9 @@ bool getDuplicatedPointIdx(
/**
* @brief get objective polygons for detection area
*/
bool getDetectionLanelets(
std::tuple<lanelet::ConstLanelets, lanelet::ConstLanelets> getObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const int lane_id, const double detection_area_length,
lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on = false);
const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false);

struct StopLineIdx
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -98,20 +98,19 @@ bool IntersectionModule::modifyPathVelocity(
/* dynamically change detection area based on tl_arrow_solid_on */
const bool tl_arrow_solid_on =
util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
lanelet::ConstLanelets detection_area_lanelets;
util::getDetectionLanelets(
auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
&detection_area_lanelets, tl_arrow_solid_on);
if (detection_area_lanelets.empty()) {
tl_arrow_solid_on);
if (detection_lanelets.empty()) {
RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
setSafe(true);
setDistance(std::numeric_limits<double>::lowest());
return true;
}
const std::vector<lanelet::CompoundPolygon3d> detection_areas =
util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
const std::vector<lanelet::CompoundPolygon3d> detection_area =
util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);

debug_data_.detection_area = detection_areas;
debug_data_.detection_area = detection_area;

/* get intersection area */
const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
Expand All @@ -128,7 +127,7 @@ bool IntersectionModule::modifyPathVelocity(
/* set stop-line and stop-judgement-line for base_link */
util::StopLineIdx stop_line_idxs;
if (!util::generateStopLine(
lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin,
lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin,
planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs,
logger_.get_child("util"))) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
Expand Down Expand Up @@ -195,8 +194,8 @@ bool IntersectionModule::modifyPathVelocity(
planner_param_.stuck_vehicle_detect_dist);
bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
bool has_collision = checkCollision(
lanelet_map_ptr, *path, detection_area_lanelets, adjacent_lanelets, intersection_area,
objects_ptr, closest_idx, stuck_vehicle_detect_area);
lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
closest_idx, stuck_vehicle_detect_area);
bool is_entry_prohibited = (has_collision || is_stuck);
if (external_go) {
is_entry_prohibited = false;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,24 +66,23 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();

/* get detection area */
lanelet::ConstLanelets detection_area_lanelets;
util::getDetectionLanelets(
auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets(
lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
&detection_area_lanelets);
std::vector<lanelet::CompoundPolygon3d> detection_areas =
util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
if (detection_areas.empty()) {
false /* tl_arrow_solid on does not matter here*/);
if (detection_lanelets.empty()) {
RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
return true;
}
debug_data_.detection_area = detection_areas;
const auto detection_area =
util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length);
debug_data_.detection_area = detection_area;

/* set stop-line and stop-judgement-line for base_link */
util::StopLineIdx stop_line_idxs;
const auto private_path =
extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
if (!util::generateStopLine(
lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin,
lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin,
0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs,
logger_.get_child("util"))) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -293,12 +293,13 @@ bool getStopLineIndexFromMap(
return true;
}

bool getDetectionLanelets(
// TODO(Mamoru Sobue): return std::tuple<bool, lanelet::ConstLanelets, lanelet::ConstLanelets>
std::tuple<lanelet::ConstLanelets, lanelet::ConstLanelets> getObjectiveLanelets(
lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
const int lane_id, const double detection_area_length,
lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on)
const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on)
{
const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);
const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");

// retrieve a stopline associated with a traffic light
bool has_traffic_light = false;
Expand All @@ -309,13 +310,10 @@ bool getDetectionLanelets(
if (!!stop_line_opt) has_traffic_light = true;
}

const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");

lanelet::ConstLanelets yield_lanelets;

// for low priority lane
// If ego_lane has right of way (i.e. is high priority),
// ignore yieldLanelets (i.e. low priority lanes)
lanelet::ConstLanelets yield_lanelets{};
const auto right_of_ways = assigned_lanelet.regulatoryElementsAs<lanelet::RightOfWay>();
for (const auto & right_of_way : right_of_ways) {
if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) {
Expand All @@ -328,9 +326,8 @@ bool getDetectionLanelets(
}
}

lanelet::ConstLanelets ego_lanelets;

// for the behind ego-car lane.
// get all following lanes of previous lane
lanelet::ConstLanelets ego_lanelets{};
for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) {
ego_lanelets.push_back(previous_lanelet);
for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) {
Expand All @@ -347,6 +344,12 @@ bool getDetectionLanelets(

// final objective lanelets
lanelet::ConstLanelets detection_lanelets;
lanelet::ConstLanelets conflicting_ex_ego_lanelets;
// conflicting lanes is necessary to get stop_line for stuck vehicle
for (auto && conflicting_lanelet : conflicting_lanelets) {
if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet))
conflicting_ex_ego_lanelets.push_back(conflicting_lanelet);
}

// exclude yield lanelets and ego lanelets from detection_lanelets
if (turn_direction == std::string("straight") && has_traffic_light) {
Expand Down Expand Up @@ -384,11 +387,10 @@ bool getDetectionLanelets(
}
}
}
*detection_lanelets_result = detection_and_preceding_lanelets;
return {std::move(detection_and_preceding_lanelets), std::move(conflicting_ex_ego_lanelets)};
} else {
*detection_lanelets_result = detection_lanelets;
return {std::move(detection_lanelets), std::move(conflicting_ex_ego_lanelets)};
}
return true;
}

std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLanelets(
Expand Down

0 comments on commit 41bb84b

Please sign in to comment.