tf2: tf2: tf2_py.cpp Source File

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