Scale mode - Part2

include/ignition/gazebo/components/Model.hh

@@ -17,7 +17,10 @@
 #ifndef IGNITION_GAZEBO_COMPONENTS_MODEL_HH_
 #define IGNITION_GAZEBO_COMPONENTS_MODEL_HH_
 
+#include <string>
+
 #include <sdf/Model.hh>
+#include <sdf/Root.hh>
 
 #include <ignition/gazebo/components/Factory.hh>
 #include <ignition/gazebo/components/Component.hh>
@@ -29,14 +32,64 @@ namespace gazebo
 {
 // Inline bracket to help doxygen filtering.
 inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace serializers
+{
+ class SdfModelSerializer
+ {
+ /// \brief Serialization for `sdf::Model`.
+ /// \param[in] _out Output stream.
+ /// \param[in] _time Model to stream
+ /// \return The stream.
+ public: static std::ostream &Serialize(std::ostream &_out,
+ const sdf::Model &_model)
+ {
+ sdf::ElementPtr modelElem = _model.Element();
+ if (!modelElem)
+ {
+ ignerr << "Unable to serialize sdf::Model" << std::endl;
+ return _out;
+ }
+
+ _out << "<?xml version=\"1.0\" ?>"
+ << "<sdf version='" << SDF_PROTOCOL_VERSION << "'>"
+ << modelElem->ToString("")
+ << "</sdf>";
+ return _out;
+ }
+
+ /// \brief Deserialization for `sdf::Model`.
+ /// \param[in] _in Input stream.
+ /// \param[out] _model Model to populate
+ /// \return The stream.
+ public: static std::istream &Deserialize(std::istream &_in,
+ sdf::Model &_model)
+ {
+ sdf::Root root;
+ std::string sdf(std::istreambuf_iterator<char>(_in), {});
+
+ sdf::Errors errors = root.LoadSdfString(sdf);
+ if (!errors.empty())
+ {
+ ignerr << "Unable to unserialize sdf::Model" << std::endl;
+ return _in;
+ }
+
+ _model.Load(root.Element()->GetElement("model"));
+ return _in;
+ }
+ };
+}
+
 namespace components
 {
  /// \brief A component that identifies an entity as being a model.
  using Model = Component<NoData, class ModelTag>;
  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Model", Model)
 
  /// \brief A component that holds the model's SDF DOM
- using ModelSdf = Component<sdf::Model, class ModelTag>;
+ using ModelSdf = Component<sdf::Model,
+ class ModelTag,
+ serializers::SdfModelSerializer>;
  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ModelSdf", ModelSdf)
 }
 }

include/ignition/gazebo/rendering/RenderUtil.hh

@@ -158,6 +158,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
  /// \param[in] _active True if active.
  public: void SetTransformActive(bool _active);
 
+ /// Set the scale of a given wirebox.
+ /// \param[in] _entityId The entity Id.
+ /// \param[in] _scale Scale vector in absolute coordinates.
+ public: void SetWireBoxScale(const Entity &_entityId,
+ const math::Vector3d &_scale);
+
  /// \brief Private data pointer.
  private: std::unique_ptr<RenderUtilPrivate> dataPtr;
  };

src/gui/plugins/scene3d/Scene3D.cc

@@ -23,6 +23,7 @@
 #include <limits>
 #include <map>
 #include <mutex>
+#include <queue>
 #include <sstream>
 #include <string>
 #include <utility>
@@ -1341,6 +1342,52 @@ void IgnRenderer::XYZConstraint(math::Vector3d &_axis)
  }
 }
 
+/////////////////////////////////////////////////
+rendering::VisualPtr IgnRenderer::ContainsSimpleShape(
+ const rendering::NodePtr &_node) const
+{
+ std::queue<rendering::NodePtr> q;
+
+ // Adding the root node.
+ q.push(_node);
+
+ while (!q.empty())
+ {
+ // Process the next node.
+ auto n = q.front();
+ q.pop();
+ if (n)
+ {
+ auto v = std::dynamic_pointer_cast<ignition::rendering::Visual>(n);
+ if (v)
+ {
+ try
+ {
+ int userData = std::get<int>(v->UserData("geometry-type"));
+
+ /// \TODO(anyone) Consider sdf::GeometryType::CAPSULE and
+ /// sdf::GeometryType::ELLIPSOID in Edifice.
+ if (userData == static_cast<int>(sdf::GeometryType::BOX) ||
+ userData == static_cast<int>(sdf::GeometryType::CYLINDER) ||
+ userData == static_cast<int>(sdf::GeometryType::SPHERE))
+ {
+ return v;
+ }
+ }
+ catch (const std::bad_variant_access& ex)
+ {
+ }
+ }
+
+ // Add all children to the queue to be considered.
+ for (auto i = 0u; i < n->ChildCount(); ++i)
+ q.push(n->ChildByIndex(i));
+ }
+ }
+
+ return nullptr;
+}
+
 /////////////////////////////////////////////////
 void IgnRenderer::HandleMouseTransformControl()
 {
@@ -1607,10 +1654,20 @@ void IgnRenderer::HandleMouseTransformControl()
  else if (this->dataPtr->transformControl.Mode() ==
  rendering::TransformMode::TM_SCALE)
  {
+ // Check if the model that we're trying to scale looks like a simple shape
+ auto node = this->dataPtr->transformControl.Node();
+ auto v = std::dynamic_pointer_cast<ignition::rendering::Visual>(node);
+ rendering::VisualPtr visualSimple = this->ContainsSimpleShape(node);
+ if (!visualSimple)
+ return;
+
+ int userData = std::get<int>(v->UserData("geometry-type"));
+ sdf::GeometryType geomType = static_cast<sdf::GeometryType>(userData);
+
  this->XYZConstraint(axis);
  // note: scaling is limited to local space
  math::Vector3d scale =
-  this->dataPtr->transformControl.ScaleFrom2d(axis, start, end);
+ this->dataPtr->transformControl.ScaleFrom2d(axis, start, end);
  if (this->dataPtr->keyEvent.Control())
  {
  math::Vector3d snapVals = this->ScaleSnap();
@@ -1624,14 +1681,54 @@ void IgnRenderer::HandleMouseTransformControl()
 
  SnapPoint(scale, snapVals);
  }
+
+ // Apply geometry constaints to the scaling.
+ if (geomType == sdf::GeometryType::SPHERE)
+ {
+ if (axis.X() > 0)
+ {
+ scale.Y(scale.X());
+ scale.Z(scale.X());
+ }
+ else if (axis.Y() > 0)
+ {
+ scale.X(scale.Y());
+ scale.Z(scale.Y());
+ }
+ else if (axis.Z() > 0)
+ {
+ scale.X(scale.Z());
+ scale.Y(scale.Z());
+ }
+ }
+ else if (geomType == sdf::GeometryType::CYLINDER)
+ {
+ if (axis.X() > 0)
+ {
+ scale.Y(scale.X());
+ }
+ else if (axis.Y() > 0)
+ {
+ scale.X(scale.Y());
+ }
+ }
+
+ // Scale.
  this->dataPtr->transformControl.Scale(scale);
+
+ // Update the bounding box.
+ if (v)
+ {
+ Entity entityId = std::get<int>(v->UserData("gazebo-entity"));
+ auto s = this->dataPtr->transformControl.Node()->LocalScale();
+ this->dataPtr->renderUtil.SetWireBoxScale(entityId, s);
+ }
  }
  this->dataPtr->drag = 0;
  this->dataPtr->mouseDirty = false;
  }
 }
 
-
 /////////////////////////////////////////////////
 void IgnRenderer::HandleMouseViewControl()
 {

src/gui/plugins/scene3d/Scene3D.hh

@@ -439,6 +439,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
  /// when the deselection is initiated from this plugin.
  private: void DeselectAllEntities(bool _sendEvent);
 
+ /// \brief Helper function that traverses the node tree looking for any
+ /// child with the "geometry-type" user data. If found, the function also
+ /// checks if the value is a simple shape (box, cylinder or sphere).
+ /// \param[in] _node Root node.
+ /// \return The visual pointer to the simple shape if found or nullptr
+ /// otherwise.
+ private: rendering::VisualPtr ContainsSimpleShape(
+ const rendering::NodePtr &_node) const;
+
  /// \brief Signal fired when context menu event is triggered
  signals: void ContextMenuRequested(QString _entity);
 

src/gui/plugins/transform_control/TransformControl.cc

@@ -245,6 +245,10 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event)
  {
  this->activateRotate();
  }
+ else if (keyEvent->key() == Qt::Key_S)
+ {
+ this->activateScale();
+ }
  }
  else if (_event->type() == QEvent::KeyRelease)
  {

src/gui/plugins/transform_control/TransformControl.hh

@@ -142,6 +142,9 @@ namespace gazebo
  /// \brief Notify that rotation has been activated
  signals: void activateRotate();
 
+ /// \brief Notify that scale has been activated
+ signals: void activateScale();
+
  /// \internal
  /// \brief Pointer to private data.
  private: std::unique_ptr<TransformControlPrivate> dataPtr;

src/gui/plugins/transform_control/TransformControl.qml

@@ -29,10 +29,10 @@ ToolBar {
  Layout.minimumHeight: 100
 
  // TODO(anyone) enable scale button when support is added in ign-physics
- // function activateScale() {
- //  scale.checked = true;
- //  TransformControl.OnMode("scale");
- // }
+ function activateScale() {
+ scale.checked = true;
+ TransformControl.OnMode("scale");
+ }
 
  property color snapTitle: (Material.theme == Material.Light) ?
  Material.color(Material.Grey, Material.Shade900) :
@@ -106,6 +106,11 @@ ToolBar {
  onActivateRotate: activateRotate();
  }
 
+ Connections {
+ target: TransformControl
+ onActivateScale: activateScale();
+ }
+
  RowLayout {
  spacing: 2
  ToolButton {
@@ -219,10 +224,6 @@ ToolBar {
  TransformControl.OnMode("rotate")
  }
  }
- // TODO(anyone) enable scale snap values below when support is added in ign-physics
- // Also be sure to replace the placeholder 0's in all of the `OnSnapUpdate` calls in
- // this file to xScaleEntry.value, yScaleEntry.value, and zScaleEntry.value, respectively
- /*
  ToolButton {
  id: scale
  text: "S"
@@ -260,7 +261,6 @@ ToolBar {
  TransformControl.OnMode("scale")
  }
  }
- */
 
  ToolButton {
  id: snap

src/rendering/RenderUtil.cc

@@ -2058,6 +2058,39 @@ void RenderUtilPrivate::LowlightNode(const rendering::NodePtr &_node)
  }
 }
 
+/////////////////////////////////////////////////
+void RenderUtil::SetWireBoxScale(const Entity &_entityId,
+ const math::Vector3d &_scale)
+{
+ /// \TODO(anyone) Consider to use this->dataPtr->sceneManager.NodeById()
+ /// when WireBoxes are tracked in the scene manager.
+ if (this->dataPtr->wireBoxes.find(_entityId) ==
+ this->dataPtr->wireBoxes.end())
+ {
+ ignerr << "Trying to scale a wirebox with unknown id [" << _entityId
+ << "]" << std::endl;
+ return;
+ }
+
+ ignition::rendering::WireBoxPtr wireBox = this->dataPtr->wireBoxes[_entityId];
+ if (!wireBox)
+ {
+ ignerr << "Null wirebox associated to id [" << _entityId
+ << "]" << std::endl;
+ return;
+ }
+
+ auto visParent = wireBox->Parent();
+ if (!visParent)
+ {
+ ignerr << "Trying to scale a wirebox with null parent [" << _entityId
+ << "]" << std::endl;
+ return;
+ }
+
+ visParent->SetLocalScale(_scale);
+}
+
 /////////////////////////////////////////////////
 void RenderUtil::ViewCollisions(const Entity &_entity)
 {

src/rendering/SceneManager.cc

@@ -265,6 +265,11 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id,
  rendering::VisualPtr visualVis = this->dataPtr->scene->CreateVisual(name);
  visualVis->SetUserData("gazebo-entity", static_cast<int>(_id));
  visualVis->SetUserData("pause-update", static_cast<int>(0));
+
+ auto geometry = _visual.Geom();
+ if (geometry)
+ visualVis->SetUserData("geometry-type", static_cast<int>(geometry->Type()));
+
  visualVis->SetLocalPose(_visual.RawPose());
 
  if (_visual.HasLaserRetro())