Skip to content

Commit

Permalink
feat(behavior_velocity): support the new traffic signal interface (#675)
Browse files Browse the repository at this point in the history 
* feat(behavior_velocity): support new traffic signal interface

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

style(pre-commit): autofix

add missing dependency

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

style(pre-commit): autofix

remove the external signal input source in behavior_planning_launch.py

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

replace TrafficLightElement with TrafficSignalElement

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

style(pre-commit): autofix

use the regulatory element id instead of traffic light id

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

change the input of traffic signal to traffic light arbiter

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

do not return until the all regulatory elements are checked

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* docs(traffic_light_arbiter): fix the datatype of input topics (autowarefoundation#4156)

docs(traffic_light_arbiter): fix the type of input topics

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* feat: rename traffic signal element (autowarefoundation#4129)

* feat(traffic_light_arbiter): rename traffic signal element

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(traffic_light_selector): remove traffic light selector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix(traffic_light_arbiter): fix node name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* update topic name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* update message type

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* update message type

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* feat(traffic_light_selector): remove traffic light selector (autowarefoundation#4130)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

---------

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>
  • Loading branch information
@TomohitoAndo @isamu-takagi @knzo25
3 people authored Jul 25, 2023
1 parent 24ec919 commit 014a92c
Show file tree
Hide file tree
Showing 26 changed files with 178 additions and 297 deletions.
6 changes: 3 additions & 3 deletions common/tier4_traffic_light_rviz_plugin/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals.

### Output

| Name | Type | Description |
| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- |
| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
| Name | Type | Description |
| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- |
| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |

## HowToUse

Expand Down
2 changes: 1 addition & 1 deletion common/tier4_traffic_light_rviz_plugin/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>lanelet2_extension</depend>
<depend>libqt5-core</depend>
<depend>libqt5-gui</depend>
Expand Down
98 changes: 47 additions & 51 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 @@ -34,6 +34,7 @@
#include <utility>
#include <vector>

#undef signals
namespace rviz_plugins
{
TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent)
Expand Down Expand Up @@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState()
const auto shape = light_shape_combo_->currentText();
const auto status = light_status_combo_->currentText();

TrafficLight traffic_light;
TrafficSignalElement traffic_light;
traffic_light.confidence = traffic_light_confidence_input_->value();

if (color == "RED") {
traffic_light.color = TrafficLight::RED;
traffic_light.color = TrafficSignalElement::RED;
} else if (color == "AMBER") {
traffic_light.color = TrafficLight::AMBER;
traffic_light.color = TrafficSignalElement::AMBER;
} else if (color == "GREEN") {
traffic_light.color = TrafficLight::GREEN;
traffic_light.color = TrafficSignalElement::GREEN;
} else if (color == "WHITE") {
traffic_light.color = TrafficLight::WHITE;
traffic_light.color = TrafficSignalElement::WHITE;
} else if (color == "UNKNOWN") {
traffic_light.color = TrafficLight::UNKNOWN;
traffic_light.color = TrafficSignalElement::UNKNOWN;
}

if (shape == "CIRCLE") {
traffic_light.shape = TrafficLight::CIRCLE;
traffic_light.shape = TrafficSignalElement::CIRCLE;
} else if (shape == "LEFT_ARROW") {
traffic_light.shape = TrafficLight::LEFT_ARROW;
traffic_light.shape = TrafficSignalElement::LEFT_ARROW;
} else if (shape == "RIGHT_ARROW") {
traffic_light.shape = TrafficLight::RIGHT_ARROW;
traffic_light.shape = TrafficSignalElement::RIGHT_ARROW;
} else if (shape == "UP_ARROW") {
traffic_light.shape = TrafficLight::UP_ARROW;
traffic_light.shape = TrafficSignalElement::UP_ARROW;
} else if (shape == "DOWN_ARROW") {
traffic_light.shape = TrafficLight::DOWN_ARROW;
traffic_light.shape = TrafficSignalElement::DOWN_ARROW;
} else if (shape == "DOWN_LEFT_ARROW") {
traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW;
} else if (shape == "DOWN_RIGHT_ARROW") {
traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW;
} else if (shape == "UNKNOWN") {
traffic_light.shape = TrafficLight::UNKNOWN;
traffic_light.shape = TrafficSignalElement::UNKNOWN;
}

if (status == "SOLID_OFF") {
traffic_light.status = TrafficLight::SOLID_OFF;
traffic_light.status = TrafficSignalElement::SOLID_OFF;
} else if (status == "SOLID_ON") {
traffic_light.status = TrafficLight::SOLID_ON;
traffic_light.status = TrafficSignalElement::SOLID_ON;
} else if (status == "FLASHING") {
traffic_light.status = TrafficLight::FLASHING;
traffic_light.status = TrafficSignalElement::FLASHING;
} else if (status == "UNKNOWN") {
traffic_light.status = TrafficLight::UNKNOWN;
traffic_light.status = TrafficSignalElement::UNKNOWN;
}

TrafficSignal traffic_signal;
traffic_signal.lights.push_back(traffic_light);
traffic_signal.map_primitive_id = traffic_light_id;
traffic_signal.elements.push_back(traffic_light);
traffic_signal.traffic_signal_id = traffic_light_id;

for (auto & signal : extra_traffic_signals_.signals) {
if (signal.map_primitive_id == traffic_light_id) {
if (signal.traffic_signal_id == traffic_light_id) {
signal = traffic_signal;
return;
}
Expand Down Expand Up @@ -218,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 @@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer()
void TrafficLightPublishPanel::onTimer()
{
if (enable_publish_) {
extra_traffic_signals_.header.stamp = rclcpp::Clock().now();
extra_traffic_signals_.stamp = rclcpp::Clock().now();
pub_traffic_signals_->publish(extra_traffic_signals_);
}

Expand All @@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer()
for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) {
const auto & signal = extra_traffic_signals_.signals.at(i);

if (signal.lights.empty()) {
if (signal.elements.empty()) {
continue;
}

auto id_label = new QLabel(QString::number(signal.map_primitive_id));
auto id_label = new QLabel(QString::number(signal.traffic_signal_id));
id_label->setAlignment(Qt::AlignCenter);

auto color_label = new QLabel();
color_label->setAlignment(Qt::AlignCenter);

const auto & light = signal.lights.front();
const auto & light = signal.elements.front();
switch (light.color) {
case TrafficLight::RED:
case TrafficSignalElement::RED:
color_label->setText("RED");
color_label->setStyleSheet("background-color: #FF0000;");
break;
case TrafficLight::AMBER:
case TrafficSignalElement::AMBER:
color_label->setText("AMBER");
color_label->setStyleSheet("background-color: #FFBF00;");
break;
case TrafficLight::GREEN:
case TrafficSignalElement::GREEN:
color_label->setText("GREEN");
color_label->setStyleSheet("background-color: #7CFC00;");
break;
case TrafficLight::WHITE:
case TrafficSignalElement::WHITE:
color_label->setText("WHITE");
color_label->setStyleSheet("background-color: #FFFFFF;");
break;
case TrafficLight::UNKNOWN:
case TrafficSignalElement::UNKNOWN:
color_label->setText("UNKNOWN");
color_label->setStyleSheet("background-color: #808080;");
break;
Expand All @@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer()
shape_label->setAlignment(Qt::AlignCenter);

switch (light.shape) {
case TrafficLight::CIRCLE:
case TrafficSignalElement::CIRCLE:
shape_label->setText("CIRCLE");
break;
case TrafficLight::LEFT_ARROW:
case TrafficSignalElement::LEFT_ARROW:
shape_label->setText("LEFT_ARROW");
break;
case TrafficLight::RIGHT_ARROW:
case TrafficSignalElement::RIGHT_ARROW:
shape_label->setText("RIGHT_ARROW");
break;
case TrafficLight::UP_ARROW:
case TrafficSignalElement::UP_ARROW:
shape_label->setText("UP_ARROW");
break;
case TrafficLight::DOWN_ARROW:
case TrafficSignalElement::DOWN_ARROW:
shape_label->setText("DOWN_ARROW");
break;
case TrafficLight::DOWN_LEFT_ARROW:
case TrafficSignalElement::DOWN_LEFT_ARROW:
shape_label->setText("DOWN_LEFT_ARROW");
break;
case TrafficLight::DOWN_RIGHT_ARROW:
case TrafficSignalElement::DOWN_RIGHT_ARROW:
shape_label->setText("DOWN_RIGHT_ARROW");
break;
case TrafficLight::FLASHING:
shape_label->setText("FLASHING");
break;
case TrafficLight::UNKNOWN:
case TrafficSignalElement::UNKNOWN:
shape_label->setText("UNKNOWN");
break;
default:
Expand All @@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer()
status_label->setAlignment(Qt::AlignCenter);

switch (light.status) {
case TrafficLight::SOLID_OFF:
case TrafficSignalElement::SOLID_OFF:
status_label->setText("SOLID_OFF");
break;
case TrafficLight::SOLID_ON:
case TrafficSignalElement::SOLID_ON:
status_label->setText("SOLID_ON");
break;
case TrafficLight::FLASHING:
case TrafficSignalElement::FLASHING:
status_label->setText("FLASHING");
break;
case TrafficLight::UNKNOWN:
case TrafficSignalElement::UNKNOWN:
status_label->setText("UNKNOWN");
break;
default:
Expand Down Expand Up @@ -375,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
9 changes: 4 additions & 5 deletions common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
#include <rviz_common/panel.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
#endif

#include <set>
Expand All @@ -36,10 +36,9 @@ namespace rviz_plugins
{

using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_perception_msgs::msg::TrafficLight;
using autoware_auto_perception_msgs::msg::TrafficSignal;
using autoware_auto_perception_msgs::msg::TrafficSignalArray;

using autoware_perception_msgs::msg::TrafficSignal;
using autoware_perception_msgs::msg::TrafficSignalArray;
using autoware_perception_msgs::msg::TrafficSignalElement;
class TrafficLightPublishPanel : public rviz_common::Panel
{
Q_OBJECT
Expand Down
6 changes: 1 addition & 5 deletions ...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,11 +173,7 @@ def launch_setup(context, *args, **kwargs):
),
(
"~/input/traffic_signals",
"/perception/traffic_light_recognition/traffic_signals",
),
(
"~/input/external_traffic_signals",
"/external/traffic_light_recognition/traffic_signals",
"/perception/traffic_light_arbiter/traffic_signals",
),
(
"~/input/external_velocity_limit_mps",
Expand Down
10 changes: 5 additions & 5 deletions perception/traffic_light_arbiter/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@ A node that merges traffic light/signal state from image recognition and externa

#### Input

| Name | Type | Description |
| -------------------------------- | -------------------------------------------- | -------------------------------------------------------- |
| ~/sub/vector_map | autoware_auto_mapping_msgs::msg::HADMapBin | The vector map to get valid traffic signal ids. |
| ~/sub/perception_traffic_signals | autoware_perception_msgs::msg::TrafficSignal | The traffic signals from the image recognition pipeline. |
| ~/sub/external_traffic_signals | autoware_perception_msgs::msg::TrafficSignal | The traffic signals from an external system. |
| Name | Type | Description |
| -------------------------------- | ------------------------------------------------- | -------------------------------------------------------- |
| ~/sub/vector_map | autoware_auto_mapping_msgs::msg::HADMapBin | The vector map to get valid traffic signal ids. |
| ~/sub/perception_traffic_signals | autoware_perception_msgs::msg::TrafficSignalArray | The traffic signals from the image recognition pipeline. |
| ~/sub/external_traffic_signals | autoware_perception_msgs::msg::TrafficSignalArray | The traffic signals from an external system. |

#### Output

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_crosswalk_module/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_tf2</depend>
<depend>autoware_perception_msgs</depend>
<depend>behavior_velocity_planner_common</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
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) {
continue;
}

if (
planner_param_.tl_state_timeout <
(clock_->now() - traffic_signal_stamped->header.stamp).seconds()) {
continue;
}
if (
planner_param_.tl_state_timeout < (clock_->now() - traffic_signal_stamped->stamp).seconds()) {
continue;
}

const auto & lights = traffic_signal_stamped->signal.lights;
if (lights.empty()) {
continue;
}
const auto & lights = traffic_signal_stamped->signal.elements;
if (lights.empty()) {
continue;
}

if (lights.front().color == TrafficLight::RED) {
return true;
}
if (lights.front().color == TrafficSignalElement::RED) {
return true;
}
}

Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ namespace behavior_velocity_planner
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_perception_msgs::msg::TrafficLight;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_perception_msgs::msg::TrafficSignalElement;
using lanelet::autoware::Crosswalk;
using tier4_api_msgs::msg::CrosswalkStatus;
using tier4_autoware_utils::StopWatch;
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_intersection_module/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>behavior_velocity_planner_common</depend>
<depend>cv_bridge</depend>
<depend>geometry_msgs</depend>
Expand Down
Loading

0 comments on commit 014a92c

Please sign in to comment.