diff --git a/include/ignition/gazebo/components/AirPressureSensor.hh b/include/ignition/gazebo/components/AirPressureSensor.hh index 7ac0308234..02f825ffd5 100644 --- a/include/ignition/gazebo/components/AirPressureSensor.hh +++ b/include/ignition/gazebo/components/AirPressureSensor.hh @@ -34,7 +34,8 @@ namespace components { /// \brief A component type that contains an air pressure sensor, /// sdf::AirPressure, information. - using AirPressureSensor = Component; + using AirPressureSensor = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.AirPressureSensor", AirPressureSensor) } diff --git a/include/ignition/gazebo/components/Altimeter.hh b/include/ignition/gazebo/components/Altimeter.hh index 0cf146c767..2aa05827f0 100644 --- a/include/ignition/gazebo/components/Altimeter.hh +++ b/include/ignition/gazebo/components/Altimeter.hh @@ -34,7 +34,8 @@ namespace components { /// \brief A component type that contains an altimeter sensor, /// sdf::Altimeter, information. - using Altimeter = Component; + using Altimeter = + Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Altimeter", Altimeter) } } diff --git a/include/ignition/gazebo/components/Camera.hh b/include/ignition/gazebo/components/Camera.hh index 06459b585c..b964988692 100644 --- a/include/ignition/gazebo/components/Camera.hh +++ b/include/ignition/gazebo/components/Camera.hh @@ -21,6 +21,7 @@ #include #include +#include #include namespace ignition @@ -33,7 +34,8 @@ namespace components { /// \brief A component type that contains a camera sensor, /// sdf::Camera, information. - using Camera = Component; + using Camera = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Camera", Camera) } } diff --git a/include/ignition/gazebo/components/Component.hh b/include/ignition/gazebo/components/Component.hh index 1c2e7bae87..7080adf36f 100644 --- a/include/ignition/gazebo/components/Component.hh +++ b/include/ignition/gazebo/components/Component.hh @@ -29,223 +29,225 @@ #include #include -/// \brief Helper trait to determine if a type is shared_ptr or not -template struct IsSharedPtr: - std::false_type +namespace ignition { -}; - -/// \brief Helper trait to determine if a type is shared_ptr or not -template struct IsSharedPtr>: - std::true_type +namespace gazebo { -}; - -/// \brief Helper template to call stream operators only on types that support -/// them. -/// This version is called for types that have operator<< -/// \tparam DataType Type on which the operator will be called. -/// \tparam Identifier Unique identifier for the component class. -/// \tparam Stream Type used to check if component has operator<< -/// \param[in] _out Out stream. -/// \param[in] _data Data to be serialized. -template() << std::declval()), - typename std::enable_if< - !IsSharedPtr::value && - std::is_convertible::value, - int>::type = 0> -std::ostream &toStream(std::ostream &_out, DataType const &_data) +// namespace ignition +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace traits { - _out << _data; - return _out; -} + /// \brief Helper trait to determine if a type is shared_ptr or not + template + struct IsSharedPtr : std::false_type + { + }; -/// \brief Helper template to call stream operators only on types that support -/// them. -/// This version is called for types that are pointers to types that have -/// operator<< -/// \tparam DataType Type on which the operator will be called. -/// \tparam Identifier Unique identifier for the component class. -/// \tparam Stream Type used to check if component has operator<< -/// \param[in] _out Out stream. -/// \param[in] _data Data to be serialized. -template() << std::declval< - typename DataType::element_type const &>()), - typename std::enable_if< - IsSharedPtr::value && - std::is_convertible::value, - int>::type = 0> -std::ostream &toStream(std::ostream &_out, DataType const &_data) -{ - _out << *_data; - return _out; -} + /// \brief Helper trait to determine if a type is shared_ptr or not + template + struct IsSharedPtr> : std::true_type + { + }; -/// \brief Helper template to call stream operators only on types that support -/// them. -/// This version is called for types that don't have operator<< -/// \tparam DataType Type on which the operator will be called. -/// \tparam Identifier Unique identifier for the component class. -/// \tparam Ignored All other template parameters are ignored. -/// \param[in] _out Out stream. -/// \param[in] _data Data to be serialized. -template -std::ostream &toStream(std::ostream &_out, DataType const &, - Ignored const &..., ...) -{ - static bool warned{false}; - if (!warned) + /// \brief Type trait that determines if a operator<< is defined on `Stream` + /// and `DataType`, i.e, it checks if the function + /// `Stream& operator<<(Stream&, const DataType&)` exists. + /// Example: + /// \code + /// constexpr bool isDoubleOutStreamable = + /// IsOutStreamable::value + /// \endcode + template + class IsOutStreamable { - ignwarn << "Trying to serialize component with data type [" - << typeid(DataType).name() << "], which doesn't have " - << "`operator<<`. Component will not be serialized." << std::endl; - warned = true; - } - return _out; -} + private: template + static auto Test(int _test) + -> decltype(std::declval() + << std::declval(), std::true_type()); -/// \brief Helper template to call extract operators only on types that support -/// them. -/// This version is called for types that have operator>> -/// \tparam DataType Type on which the operator will be called. -/// \tparam Identifier Unique identifier for the component class. -/// \tparam Stream Type used to check if component has operator>> -/// \param[in] _in In stream. -/// \param[in] _data Data resulting from deserialization. -template() >> std::declval()), - typename std::enable_if< - !IsSharedPtr::value && - std::is_convertible::value, - int>::type = 0> -std::istream &fromStream(std::istream &_in, DataType &_data) -{ - _in >> _data; - return _in; -} + private: template + static auto Test(...) -> std::false_type; -/// \brief Helper template to call stream operators only on types that support -/// them. -/// This version is called for types that are pointers to types that have -/// operator>> -/// \tparam DataType Type on which the operator will be called. -/// \tparam Identifier Unique identifier for the component class. -/// \tparam Stream Type used to check if component has operator<< -/// \param[in] _out Out stream. -/// \param[in] _data Data to be serialized. -template() >> std::declval< - typename DataType::element_type &>()), - typename std::enable_if< - IsSharedPtr::value && - std::is_convertible::value, - int>::type = 0> -std::istream &fromStream(std::istream &_in, DataType &_data) -{ - _in >> *_data; - return _in; + public: static constexpr bool value = // NOLINT + decltype(Test(true))::value; + }; + + /// \brief Type trait that determines if a operator>> is defined on `Stream` + /// and `DataType`, i.e, it checks if the function + /// `Stream& operator>>(Stream&, DataType&)` exists. + /// Example: + /// \code + /// constexpr bool isDoubleInStreamable = + /// IsInStreamable::value + /// \endcode + /// + template + class IsInStreamable + { + private: template + static auto Test(int _test) + -> decltype(std::declval() >> std::declval(), + std::true_type()); + + private: template + static auto Test(...) -> std::false_type; + + public: static constexpr bool value = // NOLINT + decltype(Test(0))::value; + }; } -/// \brief Helper template to call extract operators only on types that support -/// them. -/// \tparam DataType Type on which the operator will be called. -/// \tparam Identifier Unique identifier for the component class. -/// \tparam Ignored All other template parameters are ignored. -/// This version is called for types that don't have operator>> -/// \param[in] _in In stream. -/// \param[in] _data Data resulting from deserialization. -template -std::istream &fromStream(std::istream &_in, DataType const &, - Ignored const &..., ...) +namespace serializers { - static bool warned{false}; - if (!warned) + /// \brief Default serializer template to call stream operators only on types + /// that support them. If the stream operator is not available, a warning + /// message is printed. + /// \tparam DataType Type on which the operator will be called. + template + class DefaultSerializer { - ignwarn << "Trying to deserialize component with data type [" - << typeid(DataType).name() << "], which doesn't have " - << "`operator>>`. Component will not be deserialized." << std::endl; - warned = true; - } - return _in; + /// Serialization + public: static std::ostream &Serialize(std::ostream &_out, + const DataType &_data) + { + // cppcheck-suppress syntaxError + if constexpr (traits::IsSharedPtr::value) // NOLINT + { + if constexpr (traits::IsOutStreamable::value) + { + _out << *_data; + } + else + { + static bool warned{false}; + if (!warned) + { + ignwarn << "Trying to serialize component with data type [" + << typeid(DataType).name() << "], which doesn't have " + << "`operator<<`. Component will not be serialized." + << std::endl; + warned = true; + } + } + } + else if constexpr (traits::IsOutStreamable::value) + { + _out << _data; + } + else + { + static bool warned{false}; + if (!warned) + { + ignwarn << "Trying to serialize component with data type [" + << typeid(DataType).name() << "], which doesn't have " + << "`operator<<`. Component will not be serialized." + << std::endl; + warned = true; + } + } + return _out; + } + + /// \brief Deserialization + /// \param[in] _in In stream. + /// \param[in] _data Data resulting from deserialization. + public: static std::istream &Deserialize(std::istream &_in, + DataType &_data) + { + if constexpr (traits::IsSharedPtr::value) + { + if constexpr (traits::IsInStreamable::value) + { + _in >> *_data; + } + else + { + static bool warned{false}; + if (!warned) + { + ignwarn << "Trying to deserialize component with data type [" + << typeid(DataType).name() << "], which doesn't have " + << "`operator>>`. Component will not be deserialized." + << std::endl; + warned = true; + } + } + } + else if constexpr (traits::IsInStreamable::value) + { + _in >> _data; + } + else + { + static bool warned{false}; + if (!warned) + { + ignwarn << "Trying to deserialize component with data type [" + << typeid(DataType).name() << "], which doesn't have " + << "`operator>>`. Component will not be deserialized." + << std::endl; + warned = true; + } + } + return _in; + } + }; } -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief Convenient type to be used by components that don't wrap any data. /// I.e. they act as tags and their presence is enough to infer something /// about the entity. using NoData = std::add_lvalue_reference; +} - /// \brief Base class for all components. - class BaseComponent +namespace serializers +{ + /// \brief Specialization of DefaultSerializer for NoData + template<> class DefaultSerializer { - /// \brief Default constructor. - public: BaseComponent() = default; - - /// \brief Default destructor. - public: virtual ~BaseComponent() = default; - - /// \brief Stream insertion operator. It exposes the component's serialized - /// state which can be recreated by `operator>>`. - /// - /// \internal This function is called when using the base class, even if - /// the component can be casted to a derived class. - /// - /// \param[in] _out Output stream. - /// \param[in] _component The component to be streamed. - /// \return The stream. - public: friend std::ostream &operator<<( - std::ostream &_out, const BaseComponent &_component) + public: static std::ostream &Serialize(std::ostream &_out) { - _component.Serialize(_out); + _out << "-"; return _out; } - /// \brief Stream extraction operator. It parses the component's serialized - /// state which is created by `operator<<`. - /// - /// \internal This function is called when using the base class, even if - /// the component can be casted to a derived class. - /// - /// \param[in] _in Input stream. - /// \param[in] _component The component to be populated. - /// \return The stream. - public: friend std::istream &operator>>( - std::istream &_in, BaseComponent &_component) + public: static std::istream &Deserialize(std::istream &_in) { - _component.Deserialize(_in); return _in; } + }; +} + +namespace components +{ + /// \brief Base class for all components. + class BaseComponent + { + /// \brief Default constructor. + public: BaseComponent() = default; + + /// \brief Default destructor. + public: virtual ~BaseComponent() = default; /// \brief Fills a stream with a serialized version of the component. /// By default, it will leave the stream empty. Derived classes should /// override this function to support serialization. /// - /// \internal This function is used by `operator<<`, which can't be - /// overridden by derived classes. - /// /// \param[in] _out Out stream. - protected: virtual void Serialize(std::ostream &/*_out*/) const + public: virtual void Serialize(std::ostream &/*_out*/) const { static bool warned{false}; if (!warned) { - ignwarn << "Trying to serialize copmponent of type [" << this->TypeId() + ignwarn << "Trying to serialize component of type [" << this->TypeId() << "], which hasn't implemented the `Serialize` function. " - << "Component will not be serialized." - << std::endl; + << "Component will not be serialized." << std::endl; warned = true; } }; @@ -254,16 +256,13 @@ namespace components /// By default, it will do nothing. Derived classes should /// override this function to support deserialization. /// - /// \internal This function is used by `operator>>`, which can't be - /// overridden by derived classes. - /// /// \param[in] _in In stream. - protected: virtual void Deserialize(std::istream &/*_in*/) + public: virtual void Deserialize(std::istream &/*_in*/) { static bool warned{false}; if (!warned) { - ignwarn << "Trying to deserialize copmponent of type [" + ignwarn << "Trying to deserialize component of type [" << this->TypeId() << "], which hasn't implemented the " << "`Deserialize` function. Component will not be deserialized." << std::endl; @@ -284,58 +283,48 @@ namespace components /// aliases can be used to create new components. However the type does not /// need to be defined anywhere /// eg. + /// \code /// using Static = Component; + /// \endcode /// /// Note, however, that this scheme does not have a mechanism to stop someone /// accidentally defining another component that wraps a bool as such: + /// \code /// using AnotherComp = Component; + /// \endcode /// In this case, Static and AnotherComp are exactly the same types and would /// not be differentiable by the EntityComponentManager. /// + /// A third template argument can be passed to Component to specify the + /// serializer class to use. If this argument is not provided, Component will + /// use DefaultSerializer where DataType is the first template + /// argument to Component. + /// eg. + /// \code + /// class BoolSerializer; // Defined elsewhere + /// using Static = Component; + /// \endcode + /// /// \tparam DataType Type of the data being wrapped by this component. /// \tparam Identifier Unique identifier for the component class, to avoid /// collision. - /// - // Dev Note: Copy and move constructors/assignment operators should not be - // removed even though it may seem possible to do so. Removing them will - // result in a segfault when a copy or move is used with DataType set to a - // type that is incomplete during construction of the component - // (eg. sdf::Geometry). - template - class Component: public BaseComponent + /// \tparam Serializer A class that can serialize `DataType`. Defaults to a + /// serializer that uses stream operators `<<` and `>>` on the data if they + /// exist. + template > + class Component : public BaseComponent { /// \brief Default constructor public: Component() = default; /// \brief Constructor /// \param[in] _data Data to copy - public: explicit Component(const DataType &_data); - - /// \brief Constructor data to be moved - /// \param[in] _data Data to moved - public: explicit Component(DataType &&_data); - - /// \brief Copy Constructor - /// \param[in] _component Component component to copy. - public: Component(const Component &_component) = default; - - /// \brief Move Constructor - /// \param[in] _component Component component to move. - public: Component(Component &&_component) = default; + public: explicit Component(DataType _data); /// \brief Destructor. public: ~Component() override = default; - /// \brief Move assignment operator. - /// \param[in] _component Component component to move. - /// \return Reference to this. - public: Component &operator=(Component &&_component) = default; - - /// \brief Copy assignment operator. - /// \param[in] _component Component component to copy. - /// \return Reference to this. - public: Component &operator=(const Component &_component) = default; - /// \brief Equality operator. /// \param[in] _component Component to compare to. /// \return True if equal. @@ -383,29 +372,31 @@ namespace components /// /// using Joint = Component; /// - template - class Component : public BaseComponent + template + class Component : public BaseComponent { /// \brief Components with no data are always equal to another instance of /// the same type. /// \param[in] _component Component to compare to /// \return True. - public: bool operator==(const Component &) const; + public: bool operator==(const Component &) const; /// \brief Components with no data are always equal to another instance of /// the same type. /// \param[in] _component Component to compare to /// \return False. - public: bool operator!=(const Component &) const; + public: bool operator!=(const Component &) const; // Documentation inherited - public: void Serialize(std::ostream &_out) const override; + public: ComponentTypeId TypeId() const override; // Documentation inherited - public: void Deserialize(std::istream &_in) override; + public: void Serialize(std::ostream &_out) const override; // Documentation inherited - public: ComponentTypeId TypeId() const override; + public: void Deserialize(std::istream &_in) override; /// \brief Unique ID for this component type. This is set through the /// Factory registration. @@ -417,104 +408,102 @@ namespace components }; ////////////////////////////////////////////////// - template - Component::Component(const DataType &_data) - : data(_data) - { - } - - ////////////////////////////////////////////////// - template - Component::Component(DataType &&_data) + template + Component::Component(DataType _data) : data(std::move(_data)) { } ////////////////////////////////////////////////// - template - DataType &Component::Data() + template + DataType &Component::Data() { return this->data; } ////////////////////////////////////////////////// - template - const DataType &Component::Data() const + template + const DataType &Component::Data() const { return this->data; } ////////////////////////////////////////////////// - template - bool Component:: - operator==(const Component &_component) const + template + bool Component::operator==( + const Component &_component) const { return this->data == _component.Data(); } ////////////////////////////////////////////////// - template - bool Component:: - operator!=(const Component &_component) const + template + bool Component::operator!=( + const Component &_component) const { return this->data != _component.Data(); } ////////////////////////////////////////////////// - template - void Component::Serialize(std::ostream &_out) const + template + void Component::Serialize( + std::ostream &_out) const { - toStream(_out, this->Data()); + Serializer::Serialize(_out, this->Data()); } ////////////////////////////////////////////////// - template - void Component::Deserialize(std::istream &_in) + template + void Component::Deserialize( + std::istream &_in) { - fromStream(_in, this->Data()); + Serializer::Deserialize(_in, this->Data()); } ////////////////////////////////////////////////// - template - ComponentTypeId Component::TypeId() const + template + ComponentTypeId Component::TypeId() const { return typeId; } ////////////////////////////////////////////////// - template - bool Component::operator==( - const Component &) const + template + bool Component::operator==( + const Component &) const { return true; } ////////////////////////////////////////////////// - template - bool Component::operator!=( - const Component &) const + template + bool Component::operator!=( + const Component &) const { return false; } ////////////////////////////////////////////////// - template - ComponentTypeId Component::TypeId() const + template + ComponentTypeId Component::TypeId() const { return typeId; } ////////////////////////////////////////////////// - template - void Component::Serialize(std::ostream &_out) const + template + void Component::Serialize( + std::ostream &_out) const { - _out << "-"; + Serializer::Serialize(_out); } ////////////////////////////////////////////////// - template - void Component::Deserialize(std::istream &) + template + void Component::Deserialize( + std::istream &_in) { + Serializer::Deserialize(_in); } } } diff --git a/include/ignition/gazebo/components/DepthCamera.hh b/include/ignition/gazebo/components/DepthCamera.hh index 8da3af403b..43e5b0cb81 100644 --- a/include/ignition/gazebo/components/DepthCamera.hh +++ b/include/ignition/gazebo/components/DepthCamera.hh @@ -22,6 +22,7 @@ #include #include +#include #include namespace ignition @@ -34,7 +35,8 @@ namespace components { /// \brief A component type that contains a depth camera sensor, /// sdf::Camera, information. - using DepthCamera = Component; + using DepthCamera = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.DepthCamera", DepthCamera) } diff --git a/include/ignition/gazebo/components/Geometry.hh b/include/ignition/gazebo/components/Geometry.hh index bedc831096..dcd1fe8dfa 100644 --- a/include/ignition/gazebo/components/Geometry.hh +++ b/include/ignition/gazebo/components/Geometry.hh @@ -23,47 +23,30 @@ #include #include +#include #include #include -namespace sdf -{ -/// \brief Stream insertion operator for `sdf::Geometry`. -/// \param[in] _out Output stream. -/// \param[in] _geometry Geometry to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const Geometry &_geometry) -{ - auto msg = ignition::gazebo::convert(_geometry); - msg.SerializeToOstream(&_out); - return _out; -} - -/// \brief Stream extraction operator for `sdf::Geometry`. -/// \param[in] _in Input stream. -/// \param[out] _geometry Geometry to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, Geometry &_geometry) -{ - ignition::msgs::Geometry msg; - msg.ParseFromIstream(&_in); - - _geometry = ignition::gazebo::convert(msg); - return _in; -} -} - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using GeometrySerialzer = + serializers::ComponentToMsgSerializer; +} + namespace components { /// \brief This component holds an entity's geometry. - using Geometry = Component; + using Geometry = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Geometry", Geometry) + } } } diff --git a/include/ignition/gazebo/components/GpuLidar.hh b/include/ignition/gazebo/components/GpuLidar.hh index 6abae4d472..ba1f50f047 100644 --- a/include/ignition/gazebo/components/GpuLidar.hh +++ b/include/ignition/gazebo/components/GpuLidar.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace ignition @@ -32,7 +33,8 @@ namespace components { /// \brief A component type that contains a GPU Lidar sensor, /// sdf::Lidar, information. - using GpuLidar = Component; + using GpuLidar = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.GpuLidar", GpuLidar) } } diff --git a/include/ignition/gazebo/components/Imu.hh b/include/ignition/gazebo/components/Imu.hh index 105c30449d..fbd023c345 100644 --- a/include/ignition/gazebo/components/Imu.hh +++ b/include/ignition/gazebo/components/Imu.hh @@ -23,7 +23,8 @@ #include #include -#include "ignition/gazebo/components/Component.hh" +#include +#include namespace ignition { @@ -35,7 +36,8 @@ namespace components { /// \brief A component type that contains an IMU sensor, /// sdf::IMU, information. - using Imu = Component; + using Imu = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Imu", Imu) } } diff --git a/include/ignition/gazebo/components/Inertial.hh b/include/ignition/gazebo/components/Inertial.hh index 581778506c..becaa18acc 100644 --- a/include/ignition/gazebo/components/Inertial.hh +++ b/include/ignition/gazebo/components/Inertial.hh @@ -21,45 +21,27 @@ #include #include #include +#include #include #include namespace ignition { -namespace math -{ -/// \brief Stream insertion operator for `math::Inertiald`. -/// \param[in] _out Output stream. -/// \param[in] _inertial Inertiald to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const Inertiald &_inertial) -{ - auto msg = gazebo::convert(_inertial); - msg.SerializeToOstream(&_out); - return _out; -} - -/// \brief Stream extraction operator for `math::Inertiald`. -/// \param[in] _in Input stream. -/// \param[out] _inertial Inertiald to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, Inertiald &_inertial) -{ - msgs::Inertial msg; - msg.ParseFromIstream(&_in); - - _inertial = gazebo::convert(msg); - return _in; -} -} namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using InertialSerialzer = + serializers::ComponentToMsgSerializer; +} + namespace components { /// \brief This component holds an entity's inertial. - using Inertial = Component; + using Inertial = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Inertial", Inertial) } } diff --git a/include/ignition/gazebo/components/JointAxis.hh b/include/ignition/gazebo/components/JointAxis.hh index 4e8b54c749..bd4113f55c 100644 --- a/include/ignition/gazebo/components/JointAxis.hh +++ b/include/ignition/gazebo/components/JointAxis.hh @@ -21,52 +21,33 @@ #include #include #include +#include #include -namespace sdf -{ -/// \brief Stream insertion operator for `sdf::JointAxis`. -/// \param[in] _out Output stream. -/// \param[in] _set Set to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const JointAxis &_axis) -{ - auto msg = ignition::gazebo::convert(_axis); - msg.SerializeToOstream(&_out); - return _out; -} - -/// \brief Stream extraction operator for `sdf::JointAxis`. -/// \param[in] _in Input stream. -/// \param[out] _set Set to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, JointAxis &_axis) -{ - ignition::msgs::Axis msg; - msg.ParseFromIstream(&_in); - - _axis = ignition::gazebo::convert(msg); - return _in; -} -} - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using JointAxisSerialzer = + serializers::ComponentToMsgSerializer; +} namespace components { /// \brief A component that contains the joint axis . This is a simple wrapper /// around sdf::JointAxis - using JointAxis = Component; + using JointAxis = Component; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointAxis", JointAxis) /// \brief A component that contains the second joint axis for joints with two /// axes. This is a simple wrapper around sdf::JointAxis - using JointAxis2 = Component; + using JointAxis2 = Component; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointAxis2", JointAxis2) } diff --git a/include/ignition/gazebo/components/JointForce.hh b/include/ignition/gazebo/components/JointForce.hh index b1fcce3266..2152d25d98 100644 --- a/include/ignition/gazebo/components/JointForce.hh +++ b/include/ignition/gazebo/components/JointForce.hh @@ -34,7 +34,8 @@ namespace components { /// \brief Force applied to a joint in SI units (Nm for revolute, N for /// prismatic). - using JointForce = Component, class JointForceTag>; + using JointForce = Component, class JointForceTag, + serializers::VectorDoubleSerializer>; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointForce", JointForce) } diff --git a/include/ignition/gazebo/components/JointPosition.hh b/include/ignition/gazebo/components/JointPosition.hh index dee0c0f690..2e9a717252 100644 --- a/include/ignition/gazebo/components/JointPosition.hh +++ b/include/ignition/gazebo/components/JointPosition.hh @@ -21,6 +21,7 @@ #include #include +#include #include namespace ignition @@ -34,7 +35,8 @@ namespace components /// \brief Joint positions in SI units (rad/s for revolute, m/s for /// prismatic). The component wraps a std::vector of size equal to the degrees /// of freedom of the joint. - using JointPosition = Component, class JointPositionTag>; + using JointPosition = Component, class JointPositionTag, + serializers::VectorDoubleSerializer>; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointPosition", JointPosition) } diff --git a/include/ignition/gazebo/components/JointType.hh b/include/ignition/gazebo/components/JointType.hh index efa9c75d4b..1d63995ccf 100644 --- a/include/ignition/gazebo/components/JointType.hh +++ b/include/ignition/gazebo/components/JointType.hh @@ -23,42 +23,47 @@ #include #include -namespace sdf -{ -/// \brief Stream insertion operator for `sdf::JointType`. -/// \param[in] _out Output stream. -/// \param[in] _type JointType to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const JointType &_type) -{ - _out << static_cast(_type); - return _out; -} - -/// \brief Stream extraction operator for `sdf::JointType`. -/// \param[in] _in Input stream. -/// \param[out] _type JointType to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, JointType &_type) -{ - int type; - _in >> type; - _type = sdf::JointType(type); - return _in; -} -} - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + class JointTypeSerializer + { + /// \brief Serialization for `sdf::JointType`. + /// \param[in] _out Output stream. + /// \param[in] _type JointType to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const sdf::JointType &_type) + { + _out << static_cast(_type); + return _out; + } + + /// \brief Deserialization for `sdf::JointType`. + /// \param[in] _in Input stream. + /// \param[out] _type JointType to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + sdf::JointType &_type) + { + int type; + _in >> type; + _type = sdf::JointType(type); + return _in; + } + }; +} namespace components { /// \brief A component that contains the joint type. This is a simple wrapper /// around sdf::JointType - using JointType = Component; + using JointType = Component; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointType", JointType) } diff --git a/include/ignition/gazebo/components/JointVelocity.hh b/include/ignition/gazebo/components/JointVelocity.hh index ff09aefc4d..3b86501f52 100644 --- a/include/ignition/gazebo/components/JointVelocity.hh +++ b/include/ignition/gazebo/components/JointVelocity.hh @@ -34,8 +34,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief Base class which can be extended to add serialization - using JointVelocity = Component, - class JointVelocityTag>; + using JointVelocity = Component, class JointVelocityTag, + serializers::VectorDoubleSerializer>; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointVelocity", JointVelocity) diff --git a/include/ignition/gazebo/components/JointVelocityCmd.hh b/include/ignition/gazebo/components/JointVelocityCmd.hh index 23f9b64192..d37b3b263d 100644 --- a/include/ignition/gazebo/components/JointVelocityCmd.hh +++ b/include/ignition/gazebo/components/JointVelocityCmd.hh @@ -34,8 +34,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief Base class which can be extended to add serialization - using JointVelocityCmd = Component, - class JointVelocityCmdTag>; + using JointVelocityCmd = + Component, class JointVelocityCmdTag, + serializers::VectorDoubleSerializer>; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.JointVelocityCmd", JointVelocityCmd) diff --git a/include/ignition/gazebo/components/LevelEntityNames.hh b/include/ignition/gazebo/components/LevelEntityNames.hh index f8d3e95ecb..9c45bc31a2 100644 --- a/include/ignition/gazebo/components/LevelEntityNames.hh +++ b/include/ignition/gazebo/components/LevelEntityNames.hh @@ -26,53 +26,58 @@ #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Component.hh" -namespace std +namespace ignition +{ +namespace gazebo { -/// \brief Stream insertion operator for `std::set`. -/// \param[in] _out Output stream. -/// \param[in] _set Set to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, - const std::set &_set) +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers { - for (const auto &entity : _set) + class LevelEntityNamesSerializer { - _out << entity << " "; - } - return _out; -} + /// \brief Serialization for `std::set`. + /// \param[in] _out Output stream. + /// \param[in] _set Set to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const std::set &_set) + { + for (const auto &entity : _set) + { + _out << entity << " "; + } + return _out; + } -/// \brief Stream extraction operator for `std::set`. -/// \param[in] _in Input stream. -/// \param[out] _set Set to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, std::set &_set) -{ - _in.setf(std::ios_base::skipws); + /// \brief Deserialization for `std::set`. + /// \param[in] _in Input stream. + /// \param[out] _set Set to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + std::set &_set) + { + _in.setf(std::ios_base::skipws); - _set.clear(); + _set.clear(); - for (auto it = std::istream_iterator(_in); - it != std::istream_iterator(); ++it) - { - _set.insert(*it); - } - return _in; -} + for (auto it = std::istream_iterator(_in); + it != std::istream_iterator(); ++it) + { + _set.insert(*it); + } + return _in; + } + }; } -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief A component that holds a list of names of entities to be loaded in /// a level. using LevelEntityNames = - Component, class LevelEntityNamesTag>; + Component, class LevelEntityNamesTag, + serializers::LevelEntityNamesSerializer>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.LevelEntityNames", LevelEntityNames) diff --git a/include/ignition/gazebo/components/Lidar.hh b/include/ignition/gazebo/components/Lidar.hh index 1ea0cee604..770c4b9664 100644 --- a/include/ignition/gazebo/components/Lidar.hh +++ b/include/ignition/gazebo/components/Lidar.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace ignition @@ -32,7 +33,8 @@ namespace components { /// \brief A component type that contains a Lidar sensor, /// sdf::Lidar, information. - using Lidar = Component; + using Lidar = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Lidar", Lidar) } } diff --git a/include/ignition/gazebo/components/Light.hh b/include/ignition/gazebo/components/Light.hh index d6635877fb..fc27c8b0ee 100644 --- a/include/ignition/gazebo/components/Light.hh +++ b/include/ignition/gazebo/components/Light.hh @@ -23,48 +23,29 @@ #include #include +#include #include #include -namespace sdf -{ -/// \brief Stream insertion operator for `sdf::Light`. -/// \param[in] _out Output stream. -/// \param[in] _light Light to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const Light &_light) -{ - auto msg = ignition::gazebo::convert(_light); - msg.SerializeToOstream(&_out); - return _out; -} - -/// \brief Stream extraction operator for `sdf::Light`. -/// \param[in] _in Input stream. -/// \param[out] _light Light to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, Light &_light) -{ - ignition::msgs::Light msg; - msg.ParseFromIstream(&_in); - - _light = ignition::gazebo::convert(msg); - return _in; -} -} - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using LightSerialzer = + serializers::ComponentToMsgSerializer; +} + namespace components { /// \brief This component contains light source information. For more /// information on lights, see [SDF's Light /// element](http://sdformat.org/spec?ver=1.6&elem=light). - using Light = Component; + using Light = + Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Light", Light) } } diff --git a/include/ignition/gazebo/components/Magnetometer.hh b/include/ignition/gazebo/components/Magnetometer.hh index c51173d663..daa7c5a860 100644 --- a/include/ignition/gazebo/components/Magnetometer.hh +++ b/include/ignition/gazebo/components/Magnetometer.hh @@ -36,7 +36,8 @@ namespace components { /// \brief A component type that contains a magnetometer sensor, /// sdf::Magnetometer, information. - using Magnetometer = Component; + using Magnetometer = Component; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.Magnetometer", Magnetometer) diff --git a/include/ignition/gazebo/components/Material.hh b/include/ignition/gazebo/components/Material.hh index a37a841afe..ff55a73b35 100644 --- a/include/ignition/gazebo/components/Material.hh +++ b/include/ignition/gazebo/components/Material.hh @@ -23,45 +23,25 @@ #include #include +#include #include -namespace sdf -{ -/// \brief Stream insertion operator for `sdf::Material`. -/// \param[in] _out Output stream. -/// \param[in] _material Material to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const Material &_material) -{ - auto msg = ignition::gazebo::convert(_material); - msg.SerializeToOstream(&_out); - return _out; -} - -/// \brief Stream extraction operator for `sdf::Material`. -/// \param[in] _in Input stream. -/// \param[out] _material Material to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, Material &_material) -{ - ignition::msgs::Material msg; - msg.ParseFromIstream(&_in); - - _material = ignition::gazebo::convert(msg); - return _in; -} -} - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using MaterialSerialzer = + serializers::ComponentToMsgSerializer; +} namespace components { /// \brief This component holds an entity's material. - using Material = Component; + using Material = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Material", Material) } } diff --git a/include/ignition/gazebo/components/PerformerLevels.hh b/include/ignition/gazebo/components/PerformerLevels.hh index 67757bab62..a9f84faa94 100644 --- a/include/ignition/gazebo/components/PerformerLevels.hh +++ b/include/ignition/gazebo/components/PerformerLevels.hh @@ -25,53 +25,56 @@ #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Component.hh" -namespace std +namespace ignition +{ +namespace gazebo { -/// \brief Stream insertion operator for `std::set`. -/// \param[in] _out Output stream. -/// \param[in] _set Set to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, - const std::set &_set) +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers { - for (const auto &level : _set) + class PerformerLevelsSerializer { - _out << level << " "; - } - return _out; -} + /// \brief Serialization for `std::set`. + /// \param[in] _out Output stream. + /// \param[in] _set Set to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const std::set &_set) + { + for (const auto &level : _set) + { + _out << level << " "; + } + return _out; + } -/// \brief Stream extraction operator for `std::set`. -/// \param[in] _in Input stream. -/// \param[out] _set Set to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, - std::set &_set) -{ - _in.setf(std::ios_base::skipws); + /// \brief Deserialization for `std::set`. + /// \param[in] _in Input stream. + /// \param[out] _set Set to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + std::set &_set) + { + _in.setf(std::ios_base::skipws); - _set.clear(); + _set.clear(); - for (auto it = std::istream_iterator(_in); - it != std::istream_iterator(); ++it) - { - _set.insert(*it); - } - return _in; -} + for (auto it = std::istream_iterator(_in); + it != std::istream_iterator(); ++it) + { + _set.insert(*it); + } + return _in; + } + }; } -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { /// \brief Holds all the levels which a performer is in. - using PerformerLevels = - Component, class PerformerLevelsTag>; + using PerformerLevels = Component, class PerformerLevelsTag, + serializers::PerformerLevelsSerializer>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PerformerLevels", PerformerLevels) } diff --git a/include/ignition/gazebo/components/Scene.hh b/include/ignition/gazebo/components/Scene.hh index bdfa2949d8..7211a183e7 100644 --- a/include/ignition/gazebo/components/Scene.hh +++ b/include/ignition/gazebo/components/Scene.hh @@ -22,46 +22,27 @@ #include #include #include +#include #include #include -namespace sdf -{ -/// \brief Stream insertion operator for `sdf::Scene`. -/// \param[in] _out Output stream. -/// \param[in] _scene Scene to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const Scene &_scene) -{ - auto msg = ignition::gazebo::convert(_scene); - msg.SerializeToOstream(&_out); - return _out; -} - -/// \brief Stream extraction operator for `sdf::Scene`. -/// \param[in] _in Input stream. -/// \param[out] _scene Scene to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, Scene &_scene) -{ - ignition::msgs::Scene msg; - msg.ParseFromIstream(&_in); - - _scene = ignition::gazebo::convert(msg); - return _in; -} -} - namespace ignition { namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using SceneSerialzer = + serializers::ComponentToMsgSerializer; +} + namespace components { /// \brief This component holds scene properties of the world. - using Scene = Component; + using Scene = + Component; IGN_GAZEBO_REGISTER_COMPONENT( "ign_gazebo_components.Scene", Scene) } diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index b685e1fdf8..7ff32fb941 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -18,7 +18,6 @@ #define IGNITION_GAZEBO_COMPONENTS_SERIALIZATION_HH_ #include -#include #include #include @@ -28,59 +27,110 @@ // This header holds serialization operators which are shared among several // components -namespace sdf +namespace ignition { -/// \brief Stream insertion operator for `sdf::Sensor`. -/// \param[in] _out Output stream. -/// \param[in] _sensor Sensor to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, const Sensor &_sensor) +namespace gazebo { - auto msg = ignition::gazebo::convert(_sensor); - msg.SerializeToOstream(&_out); - return _out; -} +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +/// \brief A Serializer class is used to serialize and deserialize a component. +/// It is passed in as the third template parameter to components::Component. +/// Eg. +/// \code +/// using Geometry = components::Component +/// \endcode +/// A serializer class implements two static functions: `Serialize` and +/// `Deserialize` with the following signatures +/// \code +/// class ExampleSerializer +/// { +/// public: static std::ostream &Serialize(std::ostream &_out, +/// const DataType &_data); +/// public: static std::istream &Deserialize(std::istream &_in, +/// DataType &_data) +/// }; +/// \endcode -/// \brief Stream extraction operator for `sdf::Sensor`. -/// \param[in] _in Input stream. -/// \param[out] _sensor Sensor to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, Sensor &_sensor) +namespace serializers { - ignition::msgs::Sensor msg; - msg.ParseFromIstream(&_in); + /// \brief Serialization for that converts components data types to + /// ignition::msgs. This assumes that ignition::gazebo::convert is + /// defined + /// \tparam DataType Underlying data type of the component + /// + /// This can be used for components that can be converted to ignition::msg + /// types via ignition::gazebo::convert. For example sdf::Geometry can be + /// converted to msgs::Geometry so the component can be defined as + /// \code + /// using Geometry = Component> + /// \endcode + template + class ComponentToMsgSerializer + { + /// \brief Serialization + /// \param[in] _out Output stream. + /// \param[in] _data data to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const DataType &_data) + { + auto msg = ignition::gazebo::convert(_data); + msg.SerializeToOstream(&_out); + return _out; + } - _sensor = ignition::gazebo::convert(msg); - return _in; -} -} + /// \brief Deserialization + /// \param[in] _in Input stream. + /// \param[out] _data data to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + DataType &_data) + { + MsgType msg; + msg.ParseFromIstream(&_in); -namespace std -{ -/// \brief Stream insertion operator for `std::vector`. -/// \param[in] _out Output stream. -/// \param[in] _vec Vector to stream -/// \return The stream. -inline std::ostream &operator<<(std::ostream &_out, - const std::vector &_vec) -{ - ignition::msgs::Double_V msg; - *msg.mutable_data() = {_vec.begin(), _vec.end()}; - msg.SerializeToOstream(&_out); - return _out; -} + _data = ignition::gazebo::convert(msg); + return _in; + } + }; -/// \brief Stream extraction operator for `std::vector`. -/// \param[in] _in Input stream. -/// \param[in] _vec Vector to populate -/// \return The stream. -inline std::istream &operator>>(std::istream &_in, std::vector &_vec) -{ - ignition::msgs::Double_V msg; - msg.ParseFromIstream(&_in); + /// \brief Common serializer for sensors + using SensorSerializer = ComponentToMsgSerializer; + + /// \brief Serializer for components that hold `std::vector`. + class VectorDoubleSerializer + { + /// \brief Serialization + /// \param[in] _out Output stream. + /// \param[in] _geometry Geometry to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const std::vector &_vec) + { + ignition::msgs::Double_V msg; + *msg.mutable_data() = {_vec.begin(), _vec.end()}; + msg.SerializeToOstream(&_out); + return _out; + } + + /// \brief Deserialization + /// \param[in] _in Input stream. + /// \param[in] _vec Vector to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + std::vector &_vec) + { + ignition::msgs::Double_V msg; + msg.ParseFromIstream(&_in); - _vec = {msg.data().begin(), msg.data().end()}; - return _in; + _vec = {msg.data().begin(), msg.data().end()}; + return _in; + } + }; +} +} } } diff --git a/src/Component_TEST.cc b/src/Component_TEST.cc index 0902fd2add..7bc37147e5 100644 --- a/src/Component_TEST.cc +++ b/src/Component_TEST.cc @@ -186,7 +186,7 @@ TEST_F(ComponentTest, OStream) Custom comp(data); std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("banana", ostr.str()); } @@ -200,7 +200,7 @@ TEST_F(ComponentTest, OStream) // Returns empty string and prints warning std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("", ostr.str()); } @@ -210,7 +210,7 @@ TEST_F(ComponentTest, OStream) CustomOperator comp(data); std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("simple_operator", ostr.str()); } @@ -222,13 +222,13 @@ TEST_F(ComponentTest, OStream) auto comp = new Custom(data); std::ostringstream ostr; - ostr << *comp; + comp->Serialize(ostr); EXPECT_EQ("Mass: 0", ostr.str()); // Serializable from base class auto compBase = dynamic_cast(comp); std::ostringstream ostrBase; - ostrBase << *compBase; + compBase->Serialize(ostrBase); EXPECT_EQ("Mass: 0", ostrBase.str()); } @@ -238,13 +238,13 @@ TEST_F(ComponentTest, OStream) auto comp = new InertialWrapper(data); std::ostringstream ostr; - ostr << *comp; + comp->Serialize(ostr); EXPECT_EQ("Wrapper mass: 0", ostr.str()); // Serializable from base class auto compBase = dynamic_cast(comp); std::ostringstream ostrBase; - ostrBase << *compBase; + compBase->Serialize(ostrBase); EXPECT_EQ("Wrapper mass: 0", ostrBase.str()); } @@ -258,7 +258,7 @@ TEST_F(ComponentTest, OStream) // Check the value is streamed, not the pointer address std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("123", ostr.str()); } @@ -273,7 +273,7 @@ TEST_F(ComponentTest, OStream) // Returns empty string and prints warning std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("", ostr.str()); } @@ -287,7 +287,7 @@ TEST_F(ComponentTest, OStream) // Check the value is streamed, not the pointer address std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("simple_operator", ostr.str()); } @@ -304,7 +304,7 @@ TEST_F(ComponentTest, OStream) Custom comp(data); std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("val\n", ostr.str()); } @@ -317,7 +317,7 @@ TEST_F(ComponentTest, OStream) Custom comp(data); std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("Mass: 0", ostr.str()); } @@ -326,7 +326,7 @@ TEST_F(ComponentTest, OStream) NoSerialize comp; std::ostringstream ostr; - ostr << comp; + comp.Serialize(ostr); EXPECT_EQ("", ostr.str()); } } @@ -340,7 +340,7 @@ TEST_F(ComponentTest, IStream) std::istringstream istr("banana"); Custom comp; - istr >> comp; + comp.Deserialize(istr); EXPECT_EQ("banana", comp.Data()); } @@ -352,7 +352,7 @@ TEST_F(ComponentTest, IStream) // Prints warning and doesn't modify the component std::istringstream istr("banana"); Custom comp; - istr >> comp; + comp.Deserialize(istr); } // Component with data which has custom stream operator @@ -361,7 +361,7 @@ TEST_F(ComponentTest, IStream) CustomOperator comp(data); std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); EXPECT_EQ(456, comp.Data().data); } @@ -373,7 +373,7 @@ TEST_F(ComponentTest, IStream) Custom comp(data); std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); EXPECT_DOUBLE_EQ(200, comp.Data().MassMatrix().Mass()); } @@ -383,7 +383,7 @@ TEST_F(ComponentTest, IStream) InertialWrapper comp(data); std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); EXPECT_DOUBLE_EQ(2000, comp.Data().MassMatrix().Mass()); } @@ -397,7 +397,7 @@ TEST_F(ComponentTest, IStream) // Check the value is streamed, not the pointer address std::istringstream istr("456"); - istr >> comp; + comp.Deserialize(istr); EXPECT_EQ(456, *comp.Data()); } @@ -412,7 +412,7 @@ TEST_F(ComponentTest, IStream) // Prints warning and doesn't modify the component std::istringstream istr("ignored"); - istr >> comp; + comp.Deserialize(istr); } // Component with shared_ptr data which has custom stream operator @@ -425,7 +425,7 @@ TEST_F(ComponentTest, IStream) // Check the value is changed, not the pointer address std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); EXPECT_EQ(456, comp.Data()->data); } @@ -438,7 +438,7 @@ TEST_F(ComponentTest, IStream) Custom comp(data); std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); EXPECT_EQ("\n", comp.Data()->ToString("")); } @@ -451,7 +451,7 @@ TEST_F(ComponentTest, IStream) Custom comp(data); std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); EXPECT_DOUBLE_EQ(200, comp.Data()->MassMatrix().Mass()); } @@ -460,7 +460,7 @@ TEST_F(ComponentTest, IStream) NoSerialize comp; std::istringstream istr("not used"); - istr >> comp; + comp.Deserialize(istr); } } diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 902ecbd799..88469088d4 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -690,7 +690,7 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, compMsg->set_type(compBase->TypeId()); std::ostringstream ostr; - ostr << *compBase; + compBase->Serialize(ostr); compMsg->set_component(ostr.str()); @@ -787,7 +787,8 @@ void EntityComponentManager::SetState( } std::istringstream istr(compMsg.component()); - istr >> *newComp.get(); + // istr >> *newComp.get(); + newComp->Deserialize(istr); // Get type id auto typeId = newComp->TypeId(); diff --git a/test/integration/components.cc b/test/integration/components.cc index d96ba39935..374c6021c3 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -108,10 +108,10 @@ TEST_F(ComponentsTest, AirPressureSensor) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::AirPressureSensor comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ("abc", comp3.Data().Name()); EXPECT_EQ(sdf::SensorType::AIR_PRESSURE, comp3.Data().Type()); EXPECT_EQ(ignition::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().Pose()); @@ -153,10 +153,10 @@ TEST_F(ComponentsTest, Altimeter) // Stream operator std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Altimeter comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(sdf::SensorType::ALTIMETER, comp3.Data().Type()); EXPECT_EQ(sdf::NoiseType::GAUSSIAN, comp3.Data().AltimeterSensor()->VerticalVelocityNoise().Type()); @@ -182,12 +182,12 @@ TEST_F(ComponentsTest, AngularVelocity) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1 2 3", ostr.str()); std::istringstream istr("3 2 1"); components::AngularVelocity comp3(math::Vector3d::Zero); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Vector3d(3, 2, 1), comp3.Data()); } @@ -228,12 +228,12 @@ TEST_F(ComponentsTest, CanonicalLink) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::CanonicalLink comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -254,12 +254,12 @@ TEST_F(ComponentsTest, ChildLinkName) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("comp1", ostr.str()); std::istringstream istr("comp3"); components::ChildLinkName comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ("comp3", comp3.Data()); } @@ -277,12 +277,12 @@ TEST_F(ComponentsTest, Collision) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Collision comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -306,10 +306,10 @@ TEST_F(ComponentsTest, Geometry) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Geometry comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(sdf::GeometryType::CYLINDER, comp3.Data().Type()); ASSERT_NE(nullptr, comp3.Data().CylinderShape()); EXPECT_DOUBLE_EQ(1.23, comp3.Data().CylinderShape()->Radius()); @@ -334,12 +334,12 @@ TEST_F(ComponentsTest, Gravity) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1 2 3", ostr.str()); std::istringstream istr("3 2 1"); components::Gravity comp3(math::Vector3d::Zero); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Vector3d(3, 2, 1), comp3.Data()); } @@ -374,10 +374,10 @@ TEST_F(ComponentsTest, Imu) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Imu comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ("imu_sensor", comp3.Data().Name()); EXPECT_EQ(sdf::SensorType::IMU, comp3.Data().Type()); EXPECT_EQ("imu_data", comp3.Data().Topic()); @@ -406,10 +406,10 @@ TEST_F(ComponentsTest, Inertial) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Inertial comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_DOUBLE_EQ(1.0, comp3.Data().MassMatrix().Mass()); } @@ -427,12 +427,12 @@ TEST_F(ComponentsTest, Joint) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Joint comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -459,11 +459,11 @@ TEST_F(ComponentsTest, JointAxis) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::JointAxis comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Vector3d(1, 2, 3), comp3.Data().Xyz()); EXPECT_DOUBLE_EQ(0.1, comp3.Data().Damping()); @@ -496,14 +496,14 @@ TEST_F(ComponentsTest, JointType) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ(std::to_string(static_cast(sdf::JointType::FIXED)), ostr.str()); std::istringstream istr(std::to_string(static_cast( sdf::JointType::SCREW))); components::JointType comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(sdf::JointType::SCREW, comp3.Data()); } @@ -515,11 +515,11 @@ TEST_F(ComponentsTest, JointVelocity) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::JointVelocity comp3; - istr >> comp3; + comp3.Deserialize(istr); ASSERT_EQ(3u, comp3.Data().size()); EXPECT_DOUBLE_EQ(1.2, comp3.Data()[0]); EXPECT_DOUBLE_EQ(2.3, comp3.Data()[1]); @@ -534,11 +534,11 @@ TEST_F(ComponentsTest, JointVelocityCmd) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::JointVelocityCmd comp3; - istr >> comp3; + comp3.Deserialize(istr); ASSERT_EQ(3u, comp3.Data().size()); EXPECT_DOUBLE_EQ(1.2, comp3.Data()[0]); EXPECT_DOUBLE_EQ(2.3, comp3.Data()[1]); @@ -559,12 +559,12 @@ TEST_F(ComponentsTest, Level) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Level comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -577,12 +577,12 @@ TEST_F(ComponentsTest, LevelBuffer) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1.5", ostr.str()); std::istringstream istr("3.3"); components::LevelBuffer comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_DOUBLE_EQ(3.3, comp3.Data()); } @@ -607,12 +607,12 @@ TEST_F(ComponentsTest, LevelEntityNames) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("level1 level2 ", ostr.str()); std::istringstream istr("level3 level4"); components::LevelEntityNames comp3; - istr >> comp3; + comp3.Deserialize(istr); std::set data3({"level3", "level4"}); EXPECT_EQ(data3, comp3.Data()); @@ -648,10 +648,10 @@ TEST_F(ComponentsTest, Light) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Light comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(sdf::LightType::POINT, comp3.Data().Type()); EXPECT_EQ("light_test", comp3.Data().Name()); EXPECT_EQ(math::Pose3d(1, 2, 4, 0, 0, IGN_PI), comp3.Data().Pose()); @@ -686,12 +686,12 @@ TEST_F(ComponentsTest, LinearAcceleration) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1 2 3", ostr.str()); std::istringstream istr("3 2 1"); components::LinearAcceleration comp3(math::Vector3d::Zero); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Vector3d(3, 2, 1), comp3.Data()); } @@ -713,12 +713,12 @@ TEST_F(ComponentsTest, LinearVelocity) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1 2 3", ostr.str()); std::istringstream istr("3 2 1"); components::LinearVelocity comp3(math::Vector3d::Zero); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Vector3d(3, 2, 1), comp3.Data()); } @@ -736,12 +736,12 @@ TEST_F(ComponentsTest, Link) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Link comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -774,10 +774,10 @@ TEST_F(ComponentsTest, Magnetometer) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Magnetometer comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ("banana", comp3.Data().Name()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, comp3.Data().Type()); EXPECT_EQ("grape", comp3.Data().Topic()); @@ -822,10 +822,10 @@ TEST_F(ComponentsTest, Material) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Material comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Color(1, 0, 0, 1), comp3.Data().Ambient()); EXPECT_EQ(math::Color(1, 0, 1, 1), comp3.Data().Diffuse()); EXPECT_EQ(math::Color(1, 1, 0, 1), comp3.Data().Specular()); @@ -864,12 +864,12 @@ TEST_F(ComponentsTest, Model) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Model comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -890,12 +890,12 @@ TEST_F(ComponentsTest, Name) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("comp1", ostr.str()); std::istringstream istr("comp3"); components::Name comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ("comp3", comp3.Data()); } @@ -917,12 +917,12 @@ TEST_F(ComponentsTest, ParentEntity) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1", ostr.str()); std::istringstream istr("3"); components::ParentEntity comp3(kNullEntity); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(3u, comp3.Data()); } @@ -944,12 +944,12 @@ TEST_F(ComponentsTest, ParentLinkName) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("comp1", ostr.str()); std::istringstream istr("comp3"); components::ParentLinkName comp3(std::string("")); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ("comp3", comp3.Data()); } @@ -967,12 +967,12 @@ TEST_F(ComponentsTest, Performer) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Performer comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -987,12 +987,12 @@ TEST_F(ComponentsTest, PerformerLevels) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1 2 3 ", ostr.str()); std::istringstream istr("7 8 9 10"); components::PerformerLevels comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(4u, comp3.Data().size()); EXPECT_EQ(1u, comp3.Data().count(7)); EXPECT_EQ(1u, comp3.Data().count(8)); @@ -1019,12 +1019,12 @@ TEST_F(ComponentsTest, Pose) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1 2 3 0.1 0.2 0.3", ostr.str()); std::istringstream istr("3 2 1 0.3 0.2 0.1"); components::Pose comp3(math::Pose3d::Zero); - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Pose3d(3, 2, 1, 0.3, 0.2, 0.1), comp3.Data()); } @@ -1042,12 +1042,12 @@ TEST_F(ComponentsTest, Sensor) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Sensor comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -1077,12 +1077,12 @@ TEST_F(ComponentsTest, ThreadPitch) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); EXPECT_EQ("1.2", ostr.str()); std::istringstream istr("3.4"); components::ThreadPitch comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_DOUBLE_EQ(3.4, comp3.Data()); } @@ -1100,12 +1100,12 @@ TEST_F(ComponentsTest, Visual) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::Visual comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -1122,12 +1122,12 @@ TEST_F(ComponentsTest, World) // Stream operators std::ostringstream ostr; - ostr << comp1; + comp1.Serialize(ostr); EXPECT_EQ("-", ostr.str()); std::istringstream istr("ignored"); components::World comp3; - istr >> comp3; + comp3.Deserialize(istr); } ///////////////////////////////////////////////// @@ -1147,10 +1147,10 @@ TEST_F(ComponentsTest, Scene) // Stream operators std::ostringstream ostr; - ostr << comp11; + comp11.Serialize(ostr); std::istringstream istr(ostr.str()); components::Scene comp3; - istr >> comp3; + comp3.Deserialize(istr); EXPECT_EQ(math::Color(1, 0, 1, 1), comp3.Data().Ambient()); EXPECT_EQ(math::Color(1, 1, 0, 1), comp3.Data().Background()); EXPECT_TRUE(comp3.Data().Shadows());