Commit f8a71642 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add serialization for Device and Joint.

parent 5bdf8846
......@@ -86,6 +86,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/pinocchio/simple-device.hh
include/hpp/pinocchio/util.hh
include/hpp/pinocchio/pool.hh
include/hpp/pinocchio/serialization.hh
include/hpp/pinocchio/liegroup.hh
include/hpp/pinocchio/liegroup-element.hh
......
......@@ -28,6 +28,7 @@
# include <boost/thread/condition_variable.hpp>
# include <hpp/util/debug.hh>
# include <hpp/util/serialization-fwd.hh>
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/frame.hh>
......@@ -339,6 +340,9 @@ namespace hpp {
private:
Pool<DeviceData> datas_;
Device() {}
HPP_SERIALIZABLE_SPLIT();
}; // class Device
void replaceGeometryByConvexHull (GeomModel& geomModel,
......
......@@ -24,6 +24,8 @@
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/config.hh>
# include <hpp/util/serialization-fwd.hh>
namespace hpp {
namespace pinocchio {
/// Robot joint
......@@ -252,6 +254,9 @@ namespace hpp {
void selfAssert() const;
friend class Device;
Joint() {}
HPP_SERIALIZABLE();
}; // class Joint
inline std::ostream& operator<< (std::ostream& os, const Joint& joint) { return joint.display(os); }
......
......@@ -20,8 +20,12 @@
#include <hpp/pinocchio/device.hh>
#include <boost/foreach.hpp>
#include <boost/serialization/export.hpp>
#include <Eigen/Core>
#include <hpp/util/serialization.hh>
#include <hpp/fcl/BV/AABB.h>
#include <hpp/fcl/BVH/BVH_model.h>
......@@ -29,6 +33,7 @@
#include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/serialization/model.hpp>
#include <hpp/pinocchio/fwd.hh>
#include <hpp/pinocchio/joint-collection.hh>
......@@ -40,6 +45,10 @@
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <hpp/pinocchio/serialization.hh>
// Needed to allow inheritance.
BOOST_CLASS_EXPORT(hpp::pinocchio::Device)
namespace hpp {
namespace pinocchio {
......@@ -523,5 +532,84 @@ namespace hpp {
}
}
}
#if __cplusplus > 201103L
using boost::serialization::make_nvp;
template<typename To, typename Ti, typename UnaryOp>
inline std::vector<To> serialize_to (const std::vector<Ti>& in, UnaryOp op)
{
std::vector<To> out;
out.reserve(in.size());
std::transform(in.begin(), in.end(), out.begin(), op);
return out;
}
template<class Archive>
void Device::save(Archive & ar, const unsigned int version) const
{
(void) version;
ar & BOOST_SERIALIZATION_NVP(name_);
/*
// AbstractDevice
ar & BOOST_SERIALIZATION_NVP(model_);
//ar & BOOST_SERIALIZATION_NVP(geomModel_);
// Device
ar & BOOST_SERIALIZATION_NVP(name_);
// - grippers_
ar & make_nvp("grippers", serialize_to<FrameIndex>(grippers_,
[](const GripperPtr_t& g) -> FrameIndex { return g->frameIndex(); })
);
ar & BOOST_SERIALIZATION_NVP(jointConstraints_);
ar & BOOST_SERIALIZATION_NVP(weakPtr_);
ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.dimension_);
ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.lowerBounds_);
ar & BOOST_SERIALIZATION_NVP(extraConfigSpace_.upperBounds_);
ar & serialization::make_nvp("nbDeviceData", datas_.size());
*/
}
template<class Archive>
void Device::load(Archive & ar, const unsigned int version)
{
using hpp::serialization::archive_device_wrapper;
archive_device_wrapper* adw = dynamic_cast<archive_device_wrapper*>(&ar);
if (!adw) throw std::logic_error("Not implemented.");
(void) version;
ar & BOOST_SERIALIZATION_NVP(name_);
// TODO if (adw->device->name() != name_) ?
}
#else
template<class Archive>
void Device::save(Archive &, const unsigned int) const
{
throw std::logic_error("Not implemented without C++ 11.");
}
template<class Archive>
void Device::load(Archive &, const unsigned int)
{
throw std::logic_error("Not implemented without C++ 11.");
}
#endif
HPP_SERIALIZATION_SPLIT_IMPLEMENT(Device);
} // namespace pinocchio
} // namespace hpp
namespace boost {
namespace serialization {
template<class Archive>
void serialize (Archive & ar, hpp::pinocchio::Device::JointLinearConstraint& c, const unsigned int version)
{
(void) version;
ar & make_nvp("offset", c.offset);
ar & make_nvp("multiplier", c.multiplier);
ar & make_nvp("joint", c.joint);
ar & make_nvp("reference", c.reference);
}
}
}
......@@ -20,14 +20,19 @@
# include <hpp/pinocchio/joint.hh>
# include <limits>
# include <boost/serialization/vector.hpp>
# include <pinocchio/algorithm/jacobian.hpp>
# include <hpp/util/serialization.hh>
# include <hpp/pinocchio/device.hh>
# include <hpp/pinocchio/device-data.hh>
# include <hpp/pinocchio/body.hh>
# include <hpp/pinocchio/frame.hh>
# include <hpp/pinocchio/liegroup-space.hh>
# include <hpp/pinocchio/joint-collection.hh>
# include <hpp/pinocchio/serialization.hh>
# include "joint/bound.hh"
......@@ -539,5 +544,17 @@ namespace hpp {
{
return devicePtr.lock()->d();
}
template<class Archive>
void Joint::serialize(Archive & ar, const unsigned int version)
{
(void) version;
ar & BOOST_SERIALIZATION_NVP(maximalDistanceToParent_);
ar & BOOST_SERIALIZATION_NVP(devicePtr);
ar & BOOST_SERIALIZATION_NVP(jointIndex);
ar & BOOST_SERIALIZATION_NVP(children);
}
HPP_SERIALIZATION_IMPLEMENT(Joint);
} // namespace pinocchio
} // namespace hpp
......@@ -18,6 +18,10 @@
#include <boost/test/unit_test.hpp>
#include <sstream>
#include <boost/archive/polymorphic_xml_iarchive.hpp>
#include <boost/archive/polymorphic_xml_oarchive.hpp>
#include <pinocchio/fwd.hpp>
#include <hpp/pinocchio/fwd.hh>
#include <hpp/pinocchio/joint-collection.hh>
......@@ -31,6 +35,7 @@
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/serialization.hh>
static bool verbose = true;
using namespace hpp::pinocchio;
......@@ -141,3 +146,34 @@ BOOST_AUTO_TEST_CASE(load_neutral_configuration)
*/
}
struct iarchive :
boost::archive::polymorphic_xml_iarchive, hpp::serialization::archive_device_wrapper
{
iarchive(std::istream& is) : boost::archive::polymorphic_xml_iarchive (is) {}
};
BOOST_AUTO_TEST_CASE(serialization)
{
DevicePtr_t device = unittest::makeDevice (unittest::HumanoidSimple);
JointPtr_t joint = Joint::create (device, 1);
std::stringstream ss;
{
boost::archive::polymorphic_xml_oarchive oa(ss);
oa << boost::serialization::make_nvp("device", device);
oa << boost::serialization::make_nvp("joint", joint);
}
DevicePtr_t device2;
JointPtr_t joint2;
{
iarchive ia(ss);
ia.device = device;
//boost::archive::polymorphic_xml_iarchive ia(ss);
ia >> boost::serialization::make_nvp("device", device2);
ia >> boost::serialization::make_nvp("joint", joint2);
}
BOOST_CHECK(device == device2);
BOOST_CHECK(joint2->robot() == device2);
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment