tf: tf: pytf.cpp Source File

00001 #include <Python.h>
00002 
00003 #include "tf/tf.h"
00004 
00005 // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions)
00006 //
00007 #define WRAP(x) \
00008   do { \
00009   try \
00010   { \
00011     x; \
00012   }  \
00013   catch (const tf::ConnectivityException &e) \
00014   { \
00015     PyErr_SetString(tf_connectivityexception, e.what()); \
00016     return NULL; \
00017   } \
00018   catch (const tf::LookupException &e) \
00019   { \
00020     PyErr_SetString(tf_lookupexception, e.what()); \
00021     return NULL; \
00022   } \
00023   catch (const tf::ExtrapolationException &e) \
00024   { \
00025     PyErr_SetString(tf_extrapolationexception, e.what()); \
00026     return NULL; \
00027   } \
00028   } while (0)
00029 
00030 static PyObject *pModulerospy = NULL;
00031 static PyObject *tf_exception = NULL;
00032 static PyObject *tf_connectivityexception = NULL, *tf_lookupexception = NULL, *tf_extrapolationexception = NULL;
00033 
00034 struct transformer_t {
00035   PyObject_HEAD
00036   tf::Transformer *t;
00037 };
00038 
00039 static PyTypeObject transformer_Type = {
00040   PyObject_HEAD_INIT(&PyType_Type)
00041   0,                               /*size*/
00042   "_tf.Transformer",                /*name*/
00043   sizeof(transformer_t),           /*basicsize*/
00044 };
00045 
00046 static PyObject *PyObject_BorrowAttrString(PyObject* o, const char *name)
00047 {
00048     PyObject *r = PyObject_GetAttrString(o, name);
00049     if (r != NULL)
00050       Py_DECREF(r);
00051     return r;
00052 }
00053 
00054 static int rostime_converter(PyObject *obj, ros::Time *rt)
00055 {
00056   PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL);
00057   if (tsr == NULL) {
00058     PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
00059     return 0;
00060   } else {
00061     (*rt).fromSec(PyFloat_AsDouble(tsr));
00062     Py_DECREF(tsr);
00063     return 1;
00064   }
00065 }
00066 
00067 static int rosduration_converter(PyObject *obj, ros::Duration *rt)
00068 {
00069   PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL);
00070   if (tsr == NULL) {
00071     PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
00072     return 0;
00073   } else {
00074     (*rt).fromSec(PyFloat_AsDouble(tsr));
00075     Py_DECREF(tsr);
00076     return 1;
00077   }
00078 }
00079 
00080 static int Transformer_init(PyObject *self, PyObject *args, PyObject *kw)
00081 {
00082   int interpolating = 1;
00083   ros::Duration cache_time;
00084 
00085   cache_time.fromSec(tf::Transformer::DEFAULT_CACHE_TIME);
00086 
00087   if (!PyArg_ParseTuple(args, "|iO&", &interpolating, rosduration_converter, &cache_time))
00088     return -1;
00089 
00090   ((transformer_t*)self)->t = new tf::Transformer(interpolating, cache_time);
00091   ((transformer_t*)self)->t->fall_back_to_wall_time_ = true;
00092 
00093   return 0;
00094 }
00095 
00096 static PyObject *setUsingDedicatedThread(PyObject *self, PyObject *args)
00097 {
00098   int value;
00099   if (!PyArg_ParseTuple(args, "i", &value))
00100     return NULL;
00101   tf::Transformer *t = ((transformer_t*)self)->t;
00102   t->setUsingDedicatedThread(value);
00103   return PyString_FromString(t->allFramesAsDot().c_str());
00104 }
00105 
00106 static PyObject *getTFPrefix(PyObject *self, PyObject *args)
00107 {
00108   if (!PyArg_ParseTuple(args, ""))
00109     return NULL;
00110   tf::Transformer *t = ((transformer_t*)self)->t;
00111   return PyString_FromString(t->getTFPrefix().c_str());
00112 }
00113 
00114 static PyObject *allFramesAsDot(PyObject *self, PyObject *args)
00115 {
00116   tf::Transformer *t = ((transformer_t*)self)->t;
00117   return PyString_FromString(t->allFramesAsDot().c_str());
00118 }
00119 
00120 static PyObject *allFramesAsString(PyObject *self, PyObject *args)
00121 {
00122   tf::Transformer *t = ((transformer_t*)self)->t;
00123   return PyString_FromString(t->allFramesAsString().c_str());
00124 }
00125 
00126 static PyObject *canTransform(PyObject *self, PyObject *args, PyObject *kw)
00127 {
00128   tf::Transformer *t = ((transformer_t*)self)->t;
00129   char *target_frame, *source_frame;
00130   ros::Time time;
00131   static const char *keywords[] = { "target_frame", "source_frame", "time", NULL };
00132 
00133   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time))
00134     return NULL;
00135   return PyBool_FromLong(t->canTransform(target_frame, source_frame, time));
00136 }
00137 
00138 static PyObject *canTransformFull(PyObject *self, PyObject *args, PyObject *kw)
00139 {
00140   tf::Transformer *t = ((transformer_t*)self)->t;
00141   char *target_frame, *source_frame, *fixed_frame;
00142   ros::Time target_time, source_time;
00143   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
00144 
00145   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
00146                         &target_frame,
00147                         rostime_converter,
00148                         &target_time,
00149                         &source_frame,
00150                         rostime_converter,
00151                         &source_time,
00152                         &fixed_frame))
00153     return NULL;
00154   return PyBool_FromLong(t->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame));
00155 }
00156 
00157 static PyObject *waitForTransform(PyObject *self, PyObject *args, PyObject *kw)
00158 {
00159   tf::Transformer *t = ((transformer_t*)self)->t;
00160   char *target_frame, *source_frame;
00161   ros::Time time;
00162   ros::Duration timeout;
00163   ros::Duration polling_sleep_duration(0.01);
00164   std::string error_string;
00165   static const char *keywords[] = { "target_frame", "source_frame", "time", "timeout", "polling_sleep_duration", NULL };
00166 
00167   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&|O&", (char**)keywords,
00168     &target_frame,
00169     &source_frame,
00170     rostime_converter, &time,
00171     rosduration_converter, &timeout,
00172     rosduration_converter, &polling_sleep_duration))
00173     return NULL;
00174   bool r;
00175   Py_BEGIN_ALLOW_THREADS
00176   r = t->waitForTransform(target_frame, source_frame, time, timeout, polling_sleep_duration, &error_string);
00177   Py_END_ALLOW_THREADS
00178   if (r == true) {
00179     Py_RETURN_NONE;
00180   } else {
00181     PyErr_SetString(tf_exception, error_string.c_str());
00182     return NULL;
00183   }
00184 }
00185 
00186 static PyObject *waitForTransformFull(PyObject *self, PyObject *args, PyObject *kw)
00187 {
00188   tf::Transformer *t = ((transformer_t*)self)->t;
00189   char *target_frame, *source_frame, *fixed_frame;
00190   ros::Time target_time, source_time;
00191   ros::Duration timeout;
00192   ros::Duration polling_sleep_duration(0.01);
00193   std::string error_string;
00194   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", "timeout", "polling_sleep_duration", NULL };
00195 
00196   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&sO&|O&", (char**)keywords,
00197                         &target_frame,
00198                         rostime_converter,
00199                         &target_time,
00200                         &source_frame,
00201                         rostime_converter,
00202                         &source_time,
00203                         &fixed_frame,
00204                         rosduration_converter, &timeout,
00205                         rosduration_converter, &polling_sleep_duration))
00206     return NULL;
00207   int r;
00208   Py_BEGIN_ALLOW_THREADS
00209   r = t->waitForTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout, polling_sleep_duration, &error_string);
00210   Py_END_ALLOW_THREADS
00211   if (r == true) {
00212     Py_RETURN_NONE;
00213   } else {
00214     PyErr_SetString(tf_exception, error_string.c_str());
00215     return NULL;
00216   }
00217 }
00218 
00219 static PyObject *asListOfStrings(std::vector< std::string > los)
00220 {
00221   PyObject *r = PyList_New(los.size());
00222   size_t i;
00223   for (i = 0; i < los.size(); i++) {
00224     PyList_SetItem(r, i, PyString_FromString(los[i].c_str()));
00225   }
00226   return r;
00227 }
00228 
00229 static PyObject *chain(PyObject *self, PyObject *args, PyObject *kw)
00230 {
00231   tf::Transformer *t = ((transformer_t*)self)->t;
00232   char *target_frame, *source_frame, *fixed_frame;
00233   ros::Time target_time, source_time;
00234   std::vector< std::string > output;
00235   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
00236 
00237   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
00238                         &target_frame,
00239                         rostime_converter,
00240                         &target_time,
00241                         &source_frame,
00242                         rostime_converter,
00243                         &source_time,
00244                         &fixed_frame))
00245     return NULL;
00246 
00247   WRAP(t->chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output));
00248   return asListOfStrings(output);
00249 }
00250 
00251 static PyObject *getLatestCommonTime(PyObject *self, PyObject *args, PyObject *kw)
00252 {
00253   tf::Transformer *t = ((transformer_t*)self)->t;
00254   char *source, *dest;
00255   std::string error_string;
00256   ros::Time time;
00257 
00258   if (!PyArg_ParseTuple(args, "ss", &source, &dest))
00259     return NULL;
00260   int r = t->getLatestCommonTime(source, dest, time, &error_string);
00261   if (r == 0) {
00262     PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time");
00263     PyObject *args = Py_BuildValue("ii", time.sec, time.nsec);
00264     PyObject *ob = PyObject_CallObject(rospy_time, args);
00265     Py_DECREF(args);
00266     Py_DECREF(rospy_time);
00267     return ob;
00268   } else {
00269     PyErr_SetString(tf_exception, error_string.c_str());
00270     return NULL;
00271   }
00272 }
00273 
00274 static PyObject *lookupTransform(PyObject *self, PyObject *args, PyObject *kw)
00275 {
00276   tf::Transformer *t = ((transformer_t*)self)->t;
00277   char *target_frame, *source_frame;
00278   ros::Time time;
00279   static const char *keywords[] = { "target_frame", "source_frame", "time", NULL };
00280 
00281   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time))
00282     return NULL;
00283   tf::StampedTransform transform;
00284   WRAP(t->lookupTransform(target_frame, source_frame, time, transform));
00285   btVector3 origin = transform.getOrigin();
00286   btQuaternion rotation = transform.getRotation();
00287   return Py_BuildValue("(ddd)(dddd)",
00288       origin.x(), origin.y(), origin.z(),
00289       rotation.x(), rotation.y(), rotation.z(), rotation.w());
00290 }
00291 
00292 static PyObject *lookupTransformFull(PyObject *self, PyObject *args, PyObject *kw)
00293 {
00294   tf::Transformer *t = ((transformer_t*)self)->t;
00295   char *target_frame, *source_frame, *fixed_frame;
00296   ros::Time target_time, source_time;
00297   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
00298 
00299   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
00300                         &target_frame,
00301                         rostime_converter,
00302                         &target_time,
00303                         &source_frame,
00304                         rostime_converter,
00305                         &source_time,
00306                         &fixed_frame))
00307     return NULL;
00308   tf::StampedTransform transform;
00309   WRAP(t->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, transform));
00310   btVector3 origin = transform.getOrigin();
00311   btQuaternion rotation = transform.getRotation();
00312   return Py_BuildValue("(ddd)(dddd)",
00313       origin.x(), origin.y(), origin.z(),
00314       rotation.x(), rotation.y(), rotation.z(), rotation.w());
00315 }
00316 
00317 static PyObject *lookupTwist(PyObject *self, PyObject *args, PyObject *kw)
00318 {
00319   tf::Transformer *t = ((transformer_t*)self)->t;
00320   char *tracking_frame, *observation_frame;
00321   ros::Time time;
00322   ros::Duration averaging_interval;
00323   static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", NULL };
00324 
00325   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval))
00326     return NULL;
00327   geometry_msgs::Twist twist;
00328   WRAP(t->lookupTwist(tracking_frame, observation_frame, time, averaging_interval, twist));
00329 
00330   return Py_BuildValue("(ddd)(ddd)",
00331       twist.linear.x, twist.linear.y, twist.linear.z,
00332       twist.angular.x, twist.angular.y, twist.angular.z);
00333 }
00334 
00335 static PyObject *lookupTwistFull(PyObject *self, PyObject *args)
00336 {
00337   tf::Transformer *t = ((transformer_t*)self)->t;
00338   char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame;
00339   ros::Time time;
00340   ros::Duration averaging_interval;
00341   double px, py, pz;
00342 
00343   if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&",
00344                         &tracking_frame,
00345                         &observation_frame,
00346                         &reference_frame,
00347                         &px, &py, &pz,
00348                         &reference_point_frame,
00349                         rostime_converter, &time,
00350                         rosduration_converter, &averaging_interval))
00351     return NULL;
00352   geometry_msgs::Twist twist;
00353   tf::Point pt(px, py, pz);
00354   WRAP(t->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval, twist));
00355 
00356   return Py_BuildValue("(ddd)(ddd)",
00357       twist.linear.x, twist.linear.y, twist.linear.z,
00358       twist.angular.x, twist.angular.y, twist.angular.z);
00359 }
00360 
00361 static PyObject *setTransform(PyObject *self, PyObject *args)
00362 {
00363   tf::Transformer *t = ((transformer_t*)self)->t;
00364   PyObject *py_transform;
00365   char *authority = (char*)"default_authority";
00366 
00367   if (!PyArg_ParseTuple(args, "O|s", &py_transform, &authority))
00368     return NULL;
00369   tf::StampedTransform transform;
00370   PyObject *header = PyObject_BorrowAttrString(py_transform, "header");
00371   transform.child_frame_id_ = PyString_AsString(PyObject_BorrowAttrString(py_transform, "child_frame_id"));
00372   transform.frame_id_ = PyString_AsString(PyObject_BorrowAttrString(header, "frame_id"));
00373   if (rostime_converter(PyObject_BorrowAttrString(header, "stamp"), &transform.stamp_) != 1)
00374     return NULL;
00375 
00376   PyObject *mtransform = PyObject_BorrowAttrString(py_transform, "transform");
00377   PyObject *translation = PyObject_BorrowAttrString(mtransform, "translation");
00378   double tx = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "x"));
00379   double ty = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "y"));
00380   double tz = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "z"));
00381   PyObject *rotation = PyObject_BorrowAttrString(mtransform, "rotation");
00382   double qx = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "x"));
00383   double qy = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "y"));
00384   double qz = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "z"));
00385   double qw = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "w"));
00386 
00387   transform.setData(btTransform(
00388     btQuaternion(btScalar(qx), btScalar(qy), btScalar(qz), btScalar(qw)),
00389     btVector3(btScalar(tx), btScalar(ty), btScalar(tz))));
00390   t->setTransform(transform, authority);
00391   Py_RETURN_NONE;
00392 }
00393 
00394 static PyObject *clear(PyObject *self, PyObject *args)
00395 {
00396   tf::Transformer *t = ((transformer_t*)self)->t;
00397   t->clear();
00398   Py_RETURN_NONE;
00399 }
00400 
00401 static PyObject *frameExists(PyObject *self, PyObject *args)
00402 {
00403   tf::Transformer *t = ((transformer_t*)self)->t;
00404   char *frame_id_str;
00405   if (!PyArg_ParseTuple(args, "s", &frame_id_str))
00406     return NULL;
00407   return PyBool_FromLong(t->frameExists(frame_id_str));
00408 }
00409 
00410 static PyObject *getFrameStrings(PyObject *self, PyObject *args)
00411 {
00412   tf::Transformer *t = ((transformer_t*)self)->t;
00413   std::vector< std::string > ids;
00414   t->getFrameStrings(ids);
00415   return asListOfStrings(ids);
00416 }
00417 
00418 static struct PyMethodDef transformer_methods[] =
00419 {
00420   {"allFramesAsDot", allFramesAsDot, METH_VARARGS},
00421   {"allFramesAsString", allFramesAsString, METH_VARARGS},
00422   {"setTransform", setTransform, METH_VARARGS},
00423   {"canTransform", (PyCFunction)canTransform, METH_KEYWORDS},
00424   {"canTransformFull", (PyCFunction)canTransformFull, METH_KEYWORDS},
00425   {"waitForTransform", (PyCFunction)waitForTransform, METH_KEYWORDS},
00426   {"waitForTransformFull", (PyCFunction)waitForTransformFull, METH_KEYWORDS},
00427   {"chain", (PyCFunction)chain, METH_KEYWORDS},
00428   {"clear", (PyCFunction)clear, METH_KEYWORDS},
00429   {"frameExists", (PyCFunction)frameExists, METH_VARARGS},
00430   {"getFrameStrings", (PyCFunction)getFrameStrings, METH_VARARGS},
00431   {"getLatestCommonTime", (PyCFunction)getLatestCommonTime, METH_VARARGS},
00432   {"lookupTransform", (PyCFunction)lookupTransform, METH_KEYWORDS},
00433   {"lookupTransformFull", (PyCFunction)lookupTransformFull, METH_KEYWORDS},
00434   {"lookupTwist", (PyCFunction)lookupTwist, METH_KEYWORDS},
00435   {"lookupTwistFull", lookupTwistFull, METH_VARARGS},
00436   {"setUsingDedicatedThread", (PyCFunction)setUsingDedicatedThread, METH_VARARGS},
00437   {"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS},
00438   {NULL,          NULL}
00439 };
00440 
00441 static PyMethodDef module_methods[] = {
00442   // {"Transformer", mkTransformer, METH_VARARGS},
00443   {NULL, NULL, NULL},
00444 };
00445 
00446 extern "C" void init_tf()
00447 {
00448   PyObject *item, *m, *d;
00449 
00450 #if PYTHON_API_VERSION >= 1007
00451   tf_exception = PyErr_NewException((char*)"tf.Exception", NULL, NULL);
00452   tf_connectivityexception = PyErr_NewException((char*)"tf.ConnectivityException", tf_exception, NULL);
00453   tf_lookupexception = PyErr_NewException((char*)"tf.LookupException", tf_exception, NULL);
00454   tf_extrapolationexception = PyErr_NewException((char*)"tf.ExtrapolationException", tf_exception, NULL);
00455 #else
00456   tf_exception = PyString_FromString("tf.error");
00457   tf_connectivityexception = PyString_FromString("tf.ConnectivityException");
00458   tf_lookupexception = PyString_FromString("tf.LookupException");
00459   tf_extrapolationexception = PyString_FromString("tf.ExtrapolationException");
00460 #endif
00461 
00462   pModulerospy = PyImport_Import(item= PyString_FromString("rospy")); Py_DECREF(item);
00463 
00464   transformer_Type.tp_alloc = PyType_GenericAlloc;
00465   transformer_Type.tp_new = PyType_GenericNew;
00466   transformer_Type.tp_init = Transformer_init;
00467   transformer_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE;
00468   transformer_Type.tp_methods = transformer_methods;
00469   if (PyType_Ready(&transformer_Type) != 0)
00470     return;
00471 
00472   m = Py_InitModule("_tf", module_methods);
00473   PyModule_AddObject(m, "Transformer", (PyObject *)&transformer_Type);
00474   d = PyModule_GetDict(m);
00475   PyDict_SetItemString(d, "Exception", tf_exception);
00476   PyDict_SetItemString(d, "ConnectivityException", tf_connectivityexception);
00477   PyDict_SetItemString(d, "LookupException", tf_lookupexception);
00478   PyDict_SetItemString(d, "ExtrapolationException", tf_extrapolationexception);
00479 }