tf2_py.cpp
Go to the documentation of this file.
1 #include <Python.h>
2 
3 #include <tf2/buffer_core.h>
4 #include <tf2/exceptions.h>
5 
6 #include "python_compat.h"
7 
8 // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions)
9 //
10 #define WRAP(x) \
11  do { \
12  try \
13  { \
14  x; \
15  } \
16  catch (const tf2::ConnectivityException &e) \
17  { \
18  PyErr_SetString(tf2_connectivityexception, e.what()); \
19  return NULL; \
20  } \
21  catch (const tf2::LookupException &e) \
22  { \
23  PyErr_SetString(tf2_lookupexception, e.what()); \
24  return NULL; \
25  } \
26  catch (const tf2::ExtrapolationException &e) \
27  { \
28  PyErr_SetString(tf2_extrapolationexception, e.what()); \
29  return NULL; \
30  } \
31  catch (const tf2::InvalidArgumentException &e) \
32  { \
33  PyErr_SetString(tf2_invalidargumentexception, e.what()); \
34  return NULL; \
35  } \
36  catch (const tf2::TimeoutException &e) \
37  { \
38  PyErr_SetString(tf2_timeoutexception, e.what()); \
39  return NULL; \
40  } \
41  catch (const tf2::TransformException &e) \
42  { \
43  PyErr_SetString(tf2_exception, e.what()); \
44  return NULL; \
45  } \
46  } while (0)
47 
48 static PyObject *pModulerospy = NULL;
49 static PyObject *pModulegeometrymsgs = NULL;
50 static PyObject *tf2_exception = NULL;
53 
54 struct buffer_core_t {
55  PyObject_HEAD
57 };
58 
59 
60 static PyTypeObject buffer_core_Type = {
61 #if PY_MAJOR_VERSION < 3
62  PyObject_HEAD_INIT(NULL)
63  0, /*size*/
64 # else
65  PyVarObject_HEAD_INIT(NULL, 0)
66 #endif
67  "_tf2.BufferCore", /*name*/
68  sizeof(buffer_core_t), /*basicsize*/
69 };
70 
71 static PyObject *transform_converter(const geometry_msgs::TransformStamped* transform)
72 {
73  PyObject *pclass, *pargs, *pinst = NULL;
74  pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped");
75  if(pclass == NULL)
76  {
77  printf("Can't get geometry_msgs.msg.TransformedStamped\n");
78  return NULL;
79  }
80 
81  pargs = Py_BuildValue("()");
82  if(pargs == NULL)
83  {
84  printf("Can't build argument list\n");
85  return NULL;
86  }
87 
88  pinst = PyEval_CallObject(pclass, pargs);
89  Py_DECREF(pclass);
90  Py_DECREF(pargs);
91  if(pinst == NULL)
92  {
93  printf("Can't create class\n");
94  return NULL;
95  }
96 
97  //we need to convert the time to python
98  PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time");
99  PyObject *args = Py_BuildValue("ii", transform->header.stamp.sec, transform->header.stamp.nsec);
100  PyObject *time_obj = PyObject_CallObject(rospy_time, args);
101  Py_DECREF(args);
102  Py_DECREF(rospy_time);
103 
104  PyObject* pheader = PyObject_GetAttrString(pinst, "header");
105  PyObject_SetAttrString(pheader, "stamp", time_obj);
106  Py_DECREF(time_obj);
107 
108  PyObject *frame_id = stringToPython(transform->header.frame_id);
109  PyObject_SetAttrString(pheader, "frame_id", frame_id);
110  Py_DECREF(frame_id);
111  Py_DECREF(pheader);
112 
113  PyObject *ptransform = PyObject_GetAttrString(pinst, "transform");
114  PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation");
115  PyObject *protation = PyObject_GetAttrString(ptransform, "rotation");
116  Py_DECREF(ptransform);
117 
118  PyObject *child_frame_id = stringToPython(transform->child_frame_id);
119  PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id);
120  Py_DECREF(child_frame_id);
121 
122  PyObject *trans_x = PyFloat_FromDouble(transform->transform.translation.x);
123  PyObject *trans_y = PyFloat_FromDouble(transform->transform.translation.y);
124  PyObject *trans_z = PyFloat_FromDouble(transform->transform.translation.z);
125  PyObject_SetAttrString(ptranslation, "x", trans_x);
126  PyObject_SetAttrString(ptranslation, "y", trans_y);
127  PyObject_SetAttrString(ptranslation, "z", trans_z);
128  Py_DECREF(trans_x);
129  Py_DECREF(trans_y);
130  Py_DECREF(trans_z);
131  Py_DECREF(ptranslation);
132 
133  PyObject *rot_x = PyFloat_FromDouble(transform->transform.rotation.x);
134  PyObject *rot_y = PyFloat_FromDouble(transform->transform.rotation.y);
135  PyObject *rot_z = PyFloat_FromDouble(transform->transform.rotation.z);
136  PyObject *rot_w = PyFloat_FromDouble(transform->transform.rotation.w);
137  PyObject_SetAttrString(protation, "x", rot_x);
138  PyObject_SetAttrString(protation, "y", rot_y);
139  PyObject_SetAttrString(protation, "z", rot_z);
140  PyObject_SetAttrString(protation, "w", rot_w);
141  Py_DECREF(rot_x);
142  Py_DECREF(rot_y);
143  Py_DECREF(rot_z);
144  Py_DECREF(rot_w);
145  Py_DECREF(protation);
146 
147  return pinst;
148 }
149 
150 static int rostime_converter(PyObject *obj, ros::Time *rt)
151 {
152  PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL);
153  if (tsr == NULL) {
154  PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
155  return 0;
156  } else {
157  (*rt).fromSec(PyFloat_AsDouble(tsr));
158  Py_DECREF(tsr);
159  return 1;
160  }
161 }
162 
163 static int rosduration_converter(PyObject *obj, ros::Duration *rt)
164 {
165  PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL);
166  if (tsr == NULL) {
167  PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
168  return 0;
169  } else {
170  (*rt).fromSec(PyFloat_AsDouble(tsr));
171  Py_DECREF(tsr);
172  return 1;
173  }
174 }
175 
176 static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw)
177 {
178  ros::Duration cache_time;
179 
181 
182  if (!PyArg_ParseTuple(args, "|O&", rosduration_converter, &cache_time))
183  return -1;
184 
185  ((buffer_core_t*)self)->bc = new tf2::BufferCore(cache_time);
186 
187  return 0;
188 }
189 
190 /* This may need to be implemented later if we decide to have it in the core
191 static PyObject *getTFPrefix(PyObject *self, PyObject *args)
192 {
193  if (!PyArg_ParseTuple(args, ""))
194  return NULL;
195  tf::Transformer *t = ((transformer_t*)self)->t;
196  return stringToPython(t->getTFPrefix());
197 }
198 */
199 
200 static PyObject *allFramesAsYAML(PyObject *self, PyObject *args)
201 {
202  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
203  return stringToPython(bc->allFramesAsYAML());
204 }
205 
206 static PyObject *allFramesAsString(PyObject *self, PyObject *args)
207 {
208  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
209  return stringToPython(bc->allFramesAsString());
210 }
211 
212 static PyObject *canTransformCore(PyObject *self, PyObject *args, PyObject *kw)
213 {
214  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
215  char *target_frame, *source_frame;
216  ros::Time time;
217  static const char *keywords[] = { "target_frame", "source_frame", "time", NULL };
218 
219  if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time))
220  return NULL;
221  std::string error_msg;
222  bool can_transform = bc->canTransform(target_frame, source_frame, time, &error_msg);
223  //return PyBool_FromLong(t->canTransform(target_frame, source_frame, time));
224  return Py_BuildValue("bs", can_transform, error_msg.c_str());
225 }
226 
227 static PyObject *canTransformFullCore(PyObject *self, PyObject *args, PyObject *kw)
228 {
229  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
230  char *target_frame, *source_frame, *fixed_frame;
231  ros::Time target_time, source_time;
232  static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
233 
234  if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
235  &target_frame,
237  &target_time,
238  &source_frame,
240  &source_time,
241  &fixed_frame))
242  return NULL;
243  std::string error_msg;
244  bool can_transform = bc->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, &error_msg);
245  //return PyBool_FromLong(t->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame));
246  return Py_BuildValue("bs", can_transform, error_msg.c_str());
247 }
248 
249 static PyObject *asListOfStrings(std::vector< std::string > los)
250 {
251  PyObject *r = PyList_New(los.size());
252  size_t i;
253  for (i = 0; i < los.size(); i++) {
254  PyList_SetItem(r, i, stringToPython(los[i]));
255  }
256  return r;
257 }
258 
259 static PyObject *_chain(PyObject *self, PyObject *args, PyObject *kw)
260 {
261  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
262  char *target_frame, *source_frame, *fixed_frame;
263  ros::Time target_time, source_time;
264  std::vector< std::string > output;
265  static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
266 
267  if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
268  &target_frame,
270  &target_time,
271  &source_frame,
273  &source_time,
274  &fixed_frame))
275  return NULL;
276 
277  WRAP(bc->_chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output));
278  return asListOfStrings(output);
279 }
280 
281 static PyObject *getLatestCommonTime(PyObject *self, PyObject *args)
282 {
283  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
284  char *target_frame, *source_frame;
285  tf2::CompactFrameID target_id, source_id;
286  ros::Time time;
287  std::string error_string;
288 
289  if (!PyArg_ParseTuple(args, "ss", &target_frame, &source_frame))
290  return NULL;
291  WRAP(target_id = bc->_validateFrameId("get_latest_common_time", target_frame));
292  WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame));
293  int r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string);
294  if (r == 0) {
295  PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time");
296  PyObject *args = Py_BuildValue("ii", time.sec, time.nsec);
297  PyObject *ob = PyObject_CallObject(rospy_time, args);
298  Py_DECREF(args);
299  Py_DECREF(rospy_time);
300  return ob;
301  } else {
302  PyErr_SetString(tf2_exception, error_string.c_str());
303  return NULL;
304  }
305 }
306 
307 static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *kw)
308 {
309  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
310  char *target_frame, *source_frame;
311  ros::Time time;
312  static const char *keywords[] = { "target_frame", "source_frame", "time", NULL };
313 
314  if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time))
315  return NULL;
316  geometry_msgs::TransformStamped transform;
317  WRAP(transform = bc->lookupTransform(target_frame, source_frame, time));
318  geometry_msgs::Vector3 origin = transform.transform.translation;
319  geometry_msgs::Quaternion rotation = transform.transform.rotation;
320  //TODO: Create a converter that will actually return a python message
321  return Py_BuildValue("O&", transform_converter, &transform);
322  //return Py_BuildValue("(ddd)(dddd)",
323  // origin.x, origin.y, origin.z,
324  // rotation.x, rotation.y, rotation.z, rotation.w);
325 }
326 
327 static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObject *kw)
328 {
329  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
330  char *target_frame, *source_frame, *fixed_frame;
331  ros::Time target_time, source_time;
332  static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL };
333 
334  if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords,
335  &target_frame,
337  &target_time,
338  &source_frame,
340  &source_time,
341  &fixed_frame))
342  return NULL;
343  geometry_msgs::TransformStamped transform;
344  WRAP(transform = bc->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame));
345  geometry_msgs::Vector3 origin = transform.transform.translation;
346  geometry_msgs::Quaternion rotation = transform.transform.rotation;
347  //TODO: Create a converter that will actually return a python message
348  return Py_BuildValue("O&", transform_converter, &transform);
349 }
350 /*
351 static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw)
352 {
353  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
354  char *tracking_frame, *observation_frame;
355  ros::Time time;
356  ros::Duration averaging_interval;
357  static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", NULL };
358 
359  if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval))
360  return NULL;
361  geometry_msgs::Twist twist;
362  WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval));
363 
364  return Py_BuildValue("(ddd)(ddd)",
365  twist.linear.x, twist.linear.y, twist.linear.z,
366  twist.angular.x, twist.angular.y, twist.angular.z);
367 }
368 
369 static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args)
370 {
371  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
372  char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame;
373  ros::Time time;
374  ros::Duration averaging_interval;
375  double px, py, pz;
376 
377  if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&",
378  &tracking_frame,
379  &observation_frame,
380  &reference_frame,
381  &px, &py, &pz,
382  &reference_point_frame,
383  rostime_converter, &time,
384  rosduration_converter, &averaging_interval))
385  return NULL;
386  geometry_msgs::Twist twist;
387  tf::Point pt(px, py, pz);
388  WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval));
389 
390  return Py_BuildValue("(ddd)(ddd)",
391  twist.linear.x, twist.linear.y, twist.linear.z,
392  twist.angular.x, twist.angular.y, twist.angular.z);
393 }
394 */
395 static inline int checkTranslationType(PyObject* o)
396 {
397  PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Vector3");
398  int type_check = PyObject_TypeCheck(o, translation_type);
399  int attr_check = PyObject_HasAttrString(o, "x") &&
400  PyObject_HasAttrString(o, "y") &&
401  PyObject_HasAttrString(o, "z");
402  if (!type_check) {
403  PyErr_WarnEx(PyExc_UserWarning, "translation should be of type Vector3", 1);
404  }
405  return attr_check;
406 }
407 
408 static inline int checkRotationType(PyObject* o)
409 {
410  PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Quaternion");
411  int type_check = PyObject_TypeCheck(o, rotation_type);
412  int attr_check = PyObject_HasAttrString(o, "w") &&
413  PyObject_HasAttrString(o, "x") &&
414  PyObject_HasAttrString(o, "y") &&
415  PyObject_HasAttrString(o, "z");
416  if (!type_check) {
417  PyErr_WarnEx(PyExc_UserWarning, "rotation should be of type Quaternion", 1);
418  }
419  return attr_check;
420 }
421 
422 static PyObject *setTransform(PyObject *self, PyObject *args)
423 {
424  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
425  PyObject *py_transform;
426  char *authority;
427 
428  if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority))
429  return NULL;
430 
431  geometry_msgs::TransformStamped transform;
432  PyObject *header = pythonBorrowAttrString(py_transform, "header");
433  transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id"));
434  transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id"));
435  if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1)
436  return NULL;
437 
438  PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform");
439 
440  PyObject *translation = pythonBorrowAttrString(mtransform, "translation");
441  if (!checkTranslationType(translation)) {
442  PyErr_SetString(PyExc_TypeError, "transform.translation must have members x, y, z");
443  return NULL;
444  }
445 
446  transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x"));
447  transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y"));
448  transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z"));
449 
450  PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation");
451  if (!checkRotationType(rotation)) {
452  PyErr_SetString(PyExc_TypeError, "transform.rotation must have members w, x, y, z");
453  return NULL;
454  }
455 
456  transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x"));
457  transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y"));
458  transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z"));
459  transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w"));
460 
461  bc->setTransform(transform, authority);
462  Py_RETURN_NONE;
463 }
464 
465 static PyObject *setTransformStatic(PyObject *self, PyObject *args)
466 {
467  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
468  PyObject *py_transform;
469  char *authority;
470 
471  if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority))
472  return NULL;
473 
474  geometry_msgs::TransformStamped transform;
475  PyObject *header = pythonBorrowAttrString(py_transform, "header");
476  transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id"));
477  transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id"));
478  if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1)
479  return NULL;
480 
481  PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform");
482  PyObject *translation = pythonBorrowAttrString(mtransform, "translation");
483  if (!checkTranslationType(translation)) {
484  PyErr_SetString(PyExc_TypeError, "transform.translation must be of type Vector3");
485  return NULL;
486  }
487 
488  transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x"));
489  transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y"));
490  transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z"));
491 
492  PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation");
493  if (!checkRotationType(rotation)) {
494  PyErr_SetString(PyExc_TypeError, "transform.rotation must be of type Quaternion");
495  return NULL;
496  }
497 
498  transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x"));
499  transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y"));
500  transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z"));
501  transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w"));
502 
503  // only difference to above is is_static == True
504  bc->setTransform(transform, authority, true);
505  Py_RETURN_NONE;
506 }
507 
508 static PyObject *clear(PyObject *self, PyObject *args)
509 {
510  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
511  bc->clear();
512  Py_RETURN_NONE;
513 }
514 
515 static PyObject *_frameExists(PyObject *self, PyObject *args)
516 {
517  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
518  char *frame_id_str;
519  if (!PyArg_ParseTuple(args, "s", &frame_id_str))
520  return NULL;
521  return PyBool_FromLong(bc->_frameExists(frame_id_str));
522 }
523 
524 static PyObject *_getFrameStrings(PyObject *self, PyObject *args)
525 {
526  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
527  std::vector< std::string > ids;
528  bc->_getFrameStrings(ids);
529  return asListOfStrings(ids);
530 }
531 
532 static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw)
533 {
534  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
535  static const char *keywords[] = { "time", NULL };
536  ros::Time time;
537  if (!PyArg_ParseTupleAndKeywords(args, kw, "|O&", (char**)keywords, rostime_converter, &time))
538  return NULL;
539  return stringToPython(bc->_allFramesAsDot(time.toSec()));
540 }
541 
542 
543 static struct PyMethodDef buffer_core_methods[] =
544 {
545  {"all_frames_as_yaml", allFramesAsYAML, METH_VARARGS},
546  {"all_frames_as_string", allFramesAsString, METH_VARARGS},
547  {"set_transform", setTransform, METH_VARARGS},
548  {"set_transform_static", setTransformStatic, METH_VARARGS},
549  {"can_transform_core", (PyCFunction)canTransformCore, METH_VARARGS | METH_KEYWORDS},
550  {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS},
551  {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS},
552  {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS},
553  {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS},
554  {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS},
555  {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | METH_KEYWORDS},
556  {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS},
557  {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS},
558  {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS},
559  //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS},
560  //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS},
561  //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS},
562  {NULL, NULL}
563 };
564 
565 static PyMethodDef module_methods[] = {
566  // {"Transformer", mkTransformer, METH_VARARGS},
567  {0, 0, 0},
568 };
569 
570 bool staticInit() {
571 #if PYTHON_API_VERSION >= 1007
572  tf2_exception = PyErr_NewException((char*)"tf2.TransformException", NULL, NULL);
573  tf2_connectivityexception = PyErr_NewException((char*)"tf2.ConnectivityException", tf2_exception, NULL);
574  tf2_lookupexception = PyErr_NewException((char*)"tf2.LookupException", tf2_exception, NULL);
575  tf2_extrapolationexception = PyErr_NewException((char*)"tf2.ExtrapolationException", tf2_exception, NULL);
576  tf2_invalidargumentexception = PyErr_NewException((char*)"tf2.InvalidArgumentException", tf2_exception, NULL);
577  tf2_timeoutexception = PyErr_NewException((char*)"tf2.TimeoutException", tf2_exception, NULL);
578 #else
579  tf2_exception = stringToPython("tf2.error");
580  tf2_connectivityexception = stringToPython("tf2.ConnectivityException");
581  tf2_lookupexception = stringToPython("tf2.LookupException");
582  tf2_extrapolationexception = stringToPython("tf2.ExtrapolationException");
583  tf2_invalidargumentexception = stringToPython("tf2.InvalidArgumentException");
584  tf2_timeoutexception = stringToPython("tf2.TimeoutException");
585 #endif
586 
587  pModulerospy = pythonImport("rospy");
588  pModulegeometrymsgs = pythonImport("geometry_msgs.msg");
589 
590  if(pModulegeometrymsgs == NULL)
591  {
592  printf("Cannot load geometry_msgs module");
593  return false;
594  }
595 
596  buffer_core_Type.tp_alloc = PyType_GenericAlloc;
597  buffer_core_Type.tp_new = PyType_GenericNew;
599  buffer_core_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE;
601  if (PyType_Ready(&buffer_core_Type) != 0)
602  return false;
603  return true;
604 }
605 
606 PyObject *moduleInit(PyObject *m) {
607  PyModule_AddObject(m, "BufferCore", (PyObject *)&buffer_core_Type);
608  PyObject *d = PyModule_GetDict(m);
609  PyDict_SetItemString(d, "TransformException", tf2_exception);
610  PyDict_SetItemString(d, "ConnectivityException", tf2_connectivityexception);
611  PyDict_SetItemString(d, "LookupException", tf2_lookupexception);
612  PyDict_SetItemString(d, "ExtrapolationException", tf2_extrapolationexception);
613  PyDict_SetItemString(d, "InvalidArgumentException", tf2_invalidargumentexception);
614  PyDict_SetItemString(d, "TimeoutException", tf2_timeoutexception);
615  return m;
616 }
617 
618 #if PY_MAJOR_VERSION < 3
619 extern "C" void init_tf2()
620 {
621  if (!staticInit())
622  return;
623  moduleInit(Py_InitModule("_tf2", module_methods));
624 }
625 
626 #else
627 struct PyModuleDef tf_module = {
628  PyModuleDef_HEAD_INIT, // base
629  "_tf2", // name
630  NULL, // docstring
631  -1, // state size (but we're using globals)
632  module_methods // methods
633 };
634 
635 PyMODINIT_FUNC PyInit__tf2()
636 {
637  if (!staticInit())
638  return NULL;
639  return moduleInit(PyModule_Create(&tf_module));
640 }
641 #endif
PyObject * stringToPython(const std::string &input)
Definition: python_compat.h:8
d
Definition: setup.py:6
static PyObject * _chain(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:259
static int checkTranslationType(PyObject *o)
Definition: tf2_py.cpp:395
uint32_t CompactFrameID
#define WRAP(x)
Definition: tf2_py.cpp:10
std::string allFramesAsString() const
static PyMethodDef module_methods[]
Definition: tf2_py.cpp:565
static PyObject * transform_converter(const geometry_msgs::TransformStamped *transform)
Definition: tf2_py.cpp:71
PyObject * pythonBorrowAttrString(PyObject *o, const char *name)
Definition: python_compat.h:47
static PyObject * canTransformCore(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:212
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
PyObject * moduleInit(PyObject *m)
Definition: tf2_py.cpp:606
static PyObject * _allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:532
PyObject * pythonImport(const std::string &name)
Definition: python_compat.h:39
static PyObject * pModulerospy
Definition: tf2_py.cpp:48
static PyObject * tf2_connectivityexception
Definition: tf2_py.cpp:51
static PyObject * _getFrameStrings(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:524
static PyObject * clear(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:508
static int rostime_converter(PyObject *obj, ros::Time *rt)
Definition: tf2_py.cpp:150
static int checkRotationType(PyObject *o)
Definition: tf2_py.cpp:408
static PyTypeObject buffer_core_Type
Definition: tf2_py.cpp:60
static struct PyMethodDef buffer_core_methods[]
Definition: tf2_py.cpp:543
static PyObject * setTransformStatic(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:465
bool _frameExists(const std::string &frame_id_str) const
void _chainAsVector(const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
Duration & fromSec(double t)
static PyObject * tf2_exception
Definition: tf2_py.cpp:50
static PyObject * tf2_timeoutexception
Definition: tf2_py.cpp:52
static PyObject * allFramesAsYAML(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:200
PyObject_HEAD tf2::BufferCore * bc
Definition: tf2_py.cpp:56
static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:176
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
static const int DEFAULT_CACHE_TIME
static PyObject * tf2_invalidargumentexception
Definition: tf2_py.cpp:52
static PyObject * asListOfStrings(std::vector< std::string > los)
Definition: tf2_py.cpp:249
static PyObject * canTransformFullCore(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:227
std::string allFramesAsYAML(double current_time) const
int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const
static PyObject * lookupTransformFullCore(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:327
static PyObject * tf2_lookupexception
Definition: tf2_py.cpp:51
static PyObject * _frameExists(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:515
static PyObject * lookupTransformCore(PyObject *self, PyObject *args, PyObject *kw)
Definition: tf2_py.cpp:307
void _getFrameStrings(std::vector< std::string > &ids) const
static PyObject * tf2_extrapolationexception
Definition: tf2_py.cpp:51
void init_tf2()
Definition: tf2_py.cpp:619
static PyObject * getLatestCommonTime(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:281
static PyObject * setTransform(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:422
const std::string header
std::string stringFromPython(PyObject *input)
Definition: python_compat.h:26
std::string _allFramesAsDot(double current_time) const
static PyObject * pModulegeometrymsgs
Definition: tf2_py.cpp:49
static int rosduration_converter(PyObject *obj, ros::Duration *rt)
Definition: tf2_py.cpp:163
bool staticInit()
Definition: tf2_py.cpp:570
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
static PyObject * allFramesAsString(PyObject *self, PyObject *args)
Definition: tf2_py.cpp:206
CompactFrameID _validateFrameId(const char *function_name_arg, const std::string &frame_id) const


tf2_py
Author(s):
autogenerated on Fri Jun 7 2019 21:45:42