Fix GridConfig pose updates (#658)

Signed-off-by: Ian Chen <ichen@openrobotics.org>

Author: iche033

src/plugins/grid_config/GridConfig.cc

@@ -130,6 +130,7 @@ void GridConfig::LoadConfig(const tinyxml2::XMLElement *_pluginElem)
         std::stringstream poseStr;
         poseStr << std::string(elem->GetText());
         poseStr >> gridParam.pose;
+        this->emit GridPoseChanged();
       }
 
       elem = insertElem->FirstChildElement("color");
@@ -285,13 +286,12 @@ void GridConfig::ConnectToGrid()
         this->dataPtr->gridParam.vCellCount = grid->VerticalCellCount();
         this->dataPtr->gridParam.cellLength = grid->CellLength();
         this->dataPtr->gridParam.pose = grid->Parent()->LocalPose();
+        this->emit GridPoseChanged();
         this->dataPtr->gridParam.color = grid->Parent()->Material()->Ambient();
         emit this->newParams(
             grid->CellCount(),
             grid->VerticalCellCount(),
             grid->CellLength(),
-            convert(grid->Parent()->LocalPose().Pos()),
-            convert(grid->Parent()->LocalPose().Rot().Euler()),
             convert(grid->Parent()->Material()->Ambient()));
       }
     }
@@ -345,11 +345,20 @@ void GridConfig::UpdateCellLength(double _length)
 }
 
 /////////////////////////////////////////////////
-void GridConfig::SetPose(
-  double _x, double _y, double _z,
-  double _roll, double _pitch, double _yaw)
+void GridConfig::SetGridPose(const QList<double> &_gridPose)
 {
-  this->dataPtr->gridParam.pose = math::Pose3d(_x, _y, _z, _roll, _pitch, _yaw);
+  if (_gridPose == this->GridPose())
+    return;
+
+  this->dataPtr->gridParam.pose = math::Pose3d(
+    _gridPose[0],
+    _gridPose[1],
+    _gridPose[2],
+    _gridPose[3],
+    _gridPose[4],
+    _gridPose[5]);
+  emit this->GridPoseChanged();
+
   this->dataPtr->dirty = true;
 }
 
@@ -405,6 +414,20 @@ void GridConfig::RefreshList()
     this->OnName(this->dataPtr->nameList.at(0));
   emit this->NameListChanged();
 }
+
+/////////////////////////////////////////////////
+QList<double> GridConfig::GridPose() const
+{
+  return QList({
+    this->dataPtr->gridParam.pose.Pos().X(),
+    this->dataPtr->gridParam.pose.Pos().Y(),
+    this->dataPtr->gridParam.pose.Pos().Z(),
+    this->dataPtr->gridParam.pose.Rot().Roll(),
+    this->dataPtr->gridParam.pose.Rot().Pitch(),
+    this->dataPtr->gridParam.pose.Rot().Yaw()
+  });
+}
+
 }  // namespace gz::gui
 
 // Register this plugin

src/plugins/grid_config/GridConfig.hh

@@ -56,6 +56,14 @@ namespace gz::gui::plugins
       NOTIFY NameListChanged
     )
 
+    /// \brief Grid pose (QList order is x, y, z, roll, pitch, yaw)
+    Q_PROPERTY(
+      QList<double> gridPose
+      READ GridPose
+      WRITE SetGridPose
+      NOTIFY GridPoseChanged
+    )
+
     /// \brief Constructor
     public: GridConfig();
 
@@ -80,6 +88,17 @@ namespace gz::gui::plugins
     /// \brief Refresh list of grids. This is called in the rendering thread.
     public: void RefreshList();
 
+    /// \brief Get the current grid camera pose.
+    public: Q_INVOKABLE QList<double> GridPose() const;
+
+    /// \brief Set the grid pose
+    /// \param[in] _gridPose GridPose to set.
+    /// The QList elements are x, y, z, roll, pitch, yaw
+    public slots: void SetGridPose(const QList<double> &_gridPose);
+
+    /// \brief Notify that the grid pose has changed.
+    signals: void GridPoseChanged();
+
     /// \brief Callback when refresh button is pressed.
     public slots: void OnRefresh();
 
@@ -110,12 +129,6 @@ namespace gz::gui::plugins
     /// \param[in] _length new cell length
     public slots: void UpdateCellLength(double _length);
 
-    /// \brief Callback to update grid pose
-    /// \param[in] _x, _y, _z cartesion coordinates
-    /// \param[in] _roll, _pitch, _yaw principal coordinates
-    public slots: void SetPose(double _x, double _y, double _z,
-                               double _roll, double _pitch, double _yaw);
-
     /// \brief Callback to update grid color
     /// \param[in] _r, _g, _b, _a RGB color model with fourth alpha channel
     public slots: void SetColor(double _r, double _g, double _b, double _a);
@@ -128,15 +141,11 @@ namespace gz::gui::plugins
     /// \param[in] _hCellCount Horizontal cell count
     /// \param[in] _vCellCount Vertical cell count
     /// \param[in] _cellLength Cell length
-    /// \param[in] _pos XYZ Position
-    /// \param[in] _rot RPY orientation
     /// \param[in] _color Grid color
     signals: void newParams(
         int _hCellCount,
         int _vCellCount,
         double _cellLength,
-        QVector3D _pos,
-        QVector3D _rot,
         QColor _color);
 
     /// \internal

src/plugins/grid_config/GridConfig.qml

@@ -37,16 +37,10 @@ GridLayout {
 
   Connections {
     target: GridConfig
-    function onNewParams(_hCellCount, _vCellCount, _cellLength, _pos, _rot, _color) {
+    function onNewParams(_hCellCount, _vCellCount, _cellLength, _color) {
       horizontalCellCount.value = _hCellCount;
       verticalCellCount.value = _vCellCount;
       cellLength.value = _cellLength;
-      gzPoseInstance.xValue = _pos.x;
-      gzPoseInstance.yValue = _pos.y;
-      gzPoseInstance.zValue = _pos.z;
-      gzPoseInstance.rollValue = _rot.x;
-      gzPoseInstance.pitchValue = _rot.y;
-      gzPoseInstance.yawValue = _rot.z;
       gzColorGrid.r = _color.r;
       gzColorGrid.g = _color.g;
       gzColorGrid.b = _color.b;
@@ -182,17 +176,18 @@ GridLayout {
     Layout.columnSpan: 4
     Layout.fillWidth: true
     readOnly: false
-    xValue: 0.00
-    yValue: 0.00
-    zValue: 0.00
-    rollValue: 0.00
-    pitchValue: 0.00
-    yawValue: 0.00
+
+    xValue: GridConfig.gridPose[0]
+    yValue: GridConfig.gridPose[1]
+    zValue: GridConfig.gridPose[2]
+    rollValue: GridConfig.gridPose[3]
+    pitchValue: GridConfig.gridPose[4]
+    yawValue: GridConfig.gridPose[5]
 
     onGzPoseSet: {
       // _x, _y, _z, _roll, _pitch, _yaw are parameters of signal gzPoseSet
       // from gz-gui GzPose.qml
-      GridConfig.SetPose(_x, _y, _z, _roll, _pitch, _yaw)
+      GridConfig.SetGridPose([_x, _y, _z, _roll, _pitch, _yaw])
     }
     expand: true
     gzPlotEnabled: false
@@ -220,4 +215,3 @@ GridLayout {
     onGzColorSet: GridConfig.SetColor(gzColorGrid.r, gzColorGrid.g, gzColorGrid.b, gzColorGrid.a)
   }
 }
-