00001 #include "KDLTypesC.h"
00002 #include <rtt/transports/corba/CorbaConversion.hpp>
00003 #include <kdl/frames.hpp>
00004
00005 namespace RTT
00006 {
00007 namespace corba{
00008
00009 template<>
00010 struct AnyConversion< KDL::Vector >
00011 {
00012 typedef KDL::Corba::DoubleSequence CorbaType;
00013 typedef KDL::Vector StdType;
00014 static CorbaType toAny(const KDL::Vector& orig) {
00015 log(Debug)<< "Converting type 'KDL::Vector' to sequence<CORBA::Double>." <<endlog();
00016 CorbaType ret;
00017 ret.length( 3 );
00018 ret[0] = orig.x();
00019 ret[1] = orig.y();
00020 ret[2] = orig.z();
00021 return ret;
00022 }
00023
00024 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00025 any <<= toAny( t );
00026 return true;
00027 }
00028
00029
00030 static bool update(const CORBA::Any& any, StdType& _value) {
00031 log(Debug)<< "update KDL::Vector" <<endlog();
00032 CorbaType* result;
00033 if ( any >>= result ) {
00034 return toStdType(_value,*result);
00035 }
00036 return false;
00037 }
00038
00039 static CORBA::Any_ptr createAny( const StdType& t ) {
00040 CORBA::Any_ptr ret = new CORBA::Any();
00041 *ret <<= toAny( t );
00042 return ret;
00043 }
00044
00045 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00046 cb = toAny(tp);
00047 return true;
00048 }
00049
00050 static bool toStdType(StdType& tp, const CorbaType& cb){
00051 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Vector'" <<endlog();
00052 tp.x(cb[0]);
00053 tp.y(cb[1]);
00054 tp.z(cb[2]);
00055 return true;
00056 }
00057
00058 };
00059
00060 template<>
00061 struct AnyConversion< KDL::Rotation >
00062 {
00063 typedef KDL::Corba::DoubleSequence CorbaType;
00064 typedef KDL::Rotation StdType;
00065 static CorbaType toAny(const KDL::Rotation& orig) {
00066 log(Debug)<< "Converting type 'KDL::Rotation' to sequence<CORBA::Double>." <<endlog();
00067 CorbaType ret;
00068 ret.length( 9 );
00069 ret[0] = orig(0,0);
00070 ret[1] = orig(0,1);
00071 ret[2] = orig(0,2);
00072 ret[3] = orig(1,0);
00073 ret[4] = orig(1,1);
00074 ret[5] = orig(1,2);
00075 ret[6] = orig(2,0);
00076 ret[7] = orig(2,1);
00077 ret[8] = orig(2,2);
00078 return ret;
00079 }
00080
00081 static bool toStdType(StdType& tp ,const CorbaType& cb) {
00082 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Rotation'" <<endlog();
00083 tp = StdType(cb[0],cb[1],cb[2],cb[3],cb[4],cb[5],cb[6],cb[7],cb[8]);
00084 return true;
00085 }
00086
00087 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00088 cb = toAny(tp);
00089 return true;
00090 }
00091
00092 static bool update(const CORBA::Any& any, StdType& _value) {
00093 log(Debug)<< "update KDL::Rotation" <<endlog();
00094 CorbaType* result;
00095 if ( any >>= result ) {
00096 return toStdType(_value,*result);
00097 }
00098 return false;
00099 }
00100
00101 static CORBA::Any_ptr createAny( const KDL::Rotation& t ) {
00102 CORBA::Any_ptr ret = new CORBA::Any();
00103 *ret <<= toAny( t );
00104 return ret;
00105 }
00106
00107 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00108 any <<= toAny( t );
00109 return true;
00110 }
00111
00112 };
00113
00114 template<>
00115 struct AnyConversion< KDL::Frame >
00116 {
00117 typedef KDL::Corba::DoubleSequence CorbaType;
00118 typedef KDL::Frame StdType;
00119 static CorbaType toAny(const KDL::Frame& orig) {
00120 log(Debug)<< "Converting type 'KDL::Frame' to sequence<CORBA::Double>." <<endlog();
00121 CorbaType ret;
00122 ret.length( 12 );
00123 ret[0] = orig.p.x();
00124 ret[1] = orig.p.y();
00125 ret[2] = orig.p.z();
00126 ret[3] = orig.M(0,0);
00127 ret[4] = orig.M(0,1);
00128 ret[5] = orig.M(0,2);
00129 ret[6] = orig.M(1,0);
00130 ret[7] = orig.M(1,1);
00131 ret[8] = orig.M(1,2);
00132 ret[9] = orig.M(2,0);
00133 ret[10] = orig.M(2,1);
00134 ret[11] = orig.M(2,2);
00135 return ret;
00136 }
00137
00138 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00139 any <<= toAny( t );
00140 return true;
00141 }
00142 static bool update(const CORBA::Any& any, StdType& _value) {
00143 log(Debug)<< "update KDL::Frame" <<endlog();
00144 CorbaType* result;
00145 if ( any >>= result ) {
00146 return toStdType(_value,*result);
00147 }
00148 return false;
00149 }
00150
00151 static CORBA::Any_ptr createAny( const StdType& t ) {
00152 CORBA::Any_ptr ret = new CORBA::Any();
00153 *ret <<= toAny( t );
00154 return ret;
00155 }
00156
00157 static bool toStdType(StdType& tp, const CorbaType& cb) {
00158 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Frame'" <<endlog();
00159 tp.p.x(cb[0]);
00160 tp.p.y(cb[1]);
00161 tp.p.z(cb[2]);
00162 tp.M(0,0)=cb[3];
00163 tp.M(0,1)=cb[4];
00164 tp.M(0,2)=cb[5];
00165 tp.M(1,0)=cb[6];
00166 tp.M(1,1)=cb[7];
00167 tp.M(1,2)=cb[8];
00168 tp.M(2,0)=cb[9];
00169 tp.M(2,1)=cb[10];
00170 tp.M(2,2)=cb[11];
00171 return true;
00172 }
00173
00174 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00175 cb = toAny(tp);
00176 return true;
00177 }
00178
00179 };
00180
00181 template<>
00182 struct AnyConversion< KDL::Wrench >
00183 {
00184 typedef KDL::Corba::DoubleSequence CorbaType;
00185 typedef KDL::Wrench StdType;
00186 static CorbaType toAny(const KDL::Wrench& orig) {
00187 log(Debug)<< "Converting type 'KDL::Wrench' to sequence<CORBA::Double>." <<endlog();
00188 CorbaType ret;
00189 ret.length( 6 );
00190 ret[0] = orig.force.x();
00191 ret[1] = orig.force.y();
00192 ret[2] = orig.force.z();
00193 ret[3] = orig.torque.x();
00194 ret[4] = orig.torque.y();
00195 ret[5] = orig.torque.z();
00196 return ret;
00197 }
00198
00199 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00200 any <<= toAny( t );
00201 return true;
00202 }
00203
00204
00205 static bool update(const CORBA::Any& any, StdType& _value) {
00206 log(Debug)<< "update KDL::Vector" <<endlog();
00207 CorbaType* result;
00208 if ( any >>= result ) {
00209 return toStdType(_value,*result);
00210 }
00211 return false;
00212 }
00213
00214 static CORBA::Any_ptr createAny( const StdType& t ) {
00215 CORBA::Any_ptr ret = new CORBA::Any();
00216 *ret <<= toAny( t );
00217 return ret;
00218 }
00219
00220 static bool toStdType(StdType& tp, const CorbaType& cb){
00221 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Wrench'" <<endlog();
00222 tp.force.x(cb[0]);
00223 tp.force.y(cb[1]);
00224 tp.force.z(cb[2]);
00225 tp.torque.x(cb[3]);
00226 tp.torque.y(cb[4]);
00227 tp.torque.z(cb[5]);
00228 return true;
00229 }
00230 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00231 cb = toAny(tp);
00232 return true;
00233 }
00234
00235 };
00236
00237 template<>
00238 struct AnyConversion< KDL::Twist >
00239 {
00240 typedef KDL::Corba::DoubleSequence CorbaType;
00241 typedef KDL::Twist StdType;
00242 static CorbaType toAny(const KDL::Twist& orig) {
00243 log(Debug)<< "Converting type 'KDL::Twist' to sequence<CORBA::Double>." <<endlog();
00244 CorbaType ret;
00245 ret.length( 6 );
00246 ret[0] = orig.vel.x();
00247 ret[1] = orig.vel.y();
00248 ret[2] = orig.vel.z();
00249 ret[3] = orig.rot.x();
00250 ret[4] = orig.rot.y();
00251 ret[5] = orig.rot.z();
00252 return ret;
00253 }
00254
00255 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00256 any <<= toAny( t );
00257 return true;
00258 }
00259
00260
00261 static bool update(const CORBA::Any& any, StdType& _value) {
00262 log(Debug)<< "update KDL::Vector" <<endlog();
00263 CorbaType* result;
00264 if ( any >>= result ) {
00265 return toStdType(_value,*result);
00266 }
00267 return false;
00268 }
00269
00270 static CORBA::Any_ptr createAny( const StdType& t ) {
00271 CORBA::Any_ptr ret = new CORBA::Any();
00272 *ret <<= toAny( t );
00273 return ret;
00274 }
00275
00276 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00277 cb = toAny(tp);
00278 return true;
00279 }
00280
00281 static bool toStdType(StdType& tp, const CorbaType& cb){
00282 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Twist'" <<endlog();
00283 tp.vel.x(cb[0]);
00284 tp.vel.y(cb[1]);
00285 tp.vel.z(cb[2]);
00286 tp.rot.x(cb[3]);
00287 tp.rot.y(cb[4]);
00288 tp.rot.z(cb[5]);
00289 return true;
00290 }
00291 };
00292 };
00293 };
00294