Added magnetometer inspector (#1173)

* Support editing air pressure sensor in the GUI Signed-off-by: Nate Koenig <nate@openrobotics.org> * Add noise to qrc Signed-off-by: Nate Koenig <nate@openrobotics.org> * Add noise to qrc Signed-off-by: Nate Koenig <nate@openrobotics.org> * Added altimeter sensor inspector Signed-off-by: Nate Koenig <nate@openrobotics.org> * Added magnetometer inspector Signed-off-by: Nate Koenig <nate@openrobotics.org> * Fix lint Signed-off-by: Michael Carroll <michael@openrobotics.org> * Update sensor icon Signed-off-by: Nate Koenig <nate@openrobotics.org> * Move AirPressure functions out of ComponentInspector (#1179) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Fix get decimals, and address comments Signed-off-by: Nate Koenig <nate@openrobotics.org> * cleanup and simplification Signed-off-by: Nate Koenig <nate@openrobotics.org> * Require sdf 12.1.0 Signed-off-by: Nate Koenig <nate@openrobotics.org> * missign width Signed-off-by: Nate Koenig <nate@openrobotics.org> * Added simulation state aware spin box Signed-off-by: Nate Koenig <nate@openrobotics.org> * Merged Signed-off-by: Nate Koenig <nate@openrobotics.org> * merged Signed-off-by: Nate Koenig <nate@openrobotics.org> * Remove console output Signed-off-by: Nate Koenig <nate@openrobotics.org> * alphabetize Signed-off-by: Nate Koenig <nate@openrobotics.org> * Fix build Signed-off-by: Nate Koenig <nate@openrobotics.org> * alphabetize Signed-off-by: Nate Koenig <nate@openrobotics.org> * Cleanup build Signed-off-by: Nate Koenig <nate@openrobotics.org> * Missing print call Signed-off-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Michael Carroll <michael@openrobotics.org> Co-authored-by: Louise Poubel <louise@openrobotics.org>
gazebosim · Nov 19, 2021 · b1f8525 · b1f8525
1 parent 0cc0c56
commit b1f8525
Show file tree

Hide file tree

Showing 8 changed files with 458 additions and 6 deletions.
diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc
@@ -634,6 +634,7 @@ void printLinks(const uint64_t _modelEntity,
       printAltimeter(sensor, _ecm, spaces + 2);
       printCamera(sensor, _ecm, spaces + 2);
       printImu(sensor, _ecm, spaces + 2);
+      printMagnetometer(sensor, _ecm, spaces + 2);
       printRgbdCamera(sensor, _ecm, spaces + 2);
     }
   }

diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt
@@ -3,12 +3,14 @@ gz_add_gui_plugin(ComponentInspector
     AirPressure.cc
     Altimeter.cc
     ComponentInspector.cc
+    Magnetometer.cc
     ModelEditor.cc
     Types.cc
   QT_HEADERS
     AirPressure.hh
     Altimeter.hh
     ComponentInspector.hh
+    Magnetometer.hh
     ModelEditor.hh
     Types.hh
 )
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -73,6 +73,7 @@
 #include "AirPressure.hh"
 #include "Altimeter.hh"
 #include "ComponentInspector.hh"
+#include "Magnetometer.hh"
 #include "ModelEditor.hh"
 
 namespace ignition::gazebo
@@ -121,6 +122,9 @@ namespace ignition::gazebo
     /// \brief Altimeter sensor inspector elements
     public: std::unique_ptr<ignition::gazebo::Altimeter> altimeter;
 
+    /// \brief Magnetometer inspector elements
+    public: std::unique_ptr<ignition::gazebo::Magnetometer> magnetometer;
+
     /// \brief Set of callbacks to execute during the Update function.
     public: std::vector<
             std::function<void(EntityComponentManager &)>> updateCallbacks;
@@ -439,6 +443,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
 
   // Create altimeter
   this->dataPtr->altimeter = std::make_unique<Altimeter>(this);
+
+  // Create the magnetometer
+  this->dataPtr->magnetometer = std::make_unique<Magnetometer>(this);
 }
 
 //////////////////////////////////////////////////

diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc
@@ -6,6 +6,7 @@
   <file>ComponentInspector.qml</file>
   <file>ExpandingTypeHeader.qml</file>
   <file>Light.qml</file>
+  <file>Magnetometer.qml</file>
   <file>NoData.qml</file>
   <file>Noise.qml</file>
   <file>Physics.qml</file>

diff --git a/src/gui/plugins/component_inspector/Magnetometer.cc b/src/gui/plugins/component_inspector/Magnetometer.cc
@@ -0,0 +1,174 @@
+/*
+ * 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 <sdf/Magnetometer.hh>
+
+#include <ignition/common/Console.hh>
+#include <ignition/gazebo/components/Magnetometer.hh>
+
+#include "ComponentInspector.hh"
+#include "Magnetometer.hh"
+#include "Types.hh"
+
+using namespace ignition;
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+Magnetometer::Magnetometer(ComponentInspector *_inspector)
+{
+  _inspector->Context()->setContextProperty("MagnetometerImpl", this);
+  this->inspector = _inspector;
+
+  ComponentCreator creator =
+    [=](EntityComponentManager &_ecm, Entity _entity, QStandardItem *_item)
+  {
+    auto comp = _ecm.Component<components::Magnetometer>(_entity);
+    if (nullptr == _item || nullptr == comp)
+      return;
+    const sdf::Magnetometer *mag = comp->Data().MagnetometerSensor();
+
+    _item->setData(QString("Magnetometer"),
+        ComponentsModel::RoleNames().key("dataType"));
+    _item->setData(QList({
+      QVariant(mag->XNoise().Mean()),
+      QVariant(mag->XNoise().BiasMean()),
+      QVariant(mag->XNoise().StdDev()),
+      QVariant(mag->XNoise().BiasStdDev()),
+      QVariant(mag->XNoise().DynamicBiasStdDev()),
+      QVariant(mag->XNoise().DynamicBiasCorrelationTime()),
+
+      QVariant(mag->YNoise().Mean()),
+      QVariant(mag->YNoise().BiasMean()),
+      QVariant(mag->YNoise().StdDev()),
+      QVariant(mag->YNoise().BiasStdDev()),
+      QVariant(mag->YNoise().DynamicBiasStdDev()),
+      QVariant(mag->YNoise().DynamicBiasCorrelationTime()),
+
+      QVariant(mag->ZNoise().Mean()),
+      QVariant(mag->ZNoise().BiasMean()),
+      QVariant(mag->ZNoise().StdDev()),
+      QVariant(mag->ZNoise().BiasStdDev()),
+      QVariant(mag->ZNoise().DynamicBiasStdDev()),
+      QVariant(mag->ZNoise().DynamicBiasCorrelationTime()),
+
+    }), ComponentsModel::RoleNames().key("data"));
+  };
+
+  this->inspector->RegisterComponentCreator(
+      components::Magnetometer::typeId, creator);
+}
+
+/////////////////////////////////////////////////
+Q_INVOKABLE void Magnetometer::OnMagnetometerXNoise(
+    double _mean, double _meanBias, double _stdDev,
+      double _stdDevBias, double _dynamicBiasStdDev,
+      double _dynamicBiasCorrelationTime)
+{
+  UpdateCallback cb =
+      [=](EntityComponentManager &_ecm)
+  {
+    auto comp = _ecm.Component<components::Magnetometer>(
+        this->inspector->Entity());
+    if (comp)
+    {
+      sdf::Magnetometer *mag = comp->Data().MagnetometerSensor();
+      if (mag)
+      {
+        sdf::Noise noise = mag->XNoise();
+
+        setNoise(noise, _mean, _meanBias, _stdDev, _stdDevBias,
+            _dynamicBiasStdDev, _dynamicBiasCorrelationTime);
+
+        mag->SetXNoise(noise);
+      }
+      else
+        ignerr << "Unable to get the magnetometer data.\n";
+    }
+    else
+    {
+      ignerr << "Unable to get the magnetometer component.\n";
+    }
+  };
+  this->inspector->AddUpdateCallback(cb);
+}
+
+/////////////////////////////////////////////////
+Q_INVOKABLE void Magnetometer::OnMagnetometerYNoise(
+    double _mean, double _meanBias, double _stdDev,
+      double _stdDevBias, double _dynamicBiasStdDev,
+      double _dynamicBiasCorrelationTime)
+{
+  UpdateCallback cb =
+      [=](EntityComponentManager &_ecm)
+  {
+    auto comp = _ecm.Component<components::Magnetometer>(
+        this->inspector->Entity());
+    if (comp)
+    {
+      sdf::Magnetometer *mag = comp->Data().MagnetometerSensor();
+      if (mag)
+      {
+        sdf::Noise noise = mag->YNoise();
+
+        setNoise(noise, _mean, _meanBias, _stdDev, _stdDevBias,
+            _dynamicBiasStdDev, _dynamicBiasCorrelationTime);
+
+        mag->SetYNoise(noise);
+      }
+      else
+        ignerr << "Unable to get the magnetometer data.\n";
+    }
+    else
+    {
+      ignerr << "Unable to get the magnetometer component.\n";
+    }
+  };
+  this->inspector->AddUpdateCallback(cb);
+}
+
+/////////////////////////////////////////////////
+Q_INVOKABLE void Magnetometer::OnMagnetometerZNoise(
+    double _mean, double _meanBias, double _stdDev,
+      double _stdDevBias, double _dynamicBiasStdDev,
+      double _dynamicBiasCorrelationTime)
+{
+  UpdateCallback cb =
+      [=](EntityComponentManager &_ecm)
+  {
+    auto comp = _ecm.Component<components::Magnetometer>(
+        this->inspector->Entity());
+    if (comp)
+    {
+      sdf::Magnetometer *mag = comp->Data().MagnetometerSensor();
+      if (mag)
+      {
+        sdf::Noise noise = mag->ZNoise();
+
+        setNoise(noise, _mean, _meanBias, _stdDev, _stdDevBias,
+            _dynamicBiasStdDev, _dynamicBiasCorrelationTime);
+
+        mag->SetZNoise(noise);
+      }
+      else
+        ignerr << "Unable to get the magnetometer data.\n";
+    }
+    else
+    {
+      ignerr << "Unable to get the magnetometer component.\n";
+    }
+  };
+  this->inspector->AddUpdateCallback(cb);
+}
diff --git a/src/gui/plugins/component_inspector/Magnetometer.hh b/src/gui/plugins/component_inspector/Magnetometer.hh
@@ -0,0 +1,85 @@
+/*
+ * 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.
+ *
+*/
+#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_MAGNETOMETER_HH_
+#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_MAGNETOMETER_HH_
+
+#include <ignition/gazebo/gui/GuiSystem.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+  class ComponentInspector;
+
+  /// \brief A class that handles magnetometer changes.
+  class Magnetometer : public QObject
+  {
+    Q_OBJECT
+
+    /// \brief Constructor
+    /// \param[in] _inspector The component inspector.
+    public: Magnetometer(ComponentInspector *_inspector);
+
+    /// \brief This function is called when a user changes values in the
+    /// x noise elements
+    /// \param[in] _mean Mean value
+    /// \param[in] _meanBias Bias mean value
+    /// \param[in] _stdDev Standard deviation value
+    /// \param[in] _stdDevBias Bias standard deviation value
+    /// \param[in] _dynamicBiasStdDev Dynamic bias standard deviation value
+    /// \param[in] _dynamicBiasCorrelationTime Dynamic bias correlation time
+    /// value
+    public: Q_INVOKABLE void OnMagnetometerXNoise(
+        double _mean, double _meanBias, double _stdDev,
+        double _stdDevBias, double _dynamicBiasStdDev,
+        double _dynamicBiasCorrelationTime);
+
+    /// \brief This function is called when a user changes values in the
+    /// y noise elements
+    /// \param[in] _mean Mean value
+    /// \param[in] _meanBias Bias mean value
+    /// \param[in] _stdDev Standard deviation value
+    /// \param[in] _stdDevBias Bias standard deviation value
+    /// \param[in] _dynamicBiasStdDev Dynamic bias standard deviation value
+    /// \param[in] _dynamicBiasCorrelationTime Dynamic bias correlation time
+    /// value
+    public: Q_INVOKABLE void OnMagnetometerYNoise(
+        double _mean, double _meanBias, double _stdDev,
+        double _stdDevBias, double _dynamicBiasStdDev,
+        double _dynamicBiasCorrelationTime);
+
+    /// \brief This function is called when a user changes values in the
+    /// z noise elements
+    /// \param[in] _mean Mean value
+    /// \param[in] _meanBias Bias mean value
+    /// \param[in] _stdDev Standard deviation value
+    /// \param[in] _stdDevBias Bias standard deviation value
+    /// \param[in] _dynamicBiasStdDev Dynamic bias standard deviation value
+    /// \param[in] _dynamicBiasCorrelationTime Dynamic bias correlation time
+    /// value
+    public: Q_INVOKABLE void OnMagnetometerZNoise(
+        double _mean, double _meanBias, double _stdDev,
+        double _stdDevBias, double _dynamicBiasStdDev,
+        double _dynamicBiasCorrelationTime);
+
+    /// \brief Pointer to the component inspector. This is used to add
+    /// update callbacks that modify the ECM.
+    private: ComponentInspector *inspector{nullptr};
+  };
+}
+}
+#endif