WIP agent_info_display 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,242 @@
+// 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/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;
+  bool visible{true};
+};
+
+class AgentInfoDisplayPrivate {
+ public:
+  QWidget* widget;
+
+ public:
+  QSignalMapper* signalMapper;
+
+  /// \brief Color of the text
+ public:
+  ignition::math::Color textColor = ignition::math::Color::White;
+};
+
+static constexpr double charHeight = 0.3;
+
+/////////////////////////////////////////////////
+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);
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::LoadConfig(const tinyxml2::XMLElement* _pluginElem) {
+  title = "Agent Info Display";
+  this->node.Subscribe("agents/state", &AgentInfoDisplay::OnAgentState, this);
+
+  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();
+    }
+  }
+
+  // Get the render engine.
+  // Note: we don't support other engines than Ogre.
+  auto engine = ignition::rendering::engine(kEngineName);
+  if (!engine) {
+    ignerr << "Engine \"" << kEngineName << "\" not supported, agent info display plugin won't work." << std::endl;
+    return;
+  }
+  // Get the scene.
+  auto scene = engine->SceneByName(this->sceneName);
+  if (!scene) {
+    ignwarn << "Scene \"" << this->sceneName << "\" not found, agent info display plugin won't work until the scene is created."
+            << " Trying again in " << kTimerPeriodInMs << "ms" << std::endl;
+    timer.start(kTimerPeriodInMs, this);
+    return;
+  }
+  DrawAgentInfo(scene);
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::timerEvent(QTimerEvent* _event) {
+  if (_event->timerId() != timer.timerId()) {
+    return;
+  }
+
+  // Get the render engine.
+  // Note: we don't support other engines than Ogre.
+  auto engine = ignition::rendering::engine(kEngineName);
+  auto scene = engine->SceneByName(this->sceneName);
+  if (!scene) {
+    ignwarn << "Scene \"" << this->sceneName << "\" not found yet. Trying again in "
+            << " Trying again in " << kTimerPeriodInMs << "ms" << std::endl;
+    return;
+  }
+  timer.stop();
+  DrawAgentInfo(scene);
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::DrawAgentInfo(ignition::rendering::ScenePtr scene) {
+
+  // Set the visibility of the visuals
+  ChangeAgentInfoVisibility();
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::ChangeAgentInfoVisibility() {
+  const bool newIsVisibleValue = isVisible;
+  //if (axes[0] != nullptr) {
+  //  for (auto& axis : axes) {
+  //    axis->SetVisible(newIsVisibleValue);
+  //  }
+  //}
+}
+
+/////////////////////////////////////////////////
+//void AgentInfoDisplay::ToggleText(const QString& _agentName) {
+//  std::shared_ptr<AgentInfoText> agentInfoText = this->dataPtr->agentInfoText[_agentName.toStdString()];
+//
+//  agentInfoText->visible = !agentInfoText->visible;
+//
+//  if (agentInfoText->visible) {
+//    agentInfoText->text->SetCharHeight(charHeight);
+//  } else {
+//    agentInfoText->text->SetCharHeight(0.0);
+//  }
+//}
+
+/////////////////////////////////////////////////
+//std::shared_ptr<AgentInfoText> AgentInfoDisplay::CreateAgentText(
+//    const std::string& _agentName, QVBoxLayout* _layout, std::shared_ptr<ignition::rendering::Scene> _scenePtr) {
+//  auto visibleCheck = new QCheckBox(QString::fromStdString(_agentName), this);
+//  visibleCheck->setChecked(true);
+//  _layout->addWidget(visibleCheck);
+//
+//  this->dataPtr->signalMapper->setMapping(visibleCheck, QString::fromStdString(_agentName));
+//  connect(visibleCheck, SIGNAL(clicked()), this->dataPtr->signalMapper, SLOT(map()));
+//
+//  // Now that we've created the widgets, create the hovering text
+//  auto agentInfoText = std::make_shared<AgentInfoText>();
+//
+//  agentInfoText->visible = true;
+//
+//  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);
+//
+//  this->Visual()->AddChild(agentInfoText->textVis);
+//
+//  this->dataPtr->agentInfoText[_agentName] = agentInfoText;
+//
+//  return agentInfoText;
+//}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::UpdateAgentLabel(const ignition::msgs::AgentState& _agent, const std::string& _agentName,
+                                        std::shared_ptr<AgentInfoText> _agentInfoText) {
+  double x = 0.0;
+  double y = 0.0;
+  double z = 0.0;
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+  double vx = 0.0;
+  double vy = 0.0;
+  double vz = 0.0;
+
+  if (_agent.has_position()) {
+    x = _agent.position().x();
+    y = _agent.position().y();
+    z = _agent.position().z();
+  }
+  if (_agent.has_orientation()) {
+    roll = _agent.orientation().roll();
+    pitch = _agent.orientation().pitch();
+    yaw = _agent.orientation().yaw();
+  }
+  if (_agent.has_linear_velocity()) {
+    vx = _agent.linear_velocity().x();
+    vy = _agent.linear_velocity().y();
+    vz = _agent.linear_velocity().z();
+  }
+
+  std::stringstream ss;
+  ss << _agentName << ":\n pos:(x:" << std::setprecision(2) << x << ",y:" << y << ",z:" << z << ",yaw:" << yaw << ")"
+     << "\n vel:(x:" << vx << ",y:" << vy << ",z:" << vz << ")";
+  _agentInfoText->text->SetTextString(ss.str());
+  _agentInfoText->textVis->SetLocalPose(ignition::math::Pose3d(x, y, z + 2.6, roll, pitch, yaw));
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::ProcessMsg() {
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
+
+  auto engine = ignition::rendering::engine(kEngineName);
+  auto scenePtr = engine->SceneByName(this->sceneName);
+  if (!scenePtr) {
+    ignerr << "Scene invalid. Agent Info display not initialized." << std::endl;
+    return;
+  }
+
+  for (int i = 0; i < this->msg.states_size(); ++i) {
+    ignition::msgs::AgentState agent = this->msg.states(i);
+    std::shared_ptr<AgentInfoText> agentInfoText;
+    std::string agentName = NameFromAgent(agent);
+
+    // Step 2 from above if necessary
+    //if (layout != nullptr) {
+    //  agentInfoText = CreateAgentText(agentName, layout, scenePtr);
+    //} else {
+      agentInfoText = this->mapAgentInfoText[agentName];
+    //}
+
+    UpdateAgentLabel(agent, agentName, agentInfoText);
+  }
+}
+
+/////////////////////////////////////////////////
+void AgentInfoDisplay::OnAgentState(const ignition::msgs::AgentState_V& _msg) {
+  std::lock_guard<std::recursive_mutex> lock(this->mutex);
+
+  this->msg.CopyFrom(_msg);
+  QMetaObject::invokeMethod(this, "ProcessMsg");
+}
+
+}  // namespace gui
+}  // namespace delphyne
+
+// Register this plugin
+IGNITION_ADD_PLUGIN(delphyne::gui::AgentInfoDisplay, ignition::gui::Plugin)