5 ➡️ 6 (#1413)

Signed-off-by: Louise Poubel <[email protected]>
gazebosim · Mar 30, 2022 · 4121a5c · 4121a5c
2 parents 0996fe0 + 685a11f
commit 4121a5c
Show file tree

Hide file tree

Showing 17 changed files with 777 additions and 128 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -59,7 +59,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
 # as protobuf could be find transitively by any dependency
 set(protobuf_MODULE_COMPATIBLE TRUE)
 
-ign_find_package(sdformat12 REQUIRED VERSION 12.3)
+ign_find_package(sdformat12 REQUIRED VERSION 12.4)
 set(SDF_VER ${sdformat12_VERSION_MAJOR})
 
 #--------------------------------------

diff --git a/examples/worlds/lights.sdf b/examples/worlds/lights.sdf
@@ -52,6 +52,7 @@
         <quadratic>0.01</quadratic>
       </attenuation>
       <cast_shadows>false</cast_shadows>
+      <visualize>false</visualize>
     </light>
 
     <light type="spot" name="spot">

diff --git a/src/Conversions.cc b/src/Conversions.cc
@@ -570,6 +570,25 @@ msgs::Light ignition::gazebo::convert(const sdf::Light &_in)
   out.set_spot_inner_angle(_in.SpotInnerAngle().Radian());
   out.set_spot_outer_angle(_in.SpotOuterAngle().Radian());
   out.set_spot_falloff(_in.SpotFalloff());
+
+  {
+    // todo(ahcorde) Use the field is_light_off in light.proto from
+    // Garden on.
+    auto header = out.mutable_header()->add_data();
+    header->set_key("isLightOn");
+    std::string *value = header->add_value();
+    *value = std::to_string(_in.LightOn());
+  }
+
+  {
+    // todo(ahcorde) Use the field visualize_visual in light.proto from
+    // Garden on.
+    auto header = out.mutable_header()->add_data();
+    header->set_key("visualizeVisual");
+    std::string *value = header->add_value();
+    *value = std::to_string(_in.Visualize());
+  }
+
   if (_in.Type() == sdf::LightType::POINT)
     out.set_type(msgs::Light_LightType_POINT);
   else if (_in.Type() == sdf::LightType::SPOT)
@@ -599,6 +618,43 @@ sdf::Light ignition::gazebo::convert(const msgs::Light &_in)
   out.SetSpotInnerAngle(math::Angle(_in.spot_inner_angle()));
   out.SetSpotOuterAngle(math::Angle(_in.spot_outer_angle()));
   out.SetSpotFalloff(_in.spot_falloff());
+
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  bool visualizeVisual = true;
+  for (int i = 0; i < _in.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _in.header().data(i).value_size(); ++j)
+    {
+      if (_in.header().data(i).key() ==
+          "visualizeVisual")
+      {
+        visualizeVisual = ignition::math::parseInt(
+          _in.header().data(i).value(0));
+      }
+    }
+  }
+  out.SetVisualize(visualizeVisual);
+
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  bool isLightOn = true;
+  for (int i = 0; i < _in.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _in.header().data(i).value_size(); ++j)
+    {
+      if (_in.header().data(i).key() ==
+          "isLightOn")
+      {
+        isLightOn = ignition::math::parseInt(
+          _in.header().data(i).value(0));
+      }
+    }
+  }
+  out.SetLightOn(isLightOn);
+
   if (_in.type() == msgs::Light_LightType_POINT)
     out.SetType(sdf::LightType::POINT);
   else if (_in.type() == msgs::Light_LightType_SPOT)

diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/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
 )
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -24,7 +24,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"
@@ -56,8 +55,6 @@
 #include "ignition/gazebo/components/PerformerAffinity.hh"
 #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/RenderEngineGuiPlugin.hh"
 #include "ignition/gazebo/components/RenderEngineServerPlugin.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
@@ -75,6 +72,7 @@
 #include "ignition/gazebo/gui/GuiEvents.hh"
 
 #include "ComponentInspector.hh"
+#include "Pose3d.hh"
 
 namespace ignition::gazebo
 {
@@ -109,31 +107,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, const msgs::Light &_data)
@@ -155,6 +141,36 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
     lightType = 2;
   }
 
+  bool visualizeVisual = true;
+  for (int i = 0; i < _data.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _data.header().data(i).value_size(); ++j)
+    {
+      if (_data.header().data(i).key() ==
+          "visualizeVisual")
+      {
+        visualizeVisual = ignition::math::parseInt(
+          _data.header().data(i).value(0));
+      }
+    }
+  }
+
+  bool isLightOn = true;
+  for (int i = 0; i < _data.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _data.header().data(i).value_size(); ++j)
+    {
+      if (_data.header().data(i).key() ==
+          "isLightOn")
+      {
+        isLightOn = ignition::math::parseInt(
+          _data.header().data(i).value(0));
+      }
+    }
+  }
+
   _item->setData(QString("Light"),
       ComponentsModel::RoleNames().key("dataType"));
   _item->setData(QList({
@@ -178,7 +194,9 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
     QVariant(_data.spot_outer_angle()),
     QVariant(_data.spot_falloff()),
     QVariant(_data.intensity()),
-    QVariant(lightType)
+    QVariant(lightType),
+    QVariant(isLightOn),
+    QVariant(visualizeVisual)
   }), ComponentsModel::RoleNames().key("data"));
 }
 
@@ -454,6 +472,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);
 }
 
 //////////////////////////////////////////////////
@@ -737,12 +758,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::RenderEngineGuiPlugin::typeId)
     {
       auto comp = _ecm.Component<components::RenderEngineGuiPlugin>(
@@ -864,18 +879,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);
     }
     else if (typeId == components::Material::typeId)
     {
@@ -909,6 +916,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)
 {
@@ -998,34 +1012,15 @@ 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,
   double _rDiffuse, double _gDiffuse, double _bDiffuse, double _aDiffuse,
   double _attRange, double _attLinear, double _attConstant,
   double _attQuadratic, bool _castShadows, double _directionX,
   double _directionY, double _directionZ, double _innerAngle,
-  double _outerAngle, double _falloff, double _intensity, int _type)
+  double _outerAngle, double _falloff, double _intensity, int _type,
+  bool _isLightOn, bool _visualizeVisual)
 {
   std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
       [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
@@ -1035,6 +1030,23 @@ void ComponentInspector::OnLight(
   };
 
   ignition::msgs::Light req;
+  {
+    // todo(ahcorde) Use the field is_light_off in light.proto from
+    // Garden on.
+    auto header = req.mutable_header()->add_data();
+    header->set_key("isLightOn");
+    std::string *value = header->add_value();
+    *value = std::to_string(_isLightOn);
+  }
+  {
+    // todo(ahcorde) Use the field visualize_visual in light.proto from
+    // Garden on.
+    auto header = req.mutable_header()->add_data();
+    header->set_key("visualizeVisual");
+    std::string *value = header->add_value();
+    *value = std::to_string(_visualizeVisual);
+  }
+
   req.set_name(this->dataPtr->entityName);
   req.set_id(this->dataPtr->entity);
   ignition::msgs::Set(req.mutable_diffuse(),
@@ -1237,6 +1249,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)