diff --git a/perception/traffic_light_estimater/include/traffic_light_estimater/node.hpp b/perception/traffic_light_estimater/include/traffic_light_estimater/node.hpp index 6a3acf365dd11..0b7b9667adb4e 100644 --- a/perception/traffic_light_estimater/include/traffic_light_estimater/node.hpp +++ b/perception/traffic_light_estimater/include/traffic_light_estimater/node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include @@ -57,8 +59,9 @@ class TrafficLightEstimaterNode : public rclcpp::Node lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; - std::vector route_lanelet_traffic_lights_; + lanelet::ConstLanelets conflicting_crosswalks_; void onMap(const HADMapBin::ConstSharedPtr msg); void onRoute(const HADMapRoute::ConstSharedPtr msg); diff --git a/perception/traffic_light_estimater/launch/traffic_light_estimater.launch.xml b/perception/traffic_light_estimater/launch/traffic_light_estimater.launch.xml index db1129dc64a21..c3c6b6fb02614 100644 --- a/perception/traffic_light_estimater/launch/traffic_light_estimater.launch.xml +++ b/perception/traffic_light_estimater/launch/traffic_light_estimater.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/perception/traffic_light_estimater/src/node.cpp b/perception/traffic_light_estimater/src/node.cpp index b36ee5ad5aa10..804eb0d795e72 100644 --- a/perception/traffic_light_estimater/src/node.cpp +++ b/perception/traffic_light_estimater/src/node.cpp @@ -22,6 +22,59 @@ namespace traffic_light { +namespace +{ + +bool hasMergeLane( + const lanelet::ConstLanelet & lanelet_1, const lanelet::ConstLanelet & lanelet_2, + const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) +{ + const auto next_lanelets_1 = routing_graph_ptr->following(lanelet_1); + const auto next_lanelets_2 = routing_graph_ptr->following(lanelet_2); + + for (const auto next_lanelet_1 : next_lanelets_1) { + for (const auto next_lanelet_2 : next_lanelets_2) { + if (next_lanelet_1.id() == next_lanelet_2.id()) { + return true; + } + } + } + + return false; +} + +bool hasMergeLane( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) +{ + for (size_t i = 0; i < lanelets.size(); ++i) { + for (size_t j = i + 1; j < lanelets.size(); ++j) { + const auto lanelet_1 = lanelets.at(i); + const auto lanelet_2 = lanelets.at(j); + + if (lanelet_1.id() == lanelet_2.id()) { + continue; + } + + const std::string turn_direction_1 = lanelet_1.attributeOr("turn_direction", "none"); + const std::string turn_direction_2 = lanelet_2.attributeOr("turn_direction", "none"); + if (turn_direction_1 == turn_direction_2) { + continue; + } + + if (!hasMergeLane(lanelet_1, lanelet_2, routing_graph_ptr)) { + continue; + } + + return true; + } + } + + return false; +} + +} // namespace + TrafficLightEstimaterNode::TrafficLightEstimaterNode(const rclcpp::NodeOptions & options) : Node("traffic_light_estimater", options) { @@ -47,6 +100,17 @@ void TrafficLightEstimaterNode::onMap(const HADMapBin::ConstSharedPtr msg) lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); RCLCPP_INFO(get_logger(), "[TrafficLightEstimaterNode]: Map is loaded"); } @@ -56,6 +120,7 @@ void TrafficLightEstimaterNode::onRoute(const HADMapRoute::ConstSharedPtr msg) RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map"); return; } + lanelet::ConstLanelets route_lanelets; for (const auto & segment : msg->segments) { for (const auto & primitive : segment.primitives) { @@ -67,7 +132,17 @@ void TrafficLightEstimaterNode::onRoute(const HADMapRoute::ConstSharedPtr msg) } } } - route_lanelet_traffic_lights_ = lanelet::utils::query::autowareTrafficLights(route_lanelets); + + conflicting_crosswalks_.clear(); + + for (const auto & route_lanelet : route_lanelets) { + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflict_lls = + overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); + for (const auto & lanelet : conflict_lls) { + conflicting_crosswalks_.push_back(lanelet); + } + } } void TrafficLightEstimaterNode::onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg) @@ -79,35 +154,70 @@ void TrafficLightEstimaterNode::onTrafficLightArray(const TrafficSignalArray::Co traffic_light_id_map[traffic_signal.map_primitive_id] = traffic_signal; } - if (route_lanelet_traffic_lights_.empty()) { - pub_traffic_light_array_->publish(output); - return; - } + for (const auto & crosswalk : conflicting_crosswalks_) { + constexpr int VEHICLE_GRAPH_ID = 0; + const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); - for (const auto & traffic_lights_reg_elem : route_lanelet_traffic_lights_) { - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights(); - const auto highest_traffic_signal = - getHighestConfidenceTrafficSignal(traffic_lights, traffic_light_id_map); + bool has_left_green_lane = false; + bool has_right_green_lane = false; + bool has_straight_green_lane = false; + bool has_straight_lane = false; - const auto is_green = highest_traffic_signal == TrafficLight::GREEN || - highest_traffic_signal == TrafficLight::RIGHT_ARROW || - highest_traffic_signal == TrafficLight::LEFT_ARROW || - highest_traffic_signal == TrafficLight::UP_ARROW; + lanelet::ConstLanelets green_lanes; + for (const auto & lanelet : conflict_lls) { + const auto tl_reg_elems_for_vehicle = + lanelet.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - const std::string type = ll_traffic_light.attributeOr(lanelet::AttributeName::Type, "none"); - if (type.compare("traffic_light_crosswalk") != 0) { - continue; + bool is_green = false; + for (const auto & tl_reg_elem_for_vehicle : tl_reg_elems_for_vehicle) { + const auto traffic_lights_for_vehicle = tl_reg_elem_for_vehicle->trafficLights(); + + const auto highest_traffic_signal = + getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, traffic_light_id_map); + + if (highest_traffic_signal == TrafficLight::GREEN) { + is_green = true; + green_lanes.push_back(lanelet); + break; + } + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + if (turn_direction == "left") { + has_left_green_lane = has_left_green_lane || is_green; + } else if (turn_direction == "right") { + has_right_green_lane = has_right_green_lane || is_green; + } else { + has_straight_green_lane = has_straight_green_lane || is_green; + has_straight_lane = true; } + } + + const auto has_merge_lane = hasMergeLane(green_lanes, routing_graph_ptr_); - TrafficSignal output_traffic_signal; - TrafficLight output_traffic_light; - output_traffic_light.color = is_green ? TrafficLight::RED : TrafficLight::UNKNOWN; - output_traffic_light.confidence = 1.0; - output_traffic_signal.lights.push_back(output_traffic_light); - output_traffic_signal.map_primitive_id = ll_traffic_light.id(); - output.signals.push_back(output_traffic_signal); + bool is_red_crosswalk = false; + if (has_straight_lane) { + is_red_crosswalk = has_straight_green_lane; + } else { + is_red_crosswalk = !has_merge_lane && has_left_green_lane && has_right_green_lane; + } + + const auto tl_reg_elems_for_pedestrian = + crosswalk.regulatoryElementsAs(); + for (const auto & tl_reg_elem_for_pedestrian : tl_reg_elems_for_pedestrian) { + const auto traffic_lights_for_pedestrian = tl_reg_elem_for_pedestrian->trafficLights(); + + for (const auto & traffic_light : traffic_lights_for_pedestrian) { + const auto ll_traffic_light = static_cast(traffic_light); + + TrafficSignal output_traffic_signal; + TrafficLight output_traffic_light; + output_traffic_light.color = is_red_crosswalk ? TrafficLight::RED : TrafficLight::UNKNOWN; + output_traffic_light.confidence = 1.0; + output_traffic_signal.lights.push_back(output_traffic_light); + output_traffic_signal.map_primitive_id = ll_traffic_light.id(); + output.signals.push_back(output_traffic_signal); + } } }