diff --git a/include/ignition/gazebo/components/Model.hh b/include/ignition/gazebo/components/Model.hh index 7fdc7014ce..b1dd9f9f96 100644 --- a/include/ignition/gazebo/components/Model.hh +++ b/include/ignition/gazebo/components/Model.hh @@ -17,7 +17,10 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_MODEL_HH_ #define IGNITION_GAZEBO_COMPONENTS_MODEL_HH_ +#include + #include +#include #include #include @@ -29,6 +32,54 @@ 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 << "" + << "" + << modelElem->ToString("") + << ""; + 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(_in), {}); + + sdf::Errors errors = root.LoadSdfString(sdf); + if (!root.Element()->HasElement("model")) + { + 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. @@ -36,7 +87,9 @@ namespace components IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Model", Model) /// \brief A component that holds the model's SDF DOM - using ModelSdf = Component; + using ModelSdf = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ModelSdf", ModelSdf) } } diff --git a/test/integration/components.cc b/test/integration/components.cc index c341dfcc0d..c306ca18c1 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -30,6 +30,7 @@ #include #include #include +#include #include #include "ignition/gazebo/components/Actor.hh" @@ -1098,6 +1099,66 @@ TEST_F(ComponentsTest, Model) comp3.Deserialize(istr); } +///////////////////////////////////////////////// +TEST_F(ComponentsTest, ModelSdf) +{ + std::ostringstream stream; + std::string version = SDF_VERSION; + stream + << "" + << "" + << " " + << " " + << " 0.001" + << " 1.0" + << " " + << " " + << " " + << " " + << " " + << " " + << " 0.1 0 0 0 0 0" + << " 0.2 0.3 0.4 1" + << " 0.3 0.4 0.5 1" + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); + + // model + EXPECT_TRUE(sdfParsed->Root()->HasElement("world")); + sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); + EXPECT_TRUE(worldElem->HasElement("model")); + sdf::ElementPtr modelElem = worldElem->GetElement("model"); + EXPECT_TRUE(modelElem->HasAttribute("name")); + EXPECT_EQ(modelElem->Get("name"), "my_model"); + + sdf::Model model; + model.Load(modelElem); + EXPECT_EQ("my_model", model.Name()); + + // Create components + auto comp1 = components::ModelSdf(model); + components::ModelSdf comp2; + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + comp2.Deserialize(istr); + + EXPECT_EQ("my_model", comp2.Data().Name()); + EXPECT_EQ(1u, comp2.Data().LinkCount()); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, Name) {