feat(traffic-light-rviz-panel): select traffic light ID from Combobox (…

…tier4#1010)

* feat(traffic-light-rviz-panel): (1) select traffic light ID instead of inputting its value (2) added a workaround for tinyxml2::tinyxml2 for humble build (3) updated README and fixed mkdocs.yaml to upload .gif file to unvierse documentation (4)sort traffic light IDs, use scrollable box

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

common/tier4_traffic_light_rviz_plugin/CMakeLists.txt

@@ -2,6 +2,23 @@ cmake_minimum_required(VERSION 3.14)
 project(tier4_traffic_light_rviz_plugin)
 
 find_package(autoware_cmake REQUIRED)
+# TODO(Mamoru Sobue): workaround to avoid 'missing tinyxml2::tinyxml2'
+# From tinyxml2_vendor/cmake/Modules/FindTinyXML2.cmake
+find_package(TinyXML2 CONFIG QUIET)
+if(NOT TinyXML2_FOUND)
+  find_path(TINYXML2_INCLUDE_DIR NAMES tinyxml2.h)
+  find_library(TINYXML2_LIBRARY tinyxml2)
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TINYXML2_LIBRARY TINYXML2_INCLUDE_DIR)
+  mark_as_advanced(TINYXML2_INCLUDE_DIR TINYXML2_LIBRARY)
+  if(NOT TARGET tinyxml2::tinyxml2)
+    add_library(tinyxml2::tinyxml2 UNKNOWN IMPORTED)
+    set_property(TARGET tinyxml2::tinyxml2 PROPERTY IMPORTED_LOCATION ${TINYXML2_LIBRARY})
+    set_property(TARGET tinyxml2::tinyxml2 PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${TINYXML2_INCLUDE_DIR})
+    list(APPEND TinyXML2_TARGETS tinyxml2::tinyxml2)
+  endif()
+endif()
+
 autoware_package()
 
 find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
@@ -18,6 +35,7 @@ target_link_libraries(${PROJECT_NAME}
   ${QT_LIBRARIES}
 )
 
+target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic)
 # Export the plugin to be imported by rviz2
 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
 

common/tier4_traffic_light_rviz_plugin/README.md

@@ -20,6 +20,9 @@ This plugin panel publishes dummy traffic light signals.
 <div align="center">
   <img src="images/select_traffic_light_publish_panel.png" width=50%>
 </div>
+<div align="center">
+  <img src="images/select_traffic_light_id.png" width=50%>
+</div>
 
 1. Start rviz and select panels/Add new panel.
 2. Select TrafficLightPublishPanel and press OK.

common/tier4_traffic_light_rviz_plugin/package.xml

@@ -11,7 +11,9 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>lanelet2_extension</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp

@@ -22,10 +22,17 @@
 #include <QString>
 #include <QStringList>
 #include <QVBoxLayout>
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
 #include <rviz_common/display_context.hpp>
 
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 namespace rviz_plugins
 {
@@ -39,9 +46,8 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
   publishing_rate_input_->setSuffix("Hz");
 
   // Traffic Light ID
-  traffic_light_id_input_ = new QSpinBox();
-  traffic_light_id_input_->setRange(0, 999999);
-  traffic_light_id_input_->setValue(0);
+  traffic_light_id_input_ = new QComboBox();  // init items in first onVectorMap
+  traffic_light_id_input_->view()->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded);
 
   // Traffic Light Confidence
   traffic_light_confidence_input_ = new QDoubleSpinBox();
@@ -122,11 +128,13 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
   h_layout_5->addWidget(traffic_table_);
 
   setLayout(h_layout_5);
+  received_vector_map_ = false;
 }
 
 void TrafficLightPublishPanel::onSetTrafficLightState()
 {
-  const auto traffic_light_id = traffic_light_id_input_->value();
+  const auto traffic_light_id_str = traffic_light_id_input_->currentText();
+  const auto traffic_light_id = std::stoi(traffic_light_id_str.toStdString());
   const auto color = light_color_combo_->currentText();
   const auto shape = light_shape_combo_->currentText();
   const auto status = light_status_combo_->currentText();
@@ -207,14 +215,19 @@ void TrafficLightPublishPanel::onPublishTrafficLightState()
 
 void TrafficLightPublishPanel::onInitialize()
 {
+  using std::placeholders::_1;
   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));
 
+  sub_vector_map_ = raw_node_->create_subscription<HADMapBin>(
+    "/map/vector_map", rclcpp::QoS{1}.transient_local(),
+    std::bind(&TrafficLightPublishPanel::onVectorMap, this, _1));
   createWallTimer();
 
   enable_publish_ = false;
+  received_vector_map_ = false;
 }
 
 void TrafficLightPublishPanel::onRateChanged(int new_rate)
@@ -350,6 +363,31 @@ void TrafficLightPublishPanel::onTimer()
   }
 }
 
+void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg)
+{
+  if (received_vector_map_) return;
+  // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp
+  lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
+  lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map);
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map);
+  std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems =
+    lanelet::utils::query::trafficLights(all_lanelets);
+  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);
+    }
+  }
+  RCLCPP_INFO_STREAM(raw_node_->get_logger(), info);
+  received_vector_map_ = true;
+
+  for (auto && traffic_light_id : traffic_light_ids_) {
+    traffic_light_id_input_->addItem(QString::fromStdString(std::to_string(traffic_light_id)));
+  }
+}
 }  // namespace rviz_plugins
 
 #include <pluginlib/class_list_macros.hpp>

common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp

@@ -17,6 +17,7 @@
 #ifndef TRAFFIC_LIGHT_PUBLISH_PANEL_HPP_
 #define TRAFFIC_LIGHT_PUBLISH_PANEL_HPP_
 
+#ifndef Q_MOC_RUN
 #include <qt5/QtWidgets/QComboBox>
 #include <qt5/QtWidgets/QPushButton>
 #include <qt5/QtWidgets/QSpinBox>
@@ -25,13 +26,16 @@
 #include <rclcpp/timer.hpp>
 #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>
+#endif
 
-#include <chrono>
+#include <set>
 
 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;
@@ -53,13 +57,15 @@ public Q_SLOTS:
 protected:
   void onTimer();
   void createWallTimer();
+  void onVectorMap(const HADMapBin::ConstSharedPtr msg);
 
   rclcpp::Node::SharedPtr raw_node_;
   rclcpp::TimerBase::SharedPtr pub_timer_;
   rclcpp::Publisher<TrafficSignalArray>::SharedPtr pub_traffic_signals_;
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_vector_map_;
 
   QSpinBox * publishing_rate_input_;
-  QSpinBox * traffic_light_id_input_;
+  QComboBox * traffic_light_id_input_;
   QDoubleSpinBox * traffic_light_confidence_input_;
   QComboBox * light_color_combo_;
   QComboBox * light_shape_combo_;
@@ -72,6 +78,8 @@ public Q_SLOTS:
   TrafficSignalArray extra_traffic_signals_;
 
   bool enable_publish_;
+  std::set<int> traffic_light_ids_;
+  bool received_vector_map_;
 };
 
 }  // namespace rviz_plugins

mkdocs.yaml

@@ -50,7 +50,7 @@ plugins:
   - awesome-pages
   - exclude:
       regex:
-        - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?jpg).*$
+        - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$
         - ^(.*/)?[^.]*$
   - macros
   - same-dir