diff --git a/tf2_py/AMENT_IGNORE b/tf2_py/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 6e12ea513..4c2d42b1f 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -1,157 +1,76 @@ cmake_minimum_required(VERSION 3.5) project(tf2_py) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS rospy tf2) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -find_package(PythonLibs 2 REQUIRED) -include_directories(${PYTHON_INCLUDE_PATH} ${catkin_INCLUDE_DIRS}) - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - -####################################### -## Declare ROS messages and services ## -####################################### - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES tf2_py - CATKIN_DEPENDS rospy tf2 -# DEPENDS system_lib +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) + +find_package(python_cmake_module REQUIRED) +find_package(PythonExtra REQUIRED) + +set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}") + +if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") + set(PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}") +endif() + +ament_python_install_package(${PROJECT_NAME}) + +function(set_properties _targetname _build_type) + set_target_properties(${_targetname} PROPERTIES + PREFIX "" + LIBRARY_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/test_tf2_py" + RUNTIME_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/test_tf2_py" + OUTPUT_NAME "_${_targetname}${PythonExtra_EXTENSION_SUFFIX}" + SUFFIX "${PythonExtra_EXTENSION_EXTENSION}") +endfunction() + +add_library(${PROJECT_NAME} SHARED src/tf2_py.cpp) + +set_properties(${PROJECT_NAME} "") +if(WIN32) + set_properties(${PROJECT_NAME} "_DEBUG") + set_properties(${PROJECT_NAME} "_MINSIZEREL") + set_properties(${PROJECT_NAME} "_RELEASE") + set_properties(${PROJECT_NAME} "_RELWITHDEBINFO") +endif() + +ament_target_dependencies(${PROJECT_NAME} + "geometry_msgs" + "tf2" + "PythonExtra" ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - -## Declare a cpp library -# add_library(tf2_py -# src/${PROJECT_NAME}/tf2_py.cpp -# ) - -## Declare a cpp executable -# add_executable(tf2_py_node src/tf2_py_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(tf2_py_node tf2_py_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(tf2_py_node -# ${catkin_LIBRARIES} -# ) - - -# Check for SSE -#!!! rosbuild_check_for_sse() - -# Dynamic linking with tf worked OK, except for exception propagation, which failed in the unit test. -# so build with the objects directly instead. +install(TARGETS + ${PROJECT_NAME} + DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} +) -link_libraries(${PYTHON_LIBRARIES}) -add_library(tf2_py src/tf2_py.cpp) -target_link_libraries(tf2_py ${catkin_LIBRARIES}) -add_dependencies(tf2_py tf2_msgs_gencpp) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() -set_target_properties(tf2_py PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") -set_target_properties(tf2_py PROPERTIES COMPILE_FLAGS "-g -Wno-missing-field-initializers") -set_target_properties(tf2_py PROPERTIES - ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} - LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} -) -#!! rosbuild_add_compile_flags(tf2_py ${SSE_FLAGS}) #conditionally adds sse flags if available - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS tf2_py tf2_py_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -install(FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/_tf2.so - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} -) + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(tf2_py_test test + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + ) -############# -## Testing ## -############# + # Create importable location in build directory + file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_tf2_py/__init__.py" "") +endif() -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_tf2_py.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +set(PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 430f21ee2..4886ad85b 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -1,55 +1,31 @@ - + + tf2_py 0.9.1 The tf2_py package - Tully Foote - - - - BSD - - - - http://ros.org/wiki/tf2_py + ament_cmake + python_cmake_module - - - - + geometry_msgs + tf2 - - - - - - - - - - - - catkin - tf2 - rospy - tf2 - rospy + builtin_interfaces + geometry_msgs + rclpy + ament_lint_auto + ament_cmake_pytest - - - - - - + ament_cmake diff --git a/tf2_py/setup.py b/tf2_py/setup.py deleted file mode 100644 index a21d885d6..000000000 --- a/tf2_py/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['tf2_py'], - package_dir={'': 'src'}, - requires=['rospy', 'geometry_msgs', 'tf2_msgs'] -) - -setup(**d) diff --git a/tf2_py/src/python_compat.h b/tf2_py/src/python_compat.h index 2f431cce3..01fac8919 100644 --- a/tf2_py/src/python_compat.h +++ b/tf2_py/src/python_compat.h @@ -5,6 +5,10 @@ #include +/// \brief Converts a C++ string into a Python string. +/// \note The caller is responsible for decref'ing the returned object. +/// \note If the return value is NULL then an exception is set. +/// \return a new PyObject reference, or NULL inline PyObject *stringToPython(const std::string &input) { #if PY_MAJOR_VERSION >= 3 @@ -14,6 +18,10 @@ inline PyObject *stringToPython(const std::string &input) #endif } +/// \brief Converts a C string into a Python string. +/// \note The caller is responsible for decref'ing the returned object. +/// \note If the return value is NULL then an exception is set. +/// \return a new PyObject reference, or NULL inline PyObject *stringToPython(const char *input) { #if PY_MAJOR_VERSION >= 3 @@ -23,10 +31,16 @@ inline PyObject *stringToPython(const char *input) #endif } +/// \brief Converts a Python string into a C++ string. +/// \note The input PyObject is borrowed, and will not be decref'd. +/// \note It's possible for this function to set an exception. +/// If the returned string is empty, callers should check if an exception was +/// set using PyErr_Ocurred(). +/// \return a new std::string instance inline std::string stringFromPython(PyObject * input) { Py_ssize_t size; - char * data; + const char * data; #if PY_MAJOR_VERSION >= 3 data = PyUnicode_AsUTF8AndSize(input, &size); #else @@ -35,19 +49,30 @@ inline std::string stringFromPython(PyObject * input) return std::string(data, size); } +/// \brief Imports a python module by name. +/// \note The caller is responsible for decref'ing the returned object. +/// \note If the return value is NULL then an exception is set. +/// \return a reference to the imported module. inline PyObject *pythonImport(const std::string & name) { PyObject *py_name = stringToPython(name); + if (!py_name) { + return nullptr; + } PyObject *module = PyImport_Import(py_name); Py_XDECREF(py_name); return module; } +/// \brief Borrow an attribute on an object. +/// \note The caller must not decref the returned object. +/// \note If the return value is NULL then an exception is set. +/// \return a reference to the attribute on the object. inline PyObject *pythonBorrowAttrString(PyObject* o, const char *name) { - PyObject *r = PyObject_GetAttrString(o, name); - Py_XDECREF(r); - return r; + PyObject *r = PyObject_GetAttrString(o, name); + Py_XDECREF(r); + return r; } #endif diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index fc60ec202..0253980ce 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -45,10 +45,12 @@ } \ } while (0) -static PyObject *pModulerospy = NULL; +static PyObject *pModulerclpy = NULL; +static PyObject *pModulerclpytime = NULL; +static PyObject *pModulebuiltininterfacesmsgs = NULL; static PyObject *pModulegeometrymsgs = NULL; static PyObject *tf2_exception = NULL; -static PyObject *tf2_connectivityexception = NULL, *tf2_lookupexception = NULL, *tf2_extrapolationexception = NULL, +static PyObject *tf2_connectivityexception = NULL, *tf2_lookupexception = NULL, *tf2_extrapolationexception = NULL, *tf2_invalidargumentexception = NULL, *tf2_timeoutexception = NULL; struct buffer_core_t { @@ -56,110 +58,272 @@ struct buffer_core_t { tf2::BufferCore *bc; }; - -static PyTypeObject buffer_core_Type = { -#if PY_MAJOR_VERSION < 3 - PyObject_HEAD_INIT(NULL) - 0, /*size*/ -# else - PyVarObject_HEAD_INIT(NULL, 0) -#endif - "_tf2.BufferCore", /*name*/ - sizeof(buffer_core_t), /*basicsize*/ -}; - -static PyObject *transform_converter(const geometry_msgs::TransformStamped* transform) +static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* transform) { - PyObject *pclass, *pargs, *pinst = NULL; + PyObject * pclass = NULL; + PyObject * pargs = NULL; + PyObject * pinst = NULL; + PyObject * builtin_interfaces_time = NULL; + PyObject * args = NULL; + PyObject * kwargs = NULL; + PyObject * sec = NULL; + PyObject * nanosec = NULL; + PyObject * time_obj = NULL; + PyObject * pheader = NULL; + PyObject * pframe_id = NULL; + PyObject * ptransform = NULL; + PyObject * ptranslation = NULL; + PyObject * protation = NULL; + PyObject * child_frame_id = NULL; + PyObject * ptrans_x = NULL; + PyObject * ptrans_y = NULL; + PyObject * ptrans_z = NULL; + PyObject * prot_x = NULL; + PyObject * prot_y = NULL; + PyObject * prot_z = NULL; + PyObject * prot_w = NULL; pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped"); - if(pclass == NULL) + if (!pclass) { - printf("Can't get geometry_msgs.msg.TransformedStamped\n"); - return NULL; + goto cleanup; } pargs = Py_BuildValue("()"); - if(pargs == NULL) + if (!pargs) { - printf("Can't build argument list\n"); - return NULL; + goto cleanup; } pinst = PyEval_CallObject(pclass, pargs); - Py_DECREF(pclass); - Py_DECREF(pargs); - if(pinst == NULL) + if (!pinst) { - printf("Can't create class\n"); - return NULL; + goto cleanup; } //we need to convert the time to python - PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); - PyObject *args = Py_BuildValue("ii", transform->header.stamp.sec, transform->header.stamp.nsec); - PyObject *time_obj = PyObject_CallObject(rospy_time, args); - Py_DECREF(args); - Py_DECREF(rospy_time); + builtin_interfaces_time = PyObject_GetAttrString(pModulebuiltininterfacesmsgs, "Time"); + if (!builtin_interfaces_time) { + goto cleanup; + } + args = PyTuple_New(0); + if (!args) { + goto cleanup; + } + kwargs = PyDict_New(); + if (!kwargs) { + goto cleanup; + } + sec = Py_BuildValue("i", transform->header.stamp.sec); + if (!sec) { + goto cleanup; + } + nanosec = Py_BuildValue("i", transform->header.stamp.nanosec); + if (!nanosec) { + goto cleanup; + } + if (-1 == PyDict_SetItemString(kwargs, "sec", sec)) { + goto cleanup; + } + if (-1 == PyDict_SetItemString(kwargs, "nanosec", nanosec)) { + goto cleanup; + } + + time_obj = PyObject_Call(builtin_interfaces_time, args, kwargs); + if (!time_obj) { + goto cleanup; + } + + pheader = PyObject_GetAttrString(pinst, "header"); + if (!pheader) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(pheader, "stamp", time_obj)) { + goto cleanup; + } + + pframe_id = stringToPython(transform->header.frame_id); + if (!pframe_id) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(pheader, "frame_id", pframe_id)) { + goto cleanup; + } + + ptransform = PyObject_GetAttrString(pinst, "transform"); + if (!ptransform) { + goto cleanup; + } + ptranslation = PyObject_GetAttrString(ptransform, "translation"); + if (!ptranslation) { + goto cleanup; + } + protation = PyObject_GetAttrString(ptransform, "rotation"); + if (!ptranslation) { + goto cleanup; + } - PyObject* pheader = PyObject_GetAttrString(pinst, "header"); - PyObject_SetAttrString(pheader, "stamp", time_obj); - Py_DECREF(time_obj); + child_frame_id = stringToPython(transform->child_frame_id); + if (!child_frame_id) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id)) { + goto cleanup; + } + + ptrans_x = PyFloat_FromDouble(transform->transform.translation.x); + if (!ptrans_x) { + goto cleanup; + } + ptrans_y = PyFloat_FromDouble(transform->transform.translation.y); + if (!ptrans_y) { + goto cleanup; + } + ptrans_z = PyFloat_FromDouble(transform->transform.translation.z); + if (!ptrans_z) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(ptranslation, "x", ptrans_x)) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(ptranslation, "y", ptrans_y)) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(ptranslation, "z", ptrans_z)) { + goto cleanup; + } + + prot_x = PyFloat_FromDouble(transform->transform.rotation.x); + if (!prot_x) { + goto cleanup; + } - PyObject_SetAttrString(pheader, "frame_id", stringToPython(transform->header.frame_id)); - Py_DECREF(pheader); + prot_y = PyFloat_FromDouble(transform->transform.rotation.y); + if (!prot_y) { + goto cleanup; + } + + prot_z = PyFloat_FromDouble(transform->transform.rotation.z); + if (!prot_z) { + goto cleanup; + } - PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); - PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); - PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); - Py_DECREF(ptransform); + prot_w = PyFloat_FromDouble(transform->transform.rotation.w); + if (!prot_w) { + goto cleanup; + } - PyObject_SetAttrString(pinst, "child_frame_id", stringToPython(transform->child_frame_id)); + if (-1 == PyObject_SetAttrString(protation, "x", prot_x)) { + goto cleanup; + } - PyObject_SetAttrString(ptranslation, "x", PyFloat_FromDouble(transform->transform.translation.x)); - PyObject_SetAttrString(ptranslation, "y", PyFloat_FromDouble(transform->transform.translation.y)); - PyObject_SetAttrString(ptranslation, "z", PyFloat_FromDouble(transform->transform.translation.z)); - Py_DECREF(ptranslation); + if (-1 == PyObject_SetAttrString(protation, "y", prot_y)) { + goto cleanup; + } - PyObject_SetAttrString(protation, "x", PyFloat_FromDouble(transform->transform.rotation.x)); - PyObject_SetAttrString(protation, "y", PyFloat_FromDouble(transform->transform.rotation.y)); - PyObject_SetAttrString(protation, "z", PyFloat_FromDouble(transform->transform.rotation.z)); - PyObject_SetAttrString(protation, "w", PyFloat_FromDouble(transform->transform.rotation.w)); - Py_DECREF(protation); + if (-1 == PyObject_SetAttrString(protation, "z", prot_z)) { + goto cleanup; + } + if (-1 == PyObject_SetAttrString(protation, "w", prot_w)) { + goto cleanup; + } +cleanup: + if (PyErr_Occurred()) { + Py_XDECREF(pinst); + pinst = NULL; + } + Py_XDECREF(pclass); + Py_XDECREF(pargs); + Py_XDECREF(builtin_interfaces_time); + Py_XDECREF(args); + Py_XDECREF(kwargs); + Py_XDECREF(sec); + Py_XDECREF(nanosec); + Py_XDECREF(time_obj); + Py_XDECREF(pheader); + Py_XDECREF(pframe_id); + Py_XDECREF(ptransform); + Py_XDECREF(ptranslation); + Py_XDECREF(protation); + Py_XDECREF(child_frame_id); + Py_XDECREF(ptrans_x); + Py_XDECREF(ptrans_y); + Py_XDECREF(ptrans_z); + Py_XDECREF(prot_x); + Py_XDECREF(prot_y); + Py_XDECREF(prot_z); + Py_XDECREF(prot_w); return pinst; } -static int rostime_converter(PyObject *obj, builtin_interfaces::msg::Time *rt) +static builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t) { - PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); - if (tsr == NULL) { - PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration"); - return 0; - } else { - (*rt).fromSec(PyFloat_AsDouble(tsr)); - Py_DECREF(tsr); - return 1; + std::chrono::nanoseconds ns = t.time_since_epoch(); + std::chrono::seconds s = std::chrono::duration_cast(ns); + ns -= s; + builtin_interfaces::msg::Time time_msg; + time_msg.sec = (int32_t)s.count(); + time_msg.nanosec = (uint32_t)ns.count(); + return time_msg; +} + +static tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) +{ + std::chrono::nanoseconds ns = std::chrono::seconds(time_msg.sec) + std::chrono::nanoseconds(time_msg.nanosec); + return tf2::TimePoint(ns); +} + +static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) +{ + if (PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { + PyObject *sec = pythonBorrowAttrString(obj, "sec"); + PyObject *nanosec = pythonBorrowAttrString(obj, "nanosec"); + builtin_interfaces::msg::Time msg; + msg.sec = PyLong_AsLong(sec); + msg.nanosec = PyLong_AsUnsignedLong(nanosec); + *rt = fromMsg(msg); + return PyErr_Occurred() ? 0 : 1; + } + + if (PyObject_HasAttrString(obj, "nanoseconds")) { + PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); + const int64_t d = PyLong_AsLongLong(nanoseconds); + const std::chrono::nanoseconds ns(d); + *rt = tf2::TimePoint(ns); + return PyErr_Occurred() ? 0 : 1; } + + PyErr_SetString(PyExc_TypeError, "time must have sec and nanosec, or nanoseconds."); + return 0; } static int rosduration_converter(PyObject *obj, tf2::Duration *rt) { - PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); - if (tsr == NULL) { - PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration"); - return 0; - } else { - (*rt).fromSec(PyFloat_AsDouble(tsr)); - Py_DECREF(tsr); - return 1; + if (PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { + PyObject *sec = pythonBorrowAttrString(obj, "sec"); + PyObject *nanosec = pythonBorrowAttrString(obj, "nanosec"); + *rt = std::chrono::seconds(PyLong_AsLong(sec)) + + std::chrono::nanoseconds(PyLong_AsUnsignedLong(nanosec)); + return PyErr_Occurred() ? 0 : 1; } + + if (PyObject_HasAttrString(obj, "nanoseconds")) { + PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); + const int64_t d = PyLong_AsLongLong(nanoseconds); + const std::chrono::nanoseconds ns(d); + *rt = std::chrono::duration_cast(ns); + return PyErr_Occurred() ? 0 : 1; + } + + PyErr_SetString(PyExc_TypeError, "duration must have sec and nanosec, or nanoseconds."); + return 0; } static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) { - tf2::Duration cache_time; + (void)kw; - cache_time.fromSec(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME); + tf2::Duration cache_time(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME); if (!PyArg_ParseTuple(args, "|O&", rosduration_converter, &cache_time)) return -1; @@ -169,24 +333,16 @@ static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) return 0; } -/* This may need to be implemented later if we decide to have it in the core -static PyObject *getTFPrefix(PyObject *self, PyObject *args) -{ - if (!PyArg_ParseTuple(args, "")) - return NULL; - tf::Transformer *t = ((transformer_t*)self)->t; - return stringToPython(t->getTFPrefix()); -} -*/ - static PyObject *allFramesAsYAML(PyObject *self, PyObject *args) { + (void)args; tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; return stringToPython(bc->allFramesAsYAML()); } static PyObject *allFramesAsString(PyObject *self, PyObject *args) { + (void)args; tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; return stringToPython(bc->allFramesAsString()); } @@ -195,7 +351,7 @@ static PyObject *canTransformCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame; - builtin_interfaces::msg::Time time; + tf2::TimePoint time; static const char *keywords[] = { "target_frame", "source_frame", "time", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time)) @@ -210,7 +366,7 @@ static PyObject *canTransformFullCore(PyObject *self, PyObject *args, PyObject * { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame, *fixed_frame; - builtin_interfaces::msg::Time target_time, source_time; + tf2::TimePoint target_time, source_time; static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, @@ -242,7 +398,7 @@ static PyObject *_chain(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame, *fixed_frame; - builtin_interfaces::msg::Time target_time, source_time; + tf2::TimePoint target_time, source_time; std::vector< std::string > output; static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; @@ -265,40 +421,82 @@ static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame; tf2::CompactFrameID target_id, source_id; - builtin_interfaces::msg::Time time; + tf2::TimePoint tf2_time; std::string error_string; if (!PyArg_ParseTuple(args, "ss", &target_frame, &source_frame)) return NULL; WRAP(target_id = bc->_validateFrameId("get_latest_common_time", target_frame)); WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame)); - int r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string); - if (r == 0) { - PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); - PyObject *args = Py_BuildValue("ii", time.sec, time.nsec); - PyObject *ob = PyObject_CallObject(rospy_time, args); - Py_DECREF(args); - Py_DECREF(rospy_time); - return ob; - } else { + const tf2::TF2Error r = bc->_getLatestCommonTime(target_id, source_id, tf2_time, &error_string); + + if (r != tf2::TF2Error::NO_ERROR) { PyErr_SetString(tf2_exception, error_string.c_str()); return NULL; } + + builtin_interfaces::msg::Time time_msg; + PyObject * rclpy_time = NULL; + PyObject * call_args = NULL; + PyObject * kwargs = NULL; + PyObject * seconds = NULL; + PyObject * nanoseconds = NULL; + PyObject * ob = NULL; + + rclpy_time = PyObject_GetAttrString(pModulerclpytime, "Time"); + if (!rclpy_time) { + goto cleanup; + } + time_msg = toMsg(tf2_time); + call_args = PyTuple_New(0); + if (!call_args) { + goto cleanup; + } + kwargs = PyDict_New(); + if (!kwargs) { + goto cleanup; + } + seconds = Py_BuildValue("i", time_msg.sec); + if (!seconds) { + goto cleanup; + } + nanoseconds = Py_BuildValue("i", time_msg.nanosec); + if (!nanoseconds) { + goto cleanup; + } + if (0 != PyDict_SetItemString(kwargs, "seconds", seconds)) { + goto cleanup; + } + if (0 != PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds)) { + goto cleanup; + } + + ob = PyObject_Call(rclpy_time, call_args, kwargs); + +cleanup: + if (PyErr_Occurred()) { + Py_XDECREF(ob); + ob = NULL; + } + Py_XDECREF(rclpy_time); + Py_XDECREF(call_args); + Py_XDECREF(kwargs); + Py_XDECREF(seconds); + Py_XDECREF(nanoseconds); + return ob; } static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame; - builtin_interfaces::msg::Time time; + tf2::TimePoint time; static const char *keywords[] = { "target_frame", "source_frame", "time", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time)) return NULL; - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; WRAP(transform = bc->lookupTransform(target_frame, source_frame, time)); - geometry_msgs::Vector3 origin = transform.transform.translation; - geometry_msgs::Quaternion rotation = transform.transform.rotation; //TODO: Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); //return Py_BuildValue("(ddd)(dddd)", @@ -310,7 +508,7 @@ static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObjec { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame, *fixed_frame; - builtin_interfaces::msg::Time target_time, source_time; + tf2::TimePoint target_time, source_time; static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, @@ -322,10 +520,8 @@ static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObjec &source_time, &fixed_frame)) return NULL; - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; WRAP(transform = bc->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame)); - geometry_msgs::Vector3 origin = transform.transform.translation; - geometry_msgs::Quaternion rotation = transform.transform.rotation; //TODO: Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); } @@ -340,7 +536,7 @@ static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw) if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval)) return NULL; - geometry_msgs::Twist twist; + geometry_msgs::msg::Twist twist; WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval)); return Py_BuildValue("(ddd)(ddd)", @@ -365,7 +561,7 @@ static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args) rostime_converter, &time, rosduration_converter, &averaging_interval)) return NULL; - geometry_msgs::Twist twist; + geometry_msgs::msg::Twist twist; tf::Point pt(px, py, pz); WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval)); @@ -377,12 +573,17 @@ static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args) static inline int checkTranslationType(PyObject* o) { PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Vector3"); + if (!translation_type) { + return 0; + } int type_check = PyObject_TypeCheck(o, translation_type); + Py_DECREF((PyObject*)translation_type); int attr_check = PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && PyObject_HasAttrString(o, "z"); if (!type_check) { PyErr_WarnEx(PyExc_UserWarning, "translation should be of type Vector3", 1); + return type_check; } return attr_check; } @@ -390,7 +591,11 @@ static inline int checkTranslationType(PyObject* o) static inline int checkRotationType(PyObject* o) { PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Quaternion"); + if (!rotation_type) { + return 0; + } int type_check = PyObject_TypeCheck(o, rotation_type); + Py_DECREF((PyObject*)rotation_type); int attr_check = PyObject_HasAttrString(o, "w") && PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && @@ -406,17 +611,20 @@ static PyObject *setTransform(PyObject *self, PyObject *args) tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; PyObject *py_transform; char *authority; + tf2::TimePoint time; if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority)) return NULL; - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; PyObject *header = pythonBorrowAttrString(py_transform, "header"); transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); - if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; + transform.header.stamp = toMsg(time); + PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); @@ -449,16 +657,19 @@ static PyObject *setTransformStatic(PyObject *self, PyObject *args) tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; PyObject *py_transform; char *authority; + tf2::TimePoint time; if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority)) return NULL; - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; PyObject *header = pythonBorrowAttrString(py_transform, "header"); transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); - if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) + + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; + transform.header.stamp = toMsg(time); PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); @@ -489,6 +700,7 @@ static PyObject *setTransformStatic(PyObject *self, PyObject *args) static PyObject *clear(PyObject *self, PyObject *args) { + (void)args; tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; bc->clear(); Py_RETURN_NONE; @@ -505,6 +717,7 @@ static PyObject *_frameExists(PyObject *self, PyObject *args) static PyObject *_getFrameStrings(PyObject *self, PyObject *args) { + (void)args; tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; std::vector< std::string > ids; bc->_getFrameStrings(ids); @@ -515,38 +728,88 @@ static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; static const char *keywords[] = { "time", NULL }; - ros::Time time; + tf2::TimePoint time; if (!PyArg_ParseTupleAndKeywords(args, kw, "|O&", (char**)keywords, rostime_converter, &time)) return NULL; - return PyString_FromString(bc->_allFramesAsDot(time.toSec()).c_str()); + return stringToPython(bc->_allFramesAsDot(time).c_str()); } static struct PyMethodDef buffer_core_methods[] = { - {"all_frames_as_yaml", allFramesAsYAML, METH_VARARGS}, - {"all_frames_as_string", allFramesAsString, METH_VARARGS}, - {"set_transform", setTransform, METH_VARARGS}, - {"set_transform_static", setTransformStatic, METH_VARARGS}, - {"can_transform_core", (PyCFunction)canTransformCore, METH_KEYWORDS}, - {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_KEYWORDS}, - {"_chain", (PyCFunction)_chain, METH_KEYWORDS}, - {"clear", (PyCFunction)clear, METH_KEYWORDS}, - {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS}, - {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS}, - {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_KEYWORDS}, - {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS}, - {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_KEYWORDS}, - {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_KEYWORDS}, - //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_KEYWORDS}, + {"all_frames_as_yaml", allFramesAsYAML, METH_VARARGS, NULL}, + {"all_frames_as_string", allFramesAsString, METH_VARARGS, NULL}, + {"set_transform", setTransform, METH_VARARGS, NULL}, + {"set_transform_static", setTransformStatic, METH_VARARGS, NULL}, + {"can_transform_core", (PyCFunction)canTransformCore, METH_VARARGS | METH_KEYWORDS, NULL}, + {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS, NULL}, + {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS, NULL}, + {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS, NULL}, + {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS, NULL}, + {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS, NULL}, + {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | METH_KEYWORDS, NULL}, + {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS, NULL}, + {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS, NULL}, + {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS, NULL}, + //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS}, //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS}, - //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS}, - {NULL, NULL} + {NULL, NULL, 0, NULL} }; static PyMethodDef module_methods[] = { // {"Transformer", mkTransformer, METH_VARARGS}, - {0, 0, 0}, + {NULL, NULL, 0, NULL}, +}; + +static PyTypeObject buffer_core_Type = { + PyVarObject_HEAD_INIT(NULL, 0) + "_tf2.BufferCore", /* tp_name */ + sizeof(buffer_core_t), /* tp_basicsize */ + 0, /* tp_itemsize */ + NULL, /* tp_dealloc */ + NULL, /* tp_print */ + NULL, /* tp_getattr */ + NULL, /* tp_setattr */ + NULL, /* tp_as_async */ + NULL, /* tp_repr */ + NULL, /* tp_as_number */ + NULL, /* tp_as_sequence */ + NULL, /* tp_as_mapping */ + NULL, /* tp_hash */ + NULL, /* tp_call */ + NULL, /* tp_str */ + NULL, /* tp_getattro */ + NULL, /* tp_setattro */ + NULL, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, /* tp_flags */ + NULL, /* tp_doc */ + NULL, /* tp_traverse */ + NULL, /* tp_clear */ + NULL, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + NULL, /* tp_iter */ + NULL, /* tp_iternext */ + buffer_core_methods, /* tp_methods */ + NULL, /* tp_members */ + NULL, /* tp_getset */ + NULL, /* tp_base */ + NULL, /* tp_dict */ + NULL, /* tp_descr_get */ + NULL, /* tp_descr_set */ + 0, /* tp_dictoffset */ + BufferCore_init, /* tp_init */ + PyType_GenericAlloc, /* tp_alloc */ + PyType_GenericNew, /* tp_new */ + NULL, /* tp_free */ + NULL, /* tp_is_gc */ + NULL, /* tp_bases */ + NULL, /* tp_mro */ + NULL, /* tp_cache */ + NULL, /* tp_subclasses */ + NULL, /* tp_weaklist */ + NULL, /* tp_del */ + 0, /* tp_version_tag */ + NULL, /* tp_finalize */ }; bool staticInit() { @@ -566,20 +829,35 @@ bool staticInit() { tf2_timeoutexception = stringToPython("tf2.TimeoutException"); #endif - pModulerospy = pythonImport("rospy"); + pModulerclpy = pythonImport("rclpy"); + pModulerclpytime = pythonImport("rclpy.time"); + pModulebuiltininterfacesmsgs = pythonImport("builtin_interfaces.msg"); pModulegeometrymsgs = pythonImport("geometry_msgs.msg"); - if(pModulegeometrymsgs == NULL) + if (pModulerclpy == NULL) + { + printf("Cannot load rclpy module"); + return false; + } + + if (pModulerclpytime == NULL) + { + printf("Cannot load rclpy.time.Time module"); + return false; + } + + if (pModulegeometrymsgs == NULL) { printf("Cannot load geometry_msgs module"); return false; } - buffer_core_Type.tp_alloc = PyType_GenericAlloc; - buffer_core_Type.tp_new = PyType_GenericNew; - buffer_core_Type.tp_init = BufferCore_init; - buffer_core_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE; - buffer_core_Type.tp_methods = buffer_core_methods; + if (pModulebuiltininterfacesmsgs == NULL) + { + printf("Cannot load builtin_interfaces module"); + return false; + } + if (PyType_Ready(&buffer_core_Type) != 0) return false; return true; @@ -597,27 +875,21 @@ PyObject *moduleInit(PyObject *m) { return m; } -#if PY_MAJOR_VERSION < 3 -extern "C" void init_tf2() -{ - if (!staticInit()) - return; - moduleInit(Py_InitModule("_tf2", module_methods)); -} - -#else struct PyModuleDef tf_module = { - PyModuleDef_HEAD_INIT, // base - "_tf2", // name - NULL, // docstring - -1, // state size (but we're using globals) - module_methods // methods + PyModuleDef_HEAD_INIT, /* m_base */ + "_tf2_py", /* m_name */ + NULL, /* m_doc */ + -1, /* m_size - state size (but we're using globals) */ + module_methods, /* m_methods */ + NULL, /* m_slots */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ }; -PyMODINIT_FUNC PyInit_tf2() +PyMODINIT_FUNC PyInit__tf2_py() { if (!staticInit()) return NULL; return moduleInit(PyModule_Create(&tf_module)); } -#endif diff --git a/tf2_py/test/__init__.py b/tf2_py/test/__init__.py new file mode 100644 index 000000000..1dcf1ff02 --- /dev/null +++ b/tf2_py/test/__init__.py @@ -0,0 +1,33 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys + +assert 'tf2_py' not in sys.modules, 'tf2_py should not have been imported before running tests' diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py new file mode 100644 index 000000000..7c3234fae --- /dev/null +++ b/tf2_py/test/test_buffer_core.py @@ -0,0 +1,255 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import time +import unittest +import rclpy +from geometry_msgs.msg import TransformStamped +from test_tf2_py._tf2_py import BufferCore +from test_tf2_py._tf2_py import LookupException + +def build_transform(target_frame, source_frame, stamp): + transform = TransformStamped() + transform.header.frame_id = target_frame + transform.child_frame_id = source_frame + transform.header.stamp = stamp + transform.transform.translation.x = 2.0 + transform.transform.translation.y = 0.0 + transform.transform.translation.z = 0.0 + transform.transform.rotation.w = 1.0 + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = 0.0 + return transform + + +class TestBufferClient(unittest.TestCase): + @classmethod + def setUpClass(cls): + pass + + @classmethod + def tearDownClass(cls): + pass + + def setUp(self): + pass + + def test_all_frames_as_yaml(self): + buffer_core = BufferCore() + + transform = build_transform('bar', 'foo', rclpy.time.Time().to_msg()) + buffer_core.set_transform(transform, 'unittest') + + self.assertTrue(type(buffer_core.all_frames_as_yaml()) == str) + self.assertTrue('foo' in buffer_core.all_frames_as_yaml()) + self.assertTrue('bar' in buffer_core.all_frames_as_yaml()) + + def test_all_frames_as_string(self): + buffer_core = BufferCore() + + transform = build_transform('bar', 'foo', rclpy.time.Time().to_msg()) + buffer_core.set_transform(transform, 'unittest') + + self.assertTrue(type(buffer_core.all_frames_as_string()) == str) + self.assertTrue('foo' in buffer_core.all_frames_as_string()) + self.assertTrue('bar' in buffer_core.all_frames_as_string()) + + def test_set_transform(self): + buffer_core = BufferCore() + transform = build_transform('bar', 'foo', rclpy.time.Time().to_msg()) + + result = buffer_core.set_transform(transform, 'unittest') + self.assertEqual(result, None) + + result, _ = buffer_core.can_transform_core( + target_frame='bar', + source_frame='foo', + time=rclpy.time.Time(seconds=0.5) + ) + self.assertEqual(result, 0) + + def test_set_transform_static(self): + buffer_core = BufferCore() + + transform = build_transform('bar', 'foo', rclpy.time.Time().to_msg()) + + result = buffer_core.set_transform_static(transform, 'unittest') + self.assertEqual(result, None) + + result, _ = buffer_core.can_transform_core( + target_frame='bar', + source_frame='foo', + time=rclpy.time.Time(seconds=0.5) + ) + self.assertEqual(result, 1) + + def test_can_transform_core_pass(self): + buffer_core = BufferCore() + + transform = build_transform('bar', 'foo', rclpy.time.Time().to_msg()) + + buffer_core.set_transform(transform, 'unittest') + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=1).to_msg()) + + buffer_core.set_transform(transform, 'unittest') + + result, error_msg = buffer_core.can_transform_core( + target_frame='foo', + source_frame='bar', + time=rclpy.time.Time(seconds=0.5) + ) + self.assertEqual(result, 1) + self.assertEqual(error_msg, '') + + def test_can_transform_core_fail(self): + buffer_core = BufferCore() + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=1).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=2).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + result, error_msg = buffer_core.can_transform_core( + target_frame='foo', + source_frame='bar', + time=rclpy.time.Time(seconds=2.5) + ) + self.assertEqual(result, 0) + self.assertIn('extrapolation into the future', error_msg) + + result, error_msg = buffer_core.can_transform_core( + target_frame='foo', + source_frame='bar', + time=rclpy.time.Time(seconds=0.5) + ) + self.assertEqual(result, 0) + self.assertIn('extrapolation into the past', error_msg) + + def test_can_transform_full_core(self): + buffer_core = BufferCore() + + transform = build_transform('bar', 'foo', rclpy.time.Time().to_msg()) + buffer_core.set_transform(transform, 'unittest') + + transform = build_transform('foo', 'baz', rclpy.time.Time().to_msg()) + buffer_core.set_transform(transform, 'unittest') + + result, error_msg = buffer_core.can_transform_full_core( + target_frame='foo', + target_time=rclpy.time.Time(), + source_frame='baz', + source_time=rclpy.time.Time(), + fixed_frame='bar' + ) + + self.assertEqual(result, 1) + self.assertEqual(error_msg, '') + + def test_get_latest_common_time(self): + buffer_core = BufferCore() + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=0).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=1).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=0.5).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + latest_common_time = buffer_core.get_latest_common_time('bar', 'foo') + + self.assertEqual(latest_common_time, rclpy.time.Time(seconds=1)) + + def test_clear(self): + buffer_core = BufferCore() + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=0).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + result, _ = buffer_core.can_transform_core( + target_frame='foo', + source_frame='bar', + time=rclpy.time.Time() + ) + self.assertTrue(result) + + buffer_core.clear() + + result, _ = buffer_core.can_transform_core( + target_frame='foo', + source_frame='bar', + time=rclpy.time.Time() + ) + self.assertFalse(result) + + def test_lookup_transform_core_pass(self): + buffer_core = BufferCore() + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=0).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + lookup_transform = buffer_core.lookup_transform_core( + target_frame='bar', + source_frame='foo', + time=rclpy.time.Time() + ) + + self.assertEqual(transform, lookup_transform) + + def test_lookup_transform_core_fail(self): + buffer_core = BufferCore() + + transform = build_transform( + 'bar', 'foo', rclpy.time.Time(seconds=0).to_msg()) + buffer_core.set_transform(transform, 'unittest') + + with self.assertRaises(LookupException) as ex: + buffer_core.lookup_transform_core( + target_frame='bar', + source_frame='baz', + time=rclpy.time.Time() + ) + + self.assertEqual(LookupException, type(ex.exception)) + +if __name__ == '__main__': + unittest.main() diff --git a/tf2_py/src/tf2_py/__init__.py b/tf2_py/tf2_py/__init__.py similarity index 97% rename from tf2_py/src/tf2_py/__init__.py rename to tf2_py/tf2_py/__init__.py index ccade4e57..c9d330ff3 100644 --- a/tf2_py/src/tf2_py/__init__.py +++ b/tf2_py/tf2_py/__init__.py @@ -1,4 +1,3 @@ -#! /usr/bin/python #*********************************************************** #* Software License Agreement (BSD License) #* @@ -34,4 +33,4 @@ #* #* Author: Eitan Marder-Eppstein #*********************************************************** -from _tf2 import * +from tf2_py._tf2_py import * diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 372d2bc0e..9f24a8e7d 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) include_directories(include) +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME}) # tf2_ros library add_library(${PROJECT_NAME} SHARED @@ -178,6 +179,10 @@ add_rostest(test/transform_listener_unittest.launch) add_rostest(test/transform_listener_time_reset_test.launch) endif() + +# Python test + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(tf2_ros_py_test test/py_test) endif() ament_export_include_directories(include) diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index b19395ec1..74c9ed2c0 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -13,30 +13,30 @@ ament_cmake + actionlib_msgs geometry_msgs std_msgs + actionlib_msgs geometry_msgs std_msgs message_filters rclcpp rclcpp_action + rclpy tf2 tf2_msgs + tf2_py - - - - ament_cmake_gtest + ament_cmake_pytest ament_cmake diff --git a/tf2_ros/src/tf2_ros/__init__.py b/tf2_ros/src/tf2_ros/__init__.py index b3df91bbd..018aa88ae 100644 --- a/tf2_ros/src/tf2_ros/__init__.py +++ b/tf2_ros/src/tf2_ros/__init__.py @@ -35,9 +35,9 @@ #* Author: Eitan Marder-Eppstein #*********************************************************** from tf2_py import * -from buffer_interface import * -from buffer import * -from buffer_client import * -from transform_listener import * -from transform_broadcaster import * -from static_transform_broadcaster import * +from .buffer_interface import * +from .buffer import * +from .buffer_client import * +from .transform_listener import * +from .transform_broadcaster import * +from .static_transform_broadcaster import * diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index f652729cc..eadffc08d 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -27,12 +27,18 @@ # author: Wim Meeussen -import roslib; roslib.load_manifest('tf2_ros') -import rospy +import threading + +import rclpy import tf2_py as tf2 import tf2_ros -from tf2_msgs.srv import FrameGraph, FrameGraphResponse -import rosgraph.masterapi +from tf2_msgs.srv import FrameGraph +# TODO(vinnamkim): It seems rosgraph is not ready +# import rosgraph.masterapi +from time import sleep +from rclpy.duration import Duration +from rclpy.task import Future + class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): """ @@ -45,14 +51,11 @@ class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): known frames. """ - def __init__(self, cache_time = None, debug = True): + def __init__(self, cache_time = None): """ - .. function:: __init__(cache_time = None, debug = True) + Constructor. - Constructor. - - :param cache_time: (Optional) How long to retain past information in BufferCore. - :param debug: (Optional) If true, check if another tf2_frames service has been advertised. + :param cache_time: (Optional) How long to retain past information in BufferCore. """ if cache_time != None: tf2.BufferCore.__init__(self, cache_time) @@ -60,18 +63,35 @@ def __init__(self, cache_time = None, debug = True): tf2.BufferCore.__init__(self) tf2_ros.BufferInterface.__init__(self) - if debug: - #Check to see if the service has already been advertised in this node - try: - m = rosgraph.masterapi.Master(rospy.get_name()) - m.lookupService('~tf2_frames') - except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure): - self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames) + self._new_data_callbacks = [] + self._callbacks_to_remove = [] + self._callbacks_lock = threading.RLock() def __get_frames(self, req): - return FrameGraphResponse(self.all_frames_as_yaml()) + return FrameGraph.Response(frame_yaml=self.all_frames_as_yaml()) + + def set_transform(self, *args, **kwargs): + super().set_transform(*args, **kwargs) + self._call_new_data_callbacks() + + def set_transform_static(self, *args, **kwargs): + super().set_transform_static(*args, **kwargs) + self._call_new_data_callbacks() + + def _call_new_data_callbacks(self): + with self._callbacks_lock: + for callback in self._new_data_callbacks: + callback() + # Remove callbacks after to avoid modifying list being iterated on + for callback in self._callbacks_to_remove: + self._new_data_callbacks.remove(callback) - def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + def _remove_callback(self, callback): + with self._callbacks_lock: + # Actually remove the callback later + self._callbacks_to_remove.append(callback) + + def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Get the transform from the source frame to the target frame. @@ -86,7 +106,21 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Durat self.can_transform(target_frame, source_frame, time, timeout) return self.lookup_transform_core(target_frame, source_frame, time) - def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + async def lookup_transform_async(self, target_frame, source_frame, time): + """ + Get the transform from the source frame to the target frame asyncronously. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :return: the transform + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + await self.wait_for_transform_async(target_frame, source_frame, time) + return self.lookup_transform_core(target_frame, source_frame, time) + + + def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ Get the transform from the source frame to the target frame using the advanced API. @@ -102,8 +136,22 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) + async def lookup_transform_full_async(self, target_frame, target_time, source_frame, source_time, fixed_frame): + """ + Get the transform from the source frame to the target frame using the advanced API asyncronously. - def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False): + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :return: the transform + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + await self.wait_for_transform_full_async(target_frame, target_time, source_frame, source_time, fixed_frame) + return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) + + def can_transform(self, target_frame, source_frame, time, timeout=Duration(), return_debug_tuple=False): """ Check if a transform from the source frame to the target frame is possible. @@ -115,19 +163,24 @@ def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration :return: True if the transform is possible, false otherwise. :rtype: bool """ - if timeout != rospy.Duration(0.0): - start_time = rospy.Time.now() - r= rospy.Rate(20) - while (rospy.Time.now() < start_time + timeout and + clock = rclpy.timer.Clock() + if timeout != Duration(): + start_time = clock.now() + # TODO(vinnamkim): rclpy.Rate is not ready + # See https://github.com/ros2/rclpy/issues/186 + # r = rospy.Rate(20) + while (clock.now() < start_time + timeout and not self.can_transform_core(target_frame, source_frame, time)[0] and - (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them - r.sleep() + (clock.now() + Duration(seconds=3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them + # r.sleep() + sleep(0.02) + core_result = self.can_transform_core(target_frame, source_frame, time) if return_debug_tuple: return core_result return core_result[0] - def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0), + def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration(), return_debug_tuple=False): """ @@ -145,15 +198,76 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim :return: True if the transform is possible, false otherwise. :rtype: bool """ - if timeout != rospy.Duration(0.0): - start_time = rospy.Time.now() - r= rospy.Rate(20) - while (rospy.Time.now() < start_time + timeout and + clock = rclpy.timer.Clock() + if timeout != Duration(): + start_time = clock.now() + # TODO(vinnamkim): rclpy.Rate is not ready + # See https://github.com/ros2/rclpy/issues/186 + # r = rospy.Rate(20) + while (clock.now() < start_time + timeout and not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] and - (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them - r.sleep() + (clock.now() + Duration(seconds=3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them + # r.sleep() + sleep(0.02) core_result = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) if return_debug_tuple: return core_result return core_result[0] + def wait_for_transform_async(self, target_frame, source_frame, time): + """ + Wait for a transform from the source frame to the target frame to become possible. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :return: A future that becomes true when the transform is available + :rtype: rclpy.task.Future + """ + fut = rclpy.task.Future() + if self.can_transform_core(target_frame, source_frame, time)[0]: + # Short cut, the transform is available + fut.set_result(True) + return fut + + def _on_new_data(): + try: + if self.can_transform_core(target_frame, source_frame, time)[0]: + fut.set_result(True) + except BaseException as e: + fut.set_exception(e) + + self._new_data_callbacks.append(_on_new_data) + fut.add_done_callback(lambda _: self._remove_callback(_on_new_data)) + + return fut + + def wait_for_transform_full_async(self, target_frame, target_time, source_frame, source_time, fixed_frame): + """ + Wait for a transform from the source frame to the target frame to become possible. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :return: A future that becomes true when the transform is available + :rtype: rclpy.task.Future + """ + fut = rclpy.task.Future() + if self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0]: + # Short cut, the transform is available + fut.set_result(True) + return fut + + def _on_new_data(): + try: + if self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0]: + fut.set_result(True) + except BaseException as e: + fut.set_exception(e) + + self._new_data_callbacks.append(_on_new_data) + fut.add_done_callback(lambda _: self._remove_callback(_on_new_data)) + + return fut diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index c90fefe42..81f1f9e84 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -34,47 +34,40 @@ #* #* Author: Eitan Marder-Eppstein #*********************************************************** -PKG = 'tf2_ros' -import roslib; roslib.load_manifest(PKG) -import rospy -import actionlib +from rclpy.action.client import ActionClient +from rclpy.duration import Duration +from rclpy.clock import Clock +from time import sleep import tf2_py as tf2 import tf2_ros +import threading -from tf2_msgs.msg import LookupTransformAction, LookupTransformGoal +from tf2_msgs.action import LookupTransform from actionlib_msgs.msg import GoalStatus class BufferClient(tf2_ros.BufferInterface): """ Action client-based implementation of BufferInterface. """ - def __init__(self, ns, check_frequency = 10.0, timeout_padding = rospy.Duration.from_sec(2.0)): + def __init__(self, node, ns, check_frequency = 10.0, timeout_padding = Duration(seconds=2.0)): """ .. function:: __init__(ns, check_frequency = 10.0, timeout_padding = rospy.Duration.from_sec(2.0)) Constructor. + :param node: The ROS2 node. :param ns: The namespace in which to look for a BufferServer. :param check_frequency: How frequently to check for updates to known transforms. :param timeout_padding: A constant timeout to add to blocking calls. """ tf2_ros.BufferInterface.__init__(self) - self.client = actionlib.SimpleActionClient(ns, LookupTransformAction) + self.node = node + self.action_client = ActionClient(node, LookupTransform, action_name=ns) self.check_frequency = check_frequency self.timeout_padding = timeout_padding - def wait_for_server(self, timeout = rospy.Duration()): - """ - Block until the action server is ready to respond to requests. - - :param timeout: Time to wait for the server. - :return: True if the server is ready, false otherwise. - :rtype: bool - """ - return self.client.wait_for_server(timeout) - # lookup, simple api - def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Get the transform from the source frame to the target frame. @@ -85,17 +78,17 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Durat :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ - goal = LookupTransformGoal() - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = time; - goal.timeout = timeout; - goal.advanced = False; + goal = LookupTransform.Goal() + goal.target_frame = target_frame + goal.source_frame = source_frame + goal.source_time = time.to_msg() + goal.timeout = timeout.to_msg() + goal.advanced = False return self.__process_goal(goal) # lookup, advanced api - def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ Get the transform from the source frame to the target frame using the advanced API. @@ -108,19 +101,19 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ - goal = LookupTransformGoal() - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = source_time; - goal.timeout = timeout; - goal.target_time = target_time; - goal.fixed_frame = fixed_frame; - goal.advanced = True; + goal = LookupTransform.Goal() + goal.target_frame = target_frame + goal.source_frame = source_frame + goal.source_time = source_time.to_msg() + goal.timeout = timeout.to_msg() + goal.target_time = target_time.to_msg() + goal.fixed_frame = fixed_frame + goal.advanced = True return self.__process_goal(goal) # can, simple api - def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + def can_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Check if a transform from the source frame to the target frame is possible. @@ -140,7 +133,7 @@ def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration # can, advanced api - def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ Check if a transform from the source frame to the target frame is possible (advanced API). @@ -162,32 +155,54 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim except tf2.TransformException: return False - def __is_done(self, state): - if state == GoalStatus.REJECTED or state == GoalStatus.ABORTED or \ - state == GoalStatus.RECALLED or state == GoalStatus.PREEMPTED or \ - state == GoalStatus.SUCCEEDED or state == GoalStatus.LOST: - return True - return False - def __process_goal(self, goal): - self.client.send_goal(goal) - r = rospy.Rate(self.check_frequency) - timed_out = False - start_time = rospy.Time.now() - while not rospy.is_shutdown() and not self.__is_done(self.client.get_state()) and not timed_out: - if rospy.Time.now() > start_time + goal.timeout + self.timeout_padding: - timed_out = True - r.sleep() + # TODO(sloretz) why is this an action client? Service seems more appropriate. + if not self.action_client.server_is_ready(): + raise tf2.TimeoutException("The BufferServer is not ready") + + event = threading.Event() + + def unblock(future): + nonlocal event + event.set() + + send_goal_future = self.action_client.send_goal_async(goal) + send_goal_future.add_done_callback(unblock) + + def unblock_by_timeout(): + nonlocal send_goal_future, goal, event + clock = Clock() + start_time = clock.now() + timeout = Duration.from_msg(goal.timeout) + timeout_padding = Duration(seconds=self.timeout_padding) + while not send_goal_future.done() and not event.is_set(): + if clock.now() > start_time + timeout + timeout_padding: + break + # TODO(vinnamkim): rclpy.Rate is not ready + # See https://github.com/ros2/rclpy/issues/186 + #r = rospy.Rate(self.check_frequency) + sleep(1.0 / self.check_frequency) + + event.set() + + t = threading.Thread(target=unblock_by_timeout) + t.start() + + event.wait() #This shouldn't happen, but could in rare cases where the server hangs - if timed_out: - self.client.cancel_goal() + if not send_goal_future.done(): raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server") - if self.client.get_state() != GoalStatus.SUCCEEDED: - raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.") + # Raises if future was given an exception + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with accepted status. Something is likely wrong with the server.") + + response = self.action_client._get_result(goal_handle) - return self.__process_result(self.client.get_result()) + return self.__process_result(response.result) def __process_result(self, result): if result == None or result.error == None: diff --git a/tf2_ros/src/tf2_ros/buffer_interface.py b/tf2_ros/src/tf2_ros/buffer_interface.py index 58e2ef1d5..12e4aa85d 100644 --- a/tf2_ros/src/tf2_ros/buffer_interface.py +++ b/tf2_ros/src/tf2_ros/buffer_interface.py @@ -27,12 +27,13 @@ # author: Wim Meeussen -import roslib; roslib.load_manifest('tf2_ros') -import rospy +import rclpy import tf2_py as tf2 import tf2_ros from copy import deepcopy from std_msgs.msg import Header +from rclpy.time import Time +from rclpy.duration import Duration class BufferInterface: """ @@ -45,7 +46,7 @@ def __init__(self): self.registration = tf2_ros.TransformRegistration() # transform, simple api - def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), new_type = None): + def transform(self, object_stamped, target_frame, timeout=Duration(), new_type = None): """ Transform an input into the target frame. @@ -69,7 +70,7 @@ def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), n return convert(res, new_type) # transform, advanced api - def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=rospy.Duration(0.0), new_type = None): + def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=Duration(), new_type = None): """ Transform an input into the target frame (advanced API). @@ -98,7 +99,7 @@ def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, return convert(res, new_type) - def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Get the transform from the source frame to the target frame. @@ -113,7 +114,7 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Durat """ raise NotImplementedException() - def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ Get the transform from the source frame to the target frame using the advanced API. @@ -131,7 +132,7 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ raise NotImplementedException() # can, simple api - def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + def can_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Check if a transform from the source frame to the target frame is possible. @@ -147,7 +148,7 @@ def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration raise NotImplementedException() # can, advanced api - def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ Check if a transform from the source frame to the target frame is possible (advanced API). @@ -192,7 +193,7 @@ class TransformRegistration(): __type_map = {} def print_me(self): - print TransformRegistration.__type_map + print(TransformRegistration.__type_map) def add(self, key, callback): TransformRegistration.__type_map[key] = callback @@ -240,14 +241,14 @@ def convert(a, b_type): #check if an efficient conversion function between the types exists try: f = c.get_convert((type(a), b_type)) - print "efficient copy" + print("efficient copy") return f(a) except TypeException: if type(a) == b_type: - print "deep copy" + print("deep copy") return deepcopy(a) f_to = c.get_to_msg(type(a)) f_from = c.get_from_msg(b_type) - print "message copy" + print("message copy") return f_from(f_to(a)) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index aa33fc719..457359e36 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -30,7 +30,9 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rospy +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped @@ -40,12 +42,27 @@ class StaticTransformBroadcaster: :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. """ - def __init__(self): - self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True) + def __init__(self, node, qos=None): + """ + Constructor. + + :param node: The ROS2 node. + :param qos: A QoSProfile or a history depth to apply to the publisher. + """ + if qos is None: + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) + self.pub_tf = node.create_publisher(TFMessage, "/tf_static", qos) def sendTransform(self, transform): if not isinstance(transform, list): - transform = [transform] - self.pub_tf.publish(TFMessage(transform)) + if hasattr(transform, '__iter__'): + transform = list(transform) + else: + transform = [transform] + self.pub_tf.publish(TFMessage(transforms=transform)) diff --git a/tf2_ros/src/tf2_ros/transform_broadcaster.py b/tf2_ros/src/tf2_ros/transform_broadcaster.py index e86e83513..dfaf136a5 100644 --- a/tf2_ros/src/tf2_ros/transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/transform_broadcaster.py @@ -30,7 +30,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rospy +from rclpy.qos import QoSProfile from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped @@ -39,9 +39,16 @@ class TransformBroadcaster: """ :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. """ + def __init__(self, node, qos=QoSProfile(depth=100)): + """ + .. function:: __init__(node, qos=QoSProfile(depth=100)) + + Constructor. - def __init__(self): - self.pub_tf = rospy.Publisher("/tf", TFMessage, queue_size=100) + :param node: The ROS2 node. + :param qos: A QoSProfile or a history depth to apply to the publisher. + """ + self.pub_tf = node.create_publisher(TFMessage, "/tf", qos) def sendTransform(self, transform): """ @@ -50,7 +57,10 @@ def sendTransform(self, transform): :param transform: A transform or list of transforms to send. """ if not isinstance(transform, list): - transform = [transform] - self.pub_tf.publish(TFMessage(transform)) + if hasattr(transform, '__iter__'): + transform = list(transform) + else: + transform = [transform] + self.pub_tf.publish(TFMessage(transforms=transform)) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index daf6b6c7b..c7bbb15a8 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -27,45 +27,84 @@ # author: Wim Meeussen -import roslib; roslib.load_manifest('tf2_ros') -import rospy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import SingleThreadedExecutor +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile import tf2_ros from tf2_msgs.msg import TFMessage +from threading import Thread -class TransformListener(): +class TransformListener: """ :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info. This class takes an object that instantiates the :class:`BufferInterface` interface, to which it propagates changes to the tf frame graph. """ - def __init__(self, buffer): + def __init__(self, buffer, node, *, spin_thread=False, qos=None, static_qos=None): """ - .. function:: __init__(buffer) + Constructor. - Constructor. - - :param buffer: The buffer to propagate changes to when tf info updates. + :param buffer: The buffer to propagate changes to when tf info updates. + :param node: The ROS2 node. + :param spin_thread: Whether to create a dedidcated thread to spin this node. + :param qos: A QoSProfile or a history depth to apply to subscribers. + :param static_qos: A QoSProfile or a history depth to apply to tf_static subscribers. """ + if qos is None: + qos = QoSProfile( + depth=100, + durability=DurabilityPolicy.VOLATILE, + history=HistoryPolicy.KEEP_LAST, + ) + if static_qos is None: + static_qos = QoSProfile( + depth=100, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) self.buffer = buffer - self.tf_sub = rospy.Subscriber("tf", TFMessage, self.callback) - self.tf_static_sub = rospy.Subscriber("tf_static", TFMessage, self.static_callback) - + self.node = node + # Default callback group is mutually exclusive, which would prevent waiting for transforms + # from another callback in the same group. + self.group = ReentrantCallbackGroup() + self.tf_sub = node.create_subscription( + TFMessage, 'tf', self.callback, qos, callback_group=self.group) + self.tf_static_sub = node.create_subscription( + TFMessage, 'tf_static', self.static_callback, static_qos, callback_group=self.group) + + if spin_thread: + self.executor = SingleThreadedExecutor() + + def run_func(): + self.executor.add_node(self.node) + self.executor.spin() + self.executor.remove_node(self.node) + + self.dedicated_listener_thread = Thread(target=run_func) + self.dedicated_listener_thread.start() + def __del__(self): + if hasattr(self, 'dedicated_listener_thread') and hasattr(self, 'executor'): + self.executor.shutdown() + self.dedicated_listener_thread.join() + self.unregister() def unregister(self): """ Unregisters all tf subscribers. """ - self.tf_sub.unregister() - self.tf_static_sub.unregister() + self.node.destroy_subscription(self.tf_sub) + self.node.destroy_subscription(self.tf_static_sub) def callback(self, data): - who = data._connection_header.get('callerid', "default_authority") + who = 'default_authority' for transform in data.transforms: self.buffer.set_transform(transform, who) def static_callback(self, data): - who = data._connection_header.get('callerid', "default_authority") + who = 'default_authority' for transform in data.transforms: self.buffer.set_transform_static(transform, who) diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py new file mode 100644 index 000000000..68ff4755c --- /dev/null +++ b/tf2_ros/test/py_test/test_buffer.py @@ -0,0 +1,145 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import unittest +import rclpy + +from tf2_ros.buffer import Buffer +from geometry_msgs.msg import TransformStamped, PointStamped + + +class TestBuffer(unittest.TestCase): + @classmethod + def setUpClass(cls): + pass + + @classmethod + def tearDownClass(cls): + pass + + def build_transform(self, target, source, rclpy_time): + transform = TransformStamped() + transform.header.frame_id = target + transform.header.stamp = rclpy_time.to_msg() + transform.child_frame_id = source + transform.transform.translation.x = 42.0 + transform.transform.translation.y = -3.14 + transform.transform.translation.z = 0.0 + transform.transform.rotation.w = 1.0 + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = 0.0 + return transform + + def test_can_transform_valid_transform(self): + buffer = Buffer() + clock = rclpy.clock.Clock() + rclpy_time = clock.now() + transform = self.build_transform('foo', 'bar', rclpy_time) + + self.assertEqual(buffer.set_transform(transform, 'unittest'), None) + + self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True) + + output = buffer.lookup_transform('foo', 'bar', rclpy_time) + self.assertEqual(transform.child_frame_id, output.child_frame_id) + self.assertEqual(transform.transform.translation.x, output.transform.translation.x) + self.assertEqual(transform.transform.translation.y, output.transform.translation.y) + self.assertEqual(transform.transform.translation.z, output.transform.translation.z) + + def test_await_transform_immediately_available(self): + # wait for a transform that is already available to test short-cut code + buffer = Buffer() + clock = rclpy.clock.Clock() + rclpy_time = clock.now() + transform = self.build_transform('foo', 'bar', rclpy_time) + + buffer.set_transform(transform, 'unittest') + + coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time) + with self.assertRaises(StopIteration) as cm: + coro.send(None) + + self.assertEqual(transform, cm.exception.value) + coro.close() + + def test_await_transform_full_immediately_available(self): + # wait for a transform that is already available to test short-cut code + buffer = Buffer() + clock = rclpy.clock.Clock() + rclpy_time = clock.now() + transform = self.build_transform('foo', 'bar', rclpy_time) + + buffer.set_transform(transform, 'unittest') + + coro = buffer.lookup_transform_full_async('foo', rclpy_time, 'bar', rclpy_time, 'foo') + with self.assertRaises(StopIteration) as cm: + coro.send(None) + + self.assertEqual(transform, cm.exception.value) + coro.close() + + def test_await_transform_delayed(self): + # wait for a transform that is not yet available + buffer = Buffer() + clock = rclpy.clock.Clock() + rclpy_time = clock.now() + transform = self.build_transform('foo', 'bar', rclpy_time) + + coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time) + coro.send(None) + + buffer.set_transform(transform, 'unittest') + with self.assertRaises(StopIteration) as cm: + coro.send(None) + + self.assertEqual(transform, cm.exception.value) + coro.close() + + def test_await_transform_full_delayed(self): + # wait for a transform that is not yet available + buffer = Buffer() + clock = rclpy.clock.Clock() + rclpy_time = clock.now() + transform = self.build_transform('foo', 'bar', rclpy_time) + + coro = buffer.lookup_transform_full_async('foo', rclpy_time, 'bar', rclpy_time, 'foo') + coro.send(None) + + buffer.set_transform(transform, 'unittest') + with self.assertRaises(StopIteration) as cm: + coro.send(None) + + self.assertEqual(transform, cm.exception.value) + coro.close() + + +if __name__ == '__main__': + unittest.main() diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py new file mode 100644 index 000000000..2c2d57337 --- /dev/null +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -0,0 +1,211 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import time +import unittest +import rclpy +import threading + +from tf2_ros.buffer_client import BufferClient +from geometry_msgs.msg import TransformStamped +from tf2_msgs.action import LookupTransform +from tf2_py import BufferCore, TransformException, TimeoutException, \ + LookupException, InvalidArgumentException, ExtrapolationException, ConnectivityException +from rclpy.executors import SingleThreadedExecutor +from tf2_msgs.msg import TF2Error + + +def build_transform(target_frame, source_frame, stamp): + transform = TransformStamped() + transform.header.frame_id = target_frame + transform.header.stamp = stamp + transform.child_frame_id = source_frame + + transform.transform.translation.x = 42.0 + transform.transform.translation.y = -3.14 + transform.transform.translation.z = 0.0 + transform.transform.rotation.w = 1.0 + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = 0.0 + + return transform + + +class MockActionServer(): + def __init__(self, node, buffer_core): + self.goal_srv = node.create_service( + LookupTransform.Impl.SendGoalService, '/lookup_transform/_action/send_goal', + self.goal_callback) + self.cancel_srv = node.create_service( + LookupTransform.Impl.CancelGoalService, '/lookup_transform/_action/cancel_goal', + self.cancel_callback) + self.result_srv = node.create_service( + LookupTransform.Impl.GetResultService, '/lookup_transform/_action/get_result', + self.result_callback) + self.feedback_pub = node.create_publisher( + LookupTransform.Impl.FeedbackMessage, '/lookup_transform/_action/feedback') + self.node = node + self.buffer_core = buffer_core + self.result_buffer = {} + + def goal_callback(self, request, response): + response.accepted = True + bytes_goal_id = bytes(request.goal_id.uuid) + try: + if not request.goal.advanced: + transform = self.buffer_core.lookup_transform_core(target_frame=request.goal.target_frame, + source_frame=request.goal.source_frame, + time=request.goal.source_time) + self.result_buffer[bytes_goal_id] = ( + transform, TF2Error.NO_ERROR, '') + else: + transform = self.buffer_core.lookup_transform_full_core( + target_frame=request.goal.target_frame, + source_frame=request.goal.source_frame, + source_time=request.goal.source_time, + target_time=request.goal.target_time, + fixed_frame=request.goal.fixed_frame + ) + self.result_buffer[bytes_goal_id] = ( + transform, TF2Error.NO_ERROR, '' + ) + except TimeoutException as e: + self.result_buffer[bytes_goal_id] = ( + TransformStamped(), TF2Error.TIMEOUT_ERROR, e) + except LookupException as e: + self.result_buffer[bytes_goal_id] = ( + TransformStamped(), TF2Error.LOOKUP_ERROR, e) + except InvalidArgumentException as e: + self.result_buffer[bytes_goal_id] = ( + TransformStamped(), TF2Error.INVALID_ARGUMENT_ERROR, e) + except ExtrapolationException as e: + self.result_buffer[bytes_goal_id] = ( + TransformStamped(), TF2Error.EXTRAPOLATION_ERROR, e) + except ConnectivityException as e: + self.result_buffer[bytes_goal_id] = ( + TransformStamped(), TF2Error.CONNECTIVITY_ERROR, e) + except TransformException as e: + self.result_buffer[bytes_goal_id] = ( + TransformStamped(), TF2Error.TRANSFORM_ERROR, e) + + return response + + def cancel_callback(self, request, response): + response.goals_canceling.append(request.goal_info) + return response + + def result_callback(self, request, response): + bytes_goal_id = bytes(request.goal_id.uuid) + response.result.transform = self.result_buffer[bytes_goal_id][0] + response.result.error = TF2Error( + error=self.result_buffer[bytes_goal_id][1], + error_string=str(self.result_buffer[bytes_goal_id][2])) + return response + + def publish_feedback(self, goal_id): + feedback_message = LookupTransform.Impl.FeedbackMessage() + feedback_message.goal_id = goal_id + self.feedback_pub.publish(feedback_message) + + +class TestBufferClient(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.context = rclpy.context.Context() + rclpy.init(context=cls.context) + cls.executor = SingleThreadedExecutor(context=cls.context) + cls.node = rclpy.create_node('TestBufferClient', context=cls.context) + cls.executor.add_node(cls.node) + + buffer_core = BufferCore() + transform = build_transform('foo', 'bar', rclpy.time.Time().to_msg()) + buffer_core.set_transform(transform, 'unittest') + + cls.mock_action_server = MockActionServer(cls.node, buffer_core) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown(context=cls.context) + + def setUp(self): + self.spinning = threading.Event() + self.spin_thread = threading.Thread(target=self.spin) + self.spin_thread.start() + return + + def tearDown(self): + self.spinning.set() + self.spin_thread.join() + return + + def feedback_callback(self, feedback): + self.feedback = feedback + + def spin(self): + try: + while self.context.ok() and not self.spinning.is_set(): + self.executor.spin_once(timeout_sec=0.05) + finally: + return + + def timed_spin(self, duration): + start_time = time.time() + while (time.time() - start_time) < duration: + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + + def execute_goal_callback(self, goal_handle): + print('execute_goal_callback') + goal_handle.set_succeeded() + return LookupTransform.Result() + + def test_lookup_transform_true(self): + buffer_client = BufferClient( + self.node, 'lookup_transform', check_frequency=10.0, timeout_padding=0.0) + + result = buffer_client.lookup_transform( + 'foo', 'bar', rclpy.time.Time(), rclpy.duration.Duration(seconds=5.0)) + + self.assertEqual(build_transform( + 'foo', 'bar', rclpy.time.Time().to_msg()), result) + + def test_lookup_transform_fail(self): + buffer_client = BufferClient( + self.node, 'lookup_transform', check_frequency=10.0, timeout_padding=0.0) + + with self.assertRaises(LookupException) as ex: + result = buffer_client.lookup_transform( + 'bar', 'baz', rclpy.time.Time(), rclpy.duration.Duration(seconds=5.0)) + + self.assertEqual(LookupException, type(ex.exception)) + +if __name__ == '__main__': + unittest.main() diff --git a/tf2_ros/test/py_test/test_listener_and_broadcaster.py b/tf2_ros/test/py_test/test_listener_and_broadcaster.py new file mode 100644 index 000000000..45c334ae0 --- /dev/null +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -0,0 +1,164 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import unittest +import rclpy + +from geometry_msgs.msg import TransformStamped +from tf2_ros.buffer import Buffer +from tf2_ros.transform_broadcaster import TransformBroadcaster +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster +from tf2_ros.transform_listener import TransformListener +from tf2_ros import ExtrapolationException + + +def build_transform(target_frame, source_frame, stamp): + transform = TransformStamped() + transform.header.frame_id = target_frame + transform.header.stamp = stamp + transform.child_frame_id = source_frame + + transform.transform.translation.x = 42.0 + transform.transform.translation.y = -3.14 + transform.transform.translation.z = 0.0 + transform.transform.rotation.w = 1.0 + transform.transform.rotation.x = 0.0 + transform.transform.rotation.y = 0.0 + transform.transform.rotation.z = 0.0 + + return transform + + +class TestBroadcasterAndListener(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + cls.buffer = Buffer() + cls.node = rclpy.create_node('test_broadcaster_listener') + cls.broadcaster = TransformBroadcaster(cls.node) + cls.static_broadcaster = StaticTransformBroadcaster(cls.node) + cls.listener = TransformListener( + buffer=cls.buffer, node=cls.node, spin_thread=False) + + cls.executor = rclpy.executors.SingleThreadedExecutor() + cls.executor.add_node(cls.node) + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.buffer = Buffer() + self.listener.buffer = self.buffer + + def broadcast_transform(self, target_frame, source_frame, time_stamp): + broadcast_transform = build_transform( + target_frame=target_frame, source_frame=source_frame, stamp=time_stamp) + + self.broadcaster.sendTransform(broadcast_transform) + + self.executor.spin_once() + + return broadcast_transform + + def broadcast_static_transform(self, target_frame, source_frame, time_stamp): + broadcast_transform = build_transform( + target_frame=target_frame, source_frame=source_frame, stamp=time_stamp) + + self.static_broadcaster.sendTransform(broadcast_transform) + + self.executor.spin_once() + + return broadcast_transform + + def test_broadcaster_and_listener(self): + time_stamp = rclpy.time.Time(seconds=1, nanoseconds=0).to_msg() + + broadcasted_transform = self.broadcast_transform( + target_frame='foo', source_frame='bar', time_stamp=time_stamp) + + listened_transform = self.buffer.lookup_transform( + target_frame='foo', source_frame='bar', time=time_stamp) + + self.assertTrue(broadcasted_transform, listened_transform) + + # Execute a coroutine + listened_transform_async = None + coro = self.buffer.lookup_transform_async( + target_frame='foo', source_frame='bar', time=time_stamp) + try: + coro.send(None) + except StopIteration as e: + # The coroutine finished; store the result + coro.close() + listened_transform_async = e.value + + self.assertTrue(broadcasted_transform, listened_transform_async) + + def test_extrapolation_exception(self): + self.broadcast_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=0.3, nanoseconds=0).to_msg()) + + self.broadcast_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=0.2, nanoseconds=0).to_msg()) + + with self.assertRaises(ExtrapolationException) as e: + self.buffer.lookup_transform( + target_frame='foo', source_frame='bar', + time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg()) + + self.assertTrue( + 'Lookup would require extrapolation into the past' in str(e.exception)) + + with self.assertRaises(ExtrapolationException) as e: + self.buffer.lookup_transform( + target_frame='foo', source_frame='bar', + time=rclpy.time.Time(seconds=0.4, nanoseconds=0).to_msg()) + + self.assertTrue( + 'Lookup would require extrapolation into the future' in str(e.exception)) + + def test_static_broadcaster_and_listener(self): + broadcasted_transform = self.broadcast_static_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=1.1, nanoseconds=0).to_msg()) + + listened_transform = self.buffer.lookup_transform( + target_frame='foo', source_frame='bar', + time=rclpy.time.Time(seconds=1.5, nanoseconds=0).to_msg()) + + self.assertTrue(broadcasted_transform, listened_transform) + + +if __name__ == '__main__': + unittest.main()