From 1767ba5ef2356cd21c519cb9cd3d31f12855ce6f Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 10 Jul 2023 17:56:52 +0900 Subject: [PATCH] use the regulatory element id instead of traffic light id Signed-off-by: Tomohito Ando --- .../src/traffic_light_publish_panel.cpp | 10 +- .../src/scene_crosswalk.cpp | 34 +++---- .../src/scene.cpp | 98 +++++-------------- .../src/scene.hpp | 8 +- 4 files changed, 44 insertions(+), 106 deletions(-) diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index 796152bfb1958..71b9cb967aa2d 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -219,7 +219,7 @@ void TrafficLightPublishPanel::onInitialize() raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); pub_traffic_signals_ = raw_node_->create_publisher( - "/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); + "/perception/traffic_light_arbiter/traffic_signals", rclcpp::QoS(1)); sub_vector_map_ = raw_node_->create_subscription( "/map/vector_map", rclcpp::QoS{1}.transient_local(), @@ -373,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) std::string info = "Fetching traffic lights :"; std::string delim = " "; for (auto && tl_reg_elem_ptr : tl_reg_elems) { - for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) { - auto id = static_cast(traffic_light.id()); - info += (std::exchange(delim, ", ") + std::to_string(id)); - traffic_light_ids_.insert(id); - } + auto id = static_cast(tl_reg_elem_ptr->id()); + info += (std::exchange(delim, ", ") + std::to_string(id)); + traffic_light_ids_.insert(id); } RCLCPP_INFO_STREAM(raw_node_->get_logger(), info); received_vector_map_ = true; diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 56e4f8b008f85..5ed698ec25ea0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -868,28 +868,24 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights(); - for (const auto & traffic_light : traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id()); - if (!traffic_signal_stamped) { - continue; - } + const auto traffic_signal_stamped = + planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); + if (!traffic_signal_stamped) { + return false; + } - if ( - planner_param_.tl_state_timeout < - (clock_->now() - traffic_signal_stamped->stamp).seconds()) { - continue; - } + if ( + planner_param_.tl_state_timeout < (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + return false; + } - const auto & lights = traffic_signal_stamped->signal.elements; - if (lights.empty()) { - continue; - } + const auto & lights = traffic_signal_stamped->signal.elements; + if (lights.empty()) { + return false; + } - if (lights.front().color == TrafficSignalElement::RED) { - return true; - } + if (lights.front().color == TrafficSignalElement::RED) { + return true; } } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 8b0885239a991..b4fa9b63e165d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -159,22 +159,6 @@ bool calcStopPointAndInsertIndex( } return false; } - -geometry_msgs::msg::Point getTrafficLightPosition( - const lanelet::ConstLineStringOrPolygon3d & traffic_light) -{ - if (!traffic_light.lineString()) { - throw std::invalid_argument{"Traffic light is not LineString"}; - } - geometry_msgs::msg::Point tl_center; - auto traffic_light_ls = traffic_light.lineString().value(); - for (const auto & tl_point : traffic_light_ls) { - tl_center.x += tl_point.x() / traffic_light_ls.size(); - tl_center.y += tl_point.y() / traffic_light_ls.size(); - tl_center.z += tl_point.z() / traffic_light_ls.size(); - } - return tl_center; -} } // namespace TrafficLightModule::TrafficLightModule( @@ -205,9 +189,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto & self_pose = planner_data_->current_odometry; - // Get lanelet2 traffic lights and stop lines. + // Get lanelet2 stop lines. lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_light_reg_elem_.trafficLights(); // Calculate stop pose and insert index Eigen::Vector2d stop_line_point{}; @@ -243,7 +226,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal(traffic_lights)); + setSafe(!isStopSignal()); if (isActivated()) { is_prev_state_stop_ = false; return true; @@ -271,20 +254,19 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } -bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::isStopSignal() { - if (!updateTrafficSignal(traffic_lights)) { + if (!updateTrafficSignal()) { return false; } return isTrafficSignalStop(looking_tl_state_); } -bool TrafficLightModule::updateTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::updateTrafficSignal() { TrafficSignal signal; - bool found_signal = getHighestConfidenceTrafficSignal(traffic_lights, signal); + bool found_signal = findValidTrafficSignal(signal); if (!found_signal) { // Don't stop when UNKNOWN or TIMEOUT as discussed at #508 @@ -371,63 +353,27 @@ bool TrafficLightModule::isTrafficSignalStop( return true; } -bool TrafficLightModule::getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_perception_msgs::msg::TrafficSignal & highest_confidence_tl_state) +bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_signal) { - // search traffic light state - bool found = false; - double highest_confidence = 0.0; - std::string reason; - for (const auto & traffic_light : traffic_lights) { - // traffic light must be linestrings - if (!traffic_light.isLineString()) { - reason = "NotLineString"; - continue; - } - - const int id = static_cast(traffic_light).id(); - RCLCPP_DEBUG(logger_, "traffic light id: %d (on route)", id); - const auto tl_state_stamped = planner_data_->getTrafficSignal(id); - if (!tl_state_stamped) { - reason = "TrafficSignalNotFound"; - continue; - } - - const auto stamp = tl_state_stamped->stamp; - const auto tl_state = tl_state_stamped->signal; - if (!((clock_->now() - stamp).seconds() < planner_param_.tl_state_timeout)) { - reason = "TimeOut"; - continue; - } - - if ( - tl_state.elements.empty() || - tl_state.elements.front().color == TrafficSignalElement::UNKNOWN) { - reason = "TrafficLightUnknown"; - continue; - } - - if (highest_confidence < tl_state.elements.front().confidence) { - highest_confidence = tl_state.elements.front().confidence; - highest_confidence_tl_state = tl_state_stamped->signal; - try { - auto tl_position = getTrafficLightPosition(traffic_light); - debug_data_.traffic_light_points.push_back(tl_position); - debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position); - } catch (const std::invalid_argument & ex) { - RCLCPP_WARN_STREAM(logger_, ex.what()); - continue; - } - found = true; - } + // get traffic signal associated with the regulatory element id + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + if (!traffic_signal_stamped) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "the traffic signal data associated with regulatory element id is not received"); + return false; } - if (!found) { + + // check if the traffic signal data is outdated + const auto is_traffic_signal_timeout = + (clock_->now() - traffic_signal_stamped->stamp).seconds() > planner_param_.tl_state_timeout; + if (is_traffic_signal_timeout) { RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s).", - reason.c_str()); + logger_, *clock_, 5000 /* ms */, "the received traffic signal data is outdated"); return false; } + + valid_traffic_signal = traffic_signal_stamped->signal; return true; } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 5c3309bd00db2..a24db2c440e91 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -85,7 +85,7 @@ class TrafficLightModule : public SceneModuleInterface } private: - bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + bool isStopSignal(); bool isTrafficSignalStop(const TrafficSignal & tl_state) const; @@ -100,11 +100,9 @@ class TrafficLightModule : public SceneModuleInterface bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - TrafficSignal & highest_confidence_tl_state); + bool findValidTrafficSignal(TrafficSignal & valid_traffic_signal); - bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + bool updateTrafficSignal(); // Lane id const int64_t lane_id_;