Skip to content

Commit

Permalink
use the regulatory element id instead of traffic light id
Browse files Browse the repository at this point in the history 
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
  • Loading branch information
@TomohitoAndo
TomohitoAndo committed Jul 10, 2023
1 parent a10dc06 commit 1767ba5
Show file tree
Hide file tree
Showing 4 changed files with 44 additions and 106 deletions.
10 changes: 4 additions & 6 deletions common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,7 +219,7 @@ void TrafficLightPublishPanel::onInitialize()
raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();

pub_traffic_signals_ = raw_node_->create_publisher<TrafficSignalArray>(
"/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<HADMapBin>(
"/map/vector_map", rclcpp::QoS{1}.transient_local(),
Expand Down Expand Up @@ -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<int>(traffic_light.id());
info += (std::exchange(delim, ", ") + std::to_string(id));
traffic_light_ids_.insert(id);
}
auto id = static_cast<int>(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;
Expand Down
34 changes: 15 additions & 19 deletions planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -868,28 +868,24 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();

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<lanelet::ConstLineString3d>(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;
}
}

Expand Down
98 changes: 22 additions & 76 deletions planning/behavior_velocity_traffic_light_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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{};
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<lanelet::ConstLineString3d>(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;
}

Expand Down
8 changes: 3 additions & 5 deletions planning/behavior_velocity_traffic_light_module/src/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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_;
Expand Down

0 comments on commit 1767ba5

Please sign in to comment.