Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(intersection): code clean #2071

Merged
merged 4 commits into from
Oct 14, 2022
Merged

refactor(intersection): code clean #2071

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3e0840d
avoid lanelet <-> id as much as possible
soblin Oct 13, 2022
d68b012
compiles
soblin Oct 14, 2022
7ab8a28
return detection_lanelets and conflicting_lanelets in getObjectiveLan…
soblin Oct 14, 2022
5d3f57b
refactor stop line stuff
soblin Oct 14, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
return detection_lanelets and conflicting_lanelets in getObjectiveLan… 
…elets

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
@soblin
soblin committed Oct 14, 2022
commit 7ab8a28af512ebc94156d9c09d6fdc3987c1fe62
5 changes: 3 additions & 2 deletions planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,8 @@ class IntersectionModule : public SceneModuleInterface
bool checkCollision(
lanelet::LaneletMapConstPtr lanelet_map_ptr,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
lanelet::ConstLanelets & detection_area_lanelets, lanelet::ConstLanelets & adjacent_lanelets,
const lanelet::ConstLanelets & detection_area_lanelets,
const lanelet::ConstLanelets & adjacent_lanelets,
const std::optional<Polygon2d> & intersection_area,
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
const int closest_idx, const Polygon2d & stuck_vehicle_detect_area);
Expand Down Expand Up @@ -221,7 +222,7 @@ class IntersectionModule : public SceneModuleInterface
* @return true if the given pose belongs to any target lanelet
*/
bool checkAngleForTargetLanelets(
const geometry_msgs::msg::Pose & pose, lanelet::ConstLanelets & target_lanelet_ids,
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelet_ids,
const double margin = 0);

/**
Expand Down
6 changes: 3 additions & 3 deletions planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp
View file
Open in desktop
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
29 changes: 15 additions & 14 deletions planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
View file
Open in desktop
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 @@ -193,10 +192,11 @@ bool IntersectionModule::modifyPathVelocity(
const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon(
lanelet_map_ptr, *path, closest_idx, stop_line_idx, detect_length,
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);
const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
const bool has_collision = checkCollision(
lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
closest_idx, stuck_vehicle_detect_area);
// TODO(Mamoru Sobue): process external_go at the top of this function
bool is_entry_prohibited = (has_collision || is_stuck);
if (external_go) {
is_entry_prohibited = false;
Expand Down Expand Up @@ -282,7 +282,8 @@ void IntersectionModule::cutPredictPathWithDuration(
bool IntersectionModule::checkCollision(
lanelet::LaneletMapConstPtr lanelet_map_ptr,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
lanelet::ConstLanelets & detection_area_lanelets, lanelet::ConstLanelets & adjacent_lanelets,
const lanelet::ConstLanelets & detection_area_lanelets,
const lanelet::ConstLanelets & adjacent_lanelets,
const std::optional<Polygon2d> & intersection_area,
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
const int closest_idx, const Polygon2d & stuck_vehicle_detect_area)
Expand Down Expand Up @@ -626,7 +627,7 @@ bool IntersectionModule::isTargetExternalInputStatus(const int target_status)
}

bool IntersectionModule::checkAngleForTargetLanelets(
const geometry_msgs::msg::Pose & pose, lanelet::ConstLanelets & target_lanelets,
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
const double margin)
{
for (const auto & ll : target_lanelets) {
Expand Down
15 changes: 7 additions & 8 deletions ...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp
View file
Open in desktop
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
28 changes: 15 additions & 13 deletions planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
View file
Open in desktop
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