Skip to content

Commit

Permalink
fix(behavior_velocity_planner): keep intersection module for lanes wi…
Browse files Browse the repository at this point in the history 
…th same parent and same direction (autowarefoundation#2874)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
@soblin
soblin committed Mar 6, 2023
1 parent 825ea4a commit 71ebb4c
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 37 deletions.
15 changes: 12 additions & 3 deletions planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

#include <functional>
#include <memory>
#include <string>

namespace behavior_velocity_planner
{
Expand All @@ -43,9 +44,12 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;

bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const;
bool hasSameParentLaneletAndTurnDirectionWith(
const lanelet::ConstLanelet & lane, const size_t module_id,
const std::string & turn_direction_lane) const;

bool hasSameParentLaneletWithRegistered(const lanelet::ConstLanelet & lane) const;
bool hasSameParentLaneletAndTurnDirectionWithRegistered(
const lanelet::ConstLanelet & lane, const std::string & turn_direction) const;
};

class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
Expand All @@ -63,7 +67,12 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;

bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const;
bool hasSameParentLaneletAndTurnDirectionWith(
const lanelet::ConstLanelet & lane, const size_t module_id,
const std::string & turn_direction_lane) const;

bool hasSameParentLaneletAndTurnDirectionWithRegistered(
const lanelet::ConstLanelet & lane, const std::string & turn_direction) const;
};
} // namespace behavior_velocity_planner

Expand Down
85 changes: 51 additions & 34 deletions planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,17 +96,18 @@ void IntersectionModuleManager::launchNewModules(
const auto lane_id = ll.id();
const auto module_id = lane_id;

if (hasSameParentLaneletWithRegistered(ll)) {
continue;
}

// Is intersection?
const std::string turn_direction = ll.attributeOr("turn_direction", "else");
const auto is_intersection =
turn_direction == "right" || turn_direction == "left" || turn_direction == "straight";
if (!is_intersection) {
continue;
}

if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll, turn_direction)) {
continue;
}

registerModule(std::make_shared<IntersectionModule>(
module_id, lane_id, planner_data_, intersection_param_,
logger_.get_child("intersection_module"), clock_));
Expand Down Expand Up @@ -141,29 +142,31 @@ void MergeFromPrivateModuleManager::launchNewModules(
// Is merging from private road?
// In case the goal is in private road, check if this lanelet is conflicting with urban lanelet
const std::string lane_location = ll.attributeOr("location", "else");
if (lane_location == "private") {
if (i + 1 < lanelets.size()) {
const auto next_lane = lanelets.at(i + 1);
const std::string next_lane_location = next_lane.attributeOr("location", "else");
if (next_lane_location != "private") {
if (lane_location != "private") {
continue;
}

if (i + 1 < lanelets.size()) {
const auto next_lane = lanelets.at(i + 1);
const std::string next_lane_location = next_lane.attributeOr("location", "else");
if (next_lane_location != "private") {
registerModule(std::make_shared<MergeFromPrivateRoadModule>(
module_id, lane_id, planner_data_, merge_from_private_area_param_,
logger_.get_child("merge_from_private_road_module"), clock_));
continue;
}
} else {
const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
const auto conflicting_lanelets =
lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll);
for (auto && conflicting_lanelet : conflicting_lanelets) {
const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else");
if (conflicting_attr == "urban") {
registerModule(std::make_shared<MergeFromPrivateRoadModule>(
module_id, lane_id, planner_data_, merge_from_private_area_param_,
logger_.get_child("merge_from_private_road_module"), clock_));
continue;
}
} else {
const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
const auto conflicting_lanelets =
lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll);
for (auto && conflicting_lanelet : conflicting_lanelets) {
const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else");
if (conflicting_attr == "urban") {
registerModule(std::make_shared<MergeFromPrivateRoadModule>(
module_id, lane_id, planner_data_, merge_from_private_area_param_,
logger_.get_child("merge_from_private_road_module"), clock_));
continue;
}
}
}
}
}
Expand All @@ -184,7 +187,8 @@ IntersectionModuleManager::getModuleExpiredFunction(
if (!is_intersection) {
continue;
}
if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) {
if (hasSameParentLaneletAndTurnDirectionWith(
lane, scene_module->getModuleId(), turn_direction)) {
return false;
}
}
Expand All @@ -207,22 +211,27 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction(
if (!is_intersection) {
continue;
}
if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) {
if (hasSameParentLaneletAndTurnDirectionWith(
lane, scene_module->getModuleId(), turn_direction)) {
return false;
}
}
return true;
};
}

bool IntersectionModuleManager::hasSameParentLaneletWith(
const lanelet::ConstLanelet & lane, const size_t module_id) const
bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWith(
const lanelet::ConstLanelet & lane, const size_t module_id,
const std::string & turn_direction_lane) const
{
lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);

const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id);
const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else");
if (turn_direction_registered != turn_direction_lane) {
return false;
}
lanelet::ConstLanelets registered_parents =
planner_data_->route_handler_->getPreviousLanelets(registered_lane);
lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);
for (const auto & ll : registered_parents) {
auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll);
neighbor_lanes.push_back(ll);
Expand All @@ -233,13 +242,17 @@ bool IntersectionModuleManager::hasSameParentLaneletWith(
return false;
}

bool IntersectionModuleManager::hasSameParentLaneletWithRegistered(
const lanelet::ConstLanelet & lane) const
bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered(
const lanelet::ConstLanelet & lane, const std::string & turn_direction) const
{
lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);

for (const auto & id : registered_module_id_set_) {
const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id);
const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else");
if (turn_direction_registered != turn_direction) {
continue;
}
lanelet::ConstLanelets registered_parents =
planner_data_->route_handler_->getPreviousLanelets(registered_lane);
for (const auto & ll : registered_parents) {
Expand All @@ -253,14 +266,18 @@ bool IntersectionModuleManager::hasSameParentLaneletWithRegistered(
return false;
}

bool MergeFromPrivateModuleManager::hasSameParentLaneletWith(
const lanelet::ConstLanelet & lane, const size_t module_id) const
bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWith(
const lanelet::ConstLanelet & lane, const size_t module_id,
const std::string & turn_direction) const
{
lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);

const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id);
const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else");
if (turn_direction_registered != turn_direction) {
return false;
}
lanelet::ConstLanelets registered_parents =
planner_data_->route_handler_->getPreviousLanelets(registered_lane);
lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane);
for (const auto & ll : registered_parents) {
auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll);
neighbor_lanes.push_back(ll);
Expand Down

0 comments on commit 71ebb4c

Please sign in to comment.