1 #ifndef ORO_KDL_CORBAKDLCONVERSION_HPP 2 #define ORO_KDL_CORBAKDLCONVERSION_HPP 1 4 #include <kdl_typekit/transports/corba/KDLTypesC.h> 6 #include <kdl/frames.hpp> 7 #include <kdl/jacobian.hpp> 8 #include <kdl/jntarray.hpp> 21 log(
Debug)<<
"Converting type 'KDL::Vector' to sequence<CORBA::Double>." <<
endlog();
30 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
36 static bool update(
const CORBA::Any& any, StdType& _value) {
39 if ( any >>= result ) {
56 static bool toStdType(StdType& tp,
const CorbaType& cb){
57 log(
Debug)<<
"Converting type sequence<CORBA::Double> to 'KDL::Vector'" <<
endlog();
72 log(
Debug)<<
"Converting type 'KDL::Rotation' to sequence<CORBA::Double>." <<
endlog();
87 static bool toStdType(StdType& tp ,
const CorbaType& cb) {
88 log(
Debug)<<
"Converting type sequence<CORBA::Double> to 'KDL::Rotation'" <<
endlog();
89 tp =
StdType(cb[0],cb[1],cb[2],cb[3],cb[4],cb[5],cb[6],cb[7],cb[8]);
98 static bool update(
const CORBA::Any& any, StdType& _value) {
101 if ( any >>= result ) {
113 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
126 log(
Debug)<<
"Converting type 'KDL::Frame' to sequence<CORBA::Double>." <<
endlog();
132 ret[3] = orig.
M(0,0);
133 ret[4] = orig.
M(0,1);
134 ret[5] = orig.
M(0,2);
135 ret[6] = orig.
M(1,0);
136 ret[7] = orig.
M(1,1);
137 ret[8] = orig.
M(1,2);
138 ret[9] = orig.
M(2,0);
139 ret[10] = orig.
M(2,1);
140 ret[11] = orig.
M(2,2);
144 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
148 static bool update(
const CORBA::Any& any, StdType& _value) {
151 if ( any >>= result ) {
163 static bool toStdType(StdType& tp,
const CorbaType& cb) {
164 log(
Debug)<<
"Converting type sequence<CORBA::Double> to 'KDL::Frame'" <<
endlog();
193 log(
Debug)<<
"Converting type 'KDL::Wrench' to sequence<CORBA::Double>." <<
endlog();
205 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
211 static bool update(
const CORBA::Any& any, StdType& _value) {
214 if ( any >>= result ) {
226 static bool toStdType(StdType& tp,
const CorbaType& cb){
227 log(
Debug)<<
"Converting type sequence<CORBA::Double> to 'KDL::Wrench'" <<
endlog();
249 log(
Debug)<<
"Converting type 'KDL::Twist' to sequence<CORBA::Double>." <<
endlog();
252 ret[0] = orig.
vel.
x();
253 ret[1] = orig.
vel.
y();
254 ret[2] = orig.
vel.
z();
255 ret[3] = orig.
rot.
x();
256 ret[4] = orig.
rot.
y();
257 ret[5] = orig.
rot.
z();
261 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
267 static bool update(
const CORBA::Any& any, StdType& _value) {
270 if ( any >>= result ) {
287 static bool toStdType(StdType& tp,
const CorbaType& cb){
288 log(
Debug)<<
"Converting type sequence<CORBA::Double> to 'KDL::Twist'" <<
endlog();
303 static CorbaType*
toAny(
const StdType& tp) {
309 static bool update(
const CORBA::Any& any, StdType& _value) {
311 if ( any >>= result ) {
323 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
330 size_t rows =
static_cast<size_t>(tp.
data.rows());
331 size_t cols =
static_cast<size_t>(tp.
data.cols());
333 cb.length( (CORBA::ULong)(tp.
data.size() + 2) );
335 cb[0] =
static_cast<double>(rows);
336 cb[1] =
static_cast<double>(cols);
338 Eigen::Map<Eigen::MatrixXd>(cb.get_buffer() + 2 , rows, cols) = tp.
data;
343 static bool toStdType(StdType& tp,
const CorbaType& cb){
348 size_t rows =
static_cast<size_t>(cb[0]);
349 size_t cols =
static_cast<size_t>(cb[1]);
353 tp.
data = Eigen::MatrixXd::Map(cb.get_buffer()+2,rows,cols);
365 static CorbaType*
toAny(
const StdType& tp) {
371 static bool update(
const CORBA::Any& any, StdType& _value) {
373 if ( any >>= result ) {
385 static bool updateAny( StdType
const& t, CORBA::Any& any ) {
391 cb.length( (CORBA::ULong)(tp.
rows() ));
392 Eigen::Map<Eigen::VectorXd>(cb.get_buffer() , cb.length()) = tp.
data;
396 static bool toStdType(StdType& tp,
const CorbaType& cb){
398 tp.
data = Eigen::VectorXd::Map(cb.get_buffer() , cb.length());
405 #endif // ORO_KDL_CORBAKDLCONVERSION_HPP
KDL::Corba::DoubleSequence CorbaType
static CORBA::Any_ptr createAny(const StdType &t)
static CorbaType toAny(const KDL::Twist &orig)
static bool toStdType(StdType &tp, const CorbaType &cb)
static CorbaType * toAny(const StdType &tp)
static bool update(const CORBA::Any &any, StdType &_value)
static bool toStdType(StdType &tp, const CorbaType &cb)
static bool updateAny(StdType const &t, CORBA::Any &any)
unsigned int rows() const
static bool toCorbaType(CorbaType &cb, const StdType &tp)
void resize(unsigned int newNrOfColumns)
static CorbaType toAny(const KDL::Wrench &orig)
KDL::Corba::DoubleSequence CorbaType
static bool toCorbaType(CorbaType &cb, const StdType &tp)
static CORBA::Any_ptr createAny(const StdType &t)
static CorbaType toAny(const KDL::Vector &orig)
static CORBA::Any_ptr createAny(const StdType &t)
KDL::Corba::DoubleSequence CorbaType
static bool update(const CORBA::Any &any, StdType &_value)
static bool update(const CORBA::Any &any, StdType &_value)
static bool toCorbaType(CorbaType &cb, const StdType &tp)
static bool toStdType(StdType &tp, const CorbaType &cb)
static CorbaType toAny(const KDL::Rotation &orig)
static bool toStdType(StdType &tp, const CorbaType &cb)
static bool toStdType(StdType &tp, const CorbaType &cb)
static bool toCorbaType(CorbaType &cb, const StdType &tp)
static bool updateAny(StdType const &t, CORBA::Any &any)
static CorbaType toAny(const KDL::Frame &orig)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
KDL::Corba::DoubleSequence CorbaType
static CORBA::Any_ptr createAny(const StdType &t)
static CORBA::Any_ptr createAny(const KDL::Rotation &t)
static bool toStdType(StdType &tp, const CorbaType &cb)
static bool toCorbaType(CorbaType &cb, const StdType &tp)
static bool update(const CORBA::Any &any, StdType &_value)
static bool updateAny(StdType const &t, CORBA::Any &any)
static bool toStdType(StdType &tp, const CorbaType &cb)
static bool updateAny(StdType const &t, CORBA::Any &any)
KDL::Corba::DoubleSequence CorbaType
static bool updateAny(StdType const &t, CORBA::Any &any)
KDL::Corba::DoubleSequence CorbaType
static CorbaType * toAny(const StdType &tp)
static bool updateAny(StdType const &t, CORBA::Any &any)
static bool update(const CORBA::Any &any, StdType &_value)
static bool toCorbaType(CorbaType &cb, const StdType &tp)
void resize(unsigned int newSize)
static bool toStdType(StdType &tp, const CorbaType &cb)
static bool update(const CORBA::Any &any, StdType &_value)
static bool update(const CORBA::Any &any, StdType &_value)
KDL::Corba::DoubleSequence CorbaType
static CORBA::Any_ptr createAny(const StdType &t)
static bool toCorbaType(CorbaType &cb, const StdType &tp)
static bool toCorbaType(CorbaType &cb, const StdType &tp)
static Logger::LogFunction endlog()
static bool updateAny(StdType const &t, CORBA::Any &any)
static CORBA::Any_ptr createAny(const StdType &t)