00001 #include "kdl_mqueue.hpp"
00002 #include <rtt/transports/mqueue/MQTemplateProtocol.hpp>
00003 #include <rtt/transports/mqueue/MQSerializationProtocol.hpp>
00004 #include <rtt/types/TransportPlugin.hpp>
00005 #include <rtt/types/TypekitPlugin.hpp>
00006 #include <boost/serialization/vector.hpp>
00007 #include <boost/serialization/array.hpp>
00008 #include <kdl/frames.hpp>
00009 #include <kdl/jacobian.hpp>
00010 #include <kdl/framevel.hpp>
00011 #include <kdl/jntarray.hpp>
00012 #include <kdl/jntarrayvel.hpp>
00013 #include <Eigen/Dense>
00014
00015 using namespace std;
00016 using namespace RTT::detail;
00017 using namespace RTT;
00018 using namespace RTT::mqueue;
00019 using namespace RTT::types;
00020 using namespace Eigen;
00021
00022
00023 namespace boost{
00024 namespace serialization{
00025
00026 template< class Archive,
00027 class S,
00028 int Rows_,
00029 int Cols_,
00030 int Ops_,
00031 int MaxRows_,
00032 int MaxCols_>
00033 inline void save(
00034 Archive & ar,
00035 const Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & g,
00036 const unsigned int version)
00037 {
00038 int rows = g.rows();
00039 int cols = g.cols();
00040
00041 ar & rows;
00042 ar & cols;
00043 ar & boost::serialization::make_array(g.data(), rows * cols);
00044 }
00045
00046 template< class Archive,
00047 class S,
00048 int Rows_,
00049 int Cols_,
00050 int Ops_,
00051 int MaxRows_,
00052 int MaxCols_>
00053 inline void load(
00054 Archive & ar,
00055 Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & g,
00056 const unsigned int version)
00057 {
00058 int rows, cols;
00059 ar & rows;
00060 ar & cols;
00061 g.resize(rows, cols);
00062 ar & boost::serialization::make_array(g.data(), rows * cols);
00063 }
00064
00065 template< class Archive,
00066 class S,
00067 int Rows_,
00068 int Cols_,
00069 int Ops_,
00070 int MaxRows_,
00071 int MaxCols_>
00072 inline void serialize(
00073 Archive & ar,
00074 Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & g,
00075 const unsigned int version)
00076 {
00077 split_free(ar, g, version);
00078 }
00079
00080
00081 }
00082 }
00083
00084 namespace boost{
00085 namespace serialization{
00086 template<class Archive> inline void serialize(
00087 Archive & ar,
00088 KDL::Rotation& g,
00089 const unsigned int version)
00090 {
00091 ar & make_array(g.data,9);
00092 }
00093 template<class Archive> inline void serialize(
00094 Archive & ar,
00095 KDL::Vector& g,
00096 const unsigned int version)
00097 {
00098 ar & make_array(g.data,3);
00099 }
00100 template<class Archive> inline void serialize(
00101 Archive & ar,
00102 KDL::Twist& g,
00103 const unsigned int version)
00104 {
00105 ar & make_array(g.rot.data,3);
00106 ar & make_array(g.vel.data,3);
00107 }
00108 template<class Archive> inline void serialize(
00109 Archive & ar,
00110 KDL::Wrench& g,
00111 const unsigned int version)
00112 {
00113 ar & make_array(g.force.data,3);
00114 ar & make_array(g.torque.data,3);
00115 }
00116 template<class Archive> inline void serialize(
00117 Archive & ar,
00118 KDL::Frame& g,
00119 const unsigned int version)
00120 {
00121 ar & make_array(g.p.data,3);
00122 ar & make_array(g.M.data,9);
00123 }
00124 template<class Archive> inline void serialize(
00125 Archive & ar,
00126 KDL::Jacobian& g,
00127 const unsigned int version)
00128 {
00129 serialize(ar,g.data,version);
00130 }
00131 template<class Archive> inline void serialize(
00132 Archive & ar,
00133 KDL::JntArray& g,
00134 const unsigned int version)
00135 {
00136 serialize(ar,g.data,version);
00137 }
00138 template<class Archive> inline void serialize(
00139 Archive & ar,
00140 KDL::JntArrayVel& g,
00141 const unsigned int version)
00142 {
00143 serialize(ar,g.q.data,version);
00144 serialize(ar,g.qdot.data,version);
00145 }
00146
00147 }
00148 }
00149 namespace RTT {
00150 namespace mqueue {
00151 bool MQKDLPlugin::registerTransport(std::string name, TypeInfo* ti)
00152 {
00153 if ( name == "KDL.Vector" )
00154 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::Vector>() );
00155 if ( name == "KDL.Rotation" )
00156 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::Rotation>() );
00157 if ( name == "KDL.Frame" )
00158 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::Frame>() );
00159 if ( name == "KDL.Wrench" )
00160 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::Wrench>() );
00161 if ( name == "KDL.Twist" )
00162 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::Twist>() );
00163 if ( name == "KDL.Jacobian" )
00164 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::Jacobian>() );
00165 if ( name == "KDL.JntArray" )
00166 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::JntArray>() );
00167 if ( name == "KDL.JntArrayVel" )
00168 return ti->addProtocol(ORO_MQUEUE_PROTOCOL_ID, new MQSerializationProtocol<KDL::JntArrayVel>() );
00169
00170 return false;
00171 }
00172
00173 std::string MQKDLPlugin::getTransportName() const {
00174 return "mqueue";
00175 }
00176
00177 std::string MQKDLPlugin::getTypekitName() const {
00178 return "KDL";
00179 }
00180 std::string MQKDLPlugin::getName() const {
00181 return "kdl-mqueue-types";
00182 }
00183 }
00184 }
00185
00186 ORO_TYPEKIT_PLUGIN( RTT::mqueue::MQKDLPlugin )
00187