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