tf2_py.cpp
Go to the documentation of this file.
00001 #include <Python.h>
00002 
00003 #include <tf2/buffer_core.h>
00004 #include <tf2/exceptions.h>
00005 
00006 #include "python_compat.h"
00007 
00008 // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions)
00009 //
00010 #define WRAP(x) \
00011   do { \
00012   try \
00013   { \
00014     x; \
00015   }  \
00016   catch (const tf2::ConnectivityException &e) \
00017   { \
00018     PyErr_SetString(tf2_connectivityexception, e.what()); \
00019     return NULL; \
00020   } \
00021   catch (const tf2::LookupException &e) \
00022   { \
00023     PyErr_SetString(tf2_lookupexception, e.what()); \
00024     return NULL; \
00025   } \
00026   catch (const tf2::ExtrapolationException &e) \
00027   { \
00028     PyErr_SetString(tf2_extrapolationexception, e.what()); \
00029     return NULL; \
00030   } \
00031   catch (const tf2::InvalidArgumentException &e) \
00032   { \
00033     PyErr_SetString(tf2_invalidargumentexception, e.what()); \
00034     return NULL; \
00035   } \
00036   catch (const tf2::TimeoutException &e) \
00037   { \
00038     PyErr_SetString(tf2_timeoutexception, e.what()); \
00039     return NULL; \
00040   } \
00041   catch (const tf2::TransformException &e) \
00042   { \
00043     PyErr_SetString(tf2_exception, e.what()); \
00044     return NULL; \
00045   } \
00046   } while (0)
00047 
00048 static PyObject *pModulerospy = NULL;
00049 static PyObject *pModulegeometrymsgs = NULL;
00050 static PyObject *tf2_exception = NULL;
00051 static PyObject *tf2_connectivityexception = NULL, *tf2_lookupexception = NULL, *tf2_extrapolationexception = NULL,
00052                 *tf2_invalidargumentexception = NULL, *tf2_timeoutexception = NULL;
00053 
00054 struct buffer_core_t {
00055   PyObject_HEAD
00056   tf2::BufferCore *bc;
00057 };
00058 
00059 
00060 static PyTypeObject buffer_core_Type = {
00061 #if PY_MAJOR_VERSION < 3
00062   PyObject_HEAD_INIT(NULL)
00063   0,                               /*size*/
00064 # else
00065   PyVarObject_HEAD_INIT(NULL, 0)
00066 #endif
00067   "_tf2.BufferCore",               /*name*/
00068   sizeof(buffer_core_t),           /*basicsize*/
00069 };
00070 
00071 static PyObject *transform_converter(const geometry_msgs::TransformStamped* transform)
00072 {
00073   PyObject *pclass, *pargs, *pinst = NULL;
00074   pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped");
00075   if(pclass == NULL)
00076   {
00077     printf("Can't get geometry_msgs.msg.TransformedStamped\n");
00078     return NULL;
00079   }
00080 
00081   pargs = Py_BuildValue("()");
00082   if(pargs == NULL)
00083   {
00084     printf("Can't build argument list\n");
00085     return NULL;
00086   }
00087 
00088   pinst = PyEval_CallObject(pclass, pargs);
00089   Py_DECREF(pclass);
00090   Py_DECREF(pargs);
00091   if(pinst == NULL)
00092   {
00093     printf("Can't create class\n");
00094     return NULL;
00095   }
00096 
00097   //we need to convert the time to python
00098   PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time");
00099   PyObject *args = Py_BuildValue("ii", transform->header.stamp.sec, transform->header.stamp.nsec);
00100   PyObject *time_obj = PyObject_CallObject(rospy_time, args);
00101   Py_DECREF(args);
00102   Py_DECREF(rospy_time);
00103 
00104   PyObject* pheader = PyObject_GetAttrString(pinst, "header");
00105   PyObject_SetAttrString(pheader, "stamp", time_obj);
00106   Py_DECREF(time_obj);
00107 
00108   PyObject *frame_id = stringToPython(transform->header.frame_id);
00109   PyObject_SetAttrString(pheader, "frame_id", frame_id);
00110   Py_DECREF(frame_id);
00111   Py_DECREF(pheader);
00112 
00113   PyObject *ptransform = PyObject_GetAttrString(pinst, "transform");
00114   PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation");
00115   PyObject *protation = PyObject_GetAttrString(ptransform, "rotation");
00116   Py_DECREF(ptransform);
00117 
00118   PyObject *child_frame_id = stringToPython(transform->child_frame_id);
00119   PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id);
00120   Py_DECREF(child_frame_id);
00121 
00122   PyObject *trans_x = PyFloat_FromDouble(transform->transform.translation.x);
00123   PyObject *trans_y = PyFloat_FromDouble(transform->transform.translation.y);
00124   PyObject *trans_z = PyFloat_FromDouble(transform->transform.translation.z);
00125   PyObject_SetAttrString(ptranslation, "x", trans_x);
00126   PyObject_SetAttrString(ptranslation, "y", trans_y);
00127   PyObject_SetAttrString(ptranslation, "z", trans_z);
00128   Py_DECREF(trans_x);
00129   Py_DECREF(trans_y);
00130   Py_DECREF(trans_z);
00131   Py_DECREF(ptranslation);
00132 
00133   PyObject *rot_x = PyFloat_FromDouble(transform->transform.rotation.x);
00134   PyObject *rot_y = PyFloat_FromDouble(transform->transform.rotation.y);
00135   PyObject *rot_z = PyFloat_FromDouble(transform->transform.rotation.z);
00136   PyObject *rot_w = PyFloat_FromDouble(transform->transform.rotation.w);
00137   PyObject_SetAttrString(protation, "x", rot_x);
00138   PyObject_SetAttrString(protation, "y", rot_y);
00139   PyObject_SetAttrString(protation, "z", rot_z);
00140   PyObject_SetAttrString(protation, "w", rot_w);
00141   Py_DECREF(rot_x);
00142   Py_DECREF(rot_y);
00143   Py_DECREF(rot_z);
00144   Py_DECREF(rot_w);
00145   Py_DECREF(protation);
00146 
00147   return pinst;
00148 }
00149 
00150 static int rostime_converter(PyObject *obj, ros::Time *rt)
00151 {
00152   PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL);
00153   if (tsr == NULL) {
00154     PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
00155     return 0;
00156   } else {
00157     (*rt).fromSec(PyFloat_AsDouble(tsr));
00158     Py_DECREF(tsr);
00159     return 1;
00160   }
00161 }
00162 
00163 static int rosduration_converter(PyObject *obj, ros::Duration *rt)
00164 {
00165   PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL);
00166   if (tsr == NULL) {
00167     PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
00168     return 0;
00169   } else {
00170     (*rt).fromSec(PyFloat_AsDouble(tsr));
00171     Py_DECREF(tsr);
00172     return 1;
00173   }
00174 }
00175 
00176 static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw)
00177 {
00178   ros::Duration cache_time;
00179 
00180   cache_time.fromSec(tf2::BufferCore::DEFAULT_CACHE_TIME);
00181 
00182   if (!PyArg_ParseTuple(args, "|O&", rosduration_converter, &cache_time))
00183     return -1;
00184 
00185   ((buffer_core_t*)self)->bc = new tf2::BufferCore(cache_time);
00186 
00187   return 0;
00188 }
00189 
00190 /* This may need to be implemented later if we decide to have it in the core
00191 static PyObject *getTFPrefix(PyObject *self, PyObject *args)
00192 {
00193   if (!PyArg_ParseTuple(args, ""))
00194     return NULL;
00195   tf::Transformer *t = ((transformer_t*)self)->t;
00196   return stringToPython(t->getTFPrefix());
00197 }
00198 */
00199 
00200 static PyObject *allFramesAsYAML(PyObject *self, PyObject *args)
00201 {
00202   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00203   return stringToPython(bc->allFramesAsYAML());
00204 }
00205 
00206 static PyObject *allFramesAsString(PyObject *self, PyObject *args)
00207 {
00208   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00209   return stringToPython(bc->allFramesAsString());
00210 }
00211 
00212 static PyObject *canTransformCore(PyObject *self, PyObject *args, PyObject *kw)
00213 {
00214   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00215   char *target_frame, *source_frame;
00216   ros::Time time;
00217   static const char *keywords[] = { "target_frame", "source_frame", "time", NULL };
00218 
00219   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time))
00220     return NULL;
00221   std::string error_msg;
00222   bool can_transform = bc->canTransform(target_frame, source_frame, time, &error_msg);
00223   //return PyBool_FromLong(t->canTransform(target_frame, source_frame, time));
00224   return Py_BuildValue("bs", can_transform, error_msg.c_str());
00225 }
00226 
00227 static PyObject *canTransformFullCore(PyObject *self, PyObject *args, PyObject *kw)
00228 {
00229   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00230   char *target_frame, *source_frame, *fixed_frame;
00231   ros::Time target_time, source_time;
00232   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
00233 
00234   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
00235                         &target_frame,
00236                         rostime_converter,
00237                         &target_time,
00238                         &source_frame,
00239                         rostime_converter,
00240                         &source_time,
00241                         &fixed_frame))
00242     return NULL;
00243   std::string error_msg;
00244   bool can_transform = bc->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, &error_msg);
00245   //return PyBool_FromLong(t->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame));
00246   return Py_BuildValue("bs", can_transform, error_msg.c_str());
00247 }
00248 
00249 static PyObject *asListOfStrings(std::vector< std::string > los)
00250 {
00251   PyObject *r = PyList_New(los.size());
00252   size_t i;
00253   for (i = 0; i < los.size(); i++) {
00254     PyList_SetItem(r, i, stringToPython(los[i]));
00255   }
00256   return r;
00257 }
00258 
00259 static PyObject *_chain(PyObject *self, PyObject *args, PyObject *kw)
00260 {
00261   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00262   char *target_frame, *source_frame, *fixed_frame;
00263   ros::Time target_time, source_time;
00264   std::vector< std::string > output;
00265   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
00266 
00267   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
00268                         &target_frame,
00269                         rostime_converter,
00270                         &target_time,
00271                         &source_frame,
00272                         rostime_converter,
00273                         &source_time,
00274                         &fixed_frame))
00275     return NULL;
00276 
00277   WRAP(bc->_chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output));
00278   return asListOfStrings(output);
00279 }
00280 
00281 static PyObject *getLatestCommonTime(PyObject *self, PyObject *args)
00282 {
00283   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00284   char *target_frame, *source_frame;
00285   tf2::CompactFrameID target_id, source_id;
00286   ros::Time time;
00287   std::string error_string;
00288 
00289   if (!PyArg_ParseTuple(args, "ss", &target_frame, &source_frame))
00290     return NULL;
00291   WRAP(target_id = bc->_validateFrameId("get_latest_common_time", target_frame));
00292   WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame));
00293   int r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string);
00294   if (r == 0) {
00295     PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time");
00296     PyObject *args = Py_BuildValue("ii", time.sec, time.nsec);
00297     PyObject *ob = PyObject_CallObject(rospy_time, args);
00298     Py_DECREF(args);
00299     Py_DECREF(rospy_time);
00300     return ob;
00301   } else {
00302     PyErr_SetString(tf2_exception, error_string.c_str());
00303     return NULL;
00304   }
00305 }
00306 
00307 static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *kw)
00308 {
00309   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00310   char *target_frame, *source_frame;
00311   ros::Time time;
00312   static const char *keywords[] = { "target_frame", "source_frame", "time", NULL };
00313 
00314   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time))
00315     return NULL;
00316   geometry_msgs::TransformStamped transform;
00317   WRAP(transform = bc->lookupTransform(target_frame, source_frame, time));
00318   geometry_msgs::Vector3 origin = transform.transform.translation;
00319   geometry_msgs::Quaternion rotation = transform.transform.rotation;
00320   //TODO: Create a converter that will actually return a python message
00321   return Py_BuildValue("O&", transform_converter, &transform);
00322   //return Py_BuildValue("(ddd)(dddd)",
00323   //    origin.x, origin.y, origin.z,
00324   //    rotation.x, rotation.y, rotation.z, rotation.w);
00325 }
00326 
00327 static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObject *kw)
00328 {
00329   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00330   char *target_frame, *source_frame, *fixed_frame;
00331   ros::Time target_time, source_time;
00332   static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
00333 
00334   if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
00335                         &target_frame,
00336                         rostime_converter,
00337                         &target_time,
00338                         &source_frame,
00339                         rostime_converter,
00340                         &source_time,
00341                         &fixed_frame))
00342     return NULL;
00343   geometry_msgs::TransformStamped transform;
00344   WRAP(transform = bc->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame));
00345   geometry_msgs::Vector3 origin = transform.transform.translation;
00346   geometry_msgs::Quaternion rotation = transform.transform.rotation;
00347   //TODO: Create a converter that will actually return a python message
00348   return Py_BuildValue("O&", transform_converter, &transform);
00349 }
00350 /*
00351 static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw)
00352 {
00353   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00354   char *tracking_frame, *observation_frame;
00355   ros::Time time;
00356   ros::Duration averaging_interval;
00357   static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", NULL };
00358 
00359   if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval))
00360     return NULL;
00361   geometry_msgs::Twist twist;
00362   WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval));
00363 
00364   return Py_BuildValue("(ddd)(ddd)",
00365       twist.linear.x, twist.linear.y, twist.linear.z,
00366       twist.angular.x, twist.angular.y, twist.angular.z);
00367 }
00368 
00369 static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args)
00370 {
00371   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00372   char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame;
00373   ros::Time time;
00374   ros::Duration averaging_interval;
00375   double px, py, pz;
00376 
00377   if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&",
00378                         &tracking_frame,
00379                         &observation_frame,
00380                         &reference_frame,
00381                         &px, &py, &pz,
00382                         &reference_point_frame,
00383                         rostime_converter, &time,
00384                         rosduration_converter, &averaging_interval))
00385     return NULL;
00386   geometry_msgs::Twist twist;
00387   tf::Point pt(px, py, pz);
00388   WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval));
00389 
00390   return Py_BuildValue("(ddd)(ddd)",
00391       twist.linear.x, twist.linear.y, twist.linear.z,
00392       twist.angular.x, twist.angular.y, twist.angular.z);
00393 }
00394 */
00395 static inline int checkTranslationType(PyObject* o)
00396 {
00397   PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Vector3");
00398   int type_check = PyObject_TypeCheck(o, translation_type);
00399   int attr_check = PyObject_HasAttrString(o, "x") &&
00400                    PyObject_HasAttrString(o, "y") &&
00401                    PyObject_HasAttrString(o, "z");
00402   if (!type_check) {
00403     PyErr_WarnEx(PyExc_UserWarning, "translation should be of type Vector3", 1);
00404   }
00405   return attr_check;
00406 }
00407 
00408 static inline int checkRotationType(PyObject* o)
00409 {
00410   PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Quaternion");
00411   int type_check = PyObject_TypeCheck(o, rotation_type);
00412   int attr_check = PyObject_HasAttrString(o, "w") &&
00413                    PyObject_HasAttrString(o, "x") &&
00414                    PyObject_HasAttrString(o, "y") &&
00415                    PyObject_HasAttrString(o, "z");
00416   if (!type_check) {
00417     PyErr_WarnEx(PyExc_UserWarning, "rotation should be of type Quaternion", 1);
00418   }
00419   return attr_check;
00420 }
00421 
00422 static PyObject *setTransform(PyObject *self, PyObject *args)
00423 {
00424   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00425   PyObject *py_transform;
00426   char *authority;
00427 
00428   if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority))
00429     return NULL;
00430 
00431   geometry_msgs::TransformStamped transform;
00432   PyObject *header = pythonBorrowAttrString(py_transform, "header");
00433   transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id"));
00434   transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id"));
00435   if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1)
00436     return NULL;
00437 
00438   PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform");
00439 
00440   PyObject *translation = pythonBorrowAttrString(mtransform, "translation");
00441   if (!checkTranslationType(translation)) {
00442     PyErr_SetString(PyExc_TypeError, "transform.translation must have members x, y, z");
00443     return NULL;
00444   }
00445 
00446   transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x"));
00447   transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y"));
00448   transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z"));
00449 
00450   PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation");
00451   if (!checkRotationType(rotation)) {
00452     PyErr_SetString(PyExc_TypeError, "transform.rotation must have members w, x, y, z");
00453     return NULL;
00454   }
00455 
00456   transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x"));
00457   transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y"));
00458   transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z"));
00459   transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w"));
00460 
00461   bc->setTransform(transform, authority);
00462   Py_RETURN_NONE;
00463 }
00464 
00465 static PyObject *setTransformStatic(PyObject *self, PyObject *args)
00466 {
00467   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00468   PyObject *py_transform;
00469   char *authority;
00470 
00471   if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority))
00472     return NULL;
00473 
00474   geometry_msgs::TransformStamped transform;
00475   PyObject *header = pythonBorrowAttrString(py_transform, "header");
00476   transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id"));
00477   transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id"));
00478   if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1)
00479     return NULL;
00480 
00481   PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform");
00482   PyObject *translation = pythonBorrowAttrString(mtransform, "translation");
00483   if (!checkTranslationType(translation)) {
00484     PyErr_SetString(PyExc_TypeError, "transform.translation must be of type Vector3");
00485     return NULL;
00486   }
00487 
00488   transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x"));
00489   transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y"));
00490   transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z"));
00491 
00492   PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation");
00493   if (!checkRotationType(rotation)) {
00494     PyErr_SetString(PyExc_TypeError, "transform.rotation must be of type Quaternion");
00495     return NULL;
00496   }
00497 
00498   transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x"));
00499   transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y"));
00500   transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z"));
00501   transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w"));
00502 
00503   // only difference to above is is_static == True
00504   bc->setTransform(transform, authority, true);
00505   Py_RETURN_NONE;
00506 }
00507 
00508 static PyObject *clear(PyObject *self, PyObject *args)
00509 {
00510   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00511   bc->clear();
00512   Py_RETURN_NONE;
00513 }
00514 
00515 static PyObject *_frameExists(PyObject *self, PyObject *args)
00516 {
00517   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00518   char *frame_id_str;
00519   if (!PyArg_ParseTuple(args, "s", &frame_id_str))
00520     return NULL;
00521   return PyBool_FromLong(bc->_frameExists(frame_id_str));
00522 }
00523 
00524 static PyObject *_getFrameStrings(PyObject *self, PyObject *args)
00525 {
00526   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00527   std::vector< std::string > ids;
00528   bc->_getFrameStrings(ids);
00529   return asListOfStrings(ids);
00530 }
00531 
00532 static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw)
00533 {
00534   tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
00535   static const char *keywords[] = { "time", NULL };
00536   ros::Time time;
00537   if (!PyArg_ParseTupleAndKeywords(args, kw, "|O&", (char**)keywords, rostime_converter, &time))
00538     return NULL;
00539   return stringToPython(bc->_allFramesAsDot(time.toSec()));
00540 }
00541 
00542 
00543 static struct PyMethodDef buffer_core_methods[] =
00544 {
00545   {"all_frames_as_yaml", allFramesAsYAML, METH_VARARGS},
00546   {"all_frames_as_string", allFramesAsString, METH_VARARGS},
00547   {"set_transform", setTransform, METH_VARARGS},
00548   {"set_transform_static", setTransformStatic, METH_VARARGS},
00549   {"can_transform_core", (PyCFunction)canTransformCore, METH_VARARGS | METH_KEYWORDS},
00550   {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS},
00551   {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS},
00552   {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS},
00553   {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS},
00554   {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS},
00555   {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | METH_KEYWORDS},
00556   {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS},
00557   {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS},
00558   {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS},
00559   //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS},
00560   //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS},
00561   //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS},
00562   {NULL,          NULL}
00563 };
00564 
00565 static PyMethodDef module_methods[] = {
00566   // {"Transformer", mkTransformer, METH_VARARGS},
00567   {0, 0, 0},
00568 };
00569 
00570 bool staticInit() {
00571 #if PYTHON_API_VERSION >= 1007
00572   tf2_exception = PyErr_NewException((char*)"tf2.TransformException", NULL, NULL);
00573   tf2_connectivityexception = PyErr_NewException((char*)"tf2.ConnectivityException", tf2_exception, NULL);
00574   tf2_lookupexception = PyErr_NewException((char*)"tf2.LookupException", tf2_exception, NULL);
00575   tf2_extrapolationexception = PyErr_NewException((char*)"tf2.ExtrapolationException", tf2_exception, NULL);
00576   tf2_invalidargumentexception = PyErr_NewException((char*)"tf2.InvalidArgumentException", tf2_exception, NULL);
00577   tf2_timeoutexception = PyErr_NewException((char*)"tf2.TimeoutException", tf2_exception, NULL);
00578 #else
00579   tf2_exception = stringToPython("tf2.error");
00580   tf2_connectivityexception = stringToPython("tf2.ConnectivityException");
00581   tf2_lookupexception = stringToPython("tf2.LookupException");
00582   tf2_extrapolationexception = stringToPython("tf2.ExtrapolationException");
00583   tf2_invalidargumentexception = stringToPython("tf2.InvalidArgumentException");
00584   tf2_timeoutexception = stringToPython("tf2.TimeoutException");
00585 #endif
00586 
00587   pModulerospy        = pythonImport("rospy");
00588   pModulegeometrymsgs = pythonImport("geometry_msgs.msg");
00589 
00590   if(pModulegeometrymsgs == NULL)
00591   {
00592     printf("Cannot load geometry_msgs module");
00593     return false;
00594   }
00595 
00596   buffer_core_Type.tp_alloc = PyType_GenericAlloc;
00597   buffer_core_Type.tp_new = PyType_GenericNew;
00598   buffer_core_Type.tp_init = BufferCore_init;
00599   buffer_core_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE;
00600   buffer_core_Type.tp_methods = buffer_core_methods;
00601   if (PyType_Ready(&buffer_core_Type) != 0)
00602     return false;
00603   return true;
00604 }
00605 
00606 PyObject *moduleInit(PyObject *m) {
00607   PyModule_AddObject(m, "BufferCore", (PyObject *)&buffer_core_Type);
00608   PyObject *d = PyModule_GetDict(m);
00609   PyDict_SetItemString(d, "TransformException", tf2_exception);
00610   PyDict_SetItemString(d, "ConnectivityException", tf2_connectivityexception);
00611   PyDict_SetItemString(d, "LookupException", tf2_lookupexception);
00612   PyDict_SetItemString(d, "ExtrapolationException", tf2_extrapolationexception);
00613   PyDict_SetItemString(d, "InvalidArgumentException", tf2_invalidargumentexception);
00614   PyDict_SetItemString(d, "TimeoutException", tf2_timeoutexception);
00615   return m;
00616 }
00617 
00618 #if PY_MAJOR_VERSION < 3
00619 extern "C" void init_tf2()
00620 {
00621   if (!staticInit())
00622     return;
00623   moduleInit(Py_InitModule("_tf2", module_methods));
00624 }
00625 
00626 #else
00627 struct PyModuleDef tf_module = {
00628   PyModuleDef_HEAD_INIT, // base
00629   "_tf2",                // name
00630   NULL,                  // docstring
00631   -1,                    // state size (but we're using globals)
00632   module_methods         // methods
00633 };
00634 
00635 PyMODINIT_FUNC PyInit__tf2()
00636 {
00637   if (!staticInit())
00638     return NULL;
00639   return moduleInit(PyModule_Create(&tf_module));
00640 }
00641 #endif


tf2_py
Author(s):
autogenerated on Thu Jun 6 2019 20:22:59