Support setting pose on links, visuals, collision via the GUI

src/ModelCommandAPI_TEST.cc

@@ -462,7 +462,32 @@ TEST(ModelCommandAPI, MagnetometerSensor)
       "  - Parent: magnetometer_model [8]\n"
       "  - Pose [ XYZ (m) ] [ RPY (rad) ]:\n"
       "    [0.000000 0.000000 0.000000]\n"
-      "    [0.000000 0.000000 0.000000]\n";
+      "    [0.000000 0.000000 0.000000]\n"
+      "  - X-axis noise:\n"
+      "    - Mean: 0\n"
+      "    - Bias mean: 0\n"
+      "    - Standard deviation: 0\n"
+      "    - Bias standard deviation: 0\n"
+      "    - Precision: 0\n"
+      "    - Dynamic bias standard deviation: 0\n"
+      "    - Dynamic bias correlation time: 0\n"
+      "  - Y-axis noise:\n"
+      "    - Mean: 0\n"
+      "    - Bias mean: 0\n"
+      "    - Standard deviation: 0\n"
+      "    - Bias standard deviation: 0\n"
+      "    - Precision: 0\n"
+      "    - Dynamic bias standard deviation: 0\n"
+      "    - Dynamic bias correlation time: 0\n"
+      "  - Z-axis noise:\n"
+      "    - Mean: 0\n"
+      "    - Bias mean: 0\n"
+      "    - Standard deviation: 0\n"
+      "    - Bias standard deviation: 0\n"
+      "    - Precision: 0\n"
+      "    - Dynamic bias standard deviation: 0\n"
+      "    - Dynamic bias correlation time: 0\n";
+
       EXPECT_EQ(expectedOutput, output);
   }
 }

src/gui/plugins/component_inspector/CMakeLists.txt

@@ -8,6 +8,7 @@ gz_add_gui_plugin(ComponentInspector
     Lidar.cc
     Magnetometer.cc
     ModelEditor.cc
+    Pose3d.cc
     Types.cc
   QT_HEADERS
     AirPressure.hh
@@ -18,5 +19,6 @@ gz_add_gui_plugin(ComponentInspector
     Lidar.hh
     Magnetometer.hh
     ModelEditor.hh
+    Pose3d.hh
     Types.hh
 )

src/gui/plugins/component_inspector/ComponentInspector.cc

@@ -61,7 +61,7 @@
 #include "ignition/gazebo/components/Physics.hh"
 #include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
 #include "ignition/gazebo/components/Pose.hh"
-#include "ignition/gazebo/components/PoseCmd.hh"
+#include "ignition/gazebo/components/Recreate.hh"
 #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh"
 #include "ignition/gazebo/components/RenderEngineServerPlugin.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
@@ -77,6 +77,7 @@
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/gui/GuiEvents.hh"
+#include "ignition/gazebo/Util.hh"
 
 #include "AirPressure.hh"
 #include "Altimeter.hh"
@@ -86,6 +87,7 @@
 #include "Lidar.hh"
 #include "Magnetometer.hh"
 #include "ModelEditor.hh"
+#include "Pose3d.hh"
 
 namespace ignition::gazebo
 {
@@ -148,6 +150,9 @@ namespace ignition::gazebo
     /// \brief Magnetometer inspector elements
     public: std::unique_ptr<ignition::gazebo::Magnetometer> magnetometer;
 
+    /// \brief Pose inspector elements
+    public: std::unique_ptr<ignition::gazebo::Pose3d> pose3d;
+
     /// \brief Set of callbacks to execute during the Update function.
     public: std::vector<
             std::function<void(EntityComponentManager &)>> updateCallbacks;
@@ -160,25 +165,6 @@ namespace ignition::gazebo
 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, const msgs::Light &_data)
@@ -519,6 +505,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
 
   // Create the magnetometer
   this->dataPtr->magnetometer = std::make_unique<Magnetometer>(this);
+
+  // Create the pose3d
+  this->dataPtr->pose3d = std::make_unique<Pose3d>(this);
 }
 
 //////////////////////////////////////////////////
@@ -820,12 +809,6 @@ void ComponentInspector::Update(const UpdateInfo &_info,
       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::RenderEngineGuiPlugin::typeId)
     {
       auto comp = _ecm.Component<components::RenderEngineGuiPlugin>(
@@ -953,13 +936,6 @@ void ComponentInspector::Update(const UpdateInfo &_info,
       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());
-    }
     else if (typeId == components::Material::typeId)
     {
       auto comp = _ecm.Component<components::Material>(this->dataPtr->entity);
@@ -1123,26 +1099,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::OnLight(
   double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular,

src/gui/plugins/component_inspector/ComponentInspector.hh

@@ -26,7 +26,6 @@
 #include <sdf/Physics.hh>
 #include <sdf/Joint.hh>
 
-#include <ignition/math/Pose3.hh>
 #include <ignition/math/Vector3.hh>
 
 #include <ignition/gazebo/components/Component.hh>
@@ -70,12 +69,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 light data.
   /// \param[in] _item Item whose data will be set.
   /// \param[in] _data Data to set.
@@ -244,16 +237,6 @@ 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 Callback in Qt thread when specular changes.
     /// \param[in] _rSpecular specular red
     /// \param[in] _gSpecular specular green

src/gui/plugins/component_inspector/ComponentInspector.qml

@@ -113,13 +113,6 @@ Rectangle {
     return getDecimals(_widgetId.width)
   }
 
-  /**
-   * Forward pose changes to C++
-   */
-  function onPose(_x, _y, _z, _roll, _pitch, _yaw) {
-    ComponentInspector.OnPose(_x, _y, _z, _roll, _pitch, _yaw)
-  }
-
   /**
    * Forward light changes to C++
    */

src/gui/plugins/component_inspector/Pose3d.cc

@@ -0,0 +1,89 @@
+/*
+ * 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 <ignition/math/Pose3.hh>
+
+#include <ignition/common/Console.hh>
+#include <ignition/gazebo/Util.hh>
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Recreate.hh"
+#include <ignition/gazebo/EntityComponentManager.hh>
+
+#include "ComponentInspector.hh"
+#include "Pose3d.hh"
+#include "Types.hh"
+
+using namespace ignition;
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+Pose3d::Pose3d(ComponentInspector *_inspector)
+{
+  _inspector->Context()->setContextProperty("Pose3dImpl", this);
+  this->inspector = _inspector;
+
+  ComponentCreator creator =
+    [=](EntityComponentManager &_ecm, Entity _entity, QStandardItem *_item)
+  {
+    auto comp = _ecm.Component<components::Pose>(_entity);
+    if (nullptr == _item || nullptr == comp)
+      return;
+    math::Pose3d pose = comp->Data();
+
+    _item->setData(QString("Pose3d"),
+        ComponentsModel::RoleNames().key("dataType"));
+    _item->setData(QList({
+      QVariant(pose.Pos().X()),
+      QVariant(pose.Pos().Y()),
+      QVariant(pose.Pos().Z()),
+      QVariant(pose.Rot().Roll()),
+      QVariant(pose.Rot().Pitch()),
+      QVariant(pose.Rot().Yaw())
+    }), ComponentsModel::RoleNames().key("data"));
+
+    // Notify the qml component when the pose has changed, such as when
+    // a new entity is selected.
+    if (this->currentPose != pose)
+    {
+      this->currentPose = pose;
+      this->poseChanged();
+    }
+  };
+
+  this->inspector->RegisterComponentCreator(components::Pose::typeId, creator);
+}
+
+/////////////////////////////////////////////////
+Q_INVOKABLE void Pose3d::PoseUpdate(
+    double _x, double _y, double _z, double _roll, double _pitch, double _yaw)
+{
+  ignition::gazebo::UpdateCallback cb =
+      [=](EntityComponentManager &_ecm)
+  {
+    auto comp = _ecm.Component<components::Pose>(this->inspector->GetEntity());
+    if (comp)
+    {
+      comp->Data().Set(_x, _y, _z, _roll, _pitch, _yaw);
+      // Recreate the model. Changing link poses can cause the kinematic
+      // tree to need regreneration.
+      Entity modelEntity = topLevelModel(this->inspector->GetEntity(), _ecm);
+      _ecm.CreateComponent(modelEntity, components::Recreate());
+    }
+    else
+      ignerr << "Unable to get the pose component.\n";
+  };
+  this->inspector->AddUpdateCallback(cb);
+}

src/gui/plugins/component_inspector/Pose3d.hh

@@ -0,0 +1,61 @@
+/*
+ * 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_POSE3D_HH_
+#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_
+
+#include <ignition/gazebo/gui/GuiSystem.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+  class ComponentInspector;
+
+  /// \brief A class that handles Pose3d changes.
+  class Pose3d : public QObject
+  {
+    Q_OBJECT
+
+    /// \brief Constructor
+    /// \param[in] _inspector The component inspector.
+    public: explicit Pose3d(ComponentInspector *_inspector);
+
+    /// \brief Handle pose updates from the GUI.
+    /// \param[in] _x New x value.
+    /// \param[in] _y New y value.
+    /// \param[in] _z New z value.
+    /// \param[in] _roll New roll value.
+    /// \param[in] _pitch New pitch value.
+    /// \param[in] _yaw New yaw value.
+    public: Q_INVOKABLE void PoseUpdate(
+                double _x, double _y, double _z, double _roll,
+                double _pitch, double _yaw);
+
+    /// \brief Notify that simulation paused state has changed.
+    signals: void poseChanged();
+
+    /// \brief Pointer to the component inspector. This is used to add
+    /// update callbacks that modify the ECM.
+    private: ComponentInspector *inspector{nullptr};
+
+    /// \brief Keep track of current pose so that we can update the pose
+    /// spin boxes correctly.
+    private: math::Pose3d currentPose;
+  };
+}
+}
+#endif