Component inspector: refactor Pose3d C++ code into a separate class

src/gui/plugins/component_inspector/CMakeLists.txt

@@ -1,4 +1,8 @@
 gz_add_gui_plugin(ComponentInspector
-  SOURCES ComponentInspector.cc
-  QT_HEADERS ComponentInspector.hh
+  SOURCES
+    ComponentInspector.cc
+    Pose3d.cc
+  QT_HEADERS
+    ComponentInspector.hh
+    Pose3d.hh
 )

src/gui/plugins/component_inspector/ComponentInspector.cc

@@ -22,7 +22,6 @@
 #include <ignition/gui/Application.hh>
 #include <ignition/gui/MainWindow.hh>
 #include <ignition/plugin/Register.hh>
-#include <ignition/transport/Node.hh>
 
 #include "ignition/gazebo/components/Actor.hh"
 #include "ignition/gazebo/components/AngularAcceleration.hh"
@@ -50,8 +49,6 @@
 #include "ignition/gazebo/components/Performer.hh"
 #include "ignition/gazebo/components/PerformerAffinity.hh"
 #include "ignition/gazebo/components/Physics.hh"
-#include "ignition/gazebo/components/Pose.hh"
-#include "ignition/gazebo/components/PoseCmd.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
 #include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/SourceFilePath.hh"
@@ -66,6 +63,7 @@
 #include "ignition/gazebo/gui/GuiEvents.hh"
 
 #include "ComponentInspector.hh"
+#include "Pose3d.hh"
 
 namespace ignition::gazebo
 {
@@ -97,31 +95,19 @@ namespace ignition::gazebo
 
     /// \brief Transport node for making command requests
     public: transport::Node node;
+
+    /// \brief A map of component types to the function used to update it.
+    public: std::map<ComponentTypeId, inspector::UpdateViewCb>
+        updateViewCbs;
+
+    /// \brief Handles all components displayed as a 3D pose.
+    public: std::unique_ptr<inspector::Pose3d> pose3d;
   };
 }
 
 using namespace ignition;
 using namespace gazebo;
 
-//////////////////////////////////////////////////
-template<>
-void ignition::gazebo::setData(QStandardItem *_item, const math::Pose3d &_data)
-{
-  if (nullptr == _item)
-    return;
-
-  _item->setData(QString("Pose3d"),
-      ComponentsModel::RoleNames().key("dataType"));
-  _item->setData(QList({
-    QVariant(_data.Pos().X()),
-    QVariant(_data.Pos().Y()),
-    QVariant(_data.Pos().Z()),
-    QVariant(_data.Rot().Roll()),
-    QVariant(_data.Rot().Pitch()),
-    QVariant(_data.Rot().Yaw())
-  }), ComponentsModel::RoleNames().key("data"));
-}
-
 //////////////////////////////////////////////////
 template<>
 void ignition::gazebo::setData(QStandardItem *_item,
@@ -340,6 +326,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
   // Connect model
   this->Context()->setContextProperty(
       "ComponentsModel", &this->dataPtr->componentsModel);
+
+  // Type-specific handlers
+  this->dataPtr->pose3d = std::make_unique<inspector::Pose3d>(this);
 }
 
 //////////////////////////////////////////////////
@@ -588,12 +577,6 @@ void ComponentInspector::Update(const UpdateInfo &,
       if (comp)
         setData(item, comp->Data());
     }
-    else if (typeId == components::Pose::typeId)
-    {
-      auto comp = _ecm.Component<components::Pose>(this->dataPtr->entity);
-      if (comp)
-        setData(item, comp->Data());
-    }
     else if (typeId == components::Static::typeId)
     {
       auto comp = _ecm.Component<components::Static>(this->dataPtr->entity);
@@ -694,18 +677,10 @@ void ComponentInspector::Update(const UpdateInfo &,
         setUnit(item, "m/s");
       }
     }
-    else if (typeId == components::WorldPose::typeId)
+    else if (this->dataPtr->updateViewCbs.find(typeId) !=
+          this->dataPtr->updateViewCbs.end())
     {
-      auto comp = _ecm.Component<components::WorldPose>(this->dataPtr->entity);
-      if (comp)
-        setData(item, comp->Data());
-    }
-    else if (typeId == components::WorldPoseCmd::typeId)
-    {
-      auto comp = _ecm.Component<components::WorldPoseCmd>(
-          this->dataPtr->entity);
-      if (comp)
-        setData(item, comp->Data());
+      this->dataPtr->updateViewCbs[typeId](_ecm, item);
     }
   }
 
@@ -723,6 +698,13 @@ void ComponentInspector::Update(const UpdateInfo &,
   }
 }
 
+/////////////////////////////////////////////////
+void ComponentInspector::AddUpdateViewCb(ComponentTypeId _id,
+    inspector::UpdateViewCb _cb)
+{
+  this->dataPtr->updateViewCbs[_id] = _cb;
+}
+
 /////////////////////////////////////////////////
 bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event)
 {
@@ -812,26 +794,6 @@ void ComponentInspector::SetPaused(bool _paused)
   this->PausedChanged();
 }
 
-/////////////////////////////////////////////////
-void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll,
-    double _pitch, double _yaw)
-{
-  std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
-      [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
-  {
-    if (!_result)
-        ignerr << "Error setting pose" << std::endl;
-  };
-
-  ignition::msgs::Pose req;
-  req.set_id(this->dataPtr->entity);
-  msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z));
-  msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw));
-  auto poseCmdService = "/world/" + this->dataPtr->worldName
-      + "/set_pose";
-  this->dataPtr->node.Request(poseCmdService, req, cb);
-}
-
 /////////////////////////////////////////////////
 void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor)
 {
@@ -862,6 +824,18 @@ bool ComponentInspector::NestedModel() const
   return this->dataPtr->nestedModel;
 }
 
+/////////////////////////////////////////////////
+const std::string &ComponentInspector::WorldName() const
+{
+  return this->dataPtr->worldName;
+}
+
+/////////////////////////////////////////////////
+transport::Node &ComponentInspector::TransportNode()
+{
+  return this->dataPtr->node;
+}
+
 // Register this plugin
 IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector,
                     ignition::gui::Plugin)

src/gui/plugins/component_inspector/ComponentInspector.hh

@@ -21,15 +21,17 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <ignition/math/Pose3.hh>
 #include <ignition/math/Vector3.hh>
+#include <ignition/transport/Node.hh>
 
 #include <ignition/gazebo/components/Component.hh>
 #include <ignition/gazebo/gui/GuiSystem.hh>
 #include <ignition/gazebo/Types.hh>
 
 #include "ignition/gazebo/components/Physics.hh"
 
+#include "Types.hh"
+
 Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId)
 
 namespace ignition
@@ -65,12 +67,6 @@ namespace gazebo
   template<>
   void setData(QStandardItem *_item, const std::string &_data);
 
-  /// \brief Specialized to set pose data.
-  /// \param[in] _item Item whose data will be set.
-  /// \param[in] _data Data to set.
-  template<>
-  void setData(QStandardItem *_item, const math::Pose3d &_data);
-
   /// \brief Specialized to set vector data.
   /// \param[in] _item Item whose data will be set.
   /// \param[in] _data Data to set.
@@ -205,15 +201,12 @@ namespace gazebo
     // Documentation inherited
     public: void Update(const UpdateInfo &, EntityComponentManager &) override;
 
-    /// \brief Callback in Qt thread when pose changes.
-    /// \param[in] _x X
-    /// \param[in] _y Y
-    /// \param[in] _z Z
-    /// \param[in] _roll Roll
-    /// \param[in] _pitch Pitch
-    /// \param[in] _yaw Yaw
-    public: Q_INVOKABLE void OnPose(double _x, double _y, double _z,
-        double _roll, double _pitch, double _yaw);
+    /// \brief Add a callback that's called whenever there are updates from the
+    /// ECM to the view, for a given component type.
+    /// \param[in] _id The component type id
+    /// \param[in] _cb Function that's called when there are updates.
+    public: void AddUpdateViewCb(ComponentTypeId _id,
+                inspector::UpdateViewCb _cb);
 
     /// \brief Callback in Qt thread when physics' properties change.
     /// \param[in] _stepSize step size
@@ -275,6 +268,14 @@ namespace gazebo
     /// \brief Notify that paused has changed.
     signals: void PausedChanged();
 
+    /// \brief Name of world entity
+    /// \return World name
+    public: const std::string &WorldName() const;
+
+    /// \brief Node for communication
+    /// \return Transport node
+    public: transport::Node &TransportNode();
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<ComponentInspectorPrivate> dataPtr;

src/gui/plugins/component_inspector/Pose3d.cc

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <string>
+
+#include <ignition/msgs/boolean.pb.h>
+#include <ignition/msgs/pose.pb.h>
+
+#include "Pose3d.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace inspector;
+
+/////////////////////////////////////////////////
+Pose3d::Pose3d(ComponentInspector *_inspector)
+{
+  _inspector->Context()->setContextProperty("Pose3dImpl", this);
+  this->inspector = _inspector;
+
+  this->inspector->AddUpdateViewCb(components::Pose::typeId,
+      std::bind(&Pose3d::UpdateView<components::Pose>, this,
+      std::placeholders::_1, std::placeholders::_2));
+  this->inspector->AddUpdateViewCb(components::WorldPose::typeId,
+      std::bind(&Pose3d::UpdateView<components::WorldPose>, this,
+      std::placeholders::_1, std::placeholders::_2));
+  this->inspector->AddUpdateViewCb(components::WorldPoseCmd::typeId,
+      std::bind(&Pose3d::UpdateView<components::WorldPoseCmd>, this,
+      std::placeholders::_1, std::placeholders::_2));
+}
+
+/////////////////////////////////////////////////
+void Pose3d::OnPose(double _x, double _y, double _z, double _roll,
+    double _pitch, double _yaw)
+{
+  std::function<void(const msgs::Boolean &, const bool)> cb =
+      [](const msgs::Boolean &, const bool _result)
+  {
+    if (!_result)
+        ignerr << "Error setting pose" << std::endl;
+  };
+
+  msgs::Pose req;
+  req.set_id(this->inspector->GetEntity());
+  msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z));
+  msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw));
+  std::string poseCmdService("/world/" + this->inspector->WorldName()
+      + "/set_pose");
+  this->inspector->TransportNode().Request(poseCmdService, req, cb);
+}