Add FixedFrame tf status in GlobalOptions

ign_rviz_plugins/include/ignition/rviz/plugins/GlobalOptions.hpp

@@ -26,6 +26,7 @@
 #include <string>
 #include <memory>
 #include <utility>
+#include <vector>
 
 #include "ignition/rviz/plugins/message_display_base.hpp"
 
@@ -37,6 +38,98 @@ namespace rviz
 {
 namespace plugins
 {
+/**
+ * @brief TF status and message
+ *
+ * Displays tf status and message under GlobalOptions
+ */
+class TFStatus : public QObject
+{
+  Q_OBJECT
+
+  /**
+   *  @brief TF status
+   */
+  Q_PROPERTY(
+    QString status
+    READ getStatus
+    NOTIFY statusChanged
+  )
+
+  /**
+   *  @brief TF status message
+   */
+  Q_PROPERTY(
+    QString message
+    READ getMessage
+    NOTIFY messageChanged
+  )
+
+  /**
+   *  @brief TF status color
+   */
+  Q_PROPERTY(
+    QString color
+    READ getColor
+    NOTIFY colorChanged
+  )
+
+public:
+  // Constructor
+  TFStatus();
+
+  // Destructor
+  ~TFStatus() {}
+
+  /**
+   * @brief Update tf status and message
+   * @param[in] _fixedFrame FixedFrame name
+   * @param[in] _allFrames List of all frames
+   */
+  void update(std::string & _fixedFrame, std::vector<std::string> & _allFrames);
+
+  /**
+   * @brief Get the TF status as a string
+   * @return TF status
+   */
+  Q_INVOKABLE QString getStatus() const;
+
+  /**
+   * @brief Get the TF status message as a string
+   * @return TF status message
+   */
+  Q_INVOKABLE QString getMessage() const;
+
+  /**
+   * @brief Get the TF status color as a string
+   * @return TF status color
+   */
+  Q_INVOKABLE QString getColor() const;
+
+signals:
+  /**
+   * @brief Notify that TF status has changed
+   */
+  void statusChanged();
+
+signals:
+  /**
+   * @brief Notify that TF message has changed
+   */
+  void messageChanged();
+
+signals:
+  /**
+   * @brief Notify that TF color has changed
+   */
+  void colorChanged();
+
+private:
+  QString status;
+  QString message;
+  QString color;
+};
+
 /**
  * @brief Configure global option of ignition rviz
  *
@@ -56,6 +149,15 @@ class GlobalOptions : public MessageDisplayBase
     NOTIFY frameListChanged
   )
 
+  /**
+   *  @brief TF status
+   */
+  Q_PROPERTY(
+    TFStatus * tfStatus
+    READ getTfStatus
+    NOTIFY tfStatusChanged
+  )
+
 public:
   // Constructor
   GlobalOptions();
@@ -93,6 +195,12 @@ class GlobalOptions : public MessageDisplayBase
    */
   Q_INVOKABLE QStringList getFrameList() const;
 
+  /**
+   * @brief Get the TF status as a TFStatus object
+   * @return TFStatus object
+   */
+  Q_INVOKABLE TFStatus * getTfStatus() const;
+
   /**
    * @brief Set the frame list from a string
    * @param[in] _frameList List of frames
@@ -105,6 +213,12 @@ class GlobalOptions : public MessageDisplayBase
    */
   void frameListChanged();
 
+signals:
+  /**
+   * @brief Notify that TF status has changed
+   */
+  void tfStatusChanged();
+
 signals:
   /**
    * @brief Set combo box index
@@ -127,6 +241,7 @@ public slots:
   bool initialized;
   bool populated;
   QColor color;
+  TFStatus * tfStatus;
 };
 
 }  // namespace plugins

ign_rviz_plugins/res/qml/GlobalOptions.qml

@@ -22,7 +22,7 @@ import QtQuick.Dialogs 1.0
 
 Item {
   Layout.minimumWidth: 280
-  Layout.minimumHeight: 200
+  Layout.minimumHeight: 220
   anchors.fill: parent
   anchors.margins: 10
 
@@ -104,6 +104,27 @@ Item {
         }
       }
     }
+
+    RowLayout {
+      width: parent.width
+      spacing: 10
+
+      Label {
+        id: "status"
+        text: GlobalOptions.tfStatus.status
+        color: GlobalOptions.tfStatus.color
+        font.pointSize: 11
+      }
+
+      Label {
+        id: "message"
+        text: GlobalOptions.tfStatus.message
+        color: GlobalOptions.tfStatus.color
+        font.pointSize: 11
+        Layout.fillWidth: true
+        elide: Label.ElideRight
+      }
+    }
   }
 
   ColorDialog {

ign_rviz_plugins/src/rviz/plugins/GlobalOptions.cpp

@@ -31,6 +31,56 @@ namespace rviz
 {
 namespace plugins
 {
+////////////////////////////////////////////////////////////////////////////////
+TFStatus::TFStatus()
+: status(" "), message(" "), color("green")
+{}
+
+////////////////////////////////////////////////////////////////////////////////
+void TFStatus::update(std::string & _fixedFrame, std::vector<std::string> & _allFrames)
+{
+  // Check for tf data
+  auto framePosition = std::find(_allFrames.begin(), _allFrames.end(), _fixedFrame);
+
+  if (_allFrames.empty()) {
+    std::string msg = "No tf data. Frame [" + _fixedFrame + "] does not exist";
+
+    this->status = QString::fromStdString("Fixed Frame [Warn]");
+    this->message = QString::fromStdString(msg);
+    this->color = QString::fromStdString("orange");
+  } else if (framePosition == _allFrames.end()) {
+    std::string msg = "Frame [" + _fixedFrame + "] does not exist";
+
+    this->status = QString::fromStdString("Fixed Frame [Error]");
+    this->message = QString::fromStdString(msg);
+    this->color = QString::fromStdString("red");
+  } else {
+    std::string msg = "OK";
+
+    this->status = QString::fromStdString("Fixed Frame");
+    this->message = QString::fromStdString(msg);
+    this->color = QString::fromStdString("green");
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+QString TFStatus::getStatus() const
+{
+  return this->status;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+QString TFStatus::getMessage() const
+{
+  return this->message;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+QString TFStatus::getColor() const
+{
+  return this->color;
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 GlobalOptions::GlobalOptions()
 : dirty(false), initialized(false), populated(false), color("#303030")
@@ -42,7 +92,7 @@ GlobalOptions::GlobalOptions()
     igndbg << "Engine '" << "ogre" << "' is not supported" << std::endl;
     return;
   }
-
+  this->tfStatus = new TFStatus();
   this->frameList.push_back("world");
 }
 
@@ -97,6 +147,13 @@ bool GlobalOptions::eventFilter(QObject * _object, QEvent * _event)
       this->scene->SetBackgroundColor(math::Color(color.redF(), color.greenF(), color.blue()));
       this->dirty = false;
     }
+
+    // Update tf status and message
+    std::vector<std::string> allFrames;
+    this->frameManager->getFrames(allFrames);
+    std::string fixedFrame = this->frameManager->getFixedFrame();
+    this->tfStatus->update(fixedFrame, allFrames);
+    emit tfStatusChanged();
   }
 
   // Update combo-box on frame list change
@@ -120,6 +177,12 @@ QStringList GlobalOptions::getFrameList() const
   return this->frameList;
 }
 
+////////////////////////////////////////////////////////////////////////////////
+TFStatus * GlobalOptions::getTfStatus() const
+{
+  return this->tfStatus;
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 void GlobalOptions::onRefresh()
 {