Make viewer capable to load rules files and tree visualization of phases

delphyne-gui/visualizer/layer_selection_widget.cc

@@ -1,10 +1,13 @@
 // Copyright 2018 Toyota Research Institute
 
 #include "layer_selection_widget.hh"
+#include <ignition/common/Filesystem.hh>
 
 #include <QtCore/QDir>
+#include <QtCore/QFileInfo>
 #include <QtWidgets/QFileDialog>
 #include <QtWidgets/QGroupBox>
+#include <QtWidgets/QHBoxLayout>
 #include <QtWidgets/QVBoxLayout>
 
 using namespace delphyne;
@@ -163,6 +166,9 @@ MaliputFileSelectionWidget::MaliputFileSelectionWidget(QWidget* parent) : QWidge
   this->Build();
   // Connects all the check box events.
   QObject::connect(this->loadButton, SIGNAL(released()), this, SLOT(onLoadButtonPressed()));
+  QObject::connect(this->roadRulebookButton, SIGNAL(released()), this, SLOT(onRoadRulebookButtonPressed()));
+  QObject::connect(this->trafficLightRulesButton, SIGNAL(released()), this, SLOT(onTrafficLightrulesButtonPressed()));
+  QObject::connect(this->phaseRingButton, SIGNAL(released()), this, SLOT(onPhaseRingButtonPressed()));
 }
 
 ///////////////////////////////////////////////////////
@@ -173,24 +179,91 @@ void MaliputFileSelectionWidget::SetFileNameLabel(const std::string& fileName) {
   this->fileNameLabel->setText(QString::fromStdString(fileName));
 }
 
+void MaliputFileSelectionWidget::ClearLineEdits(bool keepOldTextsAsPlaceholders) {
+  if (keepOldTextsAsPlaceholders) {
+    QFileInfo ruleFile(this->roadRulebookLineEdit->text());
+    this->roadRulebookLineEdit->setPlaceholderText(ruleFile.baseName());
+
+    ruleFile.setFile(this->trafficLightRulesLineEdit->text());
+    this->trafficLightRulesLineEdit->setPlaceholderText(ruleFile.baseName());
+
+    ruleFile.setFile(this->phaseRingLineEdit->text());
+    this->phaseRingLineEdit->setPlaceholderText(ruleFile.baseName());
+  } else {
+    this->roadRulebookLineEdit->setPlaceholderText(QString());
+    this->trafficLightRulesLineEdit->setPlaceholderText(QString());
+    this->phaseRingLineEdit->setPlaceholderText(QString());
+  }
+  this->roadRulebookLineEdit->clear();
+  this->trafficLightRulesLineEdit->clear();
+  this->phaseRingLineEdit->clear();
+}
+
 ///////////////////////////////////////////////////////
 void MaliputFileSelectionWidget::onLoadButtonPressed() {
   QString fileName = QFileDialog::getOpenFileName(this, tr("Open Maliput XODR or YAML"),
                                                   QString::fromStdString(this->fileDialogOpenPath),
-                                                  tr("XODR Files (*.XODR);;YAML files (*.YAML)"));
+                                                  tr("XODR Files (*.xodr);;YAML files (*.yaml)"));
   if (!fileName.isEmpty()) {
     this->fileDialogOpenPath = fileName.toStdString();
   }
-  emit maliputFileChanged(fileName.toStdString());
+  emit maliputFileChanged(fileName.toStdString(), this->roadRulebookLineEdit->text().toStdString(),
+                          this->trafficLightRulesLineEdit->text().toStdString(),
+                          this->phaseRingLineEdit->text().toStdString());
+}
+
+void MaliputFileSelectionWidget::onRoadRulebookButtonPressed() {
+  QString fileName = QFileDialog::getOpenFileName(
+      this, tr("Open Road Rulebook YAML"), QString::fromStdString(this->fileDialogOpenPath), tr("YAML files (*.yaml)"));
+  this->roadRulebookLineEdit->setText(fileName);
+}
+
+void MaliputFileSelectionWidget::onTrafficLightrulesButtonPressed() {
+  QString fileName =
+      QFileDialog::getOpenFileName(this, tr("Open Traffic Lights Rulebook YAML"),
+                                   QString::fromStdString(this->fileDialogOpenPath), tr("YAML files (*.yaml)"));
+  this->trafficLightRulesLineEdit->setText(fileName);
+}
+
+void MaliputFileSelectionWidget::onPhaseRingButtonPressed() {
+  QString fileName =
+      QFileDialog::getOpenFileName(this, tr("Open Traffic Lights Rulebook YAML"),
+                                   QString::fromStdString(this->fileDialogOpenPath), tr("YAML files (*.yaml)"));
+  this->phaseRingLineEdit->setText(fileName);
 }
 
 ///////////////////////////////////////////////////////
 void MaliputFileSelectionWidget::Build() {
   this->loadButton = new QPushButton("Load", this);
   this->fileNameLabel = new QLabel("", this);
+  this->roadRulebookLineEdit = new QLineEdit(this);
+  this->trafficLightRulesLineEdit = new QLineEdit(this);
+  this->phaseRingLineEdit = new QLineEdit(this);
+  this->roadRulebookButton = new QPushButton("Select road rulebook", this);
+  this->trafficLightRulesButton = new QPushButton("Select traffic lights book", this);
+  this->phaseRingButton = new QPushButton("Select phase ring book", this);
+
+  QWidget* roadRulebookWidgetContainer = new QWidget(this);
+  QHBoxLayout* roadRulebookLayout = new QHBoxLayout(roadRulebookWidgetContainer);
+  roadRulebookLayout->addWidget(roadRulebookLineEdit);
+  roadRulebookLayout->addWidget(roadRulebookButton);
+
+  QWidget* trafficLightRulebookWidgetContainer = new QWidget(this);
+  QHBoxLayout* trafficLightRulesLayout = new QHBoxLayout(trafficLightRulebookWidgetContainer);
+  trafficLightRulesLayout->addWidget(trafficLightRulesLineEdit);
+  trafficLightRulesLayout->addWidget(trafficLightRulesButton);
+
+  QWidget* phaseRingWidgetContainer = new QWidget(this);
+  QHBoxLayout* phaseRingLayout = new QHBoxLayout(phaseRingWidgetContainer);
+  phaseRingLayout->addWidget(phaseRingLineEdit);
+  phaseRingLayout->addWidget(phaseRingButton);
 
   auto* layout = new QVBoxLayout(this);
   layout->addWidget(fileNameLabel);
   layout->addWidget(loadButton);
+  layout->addWidget(roadRulebookWidgetContainer);
+  layout->addWidget(trafficLightRulebookWidgetContainer);
+  layout->addWidget(phaseRingWidgetContainer);
+
   this->setLayout(layout);
 }

delphyne-gui/visualizer/layer_selection_widget.hh

@@ -7,6 +7,7 @@
 
 #include <QtWidgets/QCheckBox>
 #include <QtWidgets/QLabel>
+#include <QtWidgets/QLineEdit>
 #include <QtWidgets/QPushButton>
 #include <QtWidgets/QWidget>
 
@@ -91,11 +92,22 @@ class MaliputFileSelectionWidget : public QWidget {
   /// Sets @p fileName into the label to display the loaded file.
   void SetFileNameLabel(const std::string& fileName);
 
+  /// \brief Clear rules line edits.
+  /// \param[in] keepOldTextsAsPlaceholders Use the last file opened as a placeholder to remember the user which file
+  /// he used.
+  void ClearLineEdits(bool keepOldTextsAsPlaceholders = true);
+
  public slots:
   void onLoadButtonPressed();
 
  signals:
-  void maliputFileChanged(const std::string& filePath);
+  void maliputFileChanged(const std::string& filePath, const std::string& roadRulebookFilePath,
+                          const std::string& trafficLightRulesFilePath, const std::string& phaseRingFilePath);
+
+ private slots:
+  void onRoadRulebookButtonPressed();
+  void onTrafficLightrulesButtonPressed();
+  void onPhaseRingButtonPressed();
 
  private:
   /// \brief Builds the GUI with a button to load a file dialog and a label to
@@ -104,6 +116,12 @@ class MaliputFileSelectionWidget : public QWidget {
 
   QPushButton* loadButton{nullptr};
   QLabel* fileNameLabel{nullptr};
+  QPushButton* roadRulebookButton{nullptr};
+  QPushButton* trafficLightRulesButton{nullptr};
+  QPushButton* phaseRingButton{nullptr};
+  QLineEdit* roadRulebookLineEdit{nullptr};
+  QLineEdit* trafficLightRulesLineEdit{nullptr};
+  QLineEdit* phaseRingLineEdit{nullptr};
   std::string fileDialogOpenPath{};
 };
 

delphyne-gui/visualizer/maliput_viewer_model.cc

@@ -9,6 +9,8 @@
 #include <delphyne/roads/road_builder.h>
 #include <ignition/common/Console.hh>
 #include <ignition/rendering/RayQuery.hh>
+#include <malidrive/road_geometry_configuration.h>
+#include <malidrive/road_network_configuration.h>
 #include <maliput-utilities/generate_obj.h>
 #include <maliput-utilities/mesh.h>
 #include <maliput/api/lane_data.h>
@@ -301,11 +303,12 @@ maliput::api::rules::RoadRulebook::QueryResults RoadNetworkQuery::FindRulesFor(c
 }
 
 /////////////////////////////////////////////////
-bool MaliputViewerModel::Load(const std::string& _maliputFilePath) {
+bool MaliputViewerModel::Load(const std::string& _maliputFilePath, const std::string& _roadRulebookFilePath,
+                              const std::string& _trafficLightBookFilePath, const std::string& _phaseRingFilePath) {
   this->Clear();
 
   ignmsg << "About to load [" << _maliputFilePath << "] maliput file." << std::endl;
-  LoadRoadGeometry(_maliputFilePath);
+  LoadRoadGeometry(_maliputFilePath, _roadRulebookFilePath, _trafficLightBookFilePath, _phaseRingFilePath);
   const maliput::api::RoadGeometry* rg = roadGeometry == nullptr ? roadNetwork->road_geometry() : roadGeometry.get();
   ignmsg << "Loaded [" << _maliputFilePath << "] maliput file." << std::endl;
   ignmsg << "Loading RoadGeometry meshes of " << rg->id().string() << std::endl;
@@ -338,7 +341,9 @@ const std::map<std::string, std::unique_ptr<MaliputMesh>>& MaliputViewerModel::M
 const std::map<MaliputLabelType, std::vector<MaliputLabel>>& MaliputViewerModel::Labels() const { return this->labels; }
 
 /////////////////////////////////////////////////
-void MaliputViewerModel::LoadRoadGeometry(const std::string& _maliputFilePath) {
+void MaliputViewerModel::LoadRoadGeometry(const std::string& _maliputFilePath, const std::string& _roadRulebookFilePath,
+                                          const std::string& _trafficLightBookFilePath,
+                                          const std::string& _phaseRingFilePath) {
   std::ifstream fileStream(_maliputFilePath);
   if (!fileStream.is_open()) {
     throw std::runtime_error(_maliputFilePath + " doesn't exist or can't be opened.");
@@ -348,7 +353,8 @@ void MaliputViewerModel::LoadRoadGeometry(const std::string& _maliputFilePath) {
     std::getline(fileStream, line);
     if (line.find("<OpenDRIVE>") != std::string::npos) {
       this->roadNetwork = delphyne::roads::CreateMalidriveFromFile(
-          _maliputFilePath.substr(_maliputFilePath.find_last_of("/") + 1), _maliputFilePath);
+          _maliputFilePath.substr(_maliputFilePath.find_last_of("/") + 1), _maliputFilePath, _roadRulebookFilePath,
+          _trafficLightBookFilePath, _phaseRingFilePath);
       return;
     } else if (line.find("maliput_multilane_builder:") != std::string::npos) {
       this->roadGeometry = maliput::multilane::LoadFile(maliput::multilane::BuilderFactory(), _maliputFilePath);

delphyne-gui/visualizer/maliput_viewer_model.hh

@@ -175,7 +175,9 @@ class MaliputViewerModel {
   /// Gets the file path from GlobalAttributes and loads the RoadGeometry into
   /// memory. Then, converts it into a map of meshes, loading each mesh material
   /// information. Meshes that are not available, are set to kDisabled.
-  bool Load(const std::string& _maliputFilePath);
+  bool Load(const std::string& _maliputFilePath, const std::string& _roadRulebookFilePath = std::string(),
+            const std::string& _trafficLightBook = std::string(),
+            const std::string& _phaseRingFilePath = std::string());
 
   /// \brief Getter of the map of meshes.
   /// \return The map of meshes.
@@ -230,7 +232,11 @@ class MaliputViewerModel {
   /// \return rules separated by brackets.
   /// Ex: [Right of way Rule]\n.
   template <typename StringType>
-  StringType GetRulesOfLane(const std::string& _laneId) const;
+  StringType GetRulesOfLane(const std::string& _phaseRingId, const std::string& _phaseId,
+                            const std::string& _laneId) const;
+
+  template <typename StringType>
+  std::unordered_map<std::string, std::vector<StringType>> GetPhaseRings() const;
 
  private:
   /// \brief Loads a maliput RoadGeometry of multilane from
@@ -240,7 +246,8 @@ class MaliputViewerModel {
   /// If the key doesn't exist in the file, it's not valid.
   /// Otherwise the correct loader will be called to parse the file.
   /// \param _maliputFilePath The YAML file path to parse.
-  void LoadRoadGeometry(const std::string& _maliputFilePath);
+  void LoadRoadGeometry(const std::string& _maliputFilePath, const std::string& _roadRulebookFilePath,
+                        const std::string& _trafficLightBookFilePath, const std::string& _phaseRingFilePath);
 
   /// \brief Converts @p _geoMeshes into a
   ///        std::map<std::string, std::unique_ptr<ignition::common::Mesh>>
@@ -281,6 +288,10 @@ class MaliputViewerModel {
   template <typename StringType>
   StringType GetDirectionUsageRules(const maliput::api::LaneId& _laneId) const;
 
+  template <typename StringType>
+  StringType GetPhaseRightOfWayRules(const maliput::api::rules::PhaseRing::Id& _phaseRingId,
+                                     const maliput::api::rules::Phase::Id& _phaseId) const;
+
   // To support both malidrive and multilane files, we have both. roadNetwork
   // has a pointer to a RoadGeometry.
 
@@ -327,7 +338,31 @@ ContainerType MaliputViewerModel::GetAllLaneIds() const {
 }
 
 template <typename StringType>
-StringType MaliputViewerModel::GetRulesOfLane(const std::string& _laneId) const {
+std::unordered_map<std::string, std::vector<StringType>> MaliputViewerModel::GetPhaseRings() const {
+  if (this->roadNetwork == nullptr) {
+    return std::unordered_map<std::string, std::vector<StringType>>();
+  }
+  std::unordered_map<std::string, std::vector<StringType>> phase_rings;
+  const maliput::api::rules::PhaseRingBook* phase_ring_book = this->roadNetwork->phase_ring_book();
+  std::vector<maliput::api::rules::PhaseRing::Id> phase_ring_ids = phase_ring_book->GetPhaseRings();
+  phase_rings.reserve(phase_ring_ids.size());
+  for (const auto& phase_ring_id : phase_ring_ids) {
+    drake::optional<maliput::api::rules::PhaseRing> phase_ring = phase_ring_book->GetPhaseRing(phase_ring_id);
+    DELPHYNE_DEMAND(phase_ring.has_value());
+    const std::unordered_map<maliput::api::rules::Phase::Id, maliput::api::rules::Phase>& phases =
+        phase_ring.value().phases();
+    std::vector<StringType> phase_ids;
+    phase_ids.reserve(phases.size());
+    std::transform(phases.begin(), phases.end(), std::back_inserter(phase_ids),
+                   [](const auto& key_value) { return StringType(key_value.first.string().c_str()); });
+    phase_rings[phase_ring_id.string()] = std::move(phase_ids);
+  }
+  return phase_rings;
+}
+
+template <typename StringType>
+StringType MaliputViewerModel::GetRulesOfLane(const std::string& _phaseRingId, const std::string& _phaseId,
+                                              const std::string& _laneId) const {
   if (this->roadNetwork == nullptr) {
     return StringType("There are no rules for this road");
   }
@@ -337,6 +372,14 @@ StringType MaliputViewerModel::GetRulesOfLane(const std::string& _laneId) const
   StringType rules = "[Right of way rules]\n" + GetRightOfWayRules<StringType>(laneSRange) + "\n" +
                      "[Max speed limit rules]\n" + GetMaxSpeedLimitRules<StringType>(id) + "\n" +
                      "[Direction usage rules]\n" + GetDirectionUsageRules<StringType>(id) + "\n";
+
+  if (!_phaseRingId.empty() && !_phaseId.empty()) {
+    rules += "[Right of way rules by phase ring id and phase id]\n" +
+             GetPhaseRightOfWayRules<StringType>(maliput::api::rules::PhaseRing::Id(_phaseRingId),
+                                                 maliput::api::rules::Phase::Id(_phaseId)) +
+             "\n";
+  }
+
   return rules;
 }
 
@@ -364,6 +407,15 @@ StringType MaliputViewerModel::GetDirectionUsageRules(const maliput::api::LaneId
   return StringType(directionUsageRules.str().c_str());
 }
 
+template <typename StringType>
+StringType MaliputViewerModel::GetPhaseRightOfWayRules(const maliput::api::rules::PhaseRing::Id& _phaseRingId,
+                                                       const maliput::api::rules::Phase::Id& _phaseId) const {
+  std::ostringstream rightOfWayRulesPhaseRing;
+  RoadNetworkQuery query(&rightOfWayRulesPhaseRing, roadNetwork.get());
+  query.GetPhaseRightOfWay(_phaseRingId, _phaseId);
+  return StringType(rightOfWayRulesPhaseRing.str().c_str());
+}
+
 }  // namespace gui
 }  // namespace delphyne