agent_info_display plugin using ignition-gui3

delphyne_gui/visualizer/display_plugins/AgentInfoDisplay.qml

@@ -0,0 +1,24 @@
+import QtQuick 2.9
+import QtQuick.Controls 2.2
+import QtQuick.Layouts 1.3
+
+Rectangle {
+  id: agentInfoDisplay
+  color: "transparent"
+  Layout.minimumWidth: 290
+  Layout.minimumHeight: 110
+  Layout.fillWidth: true
+
+  // Checkbox to toggle axes visibility.
+  RowLayout {
+    CheckBox {
+      id: visibilityCheckbox
+      text: qsTr("Visible")
+      checked: AgentInfoDisplay.isVisible
+      onClicked : {
+        visibilityCheckbox.checked = !AgentInfoDisplay.isVisible;
+        AgentInfoDisplay.isVisible = !AgentInfoDisplay.isVisible;
+      }
+    }
+  }
+}

delphyne_gui/visualizer/display_plugins/CMakeLists.txt

@@ -66,6 +66,43 @@ install(
   ARCHIVE DESTINATION lib
 )
 
+#-------------------------------------------------------------------------------
+# AgentInfo display (ign-gui 3)
+QT5_WRAP_CPP(AgentInfoDisplay_MOC agent_info_display.hh)
+QT5_ADD_RESOURCES(AgentInfoDisplay_RCC agent_info_display.qrc)
+
+add_library(AgentInfoDisplay
+  ${CMAKE_CURRENT_SOURCE_DIR}/agent_info_display.cc
+  ${AgentInfoDisplay_MOC}
+  ${AgentInfoDisplay_RCC}
+)
+add_library(delphyne_gui::AgentInfoDisplay ALIAS AgentInfoDisplay)
+set_target_properties(AgentInfoDisplay
+  PROPERTIES
+    OUTPUT_NAME AgentInfoDisplay
+)
+
+target_link_libraries(AgentInfoDisplay
+  PUBLIC
+    delphyne::protobuf_messages
+    ignition-common3::ignition-common3
+    ignition-gui3::ignition-gui3
+    ignition-math6::ignition-math6
+    ignition-rendering3::ignition-rendering3
+    ${Qt5Core_LIBRARIES}
+    ${Qt5Widgets_LIBRARIES}
+  PRIVATE
+    ignition-plugin1::register
+)
+
+install(
+  TARGETS AgentInfoDisplay
+  EXPORT ${PROJECT_NAME}-targets
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)
+
 #-------------------------------------------------------------------------------
 # Origin display (ign-gui 3)
 QT5_WRAP_CPP(OriginDisplay_MOC origin_display.hh)

delphyne_gui/visualizer/display_plugins/agent_info_display.cc

@@ -0,0 +1,182 @@
+// Copyright 2021 Toyota Research Institute
+#include "agent_info_display.hh"
+
+#include <iomanip>
+#include <map>
+#include <mutex>
+#include <sstream>
+
+#include <ignition/common/Console.hh>
+#include <ignition/gui/Application.hh>
+#include <ignition/gui/GuiEvents.hh>
+#include <ignition/gui/MainWindow.hh>
+#include <ignition/plugin/Register.hh>
+#include <ignition/rendering/RenderEngine.hh>
+#include <ignition/rendering/RenderingIface.hh>
+#include <ignition/rendering/Scene.hh>
+#include <ignition/rendering/Text.hh>
+#include <ignition/rendering/Visual.hh>
+
+#include "delphyne/protobuf/agent_state_v.pb.h"
+
+namespace delphyne {
+namespace gui {
+struct AgentInfoText {
+  /// \brief The text display
+  ignition::rendering::TextPtr text;
+  ignition::rendering::VisualPtr textVis;
+};
+
+static constexpr double charHeight = 0.3;
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::LoadConfig(const tinyxml2::XMLElement* _pluginElem) {
+  title = "Agent Info Display";
+
+  if (_pluginElem) {
+    // Update the requested scene name even it fails to load.
+    if (auto elem = _pluginElem->FirstChildElement("scene")) {
+      this->sceneName = elem->GetText();
+    }
+    // Similarly with the visibility flag.
+    if (auto elem = _pluginElem->FirstChildElement("visible")) {
+      elem->QueryBoolText(&isVisible);
+      IsVisibleChanged();
+    }
+  }
+
+  ignition::gui::App()->findChild<ignition::gui::MainWindow*>()->installEventFilter(this);
+}
+
+/////////////////////////////////////////////////
+bool AgentInfoDisplay::eventFilter(QObject* _obj, QEvent* _event) {
+  if (_event->type() == ignition::gui::events::Render::kType) {
+    if (nullptr == this->scenePtr) {
+      auto engine = ignition::rendering::engine(kEngineName);
+      this->scenePtr = engine->SceneByName(this->sceneName);
+      if (nullptr == this->scenePtr) {
+        ignwarn << "Scene \"" << this->sceneName << "\" not found, "
+                << "agent info display plugin won't work until the scene is created." << std::endl;
+      } else {
+        // Subscribe to agent info once the scene pointer is found
+        this->node.Subscribe("agents/state", &AgentInfoDisplay::OnAgentState, this);
+      }
+    } else if (this->dirty) {
+      this->ProcessMsg();
+    }
+  }
+
+  // Standard event processing
+  return QObject::eventFilter(_obj, _event);
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::OnAgentState(const ignition::msgs::AgentState_V& _msg) {
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
+
+  this->msg.CopyFrom(_msg);
+  this->dirty = true;
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::ProcessMsg() {
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
+
+  for (int i = 0; i < this->msg.states_size(); ++i) {
+    ignition::msgs::AgentState agent = this->msg.states(i);
+    std::shared_ptr<AgentInfoText> agentInfoText;
+    const std::string agentName = NameFromAgent(agent);
+
+    // Step 2 from above if necessary
+    if (mapAgentInfoText.find(agentName) == mapAgentInfoText.end()) {
+      agentInfoText = CreateAgentText(agentName, scenePtr);
+    } else {
+      agentInfoText = this->mapAgentInfoText[agentName];
+    }
+
+    UpdateAgentLabel(agent, agentName, agentInfoText);
+  }
+
+  ChangeAgentInfoVisibility();
+
+  this->dirty = false;
+}
+
+/////////////////////////////////////////////////
+std::string AgentInfoDisplay::NameFromAgent(const ignition::msgs::AgentState& agent) {
+  // The names that we get from the agents are of the form:
+  //
+  // "/agent/0/state"
+  //
+  // To reduce screen real estate, remove the "/agent/" from the start and
+  // "/state" from the rear.
+  return agent.name().substr(7, agent.name().length() - 7 - 6);
+}
+
+/////////////////////////////////////////////////
+std::shared_ptr<AgentInfoText> AgentInfoDisplay::CreateAgentText(const std::string& _agentName,
+                                                                 ignition::rendering::ScenePtr _scenePtr) {
+  auto agentInfoText = std::make_shared<AgentInfoText>();
+
+  agentInfoText->text = _scenePtr->CreateText();
+  agentInfoText->text->SetShowOnTop(true);
+  agentInfoText->text->SetCharHeight(charHeight);
+
+  agentInfoText->textVis = _scenePtr->CreateVisual();
+  agentInfoText->textVis->SetLocalScale(1.0, 1.0, 1.0);
+  agentInfoText->textVis->AddGeometry(agentInfoText->text);
+
+  _scenePtr->RootVisual()->AddChild(agentInfoText->textVis);
+
+  mapAgentInfoText[_agentName] = agentInfoText;
+
+  ChangeAgentInfoVisibility();
+
+  return agentInfoText;
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::UpdateAgentLabel(const ignition::msgs::AgentState& _agent, const std::string& _agentName,
+                                        std::shared_ptr<AgentInfoText> _agentInfoText) {
+  ignition::math::Vector3d pos;
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+  ignition::math::Vector3d linear_velocity;
+
+  if (_agent.has_position()) {
+    pos = ignition::msgs::Convert(_agent.position());
+  }
+  if (_agent.has_orientation()) {
+    roll = _agent.orientation().roll();
+    pitch = _agent.orientation().pitch();
+    yaw = _agent.orientation().yaw();
+  }
+  if (_agent.has_linear_velocity()) {
+    linear_velocity = ignition::msgs::Convert(_agent.linear_velocity());
+  }
+
+  std::stringstream ss;
+  ss << std::setprecision(2);
+  ss << _agentName << ":\n pos:(" << pos << "), yaw:(" << yaw << ")\n vel:(" << linear_velocity << ")";
+  _agentInfoText->text->SetTextString(ss.str());
+  _agentInfoText->textVis->SetLocalPose(ignition::math::Pose3d(pos.X(), pos.Y(), pos.Z() + 2.6, roll, pitch, yaw));
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::ChangeAgentInfoVisibility() {
+  const bool newIsVisibleValue = isVisible;
+  for (auto& nameToAgentInfoText : mapAgentInfoText) {
+    auto name = nameToAgentInfoText.first;
+    auto agentInfoText = nameToAgentInfoText.second;
+    if (agentInfoText && agentInfoText->textVis) {
+      agentInfoText->textVis->SetVisible(newIsVisibleValue);
+    }
+  }
+}
+
+}  // namespace gui
+}  // namespace delphyne
+
+// Register this plugin
+IGNITION_ADD_PLUGIN(delphyne::gui::AgentInfoDisplay, ignition::gui::Plugin)

delphyne_gui/visualizer/display_plugins/agent_info_display.hh

@@ -0,0 +1,112 @@
+// Copyright 2021 Toyota Research Institute
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <ignition/gui/Plugin.hh>
+#include <ignition/gui/qt.h>
+#include <ignition/rendering/RenderTypes.hh>
+#include <ignition/transport.hh>
+
+#include "delphyne/protobuf/agent_state_v.pb.h"
+
+namespace delphyne {
+namespace gui {
+
+struct AgentInfoText;
+
+/// @brief Implements a plugin to display the state information for agents in the scene.
+/// @details ign-gui3 does not have DisplayPlugins, so this plugin
+///          implements a slightly different logic to what the original ign-gui0
+///          DisplayPlugin did. It subscribes to events emitted by the MainWindow and
+///          checks for `ignition::gui::events::Render` to make rendering calls. On its
+///          first Render event, it gets a pointer to the scene and subscribes to the
+///          agent info topic. On subsequent Render events, it checks for new agent info
+///          data and creates or updates a text geometry to display this data.
+///          Typically, this plugin goes hand in hand with the Scene3D plugin.
+///          The plugin UI has a checkbox to toggle visibility. It is paired
+///          with `isVisible`
+class AgentInfoDisplay : public ignition::gui::Plugin {
+  Q_OBJECT
+
+  Q_PROPERTY(bool isVisible READ IsVisible WRITE SetIsVisible NOTIFY IsVisibleChanged)
+
+ public:
+  AgentInfoDisplay() = default;
+
+  /// @brief Loads the plugin configuration.
+  void LoadConfig(const tinyxml2::XMLElement* _pluginElem) override;
+
+  /// @{ isVisible accessors.
+  Q_INVOKABLE bool IsVisible() const { return isVisible; }
+
+  Q_INVOKABLE void SetIsVisible(bool _isVisible) {
+    isVisible = _isVisible;
+    IsVisibleChanged();
+    dirty = true;
+  }
+  /// @}
+
+ private slots:
+  void ProcessMsg();
+
+ signals:
+  void IsVisibleChanged();
+
+ private:
+  /// @brief Callback for all installed event filters. On Render events, if the scene pointer
+  /// is not yet available, it will try to get it and then subscibe to the agent info topic if
+  /// successful. On subsequent calls, it will create and update text geometries if new data
+  /// is available (indicated by the dirty flag).
+  /// @param[in] _obj Object that received the event
+  /// @param[in] _event Event
+  bool eventFilter(QObject* _obj, QEvent* _event) override;
+
+  /// @brief The rendering engine name.
+  const std::string kEngineName{"ogre"};
+
+  /// @brief The scene name.
+  std::string sceneName{"scene"};
+
+  /// @brief The scene pointer.
+  ignition::rendering::ScenePtr scenePtr;
+
+  /// @brief Holds the visibility status of the agent info.
+  bool isVisible{true};
+
+  /// @brief Flag to indicate that new data is available for rendering.
+  bool dirty{false};
+
+  /// @brief Message holding latest agent states
+  ignition::msgs::AgentState_V msg;
+
+  /// @brief Mutex to protect msg
+  std::recursive_mutex mutex;
+
+  /// @brief Map from agent name to AgentInfoText.
+  std::map<std::string, std::shared_ptr<AgentInfoText>> mapAgentInfoText;
+
+  /// \brief A transport node.
+  ignition::transport::Node node;
+
+  /// @brief Toggles the visibility of the agent info.
+  void ChangeAgentInfoVisibility();
+
+  /// @brief Callback for agent info subscriber
+  void OnAgentState(const ignition::msgs::AgentState_V& _msg);
+
+  /// @brief Extract the agent name from the topic name.
+  std::string NameFromAgent(const ignition::msgs::AgentState& agent);
+
+  /// @brief Create floating text visuals for each agent.
+  std::shared_ptr<AgentInfoText> CreateAgentText(const std::string& _agentName,
+                                                 ignition::rendering::ScenePtr _scenePtr);
+
+  /// @brief Update pose and content of floating text visuals.
+  void UpdateAgentLabel(const ignition::msgs::AgentState& _agent, const std::string& _agentName,
+                        std::shared_ptr<AgentInfoText> _agentInfoText);
+};
+
+}  // namespace gui
+}  // namespace delphyne

delphyne_gui/visualizer/display_plugins/agent_info_display.qrc

@@ -0,0 +1,5 @@
+<!DOCTYPE RCC><RCC version="1.0">
+  <qresource prefix="AgentInfoDisplay/">
+    <file>AgentInfoDisplay.qml</file>
+  </qresource>
+</RCC>

delphyne_gui/visualizer/display_plugins/agent_info_display0.cc

@@ -49,22 +49,6 @@ class AgentInfoDisplayPrivate {
 
  public:
   std::map<std::string, std::shared_ptr<AgentInfoText>> agentInfoText;
-
-  /// \brief Text size in pixels
- public:
-  unsigned int textSize = 15;
-
-  /// \brief Horizontal padding away from the image border
- public:
-  int horizontalPadding = 20;
-
-  /// \brief Vertical padding away from the image border
- public:
-  int verticalPadding = 20;
-
-  /// \brief Color of the text
- public:
-  ignition::math::Color textColor = ignition::math::Color::White;
 };
 }  // namespace display_plugins
 }  // namespace gui