16 catch (const tf2::ConnectivityException &e) \
18 PyErr_SetString(tf2_connectivityexception, e.what()); \
21 catch (const tf2::LookupException &e) \
23 PyErr_SetString(tf2_lookupexception, e.what()); \
26 catch (const tf2::ExtrapolationException &e) \
28 PyErr_SetString(tf2_extrapolationexception, e.what()); \
31 catch (const tf2::InvalidArgumentException &e) \
33 PyErr_SetString(tf2_invalidargumentexception, e.what()); \
36 catch (const tf2::TimeoutException &e) \
38 PyErr_SetString(tf2_timeoutexception, e.what()); \
41 catch (const tf2::TransformException &e) \
43 PyErr_SetString(tf2_exception, e.what()); \
61 #if PY_MAJOR_VERSION < 3
62 PyObject_HEAD_INIT(NULL)
65 PyVarObject_HEAD_INIT(NULL, 0)
73 PyObject *pclass, *pargs, *pinst = NULL;
77 printf(
"Can't get geometry_msgs.msg.TransformedStamped\n");
81 pargs = Py_BuildValue(
"()");
85 printf(
"Can't build argument list\n");
89 pinst = PyEval_CallObject(pclass, pargs);
94 printf(
"Can't create class\n");
99 PyObject *rospy_time = PyObject_GetAttrString(
pModulerospy,
"Time");
100 PyObject *args = Py_BuildValue(
"ii", transform->header.stamp.sec, transform->header.stamp.nsec);
101 PyObject *time_obj = PyObject_CallObject(rospy_time, args);
103 Py_DECREF(rospy_time);
105 PyObject* pheader = PyObject_GetAttrString(pinst,
"header");
106 PyObject_SetAttrString(pheader,
"stamp", time_obj);
110 PyObject_SetAttrString(pheader,
"frame_id", frame_id);
114 PyObject *ptransform = PyObject_GetAttrString(pinst,
"transform");
115 PyObject *ptranslation = PyObject_GetAttrString(ptransform,
"translation");
116 PyObject *protation = PyObject_GetAttrString(ptransform,
"rotation");
117 Py_DECREF(ptransform);
119 PyObject *child_frame_id =
stringToPython(transform->child_frame_id);
120 PyObject_SetAttrString(pinst,
"child_frame_id", child_frame_id);
121 Py_DECREF(child_frame_id);
123 PyObject *trans_x = PyFloat_FromDouble(transform->transform.translation.x);
124 PyObject *trans_y = PyFloat_FromDouble(transform->transform.translation.y);
125 PyObject *trans_z = PyFloat_FromDouble(transform->transform.translation.z);
126 PyObject_SetAttrString(ptranslation,
"x", trans_x);
127 PyObject_SetAttrString(ptranslation,
"y", trans_y);
128 PyObject_SetAttrString(ptranslation,
"z", trans_z);
132 Py_DECREF(ptranslation);
134 PyObject *rot_x = PyFloat_FromDouble(transform->transform.rotation.x);
135 PyObject *rot_y = PyFloat_FromDouble(transform->transform.rotation.y);
136 PyObject *rot_z = PyFloat_FromDouble(transform->transform.rotation.z);
137 PyObject *rot_w = PyFloat_FromDouble(transform->transform.rotation.w);
138 PyObject_SetAttrString(protation,
"x", rot_x);
139 PyObject_SetAttrString(protation,
"y", rot_y);
140 PyObject_SetAttrString(protation,
"z", rot_z);
141 PyObject_SetAttrString(protation,
"w", rot_w);
146 Py_DECREF(protation);
153 PyObject *tsr = PyObject_CallMethod(obj, (
char*)
"to_sec", NULL);
155 PyErr_SetString(PyExc_TypeError,
"time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
158 (*rt).fromSec(PyFloat_AsDouble(tsr));
166 PyObject *tsr = PyObject_CallMethod(obj, (
char*)
"to_sec", NULL);
168 PyErr_SetString(PyExc_TypeError,
"time must have a to_sec method, e.g. rospy.Time or rospy.Duration");
171 (*rt).fromSec(PyFloat_AsDouble(tsr));
216 char *target_frame, *source_frame;
218 static const char *keywords[] = {
"target_frame",
"source_frame",
"time", NULL };
220 if (!PyArg_ParseTupleAndKeywords(args, kw,
"ssO&", (
char**)keywords, &target_frame, &source_frame,
rostime_converter, &time))
222 std::string error_msg;
223 bool can_transform = bc->
canTransform(target_frame, source_frame, time, &error_msg);
225 return Py_BuildValue(
"bs", can_transform, error_msg.c_str());
231 char *target_frame, *source_frame, *fixed_frame;
233 static const char *keywords[] = {
"target_frame",
"target_time",
"source_frame",
"source_time",
"fixed_frame", NULL };
235 if (!PyArg_ParseTupleAndKeywords(args, kw,
"sO&sO&s", (
char**)keywords,
244 std::string error_msg;
245 bool can_transform = bc->
canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, &error_msg);
247 return Py_BuildValue(
"bs", can_transform, error_msg.c_str());
252 PyObject *r = PyList_New(los.size());
254 for (i = 0; i < los.size(); i++) {
260 static PyObject *
_chain(PyObject *
self, PyObject *args, PyObject *kw)
263 char *target_frame, *source_frame, *fixed_frame;
265 std::vector< std::string > output;
266 static const char *keywords[] = {
"target_frame",
"target_time",
"source_frame",
"source_time",
"fixed_frame", NULL };
268 if (!PyArg_ParseTupleAndKeywords(args, kw,
"sO&sO&s", (
char**)keywords,
278 WRAP(bc->
_chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output));
285 char *target_frame, *source_frame;
288 std::string error_string;
290 if (!PyArg_ParseTuple(args,
"ss", &target_frame, &source_frame))
296 PyObject *rospy_time = PyObject_GetAttrString(
pModulerospy,
"Time");
297 PyObject *args = Py_BuildValue(
"ii", time.
sec, time.
nsec);
298 PyObject *ob = PyObject_CallObject(rospy_time, args);
300 Py_DECREF(rospy_time);
311 char *target_frame, *source_frame;
313 static const char *keywords[] = {
"target_frame",
"source_frame",
"time", NULL };
315 if (!PyArg_ParseTupleAndKeywords(args, kw,
"ssO&", (
char**)keywords, &target_frame, &source_frame,
rostime_converter, &time))
317 geometry_msgs::TransformStamped transform;
319 geometry_msgs::Vector3 origin = transform.transform.translation;
320 geometry_msgs::Quaternion rotation = transform.transform.rotation;
331 char *target_frame, *source_frame, *fixed_frame;
333 static const char *keywords[] = {
"target_frame",
"target_time",
"source_frame",
"source_time",
"fixed_frame", NULL };
335 if (!PyArg_ParseTupleAndKeywords(args, kw,
"sO&sO&s", (
char**)keywords,
344 geometry_msgs::TransformStamped transform;
345 WRAP(transform = bc->
lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame));
346 geometry_msgs::Vector3 origin = transform.transform.translation;
347 geometry_msgs::Quaternion rotation = transform.transform.rotation;
398 PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(
pModulegeometrymsgs,
"Vector3");
399 int type_check = PyObject_TypeCheck(o, translation_type);
400 int attr_check = PyObject_HasAttrString(o,
"x") &&
401 PyObject_HasAttrString(o,
"y") &&
402 PyObject_HasAttrString(o,
"z");
404 PyErr_WarnEx(PyExc_UserWarning,
"translation should be of type Vector3", 1);
411 PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(
pModulegeometrymsgs,
"Quaternion");
412 int type_check = PyObject_TypeCheck(o, rotation_type);
413 int attr_check = PyObject_HasAttrString(o,
"w") &&
414 PyObject_HasAttrString(o,
"x") &&
415 PyObject_HasAttrString(o,
"y") &&
416 PyObject_HasAttrString(o,
"z");
418 PyErr_WarnEx(PyExc_UserWarning,
"rotation should be of type Quaternion", 1);
426 PyObject *py_transform;
429 if (!PyArg_ParseTuple(args,
"Os", &py_transform, &authority))
432 geometry_msgs::TransformStamped transform;
443 PyErr_SetString(PyExc_TypeError,
"transform.translation must have members x, y, z");
453 PyErr_SetString(PyExc_TypeError,
"transform.rotation must have members w, x, y, z");
469 PyObject *py_transform;
472 if (!PyArg_ParseTuple(args,
"Os", &py_transform, &authority))
475 geometry_msgs::TransformStamped transform;
485 PyErr_SetString(PyExc_TypeError,
"transform.translation must be of type Vector3");
495 PyErr_SetString(PyExc_TypeError,
"transform.rotation must be of type Quaternion");
509 static PyObject *
clear(PyObject *
self, PyObject *args)
520 if (!PyArg_ParseTuple(args,
"s", &frame_id_str))
528 std::vector< std::string > ids;
536 static const char *keywords[] = {
"time", NULL };
538 if (!PyArg_ParseTupleAndKeywords(args, kw,
"|O&", (
char**)keywords,
rostime_converter, &time))
550 {
"can_transform_core", (PyCFunction)
canTransformCore, METH_VARARGS | METH_KEYWORDS},
552 {
"_chain", (PyCFunction)
_chain, METH_VARARGS | METH_KEYWORDS},
553 {
"clear", (PyCFunction)
clear, METH_VARARGS | METH_KEYWORDS},
554 {
"_frameExists", (PyCFunction)
_frameExists, METH_VARARGS},
556 {
"_allFramesAsDot", (PyCFunction)
_allFramesAsDot, METH_VARARGS | METH_KEYWORDS},
572 #if PYTHON_API_VERSION >= 1007
573 tf2_exception = PyErr_NewException((
char*)
"tf2.TransformException", NULL, NULL);
593 printf(
"Cannot load geometry_msgs module");
609 PyObject *
d = PyModule_GetDict(m);
619 #if PY_MAJOR_VERSION < 3
631 struct PyModuleDef tf_module = {
632 PyModuleDef_HEAD_INIT,
639 PyMODINIT_FUNC PyInit__tf2()
643 return moduleInit(PyModule_Create(&tf_module));