From d29c620b9275cd76406be88438aff6858edf26e3 Mon Sep 17 00:00:00 2001 From: Brannon King Date: Wed, 21 Jun 2017 10:44:06 -0600 Subject: [PATCH 01/79] made tf2_py compile with ROS2 --- geometry2/AMENT_IGNORE | 0 geometry2/CHANGELOG.rst | 11 -- geometry2/CMakeLists.txt | 4 - geometry2/package.xml | 29 ----- geometry_experimental/AMENT_IGNORE | 0 geometry_experimental/CHANGELOG.rst | 126 -------------------- geometry_experimental/CMakeLists.txt | 4 - geometry_experimental/package.xml | 23 ---- tf2_py/AMENT_IGNORE | 0 tf2_py/CMakeLists.txt | 169 +++------------------------ tf2_py/package.xml | 42 ++----- tf2_py/setup.py | 26 +++-- tf2_py/src/tf2_py.cpp | 84 +++++++------ 13 files changed, 100 insertions(+), 418 deletions(-) delete mode 100644 geometry2/AMENT_IGNORE delete mode 100644 geometry2/CHANGELOG.rst delete mode 100644 geometry2/CMakeLists.txt delete mode 100644 geometry2/package.xml delete mode 100644 geometry_experimental/AMENT_IGNORE delete mode 100644 geometry_experimental/CHANGELOG.rst delete mode 100644 geometry_experimental/CMakeLists.txt delete mode 100644 geometry_experimental/package.xml delete mode 100644 tf2_py/AMENT_IGNORE diff --git a/geometry2/AMENT_IGNORE b/geometry2/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/geometry2/CHANGELOG.rst b/geometry2/CHANGELOG.rst deleted file mode 100644 index 4c068af87..000000000 --- a/geometry2/CHANGELOG.rst +++ /dev/null @@ -1,11 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package geometry2 -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.5.15 (2017-01-24) -------------------- - -0.5.14 (2017-01-16) -------------------- -* create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking. -* Contributors: Tully Foote diff --git a/geometry2/CMakeLists.txt b/geometry2/CMakeLists.txt deleted file mode 100644 index 83f1b03cd..000000000 --- a/geometry2/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(geometry2) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/geometry2/package.xml b/geometry2/package.xml deleted file mode 100644 index 3f4697801..000000000 --- a/geometry2/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - geometry2 - 0.9.1 - - A metapackage to bring in the default packages second generation Transform Library in ros, tf2. - - Tully Foote - Tully Foote - BSD - - http://www.ros.org/wiki/geometry2 - - catkin - - tf2 - tf2_bullet - tf2_eigen - tf2_geometry_msgs - tf2_kdl - tf2_msgs - tf2_py - tf2_ros - tf2_sensor_msgs - tf2_tools - - - - - diff --git a/geometry_experimental/AMENT_IGNORE b/geometry_experimental/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/geometry_experimental/CHANGELOG.rst b/geometry_experimental/CHANGELOG.rst deleted file mode 100644 index 2ca055e3a..000000000 --- a/geometry_experimental/CHANGELOG.rst +++ /dev/null @@ -1,126 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package geometry_experimental -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.5.15 (2017-01-24) -------------------- - -0.5.14 (2017-01-16) -------------------- -* create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking. -* Contributors: Tully Foote - -0.5.13 (2016-03-04) -------------------- -* Remove LGPL from license tags - LGPL was erroneously included in 2a38724. As there are no files with it - in the package. -* add missing dependencies in the meta-package geometry_experimental - This partly fixes the doc jobs in `#120 `_ -* Contributors: Jochen Sprickerhof, Vincent Rabaud - -0.5.12 (2015-08-05) -------------------- - -0.5.11 (2015-04-22) -------------------- - -0.5.10 (2015-04-21) -------------------- - -0.5.9 (2015-03-25) ------------------- - -0.5.8 (2015-03-17) ------------------- - -0.5.7 (2014-12-23) ------------------- - -0.5.6 (2014-09-18) ------------------- - -0.5.5 (2014-06-23) ------------------- - -0.5.4 (2014-05-07) ------------------- - -0.5.3 (2014-02-21) ------------------- - -0.5.2 (2014-02-20) ------------------- - -0.5.1 (2014-02-14) ------------------- - -0.5.0 (2014-02-14) ------------------- - -0.4.10 (2013-12-26) -------------------- - -0.4.9 (2013-11-06) ------------------- - -0.4.8 (2013-11-06) ------------------- - -0.4.7 (2013-08-28) ------------------- - -0.4.6 (2013-08-28) ------------------- - -0.4.5 (2013-07-11) ------------------- - -0.4.4 (2013-07-09) ------------------- - -0.4.3 (2013-07-05) ------------------- -* removing test_tf2 from metapackage and update description - -0.4.2 (2013-07-05) ------------------- - -0.4.1 (2013-07-05) ------------------- - -0.4.0 (2013-06-27) ------------------- -* Restoring test packages and bullet packages. - reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented - reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ -* updating for new metapackage specification - -0.3.6 (2013-03-03) ------------------- - -0.3.5 (2013-02-15 14:46) ------------------------- -* 0.3.4 -> 0.3.5 -* fixing adding new packages to metapackage - -0.3.4 (2013-02-15 13:14) ------------------------- -* 0.3.3 -> 0.3.4 - -0.3.3 (2013-02-15 11:30) ------------------------- -* 0.3.2 -> 0.3.3 - -0.3.2 (2013-02-15 00:42) ------------------------- -* 0.3.1 -> 0.3.2 - -0.3.1 (2013-02-14) ------------------- -* 0.3.0 -> 0.3.1 - -0.3.0 (2013-02-13) ------------------- -* switching to version 0.3.0 -* renaming geometry_experimental meta package in order to comply with catkin naming convention diff --git a/geometry_experimental/CMakeLists.txt b/geometry_experimental/CMakeLists.txt deleted file mode 100644 index 54074da0f..000000000 --- a/geometry_experimental/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(geometry_experimental) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/geometry_experimental/package.xml b/geometry_experimental/package.xml deleted file mode 100644 index 72c900852..000000000 --- a/geometry_experimental/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - geometry_experimental - 0.9.1 - - The second generation Transform Library in ros. This metapackage is deprecated, but is kept for backwards compatability. - - Tully Foote - Eitan Marder-Eppstein - Wim Meeussen - Tully Foote - BSD - - http://www.ros.org/wiki/geometry_experimental - - catkin - - geometry2 - - - - - - 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..f2bd3c1cf 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -1,157 +1,26 @@ 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 -) - -########### -## 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. +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(PythonLibs 3 REQUIRED) +include_directories(${PYTHON_INCLUDE_PATH}) 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) - -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} -) - - -############# -## Testing ## -############# +add_library(${PROJECT_NAME} src/tf2_py.cpp) +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) +set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") -## 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() +ament_target_dependencies(${PROJECT_NAME} "rclcpp" "rclpy" "geometry_msgs" "tf2_msgs" "python") +ament_export_include_directories("include") +ament_export_libraries(${PROJECT_NAME}) -## 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..4fdfb1868 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -1,5 +1,6 @@ - + + tf2_py 0.9.1 The tf2_py package @@ -19,37 +20,18 @@ http://ros.org/wiki/tf2_py - - - - - - - - - - - - - - - - - - - catkin - tf2 + ament_cmake rospy - tf2 - rospy - + tf2 + tf2_msgs + geometry_msgs + builtin_interfaces + tf2_msgs + tf2 + rospy - - - - - - + ament_cmake + ament_python diff --git a/tf2_py/setup.py b/tf2_py/setup.py index a21d885d6..e0cf0eaa7 100644 --- a/tf2_py/setup.py +++ b/tf2_py/setup.py @@ -1,12 +1,22 @@ #!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup -d = generate_distutils_setup( - packages=['tf2_py'], - package_dir={'': 'src'}, - requires=['rospy', 'geometry_msgs', 'tf2_msgs'] +setup( + name='tf2_py', + version='0.5.15', + packages=[], + py_modules=[], + install_requires=['setuptools', 'geometry_msgs', 'tf2_msgs'], + maintainer='Tully Foote', + maintainer_email='tfoote@osrfoundation.org', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: BSD', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='Python API to access tf2 buffers.', + license='BSD' ) - -setup(**d) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index fc60ec202..f30f5468b 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -64,11 +64,11 @@ static PyTypeObject buffer_core_Type = { # else PyVarObject_HEAD_INIT(NULL, 0) #endif - "_tf2.BufferCore", /*name*/ - sizeof(buffer_core_t), /*basicsize*/ + "_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; pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped"); @@ -96,7 +96,7 @@ static PyObject *transform_converter(const geometry_msgs::TransformStamped* tran //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 *args = Py_BuildValue("ii", transform->header.stamp.sec, transform->header.stamp.nanosec); PyObject *time_obj = PyObject_CallObject(rospy_time, args); Py_DECREF(args); Py_DECREF(rospy_time); @@ -129,14 +129,30 @@ static PyObject *transform_converter(const geometry_msgs::TransformStamped* tran return pinst; } -static int rostime_converter(PyObject *obj, builtin_interfaces::msg::Time *rt) +static int rostime_converter(PyObject *obj, tf2::TimePoint *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)); + *rt = tf2::timeFromSec(PyFloat_AsDouble(tsr)); + Py_DECREF(tsr); + return 1; + } +} + +static int rostime_converter_blt(PyObject *obj, builtin_interfaces::msg::Time *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 { + auto dbl = PyFloat_AsDouble(tsr); + auto sec = std::floor(dbl); + (*rt).sec = static_cast(sec); + (*rt).nanosec = static_cast((dbl - sec) * 1000000000.0); Py_DECREF(tsr); return 1; } @@ -149,7 +165,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) 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)); + (*rt) = tf2::durationFromSec(PyFloat_AsDouble(tsr)); Py_DECREF(tsr); return 1; } @@ -157,9 +173,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) { - tf2::Duration cache_time; - - 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; @@ -195,7 +209,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 +224,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 +256,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,17 +279,19 @@ 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 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) { + auto r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string); + if (r == tf2::TF2Error::NO_ERROR) { PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); - PyObject *args = Py_BuildValue("ii", time.sec, time.nsec); + auto dbl = tf2::timeToSec(time); + auto sec = std::floor(dbl); + PyObject *args = Py_BuildValue("ii", static_cast(sec), static_cast((dbl - sec) * 1000000000.0)); PyObject *ob = PyObject_CallObject(rospy_time, args); Py_DECREF(args); Py_DECREF(rospy_time); @@ -290,15 +306,15 @@ static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *k { 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; + auto origin = transform.transform.translation; + auto 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 +326,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 +338,10 @@ 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; + auto origin = transform.transform.translation; + auto rotation = transform.transform.rotation; //TODO: Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); } @@ -340,7 +356,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 +381,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)); @@ -410,11 +426,11 @@ static PyObject *setTransform(PyObject *self, PyObject *args) 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_blt(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) return NULL; PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); @@ -453,11 +469,12 @@ static PyObject *setTransformStatic(PyObject *self, PyObject *args) 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) + tf2::Duration tp; + if (rostime_converter_blt(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) return NULL; PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); @@ -515,10 +532,11 @@ 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()); } From ec0a7cfc995187c56b13d6593c01eace65bebd8f Mon Sep 17 00:00:00 2001 From: Brannon King Date: Sat, 24 Jun 2017 19:50:26 -0600 Subject: [PATCH 02/79] fixed package ordering, time message conversion --- tf2_py/CMakeLists.txt | 35 ++++++++++++++++++++++++++++------- tf2_py/package.xml | 11 +++++++---- tf2_py/src/tf2_py.cpp | 29 +++++++++-------------------- 3 files changed, 44 insertions(+), 31 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index f2bd3c1cf..60488abe5 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -1,26 +1,47 @@ cmake_minimum_required(VERSION 3.5) project(tf2_py) -if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wextra -Wpedantic") +# 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) endif() find_package(ament_cmake REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(PythonLibs 3 REQUIRED) +find_package(rclpy REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) -find_package(PythonLibs 3 REQUIRED) +find_package(tf2_ros REQUIRED) + include_directories(${PYTHON_INCLUDE_PATH}) link_libraries(${PYTHON_LIBRARIES}) + add_library(${PROJECT_NAME} src/tf2_py.cpp) -set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) + set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") -ament_target_dependencies(${PROJECT_NAME} "rclcpp" "rclpy" "geometry_msgs" "tf2_msgs" "python") +ament_target_dependencies(${PROJECT_NAME} + "geometry_msgs" + "rclcpp" + "rclpy" + "python" + "tf2_msgs" + "tf2_ros" +) + ament_export_include_directories("include") ament_export_libraries(${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 4fdfb1868..1284f319f 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -21,14 +21,17 @@ http://ros.org/wiki/tf2_py ament_cmake + + builtin_interfaces + geometry_msgs rospy tf2 tf2_msgs - geometry_msgs - builtin_interfaces - tf2_msgs - tf2 + tf2_ros + rospy + tf2 + tf2_ros ament_cmake diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index f30f5468b..e10343ff6 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -2,7 +2,7 @@ #include #include - +#include #include "python_compat.h" // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions) @@ -142,22 +142,6 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) } } -static int rostime_converter_blt(PyObject *obj, builtin_interfaces::msg::Time *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 { - auto dbl = PyFloat_AsDouble(tsr); - auto sec = std::floor(dbl); - (*rt).sec = static_cast(sec); - (*rt).nanosec = static_cast((dbl - sec) * 1000000000.0); - Py_DECREF(tsr); - return 1; - } -} - static int rosduration_converter(PyObject *obj, tf2::Duration *rt) { PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); @@ -422,6 +406,7 @@ 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; @@ -430,9 +415,11 @@ static PyObject *setTransform(PyObject *self, PyObject *args) 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_blt(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) - return NULL; + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) + return NULL; + transform.header.stamp = tf2_ros::toMsg(time); + PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); @@ -465,6 +452,7 @@ 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; @@ -474,8 +462,9 @@ static PyObject *setTransformStatic(PyObject *self, PyObject *args) transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); tf2::Duration tp; - if (rostime_converter_blt(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; + transform.header.stamp = tf2_ros::toMsg(time); PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); From 09ad997b9bdbb3167b464d6a2b1fb419ea9e5ed5 Mon Sep 17 00:00:00 2001 From: Brannon King Date: Mon, 26 Jun 2017 16:24:27 -0600 Subject: [PATCH 03/79] fixed dll deployment --- tf2_py/CMakeLists.txt | 15 ++++++++++----- tf2_py/package.xml | 5 ++--- tf2_py/setup.py | 22 ---------------------- tf2_py/src/tf2_py.cpp | 24 ++++++++++++------------ tf2_py/{src => }/tf2_py/__init__.py | 0 5 files changed, 24 insertions(+), 42 deletions(-) delete mode 100644 tf2_py/setup.py rename tf2_py/{src => }/tf2_py/__init__.py (100%) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 60488abe5..d1701e1cf 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -21,23 +21,28 @@ find_package(tf2_ros REQUIRED) include_directories(${PYTHON_INCLUDE_PATH}) -link_libraries(${PYTHON_LIBRARIES}) - -add_library(${PROJECT_NAME} src/tf2_py.cpp) +add_library(${PROJECT_NAME} SHARED src/tf2_py.cpp) set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") ament_target_dependencies(${PROJECT_NAME} + "builtin_interfaces" "geometry_msgs" - "rclcpp" - "rclpy" "python" + "rclpy" + "tf2" "tf2_msgs" "tf2_ros" ) ament_export_include_directories("include") ament_export_libraries(${PROJECT_NAME}) +ament_python_install_package(${PROJECT_NAME}) + +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 1284f319f..013c9e047 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -24,17 +24,16 @@ builtin_interfaces geometry_msgs - rospy + rclpy tf2 tf2_msgs tf2_ros - rospy + rclpy tf2 tf2_ros ament_cmake - ament_python diff --git a/tf2_py/setup.py b/tf2_py/setup.py deleted file mode 100644 index e0cf0eaa7..000000000 --- a/tf2_py/setup.py +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from setuptools import setup - -setup( - name='tf2_py', - version='0.5.15', - packages=[], - py_modules=[], - install_requires=['setuptools', 'geometry_msgs', 'tf2_msgs'], - maintainer='Tully Foote', - maintainer_email='tfoote@osrfoundation.org', - keywords=['ROS'], - classifiers=[ - 'Intended Audience :: Developers', - 'License :: OSI Approved :: BSD', - 'Programming Language :: Python', - 'Topic :: Software Development', - ], - description='Python API to access tf2 buffers.', - license='BSD' -) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index e10343ff6..f890d2ed6 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -133,7 +133,7 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *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"); + PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rclpy.Time or rclpy.Duration"); return 0; } else { *rt = tf2::timeFromSec(PyFloat_AsDouble(tsr)); @@ -146,7 +146,7 @@ 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"); + PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rclpy.Time or rclpy.Duration"); return 0; } else { (*rt) = tf2::durationFromSec(PyFloat_AsDouble(tsr)); @@ -535,17 +535,17 @@ static struct PyMethodDef buffer_core_methods[] = {"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}, + {"can_transform_core", (PyCFunction)canTransformCore, METH_VARARGS | METH_KEYWORDS}, + {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS}, + {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS}, + {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS}, {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS}, {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS}, - {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_KEYWORDS}, + {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | 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}, + {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS}, + {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS}, + //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS}, //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS}, //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS}, {NULL, NULL} @@ -573,7 +573,7 @@ bool staticInit() { tf2_timeoutexception = stringToPython("tf2.TimeoutException"); #endif - pModulerospy = pythonImport("rospy"); + pModulerospy = pythonImport("rclpy"); pModulegeometrymsgs = pythonImport("geometry_msgs.msg"); if(pModulegeometrymsgs == NULL) @@ -621,7 +621,7 @@ struct PyModuleDef tf_module = { module_methods // methods }; -PyMODINIT_FUNC PyInit_tf2() +PyMODINIT_FUNC PyInit__tf2() { if (!staticInit()) return NULL; diff --git a/tf2_py/src/tf2_py/__init__.py b/tf2_py/tf2_py/__init__.py similarity index 100% rename from tf2_py/src/tf2_py/__init__.py rename to tf2_py/tf2_py/__init__.py From 12addd485c00b774d1b021c16da246c7dc47d38a Mon Sep 17 00:00:00 2001 From: vinnam kim Date: Sat, 16 Mar 2019 15:46:00 +0900 Subject: [PATCH 04/79] fix the installation path of tf2_py - Also, remove tf2_ros dependency from tf2_py Signed-off-by: vinnam kim --- tf2_py/CMakeLists.txt | 4 +--- tf2_py/package.xml | 2 -- tf2_py/src/tf2_py.cpp | 17 ++++++++++++++--- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index d1701e1cf..0b1449df1 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(PythonLibs 3 REQUIRED) find_package(rclpy REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) -find_package(tf2_ros REQUIRED) include_directories(${PYTHON_INCLUDE_PATH}) @@ -32,7 +31,6 @@ ament_target_dependencies(${PROJECT_NAME} "rclpy" "tf2" "tf2_msgs" - "tf2_ros" ) ament_export_include_directories("include") @@ -41,7 +39,7 @@ ament_python_install_package(${PROJECT_NAME}) install(TARGETS ${PROJECT_NAME} - DESTINATION lib + DESTINATION ${PYTHON_INSTALL_DIR} ) if(BUILD_TESTING) diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 013c9e047..1440dc64b 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -27,11 +27,9 @@ rclpy tf2 tf2_msgs - tf2_ros rclpy tf2 - tf2_ros ament_cmake diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index f890d2ed6..b856f64a9 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -2,7 +2,6 @@ #include #include -#include #include "python_compat.h" // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions) @@ -129,6 +128,18 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* return pinst; } +static builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t) +{ + std::chrono::nanoseconds ns = \ + std::chrono::duration_cast(t.time_since_epoch()); + std::chrono::seconds s = \ + std::chrono::duration_cast(t.time_since_epoch()); + builtin_interfaces::msg::Time time_msg; + time_msg.sec = (int32_t)s.count(); + time_msg.nanosec = (uint32_t)(ns.count() % 1000000000ull); + return time_msg; +} + static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) { PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); @@ -418,7 +429,7 @@ static PyObject *setTransform(PyObject *self, PyObject *args) if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; - transform.header.stamp = tf2_ros::toMsg(time); + transform.header.stamp = toMsg(time); PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); @@ -464,7 +475,7 @@ static PyObject *setTransformStatic(PyObject *self, PyObject *args) tf2::Duration tp; if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; - transform.header.stamp = tf2_ros::toMsg(time); + transform.header.stamp = toMsg(time); PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); From 851dc26990d95efa2178132758821c8cf7ad5ee4 Mon Sep 17 00:00:00 2001 From: vinnam kim Date: Sat, 16 Mar 2019 16:45:28 +0900 Subject: [PATCH 05/79] Port tf2_ros Signed-off-by: vinnam kim --- tf2_ros/CMakeLists.txt | 1 + tf2_ros/package.xml | 12 ++++ tf2_ros/src/tf2_ros/__init__.py | 12 ++-- tf2_ros/src/tf2_ros/buffer.py | 69 +++++++++++-------- tf2_ros/src/tf2_ros/buffer_interface.py | 24 +++---- .../tf2_ros/static_transform_broadcaster.py | 17 +++-- tf2_ros/src/tf2_ros/transform_broadcaster.py | 11 +-- tf2_ros/src/tf2_ros/transform_listener.py | 19 ++--- 8 files changed, 98 insertions(+), 67 deletions(-) diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 372d2bc0e..d04d54413 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 diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index b19395ec1..e40ef6918 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -14,10 +14,22 @@ ament_cmake geometry_msgs + message_filters + rclcpp + rclpy std_msgs + tf2 + tf2_msgs + tf2_py geometry_msgs + message_filters + rclcpp + rclpy std_msgs + tf2 + tf2_msgs + tf2_py message_filters rclcpp diff --git a/tf2_ros/src/tf2_ros/__init__.py b/tf2_ros/src/tf2_ros/__init__.py index b3df91bbd..5822defe0 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..ee03756d6 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -27,12 +27,14 @@ # author: Wim Meeussen -import roslib; roslib.load_manifest('tf2_ros') -import rospy +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 class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): """ @@ -59,19 +61,19 @@ def __init__(self, cache_time = None, debug = True): else: 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) + + # 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) def __get_frames(self, req): - return FrameGraphResponse(self.all_frames_as_yaml()) + return FrameGraph.Response(frame_yaml=self.all_frames_as_yaml()) - 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. @@ -86,7 +88,7 @@ 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)): + 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. @@ -103,7 +105,7 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ 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=rospy.Duration(0.0), return_debug_tuple=False): + 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 +117,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,13 +152,17 @@ 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 diff --git a/tf2_ros/src/tf2_ros/buffer_interface.py b/tf2_ros/src/tf2_ros/buffer_interface.py index 58e2ef1d5..9c0451abf 100644 --- a/tf2_ros/src/tf2_ros/buffer_interface.py +++ b/tf2_ros/src/tf2_ros/buffer_interface.py @@ -27,12 +27,12 @@ # 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 class BufferInterface: """ @@ -45,7 +45,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 = Time(), new_type = None): """ Transform an input into the target frame. @@ -69,7 +69,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=Time(), new_type = None): """ Transform an input into the target frame (advanced API). @@ -98,7 +98,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=Time()): """ Get the transform from the source frame to the target frame. @@ -113,7 +113,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=Time()): """ Get the transform from the source frame to the target frame using the advanced API. @@ -131,7 +131,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=Time()): """ Check if a transform from the source frame to the target frame is possible. @@ -147,7 +147,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=Time()): """ Check if a transform from the source frame to the target frame is possible (advanced API). @@ -192,7 +192,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 +240,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..0f2d487f6 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -30,22 +30,27 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rospy +from rclpy.node import Node from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped +from rclpy.qos import QoSProfile, QoSDurabilityPolicy - -class StaticTransformBroadcaster: +class StaticTransformBroadcaster(Node): """ :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, name=None): + super().__init__('transform_broadcaster_impl' if name is None else name) + latching_qos = QoSProfile( + depth=1, + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + self.pub_tf = self.create_publisher(TFMessage, "/tf_static", qos_profile=latching_qos) + #self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True) def sendTransform(self, transform): if not isinstance(transform, list): transform = [transform] - self.pub_tf.publish(TFMessage(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..b23b48430 100644 --- a/tf2_ros/src/tf2_ros/transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/transform_broadcaster.py @@ -30,18 +30,19 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rospy +from rclpy.node import Node from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped -class TransformBroadcaster: +class TransformBroadcaster(Node): """ :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. """ - def __init__(self): - self.pub_tf = rospy.Publisher("/tf", TFMessage, queue_size=100) + def __init__(self, name=None): + super().__init__('transform_broadcaster_impl' if name is None else name) + self.pub_tf = self.create_publisher(TFMessage, "/tf") def sendTransform(self, transform): """ @@ -51,6 +52,6 @@ def sendTransform(self, transform): """ if not isinstance(transform, list): transform = [transform] - self.pub_tf.publish(TFMessage(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..273535987 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -27,28 +27,29 @@ # author: Wim Meeussen -import roslib; roslib.load_manifest('tf2_ros') -import rospy +from rclpy.node import Node import tf2_ros from tf2_msgs.msg import TFMessage -class TransformListener(): +class TransformListener(Node): """ :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, name=None): """ - .. function:: __init__(buffer) + .. function:: __init__(buffer, name=None) Constructor. :param buffer: The buffer to propagate changes to when tf info updates. + :param name: The name of ROS2 node. """ + super().__init__('transform_listener_impl' if name is None else name) 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.tf_sub = self.create_subscription(TFMessage, 'tf', self.callback) + self.tf_static_sub = self.create_subscription(TFMessage, 'tf_static', self.static_callback) def __del__(self): self.unregister() @@ -57,8 +58,8 @@ def unregister(self): """ Unregisters all tf subscribers. """ - self.tf_sub.unregister() - self.tf_static_sub.unregister() + self.destroy_subscription(self.tf_sub) + self.destroy_subscription(self.tf_static_sub) def callback(self, data): who = data._connection_header.get('callerid', "default_authority") From a61f8c720231cee7efe81a350ae737a57960e515 Mon Sep 17 00:00:00 2001 From: vinnam kim Date: Sat, 16 Mar 2019 20:25:41 +0900 Subject: [PATCH 06/79] Port tf2_ros/buffer_client.py Signed-off-by: vinnam kim --- tf2_ros/src/tf2_ros/buffer_client.py | 44 ++++++++++++++++------------ 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index c90fefe42..2b778c9aa 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -34,36 +34,39 @@ #* #* 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 -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.client = ActionClient(node, LookupTransform, ns) + self.node = node self.check_frequency = check_frequency self.timeout_padding = timeout_padding - def wait_for_server(self, timeout = rospy.Duration()): + def wait_for_server(self, timeout = Duration()): """ Block until the action server is ready to respond to requests. @@ -74,7 +77,7 @@ def wait_for_server(self, timeout = rospy.Duration()): 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,7 +88,7 @@ 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 = LookupTransform.Goal() goal.target_frame = target_frame; goal.source_frame = source_frame; goal.source_time = time; @@ -95,7 +98,7 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Durat 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,7 +111,7 @@ 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 = LookupTransform.Goal() goal.target_frame = target_frame; goal.source_frame = source_frame; goal.source_time = source_time; @@ -120,7 +123,7 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ 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 +143,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). @@ -171,13 +174,16 @@ def __is_done(self, state): def __process_goal(self, goal): self.client.send_goal(goal) - r = rospy.Rate(self.check_frequency) + # TODO(vinnamkim): rclpy.Rate is not ready + # See https://github.com/ros2/rclpy/issues/186 + #r = rospy.Rate(self.check_frequency) timed_out = False - start_time = rospy.Time.now() + clock = Clock() + start_time = clock.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: + if clock.now() > start_time + goal.timeout + self.timeout_padding: timed_out = True - r.sleep() + sleep(self.check_frequency / 1000.0) #This shouldn't happen, but could in rare cases where the server hangs if timed_out: From 9caefc42f32eb238e836d0d85893ce81f4cc67f4 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Sun, 17 Mar 2019 00:18:50 +0900 Subject: [PATCH 07/79] Fix build errors because of ament_lint_auto dependency Signed-off-by: vinnamkim --- tf2_py/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 1440dc64b..331e1e02a 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -31,6 +31,8 @@ rclpy tf2 + ament_lint_auto + ament_cmake From 9759d04c60f01a7110c1c4d7db86566ea9d57411 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Fri, 22 Mar 2019 20:23:21 +0900 Subject: [PATCH 08/79] Fix tf2_py - Add unit tests - Fix c extension code to make correctly Signed-off-by: vinnamkim --- tf2_py/CMakeLists.txt | 7 +- tf2_py/package.xml | 2 + tf2_py/src/tf2_py.cpp | 133 ++++++++--- tf2_py/test/test_buffer_core.py | 262 +++++++++++++++++++++ tf2_ros/src/tf2_ros/buffer_client.py | 91 +++---- tf2_ros/src/tf2_ros/buffer_interface.py | 3 +- tf2_ros/test/py_test/test_buffer.py | 84 +++++++ tf2_ros/test/py_test/test_buffer_client.py | 148 ++++++++++++ 8 files changed, 651 insertions(+), 79 deletions(-) create mode 100644 tf2_py/test/test_buffer_core.py create mode 100644 tf2_ros/test/py_test/test_buffer.py create mode 100644 tf2_ros/test/py_test/test_buffer_client.py diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 0b1449df1..20ff32dc4 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -9,8 +9,9 @@ endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() - +set(CMAKE_BUILD_TYPE Debug) find_package(ament_cmake REQUIRED) +find_package(python_cmake_module REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(PythonLibs 3 REQUIRED) @@ -45,6 +46,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(tf2_py_test test) endif() ament_package() diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 331e1e02a..7453b2a26 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -21,6 +21,7 @@ http://ros.org/wiki/tf2_py ament_cmake + python_cmake_module builtin_interfaces geometry_msgs @@ -32,6 +33,7 @@ tf2 ament_lint_auto + ament_cmake_pytest ament_cmake diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index b856f64a9..c57875313 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -44,7 +44,9 @@ } \ } 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, @@ -94,24 +96,32 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* } //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.nanosec); - PyObject *time_obj = PyObject_CallObject(rospy_time, args); + PyObject *builtin_interfaces_time = PyObject_GetAttrString(pModulebuiltininterfacesmsgs, "Time"); + PyObject *args = PyTuple_New(0); + PyObject *kwargs = PyDict_New(); + PyObject *sec = Py_BuildValue("i", transform->header.stamp.sec); + PyObject *nanosec = Py_BuildValue("i", transform->header.stamp.nanosec); + PyDict_SetItemString(kwargs, "sec", sec); + PyDict_SetItemString(kwargs, "nanosec", nanosec); + + PyObject *time_obj = PyObject_Call(builtin_interfaces_time, args, kwargs); + + Py_DECREF(builtin_interfaces_time); Py_DECREF(args); - Py_DECREF(rospy_time); + Py_DECREF(kwargs); + Py_DECREF(sec); + Py_DECREF(nanosec); PyObject* pheader = PyObject_GetAttrString(pinst, "header"); PyObject_SetAttrString(pheader, "stamp", time_obj); Py_DECREF(time_obj); - + PyObject_SetAttrString(pheader, "frame_id", stringToPython(transform->header.frame_id)); Py_DECREF(pheader); - PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); Py_DECREF(ptransform); - PyObject_SetAttrString(pinst, "child_frame_id", stringToPython(transform->child_frame_id)); PyObject_SetAttrString(ptranslation, "x", PyFloat_FromDouble(transform->transform.translation.x)); @@ -124,7 +134,6 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* PyObject_SetAttrString(protation, "z", PyFloat_FromDouble(transform->transform.rotation.z)); PyObject_SetAttrString(protation, "w", PyFloat_FromDouble(transform->transform.rotation.w)); Py_DECREF(protation); - return pinst; } @@ -140,30 +149,57 @@ static builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t) return time_msg; } +static tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) +{ + int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; + std::chrono::nanoseconds ns(d); + return tf2::TimePoint(std::chrono::duration_cast(ns)); +} + static int rostime_converter(PyObject *obj, tf2::TimePoint *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. rclpy.Time or rclpy.Duration"); - return 0; - } else { - *rt = tf2::timeFromSec(PyFloat_AsDouble(tsr)); - Py_DECREF(tsr); + 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 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(std::chrono::duration_cast(ns)); + return 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. rclpy.Time or rclpy.Duration"); - return 0; - } else { - (*rt) = tf2::durationFromSec(PyFloat_AsDouble(tsr)); - Py_DECREF(tsr); + 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 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 1; } + + PyErr_SetString(PyExc_TypeError, "time must have sec and nanosec, or nanoseconds."); + return 0; } static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) @@ -283,13 +319,22 @@ static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame)); auto r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string); if (r == tf2::TF2Error::NO_ERROR) { - PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); + PyObject *rclpy_time = PyObject_GetAttrString(pModulerclpytime, "Time"); auto dbl = tf2::timeToSec(time); auto sec = std::floor(dbl); - PyObject *args = Py_BuildValue("ii", static_cast(sec), static_cast((dbl - sec) * 1000000000.0)); - PyObject *ob = PyObject_CallObject(rospy_time, args); + PyObject *args = PyTuple_New(0); + PyObject *kwargs = PyDict_New(); + PyObject *seconds = Py_BuildValue("i", static_cast(sec)); + PyObject *nanoseconds = Py_BuildValue("i", static_cast((dbl - sec) * 1000000000.0)); + PyDict_SetItemString(kwargs, "seconds", seconds); + PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds); + + PyObject *ob = PyObject_Call(rclpy_time, args, kwargs); + Py_DECREF(seconds); + Py_DECREF(nanoseconds); Py_DECREF(args); - Py_DECREF(rospy_time); + Py_DECREF(kwargs); + Py_DECREF(rclpy_time); return ob; } else { PyErr_SetString(tf2_exception, error_string.c_str()); @@ -308,8 +353,6 @@ static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *k return NULL; geometry_msgs::msg::TransformStamped transform; WRAP(transform = bc->lookupTransform(target_frame, source_frame, time)); - auto origin = transform.transform.translation; - auto 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)", @@ -335,8 +378,6 @@ static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObjec return NULL; geometry_msgs::msg::TransformStamped transform; WRAP(transform = bc->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame)); - auto origin = transform.transform.translation; - auto rotation = transform.transform.rotation; //TODO: Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); } @@ -389,11 +430,13 @@ static inline int checkTranslationType(PyObject* o) { PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Vector3"); int type_check = PyObject_TypeCheck(o, translation_type); + Py_XDECREF((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; } @@ -402,6 +445,7 @@ static inline int checkRotationType(PyObject* o) { PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Quaternion"); int type_check = PyObject_TypeCheck(o, rotation_type); + Py_XDECREF((PyObject*)rotation_type); int attr_check = PyObject_HasAttrString(o, "w") && PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && @@ -426,7 +470,6 @@ static PyObject *setTransform(PyObject *self, PyObject *args) 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"), &time) != 1) return NULL; transform.header.stamp = toMsg(time); @@ -438,7 +481,6 @@ static PyObject *setTransform(PyObject *self, PyObject *args) PyErr_SetString(PyExc_TypeError, "transform.translation must have members x, y, z"); return NULL; } - transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x")); transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y")); transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z")); @@ -448,12 +490,11 @@ static PyObject *setTransform(PyObject *self, PyObject *args) PyErr_SetString(PyExc_TypeError, "transform.rotation must have members w, x, y, z"); return NULL; } - transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x")); transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y")); transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z")); transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w")); - + bc->setTransform(transform, authority); Py_RETURN_NONE; } @@ -584,15 +625,35 @@ bool staticInit() { tf2_timeoutexception = stringToPython("tf2.TimeoutException"); #endif - pModulerospy = pythonImport("rclpy"); + pModulerclpy = pythonImport("rclpy"); + pModulerclpytime = pythonImport("rclpy.time"); + pModulebuiltininterfacesmsgs = pythonImport("builtin_interfaces.msg"); pModulegeometrymsgs = pythonImport("geometry_msgs.msg"); + 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; } + if(pModulebuiltininterfacesmsgs == NULL) + { + printf("Cannot load builtin_interfaces module"); + return false; + } + buffer_core_Type.tp_alloc = PyType_GenericAlloc; buffer_core_Type.tp_new = PyType_GenericNew; buffer_core_Type.tp_init = BufferCore_init; diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py new file mode 100644 index 000000000..d3de4afd6 --- /dev/null +++ b/tf2_py/test/test_buffer_core.py @@ -0,0 +1,262 @@ +# *********************************************************** +# * Software License Agreement (BSD License) +# * +# * Copyright (c) 2009, Willow Garage, 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 Willow Garage, Inc. 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. +# * +# * Author: Vinnam Kim vinnam.kim@gmail.com +# *********************************************************** + +import time +import unittest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from geometry_msgs.msg import TransformStamped +from tf2_py import BufferCore +from 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_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index 2b778c9aa..1b067b6ca 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -34,12 +34,13 @@ #* #* Author: Eitan Marder-Eppstein #*********************************************************** -from rclpy.action.client import ActionClient +from rclpy.action.client import ActionClient, ClientGoalHandle 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.action import LookupTransform from actionlib_msgs.msg import GoalStatus @@ -61,21 +62,11 @@ def __init__(self, node, ns, check_frequency = 10.0, timeout_padding = Duration( """ tf2_ros.BufferInterface.__init__(self) - self.client = ActionClient(node, LookupTransform, ns) 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 = 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=Duration()): """ @@ -89,11 +80,11 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()) :rtype: :class:`geometry_msgs.msg.TransformStamped` """ goal = LookupTransform.Goal() - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = time; - goal.timeout = timeout; - goal.advanced = False; + goal.target_frame = target_frame + goal.source_frame = source_frame + goal.source_time = time + goal.timeout = timeout + goal.advanced = False return self.__process_goal(goal) @@ -112,13 +103,13 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ :rtype: :class:`geometry_msgs.msg.TransformStamped` """ goal = LookupTransform.Goal() - 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.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 return self.__process_goal(goal) @@ -173,27 +164,45 @@ def __is_done(self, state): return False def __process_goal(self, goal): - self.client.send_goal(goal) - # TODO(vinnamkim): rclpy.Rate is not ready - # See https://github.com/ros2/rclpy/issues/186 - #r = rospy.Rate(self.check_frequency) - timed_out = False - clock = Clock() - start_time = clock.now() - while not rospy.is_shutdown() and not self.__is_done(self.client.get_state()) and not timed_out: - if clock.now() > start_time + goal.timeout + self.timeout_padding: - timed_out = True - sleep(self.check_frequency / 1000.0) + 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() + while not : + if clock.now() > start_time + goal.timeout + self.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() + + event.wait() + + if send_goal_future.exception() is not None: + raise send_goal_future.exception() #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: + if send_goal_future.result().status() != 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.") - - return self.__process_result(self.client.get_result()) + + return self.__process_result(send_goal_future.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 9c0451abf..a15e30216 100644 --- a/tf2_ros/src/tf2_ros/buffer_interface.py +++ b/tf2_ros/src/tf2_ros/buffer_interface.py @@ -33,6 +33,7 @@ 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 = Time(), new_type = None): + def transform(self, object_stamped, target_frame, timeout = Duration(), new_type = None): """ Transform an input into the target frame. 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..0d2f847cb --- /dev/null +++ b/tf2_ros/test/py_test/test_buffer.py @@ -0,0 +1,84 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest +import rclpy + +from rclpy.action import ActionServer +from tf2_ros.buffer import Buffer +from tf2_msgs.action import LookupTransform +from geometry_msgs.msg import TransformStamped +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor + +TIME_FUDGE = 0.3 + +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('TestBuffer', context=cls.context) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown(context=cls.context) + + def setUp(self): + self.feedback = None + + def feedback_callback(self, feedback): + self.feedback = feedback + + 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_can_transform_valid_transform(self): + buffer = Buffer() + clock = rclpy.clock.Clock() + rclpy_time = clock.now() + + from builtin_interfaces.msg import Time + + transform = TransformStamped() + transform.header.frame_id = "foo" + transform.header.stamp = rclpy_time.to_msg() + transform.child_frame_id = "bar" + 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 + print(dir(buffer)) + self.assertTrue(buffer.set_transform(transform, "unittest")) + self.assertTrue(buffer.can_transform("bar", "foo", rclpy_time)) + output = buffer.lookupTransform("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) + +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..2f02778db --- /dev/null +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -0,0 +1,148 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest +import rclpy + +from rclpy.action import ActionServer +from tf2_ros.buffer_client import BufferClient +from tf2_msgs.action import LookupTransform + +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor + +TIME_FUDGE = 0.3 + +class MockActionServer(): + def __init__(self, node): + 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') + + def goal_callback(self, request, response): + response.accepted = True + return response + + def cancel_callback(self, request, response): + response.goals_canceling.append(request.goal_info) + return response + + def result_callback(self, request, response): + 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.mock_action_server = MockActionServer(cls.node) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown(context=cls.context) + + def setUp(self): + self.feedback = None + + def feedback_callback(self, feedback): + self.feedback = feedback + + 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_wait_for_server_5sec(self): + buffer_client = BufferClient( + self.node, 'lookup_transform', check_frequency=10.0, timeout_padding=0.0) + + try: + start = time.monotonic() + self.assertFalse(buffer_client.wait_for_server(5.0)) + end = time.monotonic() + self.assertGreater(5.0, end - start - TIME_FUDGE) + self.assertLess(5.0, end - start + TIME_FUDGE) + finally: + self.node.destroy_client(buffer_client) + + def test_wait_for_service_nowait(self): + cli = self.node.create_client(GetParameters, 'get/parameters') + try: + start = time.monotonic() + self.assertFalse(cli.wait_for_service(timeout_sec=0)) + end = time.monotonic() + self.assertGreater(0, end - start - TIME_FUDGE) + self.assertLess(0, end - start + TIME_FUDGE) + finally: + self.node.destroy_client(cli) + + def test_wait_for_service_exists(self): + cli = self.node.create_client(GetParameters, 'test_wfs_exists') + srv = self.node.create_service( + GetParameters, 'test_wfs_exists', lambda request: None) + try: + start = time.monotonic() + self.assertTrue(cli.wait_for_service(timeout_sec=1.0)) + end = time.monotonic() + self.assertGreater(0, end - start - TIME_FUDGE) + self.assertLess(0, end - start + TIME_FUDGE) + finally: + self.node.destroy_client(cli) + self.node.destroy_service(srv) + + def test_concurrent_calls_to_service(self): + cli = self.node.create_client(GetParameters, 'get/parameters') + srv = self.node.create_service( + GetParameters, 'get/parameters', + lambda request, response: response) + try: + self.assertTrue(cli.wait_for_service(timeout_sec=20)) + future1 = cli.call_async(GetParameters.Request()) + future2 = cli.call_async(GetParameters.Request()) + executor = rclpy.executors.SingleThreadedExecutor( + context=self.context) + rclpy.spin_until_future_complete( + self.node, future1, executor=executor) + rclpy.spin_until_future_complete( + self.node, future2, executor=executor) + self.assertTrue(future1.result() is not None) + self.assertTrue(future2.result() is not None) + finally: + self.node.destroy_client(cli) + self.node.destroy_service(srv) + + +if __name__ == '__main__': + unittest.main() From c02ddb12aaa0e6ca5afc82b8f9dd60a8dd4b8f9a Mon Sep 17 00:00:00 2001 From: Brannon King Date: Wed, 21 Jun 2017 10:44:06 -0600 Subject: [PATCH 09/79] made tf2_py compile with ROS2 --- tf2_py/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 20ff32dc4..67853e6ce 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -14,7 +14,6 @@ find_package(ament_cmake REQUIRED) find_package(python_cmake_module REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(PythonLibs 3 REQUIRED) find_package(rclpy REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) From 38ca50f5ef5da6a62837a05ce574fad442551008 Mon Sep 17 00:00:00 2001 From: vinnam kim Date: Sat, 16 Mar 2019 20:25:41 +0900 Subject: [PATCH 10/79] Fix some codes - Add tf2_ros unit tests - Fix tf2_ros codes to work Signed-off-by: vinnam kim --- tf2_py/CMakeLists.txt | 1 + tf2_ros/CMakeLists.txt | 4 + tf2_ros/package.xml | 1 + tf2_ros/src/tf2_ros/__init__.py | 2 +- tf2_ros/src/tf2_ros/buffer_client.py | 35 +++-- tf2_ros/src/tf2_ros/buffer_interface.py | 12 +- tf2_ros/test/py_test/test_buffer.py | 94 ++++++------ tf2_ros/test/py_test/test_buffer_client.py | 168 +++++++++++++-------- 8 files changed, 189 insertions(+), 128 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 67853e6ce..20ff32dc4 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(python_cmake_module REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(PythonLibs 3 REQUIRED) find_package(rclpy REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index d04d54413..9f24a8e7d 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -179,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 e40ef6918..ed744b298 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -49,6 +49,7 @@ 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 5822defe0..018aa88ae 100644 --- a/tf2_ros/src/tf2_ros/__init__.py +++ b/tf2_ros/src/tf2_ros/__init__.py @@ -37,7 +37,7 @@ from tf2_py import * from .buffer_interface import * from .buffer import * -#from .buffer_client 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_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index 1b067b6ca..b40d7f094 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -34,7 +34,7 @@ #* #* Author: Eitan Marder-Eppstein #*********************************************************** -from rclpy.action.client import ActionClient, ClientGoalHandle +from rclpy.action.client import ActionClient from rclpy.duration import Duration from rclpy.clock import Clock from time import sleep @@ -82,8 +82,8 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()) goal = LookupTransform.Goal() goal.target_frame = target_frame goal.source_frame = source_frame - goal.source_time = time - goal.timeout = timeout + goal.source_time = time.to_msg() + goal.timeout = timeout.to_msg() goal.advanced = False return self.__process_goal(goal) @@ -105,9 +105,9 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_ goal = LookupTransform.Goal() goal.target_frame = target_frame goal.source_frame = source_frame - goal.source_time = source_time - goal.timeout = timeout - goal.target_time = target_time + 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 @@ -172,23 +172,28 @@ def __process_goal(self, goal): 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() - while not : - if clock.now() > start_time + goal.timeout + self.timeout_padding: + 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() @@ -199,10 +204,14 @@ def unblock_by_timeout(): 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 send_goal_future.result().status() != 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.") + goal_handle = send_goal_future.result() - return self.__process_result(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(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 a15e30216..12e4aa85d 100644 --- a/tf2_ros/src/tf2_ros/buffer_interface.py +++ b/tf2_ros/src/tf2_ros/buffer_interface.py @@ -46,7 +46,7 @@ def __init__(self): self.registration = tf2_ros.TransformRegistration() # transform, simple api - def transform(self, object_stamped, target_frame, timeout = Duration(), new_type = None): + def transform(self, object_stamped, target_frame, timeout=Duration(), new_type = None): """ Transform an input into the target frame. @@ -70,7 +70,7 @@ def transform(self, object_stamped, target_frame, timeout = Duration(), new_type return convert(res, new_type) # transform, advanced api - def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=Time(), 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). @@ -99,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=Time()): + def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Get the transform from the source frame to the target frame. @@ -114,7 +114,7 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=Time()): """ raise NotImplementedException() - def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=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. @@ -132,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=Time()): + 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. @@ -148,7 +148,7 @@ def can_transform(self, target_frame, source_frame, time, timeout=Time()): raise NotImplementedException() # can, advanced api - def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Time()): + 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). diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index 0d2f847cb..005e0a75d 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -1,65 +1,59 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# *********************************************************** +# * Software License Agreement (BSD License) +# * +# * Copyright (c) 2009, Willow Garage, 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 Willow Garage, Inc. 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. +# * +# * Author: Vinnam Kim vinnam.kim@gmail.com +# *********************************************************** -import time import unittest import rclpy -from rclpy.action import ActionServer from tf2_ros.buffer import Buffer -from tf2_msgs.action import LookupTransform -from geometry_msgs.msg import TransformStamped -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from geometry_msgs.msg import TransformStamped, PointStamped -TIME_FUDGE = 0.3 - -class TestBufferClient(unittest.TestCase): +class TestBuffer(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('TestBuffer', context=cls.context) + pass @classmethod def tearDownClass(cls): - cls.node.destroy_node() - rclpy.shutdown(context=cls.context) - - def setUp(self): - self.feedback = None - - def feedback_callback(self, feedback): - self.feedback = feedback - - 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() + pass def test_can_transform_valid_transform(self): buffer = Buffer() clock = rclpy.clock.Clock() rclpy_time = clock.now() - from builtin_interfaces.msg import Time - transform = TransformStamped() transform.header.frame_id = "foo" transform.header.stamp = rclpy_time.to_msg() @@ -71,10 +65,12 @@ def test_can_transform_valid_transform(self): transform.transform.rotation.x = 0.0 transform.transform.rotation.y = 0.0 transform.transform.rotation.z = 0.0 - print(dir(buffer)) - self.assertTrue(buffer.set_transform(transform, "unittest")) - self.assertTrue(buffer.can_transform("bar", "foo", rclpy_time)) - output = buffer.lookupTransform("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) diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py index 2f02778db..35be4a8e7 100644 --- a/tf2_ros/test/py_test/test_buffer_client.py +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -15,17 +15,37 @@ import time import unittest import rclpy +import threading from rclpy.action import ActionServer 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 MultiThreadedExecutor, 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 -TIME_FUDGE = 0.3 class MockActionServer(): - def __init__(self, node): + def __init__(self, node, buffer_core): self.goal_srv = node.create_service( LookupTransform.Impl.SendGoalService, '/lookup_transform/_action/send_goal', self.goal_callback) @@ -37,9 +57,50 @@ def __init__(self, node): 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): @@ -47,6 +108,12 @@ def cancel_callback(self, request, response): return response def result_callback(self, request, response): + bytes_goal_id = bytes(request.goal_id.uuid) + #response.status = TF2Error + 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): @@ -54,6 +121,7 @@ def publish_feedback(self, goal_id): feedback_message.goal_id = goal_id self.feedback_pub.publish(feedback_message) + class TestBufferClient(unittest.TestCase): @classmethod def setUpClass(cls): @@ -61,7 +129,13 @@ def setUpClass(cls): rclpy.init(context=cls.context) cls.executor = SingleThreadedExecutor(context=cls.context) cls.node = rclpy.create_node('TestBufferClient', context=cls.context) - cls.mock_action_server = MockActionServer(cls.node) + 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): @@ -69,11 +143,27 @@ def tearDownClass(cls): rclpy.shutdown(context=cls.context) def setUp(self): - self.feedback = None + 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() + #print('teardown : ', self.spin_thread.is_alive(), self.context.ok()) + 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: @@ -84,65 +174,25 @@ def execute_goal_callback(self, goal_handle): goal_handle.set_succeeded() return LookupTransform.Result() - def test_wait_for_server_5sec(self): + def test_lookup_transform_true(self): buffer_client = BufferClient( self.node, 'lookup_transform', check_frequency=10.0, timeout_padding=0.0) - - try: - start = time.monotonic() - self.assertFalse(buffer_client.wait_for_server(5.0)) - end = time.monotonic() - self.assertGreater(5.0, end - start - TIME_FUDGE) - self.assertLess(5.0, end - start + TIME_FUDGE) - finally: - self.node.destroy_client(buffer_client) - def test_wait_for_service_nowait(self): - cli = self.node.create_client(GetParameters, 'get/parameters') - try: - start = time.monotonic() - self.assertFalse(cli.wait_for_service(timeout_sec=0)) - end = time.monotonic() - self.assertGreater(0, end - start - TIME_FUDGE) - self.assertLess(0, end - start + TIME_FUDGE) - finally: - self.node.destroy_client(cli) + result = buffer_client.lookup_transform( + 'foo', 'bar', rclpy.time.Time(), rclpy.duration.Duration(seconds=5.0)) - def test_wait_for_service_exists(self): - cli = self.node.create_client(GetParameters, 'test_wfs_exists') - srv = self.node.create_service( - GetParameters, 'test_wfs_exists', lambda request: None) - try: - start = time.monotonic() - self.assertTrue(cli.wait_for_service(timeout_sec=1.0)) - end = time.monotonic() - self.assertGreater(0, end - start - TIME_FUDGE) - self.assertLess(0, end - start + TIME_FUDGE) - finally: - self.node.destroy_client(cli) - self.node.destroy_service(srv) - - def test_concurrent_calls_to_service(self): - cli = self.node.create_client(GetParameters, 'get/parameters') - srv = self.node.create_service( - GetParameters, 'get/parameters', - lambda request, response: response) - try: - self.assertTrue(cli.wait_for_service(timeout_sec=20)) - future1 = cli.call_async(GetParameters.Request()) - future2 = cli.call_async(GetParameters.Request()) - executor = rclpy.executors.SingleThreadedExecutor( - context=self.context) - rclpy.spin_until_future_complete( - self.node, future1, executor=executor) - rclpy.spin_until_future_complete( - self.node, future2, executor=executor) - self.assertTrue(future1.result() is not None) - self.assertTrue(future2.result() is not None) - finally: - self.node.destroy_client(cli) - self.node.destroy_service(srv) + 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() From dc1dac68ddb11a896ef6059210aafb0c470f13b6 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Fri, 9 Aug 2019 16:50:54 +0900 Subject: [PATCH 11/79] Rebase to the latest ros2 - Modify some build script Signed-off-by: vinnamkim --- tf2_py/CMakeLists.txt | 1 - tf2_ros/package.xml | 19 ++----------------- 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 20ff32dc4..707f66cb0 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -28,7 +28,6 @@ set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFF ament_target_dependencies(${PROJECT_NAME} "builtin_interfaces" "geometry_msgs" - "python" "rclpy" "tf2" "tf2_msgs" diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index ed744b298..00b1e3e03 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -14,37 +14,22 @@ ament_cmake geometry_msgs - message_filters - rclcpp - rclpy std_msgs - tf2 - tf2_msgs - tf2_py geometry_msgs - message_filters - rclcpp - rclpy std_msgs - tf2 - tf2_msgs - tf2_py message_filters rclcpp rclcpp_action + rclpy tf2 tf2_msgs + tf2_py - - - - From 27129fd8ed7d1c48c4a7ab437c9ffd72234df1ec Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 14:30:49 +0900 Subject: [PATCH 12/79] Revert removing geometry2 and geometry_experimental Signed-off-by: vinnamkim --- geometry2/AMENT_IGNORE | 0 geometry2/CHANGELOG.rst | 11 +++ geometry2/CMakeLists.txt | 4 + geometry2/package.xml | 29 ++++++ geometry_experimental/AMENT_IGNORE | 0 geometry_experimental/CHANGELOG.rst | 126 +++++++++++++++++++++++++++ geometry_experimental/CMakeLists.txt | 4 + geometry_experimental/package.xml | 23 +++++ 8 files changed, 197 insertions(+) create mode 100644 geometry2/AMENT_IGNORE create mode 100644 geometry2/CHANGELOG.rst create mode 100644 geometry2/CMakeLists.txt create mode 100644 geometry2/package.xml create mode 100644 geometry_experimental/AMENT_IGNORE create mode 100644 geometry_experimental/CHANGELOG.rst create mode 100644 geometry_experimental/CMakeLists.txt create mode 100644 geometry_experimental/package.xml diff --git a/geometry2/AMENT_IGNORE b/geometry2/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/geometry2/CHANGELOG.rst b/geometry2/CHANGELOG.rst new file mode 100644 index 000000000..4c068af87 --- /dev/null +++ b/geometry2/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package geometry2 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking. +* Contributors: Tully Foote diff --git a/geometry2/CMakeLists.txt b/geometry2/CMakeLists.txt new file mode 100644 index 000000000..83f1b03cd --- /dev/null +++ b/geometry2/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(geometry2) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/geometry2/package.xml b/geometry2/package.xml new file mode 100644 index 000000000..3f4697801 --- /dev/null +++ b/geometry2/package.xml @@ -0,0 +1,29 @@ + + geometry2 + 0.9.1 + + A metapackage to bring in the default packages second generation Transform Library in ros, tf2. + + Tully Foote + Tully Foote + BSD + + http://www.ros.org/wiki/geometry2 + + catkin + + tf2 + tf2_bullet + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_msgs + tf2_py + tf2_ros + tf2_sensor_msgs + tf2_tools + + + + + diff --git a/geometry_experimental/AMENT_IGNORE b/geometry_experimental/AMENT_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/geometry_experimental/CHANGELOG.rst b/geometry_experimental/CHANGELOG.rst new file mode 100644 index 000000000..2ca055e3a --- /dev/null +++ b/geometry_experimental/CHANGELOG.rst @@ -0,0 +1,126 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package geometry_experimental +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking. +* Contributors: Tully Foote + +0.5.13 (2016-03-04) +------------------- +* Remove LGPL from license tags + LGPL was erroneously included in 2a38724. As there are no files with it + in the package. +* add missing dependencies in the meta-package geometry_experimental + This partly fixes the doc jobs in `#120 `_ +* Contributors: Jochen Sprickerhof, Vincent Rabaud + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ + +0.5.7 (2014-12-23) +------------------ + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ + +0.4.3 (2013-07-05) +------------------ +* removing test_tf2 from metapackage and update description + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ + +0.4.0 (2013-06-27) +------------------ +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ +* updating for new metapackage specification + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 +* fixing adding new packages to metapackage + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 + +0.3.1 (2013-02-14) +------------------ +* 0.3.0 -> 0.3.1 + +0.3.0 (2013-02-13) +------------------ +* switching to version 0.3.0 +* renaming geometry_experimental meta package in order to comply with catkin naming convention diff --git a/geometry_experimental/CMakeLists.txt b/geometry_experimental/CMakeLists.txt new file mode 100644 index 000000000..54074da0f --- /dev/null +++ b/geometry_experimental/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.5) +project(geometry_experimental) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/geometry_experimental/package.xml b/geometry_experimental/package.xml new file mode 100644 index 000000000..72c900852 --- /dev/null +++ b/geometry_experimental/package.xml @@ -0,0 +1,23 @@ + + geometry_experimental + 0.9.1 + + The second generation Transform Library in ros. This metapackage is deprecated, but is kept for backwards compatability. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/geometry_experimental + + catkin + + geometry2 + + + + + + From 821a7a76e20458983c7ebc46c22334e0e00df369 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 17:01:59 +0900 Subject: [PATCH 13/79] Fix build script of tf2_py with respect to the review Signed-off-by: vinnamkim --- tf2_py/CMakeLists.txt | 57 ++++++++++++++++++++++----------- tf2_py/package.xml | 10 +++--- tf2_py/src/tf2_py.cpp | 6 ++-- tf2_py/test/__init__.py | 17 ++++++++++ tf2_py/test/test_buffer_core.py | 4 +-- tf2_py/tf2_py/__init__.py | 2 +- 6 files changed, 65 insertions(+), 31 deletions(-) create mode 100644 tf2_py/test/__init__.py diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 707f66cb0..9896c67ce 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -9,37 +9,51 @@ endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() -set(CMAKE_BUILD_TYPE Debug) + find_package(ament_cmake REQUIRED) -find_package(python_cmake_module REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(PythonLibs 3 REQUIRED) -find_package(rclpy REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(tf2 REQUIRED) -find_package(tf2_msgs REQUIRED) -include_directories(${PYTHON_INCLUDE_PATH}) +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() + +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_tf2_py/__init__.py" "") + +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_${PROJECT_NAME}" + RUNTIME_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/test_${PROJECT_NAME}" + OUTPUT_NAME "_${_targetname}${PythonExtra_EXTENSION_SUFFIX}" + SUFFIX "${PythonExtra_EXTENSION_EXTENSION}") +endfunction() add_library(${PROJECT_NAME} SHARED src/tf2_py.cpp) -set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") +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} - "builtin_interfaces" - "geometry_msgs" - "rclpy" + "PythonExtra" "tf2" - "tf2_msgs" ) -ament_export_include_directories("include") -ament_export_libraries(${PROJECT_NAME}) -ament_python_install_package(${PROJECT_NAME}) - install(TARGETS ${PROJECT_NAME} - DESTINATION ${PYTHON_INSTALL_DIR} + DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -48,7 +62,12 @@ if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) - ament_add_pytest_test(tf2_py_test test) + ament_add_pytest_test(tf2_py_test test + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + ) endif() +set(PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") + ament_package() diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 7453b2a26..225d40af3 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -23,17 +23,15 @@ ament_cmake python_cmake_module - builtin_interfaces - geometry_msgs - rclpy - tf2 - tf2_msgs + tf2 rclpy - tf2 + builtin_interfaces ament_lint_auto ament_cmake_pytest + geometry_msgs + rclpy ament_cmake diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index c57875313..7337f7ba2 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -681,19 +681,19 @@ extern "C" void init_tf2() { if (!staticInit()) return; - moduleInit(Py_InitModule("_tf2", module_methods)); + moduleInit(Py_InitModule("_tf2_py", module_methods)); } #else struct PyModuleDef tf_module = { PyModuleDef_HEAD_INIT, // base - "_tf2", // name + "_tf2_py", // name NULL, // docstring -1, // state size (but we're using globals) module_methods // methods }; -PyMODINIT_FUNC PyInit__tf2() +PyMODINIT_FUNC PyInit__tf2_py() { if (!staticInit()) return NULL; diff --git a/tf2_py/test/__init__.py b/tf2_py/test/__init__.py new file mode 100644 index 000000000..9a0eb79bd --- /dev/null +++ b/tf2_py/test/__init__.py @@ -0,0 +1,17 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +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 index d3de4afd6..44bf3d8d7 100644 --- a/tf2_py/test/test_buffer_core.py +++ b/tf2_py/test/test_buffer_core.py @@ -39,8 +39,8 @@ import rclpy from rclpy.executors import SingleThreadedExecutor from geometry_msgs.msg import TransformStamped -from tf2_py import BufferCore -from tf2_py import LookupException +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() diff --git a/tf2_py/tf2_py/__init__.py b/tf2_py/tf2_py/__init__.py index ccade4e57..5202a0755 100644 --- a/tf2_py/tf2_py/__init__.py +++ b/tf2_py/tf2_py/__init__.py @@ -34,4 +34,4 @@ #* #* Author: Eitan Marder-Eppstein #*********************************************************** -from _tf2 import * +from tf2_py._tf2_py import * From f034a4e9fbf9f0f5ac7ed060286d95c65b172e86 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 17:21:51 +0900 Subject: [PATCH 14/79] Remove unnecessary whitespace changes Signed-off-by: vinnamkim --- tf2_py/src/tf2_py.cpp | 15 ++++++++++----- tf2_ros/src/tf2_ros/buffer.py | 2 +- tf2_ros/src/tf2_ros/buffer_client.py | 3 +-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 7337f7ba2..053147d3a 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -2,6 +2,7 @@ #include #include + #include "python_compat.h" // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions) @@ -65,8 +66,8 @@ static PyTypeObject buffer_core_Type = { # else PyVarObject_HEAD_INIT(NULL, 0) #endif - "_tf2.BufferCore", /* name */ - sizeof(buffer_core_t), /* basicsize */ + "_tf2.BufferCore", /*name*/ + sizeof(buffer_core_t), /*basicsize*/ }; static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* transform) @@ -115,9 +116,10 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* PyObject* pheader = PyObject_GetAttrString(pinst, "header"); PyObject_SetAttrString(pheader, "stamp", time_obj); Py_DECREF(time_obj); - + PyObject_SetAttrString(pheader, "frame_id", stringToPython(transform->header.frame_id)); Py_DECREF(pheader); + PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); @@ -134,6 +136,7 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* PyObject_SetAttrString(protation, "z", PyFloat_FromDouble(transform->transform.rotation.z)); PyObject_SetAttrString(protation, "w", PyFloat_FromDouble(transform->transform.rotation.w)); Py_DECREF(protation); + return pinst; } @@ -472,6 +475,7 @@ static PyObject *setTransform(PyObject *self, PyObject *args) transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; + transform.header.stamp = toMsg(time); PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); @@ -481,6 +485,7 @@ static PyObject *setTransform(PyObject *self, PyObject *args) PyErr_SetString(PyExc_TypeError, "transform.translation must have members x, y, z"); return NULL; } + transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x")); transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y")); transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z")); @@ -490,11 +495,12 @@ static PyObject *setTransform(PyObject *self, PyObject *args) PyErr_SetString(PyExc_TypeError, "transform.rotation must have members w, x, y, z"); return NULL; } + transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x")); transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y")); transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z")); transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w")); - + bc->setTransform(transform, authority); Py_RETURN_NONE; } @@ -573,7 +579,6 @@ static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; static const char *keywords[] = { "time", NULL }; - tf2::TimePoint time; if (!PyArg_ParseTupleAndKeywords(args, kw, "|O&", (char**)keywords, rostime_converter, &time)) return NULL; diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index ee03756d6..67b8b14c5 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -61,7 +61,7 @@ def __init__(self, cache_time = None, debug = True): else: 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: diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index b40d7f094..73c8e73c3 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -54,14 +54,13 @@ def __init__(self, node, ns, check_frequency = 10.0, timeout_padding = Duration( .. 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.node = node self.action_client = ActionClient(node, LookupTransform, action_name=ns) self.check_frequency = check_frequency From 9d421a2d056c533be3307511faa9deec3311a4db Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 17:31:56 +0900 Subject: [PATCH 15/79] Trim trailing whitespace Signed-off-by: vinnamkim --- tf2_py/CMakeLists.txt | 2 +- tf2_py/test/test_buffer_core.py | 2 +- tf2_ros/test/py_test/test_buffer.py | 6 +++--- tf2_ros/test/py_test/test_buffer_client.py | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 9896c67ce..8d1c77535 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -46,7 +46,7 @@ if(WIN32) set_properties(${PROJECT_NAME} "_RELWITHDEBINFO") endif() -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies(${PROJECT_NAME} "PythonExtra" "tf2" ) diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py index 44bf3d8d7..fd93e16a5 100644 --- a/tf2_py/test/test_buffer_core.py +++ b/tf2_py/test/test_buffer_core.py @@ -255,7 +255,7 @@ def test_lookup_transform_core_fail(self): source_frame='baz', time=rclpy.time.Time() ) - + self.assertEqual(LookupException, type(ex.exception)) if __name__ == '__main__': diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index 005e0a75d..48b281036 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -53,7 +53,7 @@ def test_can_transform_valid_transform(self): buffer = Buffer() clock = rclpy.clock.Clock() rclpy_time = clock.now() - + transform = TransformStamped() transform.header.frame_id = "foo" transform.header.stamp = rclpy_time.to_msg() @@ -65,11 +65,11 @@ def test_can_transform_valid_transform(self): transform.transform.rotation.x = 0.0 transform.transform.rotation.y = 0.0 transform.transform.rotation.z = 0.0 - + 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) diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py index 35be4a8e7..8b1cd28c5 100644 --- a/tf2_ros/test/py_test/test_buffer_client.py +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -183,7 +183,7 @@ def test_lookup_transform_true(self): 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) From 60bde6c20e48ca0b15b79514b6eca5b38e6be3a5 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 17:36:59 +0900 Subject: [PATCH 16/79] Fix Py_DECREF order Signed-off-by: vinnamkim --- tf2_py/src/tf2_py.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 053147d3a..676bebd28 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -107,11 +107,11 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* PyObject *time_obj = PyObject_Call(builtin_interfaces_time, args, kwargs); - Py_DECREF(builtin_interfaces_time); - Py_DECREF(args); - Py_DECREF(kwargs); - Py_DECREF(sec); Py_DECREF(nanosec); + Py_DECREF(sec); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(builtin_interfaces_time); PyObject* pheader = PyObject_GetAttrString(pinst, "header"); PyObject_SetAttrString(pheader, "stamp", time_obj); @@ -333,10 +333,10 @@ static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds); PyObject *ob = PyObject_Call(rclpy_time, args, kwargs); - Py_DECREF(seconds); Py_DECREF(nanoseconds); - Py_DECREF(args); + Py_DECREF(seconds); Py_DECREF(kwargs); + Py_DECREF(args); Py_DECREF(rclpy_time); return ob; } else { From cb9542bc10be0db661267baaf9152a43ce21da09 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 17:41:12 +0900 Subject: [PATCH 17/79] Remove the unused variable Signed-off-by: vinnamkim --- tf2_py/src/tf2_py.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 676bebd28..f45aac0eb 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -519,7 +519,7 @@ static PyObject *setTransformStatic(PyObject *self, PyObject *args) 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")); - tf2::Duration tp; + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &time) != 1) return NULL; transform.header.stamp = toMsg(time); From e44dbb77254fb653410c537276fc51c4ba789a15 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 18:02:16 +0900 Subject: [PATCH 18/79] Prevent floating number conversion from getLatestCommonTime Signed-off-by: vinnamkim --- tf2_py/src/tf2_py.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index f45aac0eb..dae0a8fa8 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -313,22 +313,21 @@ 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; - tf2::TimePoint 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)); - auto r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string); + const tf2::TF2Error r = bc->_getLatestCommonTime(target_id, source_id, tf2_time, &error_string); if (r == tf2::TF2Error::NO_ERROR) { PyObject *rclpy_time = PyObject_GetAttrString(pModulerclpytime, "Time"); - auto dbl = tf2::timeToSec(time); - auto sec = std::floor(dbl); + const builtin_interfaces::msg::Time time_msg = toMsg(tf2_time); PyObject *args = PyTuple_New(0); PyObject *kwargs = PyDict_New(); - PyObject *seconds = Py_BuildValue("i", static_cast(sec)); - PyObject *nanoseconds = Py_BuildValue("i", static_cast((dbl - sec) * 1000000000.0)); + PyObject *seconds = Py_BuildValue("i", time_msg.sec); + PyObject *nanoseconds = Py_BuildValue("i", time_msg.nanosec); PyDict_SetItemString(kwargs, "seconds", seconds); PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds); From 170e4e327d2563f3b323060cb00c6de0d058c7c5 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 18:08:48 +0900 Subject: [PATCH 19/79] Remove unused imports Signed-off-by: vinnamkim --- tf2_py/test/test_buffer_core.py | 1 - .../tf2_ros/static_transform_broadcaster.py | 1 - tf2_ros/test/py_test/test_buffer.py | 48 +++++-------------- tf2_ros/test/py_test/test_buffer_client.py | 5 +- 4 files changed, 15 insertions(+), 40 deletions(-) diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py index fd93e16a5..45ac701fa 100644 --- a/tf2_py/test/test_buffer_core.py +++ b/tf2_py/test/test_buffer_core.py @@ -37,7 +37,6 @@ import time import unittest import rclpy -from rclpy.executors import SingleThreadedExecutor from geometry_msgs.msg import TransformStamped from test_tf2_py._tf2_py import BufferCore from test_tf2_py._tf2_py import LookupException diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index 0f2d487f6..6fd8b162c 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -46,7 +46,6 @@ def __init__(self, name=None): depth=1, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) self.pub_tf = self.create_publisher(TFMessage, "/tf_static", qos_profile=latching_qos) - #self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True) def sendTransform(self, transform): if not isinstance(transform, list): diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index 48b281036..14d68d084 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -1,38 +1,16 @@ -# *********************************************************** -# * Software License Agreement (BSD License) -# * -# * Copyright (c) 2009, Willow Garage, 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 Willow Garage, Inc. 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. -# * -# * Author: Vinnam Kim vinnam.kim@gmail.com -# *********************************************************** +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. import unittest import rclpy diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py index 8b1cd28c5..d50fc01c0 100644 --- a/tf2_ros/test/py_test/test_buffer_client.py +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -17,13 +17,12 @@ import rclpy import threading -from rclpy.action import ActionServer 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 MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.executors import SingleThreadedExecutor from tf2_msgs.msg import TF2Error From 10232f5b55b36da07b1e586edeba5f17d9f021bb Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 12 Aug 2019 18:28:37 +0900 Subject: [PATCH 20/79] Fix colcon test error on pytest - Declare dependency on actionlib_msgs Signed-off-by: vinnamkim --- tf2_ros/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index 00b1e3e03..74c9ed2c0 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -13,9 +13,11 @@ ament_cmake + actionlib_msgs geometry_msgs std_msgs + actionlib_msgs geometry_msgs std_msgs From 7cc4d8388c703f6cdaccbf8d5dcc036a3532bdf0 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Tue, 13 Aug 2019 00:02:34 +0900 Subject: [PATCH 21/79] Add geometry_msgs dependency to tf2_py Signed-off-by: vinnamkim --- tf2_py/CMakeLists.txt | 4 +++- tf2_py/package.xml | 5 ++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 8d1c77535..9abebacc3 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -12,6 +12,7 @@ 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) @@ -47,8 +48,9 @@ if(WIN32) endif() ament_target_dependencies(${PROJECT_NAME} - "PythonExtra" + "geometry_msgs" "tf2" + "PythonExtra" ) install(TARGETS diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 225d40af3..9923f6815 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -23,10 +23,13 @@ ament_cmake python_cmake_module + geometry_msgs + tf2 - rclpy builtin_interfaces + geometry_msgs + rclpy ament_lint_auto ament_cmake_pytest From 94a82ca56eca5f825ff5608af03424e131490fd5 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Aug 2019 13:15:59 +0000 Subject: [PATCH 22/79] Cleanup. Signed-off-by: Chris Lalancette --- tf2_py/package.xml | 4 ++-- tf2_py/src/tf2_py.cpp | 15 ++++++++------- tf2_ros/src/tf2_ros/buffer.py | 10 +++++----- tf2_ros/src/tf2_ros/buffer_client.py | 14 +++++++------- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 9923f6815..e917e1f38 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -5,7 +5,7 @@ 0.9.1 The tf2_py package - + Tully Foote @@ -35,7 +35,7 @@ ament_cmake_pytest geometry_msgs rclpy - + ament_cmake diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index dae0a8fa8..52b9fa1cb 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -50,7 +50,7 @@ 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 { @@ -124,6 +124,7 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); Py_DECREF(ptransform); + PyObject_SetAttrString(pinst, "child_frame_id", stringToPython(transform->child_frame_id)); PyObject_SetAttrString(ptranslation, "x", PyFloat_FromDouble(transform->transform.translation.x)); @@ -178,7 +179,7 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) *rt = tf2::TimePoint(std::chrono::duration_cast(ns)); return 1; } - + PyErr_SetString(PyExc_TypeError, "time must have sec and nanosec, or nanoseconds."); return 0; } @@ -188,7 +189,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) 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)) + + *rt = std::chrono::seconds(PyLong_AsLong(sec)) + std::chrono::nanoseconds(PyLong_AsUnsignedLong(nanosec)); return 1; } @@ -200,7 +201,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) *rt = std::chrono::duration_cast(ns); return 1; } - + PyErr_SetString(PyExc_TypeError, "time must have sec and nanosec, or nanoseconds."); return 0; } @@ -330,7 +331,7 @@ static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) PyObject *nanoseconds = Py_BuildValue("i", time_msg.nanosec); PyDict_SetItemString(kwargs, "seconds", seconds); PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds); - + PyObject *ob = PyObject_Call(rclpy_time, args, kwargs); Py_DECREF(nanoseconds); Py_DECREF(seconds); @@ -476,7 +477,7 @@ static PyObject *setTransform(PyObject *self, PyObject *args) return NULL; transform.header.stamp = toMsg(time); - + PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); @@ -640,7 +641,7 @@ bool staticInit() { return false; } - if(pModulerclpytime == NULL) + if(pModulerclpytime == NULL) { printf("Cannot load rclpy.time.Time module"); return false; diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 67b8b14c5..808a44a3d 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -67,7 +67,7 @@ def __init__(self, cache_time = None, debug = True): # try: # m = rosgraph.masterapi.Master(rospy.get_name()) # m.lookupService('~tf2_frames') - # except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure): + # except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure): # self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames) def __get_frames(self, req): @@ -120,10 +120,10 @@ def can_transform(self, target_frame, source_frame, time, timeout=Duration(), re clock = rclpy.timer.Clock() if timeout != Duration(): start_time = clock.now() - # TODO(vinnamkim): rclpy.Rate is not ready + # 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 + while (clock.now() < start_time + timeout and not self.can_transform_core(target_frame, source_frame, time)[0] and (clock.now() + Duration(seconds=3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them # r.sleep() @@ -155,10 +155,10 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim clock = rclpy.timer.Clock() if timeout != Duration(): start_time = clock.now() - # TODO(vinnamkim): rclpy.Rate is not ready + # 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 + 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 (clock.now() + Duration(seconds=3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them # r.sleep() diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index 73c8e73c3..1980bb77f 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -65,7 +65,7 @@ def __init__(self, node, ns, check_frequency = 10.0, timeout_padding = Duration( self.action_client = ActionClient(node, LookupTransform, action_name=ns) self.check_frequency = check_frequency self.timeout_padding = timeout_padding - + # lookup, simple api def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()): """ @@ -165,16 +165,16 @@ def __is_done(self, state): def __process_goal(self, goal): 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() @@ -184,13 +184,13 @@ def unblock_by_timeout(): 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 + # 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() @@ -204,7 +204,7 @@ def unblock_by_timeout(): 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") 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.") From 8f68f220abd1d36caeaeee31d8acfdbfe86740f9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 16 Aug 2019 16:14:44 +0000 Subject: [PATCH 23/79] Fixes for Windows. Signed-off-by: Chris Lalancette --- tf2_py/src/python_compat.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tf2_py/src/python_compat.h b/tf2_py/src/python_compat.h index 2f431cce3..cdd4c3f10 100644 --- a/tf2_py/src/python_compat.h +++ b/tf2_py/src/python_compat.h @@ -26,7 +26,7 @@ inline PyObject *stringToPython(const char *input) 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 @@ -45,9 +45,9 @@ inline PyObject *pythonImport(const std::string & name) 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 From afe44118f49b382072bbe60e4ab3b68044fa93a9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 16 Aug 2019 16:31:21 +0000 Subject: [PATCH 24/79] Fix some initializer warnings. Signed-off-by: Chris Lalancette --- tf2_py/src/tf2_py.cpp | 141 ++++++++++++++++++++++++------------------ 1 file changed, 82 insertions(+), 59 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 52b9fa1cb..907419eb6 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -58,18 +58,6 @@ 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::msg::TransformStamped* transform) { PyObject *pclass, *pargs, *pinst = NULL; @@ -208,6 +196,8 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) { + (void)kw; + tf2::Duration cache_time(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME); if (!PyArg_ParseTuple(args, "|O&", rosduration_converter, &cache_time)) @@ -218,24 +208,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()); } @@ -553,6 +535,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; @@ -569,6 +552,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); @@ -588,29 +572,79 @@ static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw) 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_VARARGS | METH_KEYWORDS}, - {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS}, - {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS}, - {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS}, - {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS}, - {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS}, - {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | METH_KEYWORDS}, - {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS}, - {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS}, - {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | 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() { @@ -659,11 +693,6 @@ bool staticInit() { 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 (PyType_Ready(&buffer_core_Type) != 0) return false; return true; @@ -681,21 +710,16 @@ PyObject *moduleInit(PyObject *m) { return m; } -#if PY_MAJOR_VERSION < 3 -extern "C" void init_tf2() -{ - if (!staticInit()) - return; - moduleInit(Py_InitModule("_tf2_py", module_methods)); -} - -#else struct PyModuleDef tf_module = { - PyModuleDef_HEAD_INIT, // base - "_tf2_py", // 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_py() @@ -704,4 +728,3 @@ PyMODINIT_FUNC PyInit__tf2_py() return NULL; return moduleInit(PyModule_Create(&tf_module)); } -#endif From a0b23f0d1ab4d5337817fd816b9dabbc0a9237a3 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 19 Aug 2019 17:21:24 +0900 Subject: [PATCH 25/79] Prevent transform_listener.py from finding an attribute not existed in tf_message Signed-off-by: vinnamkim --- tf2_ros/src/tf2_ros/transform_listener.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 273535987..8185ff3aa 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -62,11 +62,11 @@ def unregister(self): self.destroy_subscription(self.tf_static_sub) def callback(self, data): - who = data._connection_header.get('callerid', "default_authority") + who = str("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 = str("default_authority") for transform in data.transforms: self.buffer.set_transform_static(transform, who) From 907f915135044f277af1854b76681baa50d3b426 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 19 Aug 2019 18:14:53 +0900 Subject: [PATCH 26/79] Add unit test for broadcaster and listner Signed-off-by: vinnamkim --- .../py_test/test_listener_and_broadcaster.py | 113 ++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 tf2_ros/test/py_test/test_listener_and_broadcaster.py 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..62a9478aa --- /dev/null +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -0,0 +1,113 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +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.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.broadcaster = TransformBroadcaster() + cls.listener = TransformListener(buffer=cls.buffer) + + cls.executor = rclpy.executors.SingleThreadedExecutor() + cls.executor.add_node(cls.broadcaster) + cls.executor.add_node(cls.listener) + pass + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + pass + + def setUp(self): + self.buffer.clear() + + 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 test_broadcast_and_listen(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) + + pass + + def test_extrapolation_exception(self): + self.broadcast_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=1, nanoseconds=0).to_msg()) + + self.broadcast_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=0.5, 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.25, 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=1.25, nanoseconds=0).to_msg()) + + self.assertTrue( + 'Lookup would require extrapolation into the future' in str(e.exception)) + +if __name__ == '__main__': + unittest.main() From 48f37f8b4e1ca8ac84f884887cc0f82b24df140d Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Mon, 19 Aug 2019 20:31:10 +0900 Subject: [PATCH 27/79] Add static broadcaster unit test Signed-off-by: vinnamkim --- .../py_test/test_listener_and_broadcaster.py | 40 ++++++++++++++----- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/tf2_ros/test/py_test/test_listener_and_broadcaster.py b/tf2_ros/test/py_test/test_listener_and_broadcaster.py index 62a9478aa..1d8537d7b 100644 --- a/tf2_ros/test/py_test/test_listener_and_broadcaster.py +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -18,6 +18,7 @@ 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 @@ -46,10 +47,10 @@ def setUpClass(cls): cls.buffer = Buffer() cls.broadcaster = TransformBroadcaster() + cls.static_broadcaster = StaticTransformBroadcaster() cls.listener = TransformListener(buffer=cls.buffer) cls.executor = rclpy.executors.SingleThreadedExecutor() - cls.executor.add_node(cls.broadcaster) cls.executor.add_node(cls.listener) pass @@ -59,7 +60,8 @@ def tearDownClass(cls): pass def setUp(self): - self.buffer.clear() + self.buffer = Buffer() + self.listener.buffer = self.buffer def broadcast_transform(self, target_frame, source_frame, time_stamp): broadcast_transform = build_transform( @@ -71,7 +73,17 @@ def broadcast_transform(self, target_frame, source_frame, time_stamp): return broadcast_transform - def test_broadcast_and_listen(self): + 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( @@ -82,21 +94,19 @@ def test_broadcast_and_listen(self): self.assertTrue(broadcasted_transform, listened_transform) - pass - def test_extrapolation_exception(self): self.broadcast_transform( target_frame='foo', source_frame='bar', - time_stamp=rclpy.time.Time(seconds=1, nanoseconds=0).to_msg()) + 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.5, nanoseconds=0).to_msg()) + 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.25, nanoseconds=0).to_msg()) + time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg()) self.assertTrue( 'Lookup would require extrapolation into the past' in str(e.exception)) @@ -104,10 +114,22 @@ def test_extrapolation_exception(self): with self.assertRaises(ExtrapolationException) as e: self.buffer.lookup_transform( target_frame='foo', source_frame='bar', - time=rclpy.time.Time(seconds=1.25, nanoseconds=0).to_msg()) + 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() From c82596fd6604cc8689cf46fdade6f4131a7be85d Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Wed, 21 Aug 2019 14:32:12 +0900 Subject: [PATCH 28/79] Make tf2_ros Python consistent with transform_listener, transform_broadcaster and static_transform_broadcaster of tf2_ros C++ Signed-off-by: vinnamkim --- .../tf2_ros/static_transform_broadcaster.py | 21 ++++++---- tf2_ros/src/tf2_ros/transform_broadcaster.py | 16 ++++--- tf2_ros/src/tf2_ros/transform_listener.py | 42 ++++++++++++++----- .../py_test/test_listener_and_broadcaster.py | 10 +++-- 4 files changed, 60 insertions(+), 29 deletions(-) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index 6fd8b162c..e5d85b7b2 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -30,22 +30,25 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -from rclpy.node import Node +from rclpy.qos import QoSProfile from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped -from rclpy.qos import QoSProfile, QoSDurabilityPolicy -class StaticTransformBroadcaster(Node): +class StaticTransformBroadcaster: """ :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. """ - def __init__(self, name=None): - super().__init__('transform_broadcaster_impl' if name is None else name) - latching_qos = QoSProfile( - depth=1, - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) - self.pub_tf = self.create_publisher(TFMessage, "/tf_static", qos_profile=latching_qos) + def __init__(self, node, qos=QoSProfile(depth=100)): + """ + .. function:: __init__(node, qos=QoSProfile(depth=100)) + + Constructor. + + :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_static", qos) def sendTransform(self, transform): if not isinstance(transform, list): diff --git a/tf2_ros/src/tf2_ros/transform_broadcaster.py b/tf2_ros/src/tf2_ros/transform_broadcaster.py index b23b48430..d45c40600 100644 --- a/tf2_ros/src/tf2_ros/transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/transform_broadcaster.py @@ -30,19 +30,25 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -from rclpy.node import Node +from rclpy.qos import QoSProfile from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped -class TransformBroadcaster(Node): +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, name=None): - super().__init__('transform_broadcaster_impl' if name is None else name) - self.pub_tf = self.create_publisher(TFMessage, "/tf") + :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): """ diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 8185ff3aa..51b6d1488 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -27,39 +27,59 @@ # author: Wim Meeussen -from rclpy.node import Node +from rclpy.executors import SingleThreadedExecutor +from rclpy.qos import QoSProfile import tf2_ros from tf2_msgs.msg import TFMessage +from threading import Thread -class TransformListener(Node): +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, name=None): + def __init__(self, buffer, node, spin_thread=True, qos=QoSProfile(depth=100)): """ - .. function:: __init__(buffer, name=None) + .. function:: __init__(buffer, node, spin_thread=True, qos=QoSProfile(depth=100)) Constructor. :param buffer: The buffer to propagate changes to when tf info updates. - :param name: The name of ROS2 node. + :param node: The ROS2 node. + :param spin_thread: Whether the listener is spinning or not. + :param qos: A QoSProfile or a history depth to apply to subscribers. """ - super().__init__('transform_listener_impl' if name is None else name) self.buffer = buffer - self.tf_sub = self.create_subscription(TFMessage, 'tf', self.callback) - self.tf_static_sub = self.create_subscription(TFMessage, 'tf_static', self.static_callback) - + self.node = node + self.tf_sub = node.create_subscription(TFMessage, 'tf', self.callback, qos) + self.tf_static_sub = node.create_subscription(TFMessage, 'tf_static', self.static_callback, qos) + + if spin_thread is True: + 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): + print('del') + 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.destroy_subscription(self.tf_sub) - self.destroy_subscription(self.tf_static_sub) + self.tf_sub.destroy() + self.tf_static_sub.destroy() def callback(self, data): who = str("default_authority") diff --git a/tf2_ros/test/py_test/test_listener_and_broadcaster.py b/tf2_ros/test/py_test/test_listener_and_broadcaster.py index 1d8537d7b..4d3afb55d 100644 --- a/tf2_ros/test/py_test/test_listener_and_broadcaster.py +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -46,12 +46,14 @@ def setUpClass(cls): rclpy.init() cls.buffer = Buffer() - cls.broadcaster = TransformBroadcaster() - cls.static_broadcaster = StaticTransformBroadcaster() - cls.listener = TransformListener(buffer=cls.buffer) + cls.node = rclpy.create_node('test') + 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.listener) + cls.executor.add_node(cls.node) pass @classmethod From 3cacf52472488dc61f16b75342e8d1616d10a1c5 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Aug 2019 09:59:11 -0700 Subject: [PATCH 29/79] Document python_compat functions Signed-off-by: Shane Loretz --- tf2_py/src/python_compat.h | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/tf2_py/src/python_compat.h b/tf2_py/src/python_compat.h index cdd4c3f10..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,6 +31,12 @@ 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; @@ -35,14 +49,25 @@ 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); From 3610851c5e91d5b9a74fe3cb2a76b7e60ebd9fe4 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 27 Aug 2019 11:12:34 -0700 Subject: [PATCH 30/79] transform_convert error handling fixes Also fixed leaking PyFloat's set in python object. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 239 +++++++++++++++++++++++++++++++++++++++--- 1 file changed, 224 insertions(+), 15 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 907419eb6..b823b9775 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -86,44 +86,253 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* //we need to convert the time to python PyObject *builtin_interfaces_time = PyObject_GetAttrString(pModulebuiltininterfacesmsgs, "Time"); + if (!builtin_interfaces_time) { + Py_DECREF(pinst); + return NULL; + } PyObject *args = PyTuple_New(0); + if (!args) { + Py_DECREF(builtin_interfaces_time); + Py_DECREF(pinst); + return NULL; + } PyObject *kwargs = PyDict_New(); + if (!kwargs) { + Py_DECREF(args); + Py_DECREF(builtin_interfaces_time); + Py_DECREF(pinst); + return NULL; + } PyObject *sec = Py_BuildValue("i", transform->header.stamp.sec); + if (!sec) { + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(builtin_interfaces_time); + Py_DECREF(pinst); + return NULL; + } PyObject *nanosec = Py_BuildValue("i", transform->header.stamp.nanosec); - PyDict_SetItemString(kwargs, "sec", sec); - PyDict_SetItemString(kwargs, "nanosec", nanosec); + if (!nanosec) { + Py_DECREF(sec); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(builtin_interfaces_time); + Py_DECREF(pinst); + return NULL; + } + if (-1 == PyDict_SetItemString(kwargs, "sec", sec)) { + Py_DECREF(sec); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(builtin_interfaces_time); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(sec); + if (-1 == PyDict_SetItemString(kwargs, "nanosec", nanosec)) { + Py_DECREF(sec); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(builtin_interfaces_time); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(nanosec); PyObject *time_obj = PyObject_Call(builtin_interfaces_time, args, kwargs); - - Py_DECREF(nanosec); - Py_DECREF(sec); Py_DECREF(kwargs); Py_DECREF(args); Py_DECREF(builtin_interfaces_time); + if (!time_obj) { + Py_DECREF(pinst); + return NULL; + } PyObject* pheader = PyObject_GetAttrString(pinst, "header"); - PyObject_SetAttrString(pheader, "stamp", time_obj); + if (!pheader) { + Py_DECREF(time_obj); + Py_DECREF(pinst); + return NULL; + } + if (-1 == PyObject_SetAttrString(pheader, "stamp", time_obj)) { + Py_DECREF(time_obj); + Py_DECREF(pinst); + return NULL; + } Py_DECREF(time_obj); - PyObject_SetAttrString(pheader, "frame_id", stringToPython(transform->header.frame_id)); + PyObject * pframe_id = stringToPython(transform->header.frame_id); + if (!pframe_id) { + Py_DECREF(pinst); + return NULL; + } + if (-1 == PyObject_SetAttrString(pheader, "frame_id", pframe_id)) { + Py_DECREF(pframe_id); + Py_DECREF(pheader); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(pframe_id); Py_DECREF(pheader); PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); + if (!ptransform) { + Py_DECREF(pinst); + return NULL; + } PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); + if (!ptranslation) { + Py_DECREF(ptransform); + Py_DECREF(pinst); + return NULL; + } PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); + if (!ptranslation) { + Py_DECREF(ptranslation); + Py_DECREF(ptransform); + Py_DECREF(pinst); + return NULL; + } Py_DECREF(ptransform); - PyObject_SetAttrString(pinst, "child_frame_id", stringToPython(transform->child_frame_id)); + PyObject *child_frame_id = stringToPython(transform->child_frame_id); + if (!child_frame_id) { + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + if (-1 == PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id)) { + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } - 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)); + PyObject * ptrans_x = PyFloat_FromDouble(transform->transform.translation.x); + if (!ptrans_x) { + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + PyObject * ptrans_y = PyFloat_FromDouble(transform->transform.translation.y); + if (!ptrans_y) { + Py_DECREF(ptrans_x); + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + PyObject * ptrans_z = PyFloat_FromDouble(transform->transform.translation.z); + if (!ptrans_z) { + Py_DECREF(ptrans_y); + Py_DECREF(ptrans_x); + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + if (-1 == PyObject_SetAttrString(ptranslation, "x", ptrans_x)) { + Py_DECREF(ptrans_z); + Py_DECREF(ptrans_y); + Py_DECREF(ptrans_x); + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(ptrans_x); + if (-1 == PyObject_SetAttrString(ptranslation, "y", ptrans_y)) { + Py_DECREF(ptrans_z); + Py_DECREF(ptrans_y); + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(ptrans_y); + if (-1 == PyObject_SetAttrString(ptranslation, "z", ptrans_z)) { + Py_DECREF(ptrans_z); + Py_DECREF(ptranslation); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(ptrans_z); Py_DECREF(ptranslation); - 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)); + PyObject *prot_x = PyFloat_FromDouble(transform->transform.rotation.x); + if (!prot_x) { + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + + PyObject *prot_y = PyFloat_FromDouble(transform->transform.rotation.y); + if (!prot_y) { + Py_DECREF(prot_x); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + + PyObject *prot_z = PyFloat_FromDouble(transform->transform.rotation.z); + if (!prot_z) { + Py_DECREF(prot_y); + Py_DECREF(prot_x); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + + PyObject *prot_w = PyFloat_FromDouble(transform->transform.rotation.w); + if (!prot_w) { + Py_DECREF(prot_z); + Py_DECREF(prot_y); + Py_DECREF(prot_x); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + + if (-1 == PyObject_SetAttrString(protation, "x", prot_x)) { + Py_DECREF(prot_w); + Py_DECREF(prot_z); + Py_DECREF(prot_y); + Py_DECREF(prot_x); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(prot_x); + + if (-1 == PyObject_SetAttrString(protation, "y", prot_y)) { + Py_DECREF(prot_w); + Py_DECREF(prot_z); + Py_DECREF(prot_y); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(prot_y); + + if (-1 == PyObject_SetAttrString(protation, "z", prot_z)) { + Py_DECREF(prot_w); + Py_DECREF(prot_z); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(prot_z); + + if (-1 == PyObject_SetAttrString(protation, "w", prot_w)) { + Py_DECREF(prot_w); + Py_DECREF(protation); + Py_DECREF(pinst); + return NULL; + } + Py_DECREF(prot_w); Py_DECREF(protation); return pinst; From a9221c4bd886e5ac5d96638cb3fc1b9b0182e0d1 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Aug 2019 12:43:58 -0700 Subject: [PATCH 31/79] Eliminate constants Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index b823b9775..674f48000 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -340,21 +340,19 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* static builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t) { - std::chrono::nanoseconds ns = \ - std::chrono::duration_cast(t.time_since_epoch()); - std::chrono::seconds s = \ - std::chrono::duration_cast(t.time_since_epoch()); + 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() % 1000000000ull); + time_msg.nanosec = (uint32_t)ns.count(); return time_msg; } static tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) { - int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec; - std::chrono::nanoseconds ns(d); - return tf2::TimePoint(std::chrono::duration_cast(ns)); + 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) @@ -373,7 +371,7 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); const int64_t d = PyLong_AsLongLong(nanoseconds); const std::chrono::nanoseconds ns(d); - *rt = tf2::TimePoint(std::chrono::duration_cast(ns)); + *rt = tf2::TimePoint(ns); return 1; } From 65e571480c06045f5c2e53beb52b39b01c7a674c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Aug 2019 18:00:41 -0700 Subject: [PATCH 32/79] Static QoS uses TransientLocal Signed-off-by: Shane Loretz --- .../tf2_ros/static_transform_broadcaster.py | 15 +++++++++++++-- tf2_ros/src/tf2_ros/transform_listener.py | 18 ++++++++++++++++-- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index e5d85b7b2..81186bdbd 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -30,6 +30,8 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +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 @@ -39,7 +41,7 @@ class StaticTransformBroadcaster: :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. """ - def __init__(self, node, qos=QoSProfile(depth=100)): + def __init__(self, node, qos=None): """ .. function:: __init__(node, qos=QoSProfile(depth=100)) @@ -48,11 +50,20 @@ def __init__(self, node, qos=QoSProfile(depth=100)): :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=100, + 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] + 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 51b6d1488..2e335a1c0 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -28,6 +28,8 @@ # author: Wim Meeussen 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 @@ -39,7 +41,7 @@ class TransformListener: 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, node, spin_thread=True, qos=QoSProfile(depth=100)): + def __init__(self, buffer, node, spin_thread=True, qos=None, static_qos=None): """ .. function:: __init__(buffer, node, spin_thread=True, qos=QoSProfile(depth=100)) @@ -50,10 +52,22 @@ def __init__(self, buffer, node, spin_thread=True, qos=QoSProfile(depth=100)): :param spin_thread: Whether the listener is spinning or not. :param qos: A QoSProfile or a history depth to apply to 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.node = node self.tf_sub = node.create_subscription(TFMessage, 'tf', self.callback, qos) - self.tf_static_sub = node.create_subscription(TFMessage, 'tf_static', self.static_callback, qos) + self.tf_static_sub = node.create_subscription(TFMessage, 'tf_static', self.static_callback, static_qos) if spin_thread is True: self.executor = SingleThreadedExecutor() From 29cd0f764c3444f735e2b7d167e404f7755c0376 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Aug 2019 09:10:24 -0700 Subject: [PATCH 33/79] TransformListener uses its own callback group Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/transform_listener.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 2e335a1c0..1a7a8a8c3 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -27,6 +27,7 @@ # author: Wim Meeussen +from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import SingleThreadedExecutor from rclpy.qos import DurabilityPolicy from rclpy.qos import HistoryPolicy @@ -66,8 +67,13 @@ def __init__(self, buffer, node, spin_thread=True, qos=None, static_qos=None): ) self.buffer = buffer self.node = node - self.tf_sub = node.create_subscription(TFMessage, 'tf', self.callback, qos) - self.tf_static_sub = node.create_subscription(TFMessage, 'tf_static', self.static_callback, static_qos) + # 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 is True: self.executor = SingleThreadedExecutor() From 1d65f3d0df28cd274631952a68a8f81168a2cb91 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Aug 2019 15:35:07 -0700 Subject: [PATCH 34/79] Remove debug print Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/transform_listener.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 1a7a8a8c3..86ddb3026 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -87,7 +87,6 @@ def run_func(): self.dedicated_listener_thread.start() def __del__(self): - print('del') if hasattr(self, 'dedicated_listener_thread') and hasattr(self, 'executor'): self.executor.shutdown() self.dedicated_listener_thread.join() From dc9249f204517d6156035fe75644fd95c7953247 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Aug 2019 16:22:26 -0700 Subject: [PATCH 35/79] Ask node to destroy subscription Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/transform_listener.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 86ddb3026..7e8a055b3 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -97,8 +97,8 @@ def unregister(self): """ Unregisters all tf subscribers. """ - self.tf_sub.destroy() - self.tf_static_sub.destroy() + self.node.destroy_subscription(self.tf_sub) + self.node.destroy_subscription(self.tf_static_sub) def callback(self, data): who = str("default_authority") From c343dc498c97b60afe8cf04b11c127378665c845 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Aug 2019 17:21:38 -0700 Subject: [PATCH 36/79] Add lookup_transform_async() Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 52 +++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 808a44a3d..34ef151c6 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -35,6 +35,8 @@ # import rosgraph.masterapi from time import sleep from rclpy.duration import Duration +from rclpy.task import Future + class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): """ @@ -70,9 +72,27 @@ def __init__(self, cache_time = None, debug = True): # except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure): # self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames) + self._new_data_callbacks = [] + def __get_frames(self, req): 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): + to_remove = [] + for callback in self._new_data_callbacks: + if callback(): + to_remove.append(callback) + for callback in to_remove: + self._new_data_callbacks.remove(callback) + def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()): """ Get the transform from the source frame to the target frame. @@ -88,6 +108,38 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()) self.can_transform(target_frame, source_frame, time, timeout) return self.lookup_transform_core(target_frame, source_frame, time) + 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: A future for the given transform. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + fut = rclpy.task.Future() + + def _on_new_data(): + nonlocal target_frame + nonlocal source_frame + nonlocal time + nonlocal fut + if self.can_transform(target_frame, source_frame, time): + try: + fut.set_result(self.lookup_transform_core(target_frame, source_frame, time)) + return True + except BaseException as e: + fut.set_exception(e) + + self._new_data_callbacks.append(_on_new_data) + + await fut + transform = fut.result() + if transform is None: + raise fut.exception() + return transform + 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. From d71c1381df20e5a88397b647673c91f5cc8d1ee3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Aug 2019 13:50:10 -0700 Subject: [PATCH 37/79] Simplify async function Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 34ef151c6..7e0ffecc5 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -121,24 +121,16 @@ async def lookup_transform_async(self, target_frame, source_frame, time): fut = rclpy.task.Future() def _on_new_data(): - nonlocal target_frame - nonlocal source_frame - nonlocal time - nonlocal fut if self.can_transform(target_frame, source_frame, time): try: fut.set_result(self.lookup_transform_core(target_frame, source_frame, time)) - return True except BaseException as e: fut.set_exception(e) + return True self._new_data_callbacks.append(_on_new_data) - await fut - transform = fut.result() - if transform is None: - raise fut.exception() - return transform + return await fut def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ From a8ea8f3dc3186c248864d54a893155bfc9219ef3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Aug 2019 15:20:00 -0700 Subject: [PATCH 38/79] Full async transform methods Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 81 +++++++++++++++++++++++++++++------ 1 file changed, 68 insertions(+), 13 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 7e0ffecc5..db47ccc37 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -115,22 +115,12 @@ async def lookup_transform_async(self, target_frame, source_frame, time): :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 for the given transform. + :return: the transform :rtype: :class:`geometry_msgs.msg.TransformStamped` """ - fut = rclpy.task.Future() - - def _on_new_data(): - if self.can_transform(target_frame, source_frame, time): - try: - fut.set_result(self.lookup_transform_core(target_frame, source_frame, time)) - except BaseException as e: - fut.set_exception(e) - return True - - self._new_data_callbacks.append(_on_new_data) + await self.wait_for_transform_async(target_frame, source_frame, time) + return self.lookup_transform_core(target_frame, source_frame, time) - return await fut def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()): """ @@ -148,6 +138,20 @@ 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. + + :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): """ @@ -212,3 +216,54 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim return core_result return core_result[0] + async 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: True when the transform becomes available + :rtype: bool + """ + fut = rclpy.task.Future() + + def _on_new_data(): + try: + available = self.can_transform_core(target_frame, source_frame, time)[0] + if available: + fut.set_result(True) + except BaseException as e: + fut.set_exception(e) + return fut.done() + + self._new_data_callbacks.append(_on_new_data) + + return await fut + + async 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: True when the transform becomes available + :rtype: bool + """ + fut = rclpy.task.Future() + + def _on_new_data(): + try: + available = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] + if available: + fut.set_result(True) + except BaseException as e: + fut.set_exception(e) + return fut.done() + + self._new_data_callbacks.append(_on_new_data) + + return await fut From 77b434e0cdc50e41b00bd79d38ace8bdf903475e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Aug 2019 16:24:45 -0700 Subject: [PATCH 39/79] Add shortcut to avoid suspending if result is already available Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index db47ccc37..b22fb8cfb 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -226,6 +226,10 @@ async def wait_for_transform_async(self, target_frame, source_frame, time): :return: True when the transform becomes available :rtype: bool """ + if self.can_transform_core(target_frame, source_frame, time)[0]: + # Short cut, the transform is available + return True + fut = rclpy.task.Future() def _on_new_data(): @@ -253,6 +257,10 @@ async def wait_for_transform_full_async(self, target_frame, target_time, source_ :return: True when the transform becomes available :rtype: bool """ + if self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0]: + # Short cut, the transform is available + return True + fut = rclpy.task.Future() def _on_new_data(): From f1757525f8b9173ea3f2671f000bab1259c691ed Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 30 Aug 2019 16:25:07 -0700 Subject: [PATCH 40/79] Add test for async transform lookup Signed-off-by: Shane Loretz --- .../test/py_test/test_listener_and_broadcaster.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tf2_ros/test/py_test/test_listener_and_broadcaster.py b/tf2_ros/test/py_test/test_listener_and_broadcaster.py index 4d3afb55d..f445749e9 100644 --- a/tf2_ros/test/py_test/test_listener_and_broadcaster.py +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -96,6 +96,19 @@ def test_broadcaster_and_listener(self): 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', From 1d66d12096279f6a8387b4348dcf9ff5c318d47d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 15:42:29 -0700 Subject: [PATCH 41/79] Remove redundant code Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer_client.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index 1980bb77f..fd67a47b9 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -196,13 +196,11 @@ def unblock_by_timeout(): event.wait() - if send_goal_future.exception() is not None: - raise send_goal_future.exception() - #This shouldn't happen, but could in rare cases where the server hangs 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") + # Raises if future was given an exception goal_handle = send_goal_future.result() if not goal_handle.accepted: From a8a8a8852debf4cf7c5913a9ea4afd863bdb5a6b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 16:05:07 -0700 Subject: [PATCH 42/79] Remove unused __is_done() Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer_client.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index fd67a47b9..87f99529b 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -155,13 +155,6 @@ 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): if not self.action_client.server_is_ready(): raise tf2.TimeoutException("The BufferServer is not ready") From 17d9b230609bb55b0c2072482ad8be8cc7ec1e89 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 16:05:23 -0700 Subject: [PATCH 43/79] TODO why not service Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer_client.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tf2_ros/src/tf2_ros/buffer_client.py b/tf2_ros/src/tf2_ros/buffer_client.py index 87f99529b..81f1f9e84 100644 --- a/tf2_ros/src/tf2_ros/buffer_client.py +++ b/tf2_ros/src/tf2_ros/buffer_client.py @@ -156,6 +156,7 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim return False def __process_goal(self, goal): + # 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") From 3bc5bc5f7f292f81dba672deaa3dcd7f1c76cc0d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 16:17:11 -0700 Subject: [PATCH 44/79] Remove unnecessary list check Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/static_transform_broadcaster.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index 81186bdbd..fa4f458f6 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -59,11 +59,10 @@ def __init__(self, node, qos=None): self.pub_tf = node.create_publisher(TFMessage, "/tf_static", qos) def sendTransform(self, transform): - if not isinstance(transform, list): - if hasattr(transform, '__iter__'): - transform = list(transform) - else: - transform = [transform] + if hasattr(transform, '__iter__'): + transform = list(transform) + else: + transform = [transform] self.pub_tf.publish(TFMessage(transforms=transform)) From e88b319b8e4d8d15f6cdc390711fd97d0685e775 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 16:26:03 -0700 Subject: [PATCH 45/79] Better checks for lists of transforms Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/static_transform_broadcaster.py | 9 +++++---- tf2_ros/src/tf2_ros/transform_broadcaster.py | 5 ++++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index fa4f458f6..81186bdbd 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -59,10 +59,11 @@ def __init__(self, node, qos=None): self.pub_tf = node.create_publisher(TFMessage, "/tf_static", qos) def sendTransform(self, transform): - if hasattr(transform, '__iter__'): - transform = list(transform) - else: - transform = [transform] + if not isinstance(transform, list): + 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 d45c40600..dfaf136a5 100644 --- a/tf2_ros/src/tf2_ros/transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/transform_broadcaster.py @@ -57,7 +57,10 @@ def sendTransform(self, transform): :param transform: A transform or list of transforms to send. """ if not isinstance(transform, list): - transform = [transform] + if hasattr(transform, '__iter__'): + transform = list(transform) + else: + transform = [transform] self.pub_tf.publish(TFMessage(transforms=transform)) From 9bdc25c24142d6e2b22b65501e1f46c5848e1a6d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:05:24 -0700 Subject: [PATCH 46/79] Default spin_thread to False Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/transform_listener.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 7e8a055b3..8c880e6be 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -42,7 +42,7 @@ class TransformListener: 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, node, spin_thread=True, qos=None, static_qos=None): + def __init__(self, buffer, node, *, spin_thread=False, qos=None, static_qos=None): """ .. function:: __init__(buffer, node, spin_thread=True, qos=QoSProfile(depth=100)) @@ -50,7 +50,7 @@ def __init__(self, buffer, node, spin_thread=True, qos=None, static_qos=None): :param buffer: The buffer to propagate changes to when tf info updates. :param node: The ROS2 node. - :param spin_thread: Whether the listener is spinning or not. + :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. """ if qos is None: @@ -75,7 +75,7 @@ def __init__(self, buffer, node, spin_thread=True, qos=None, static_qos=None): self.tf_static_sub = node.create_subscription( TFMessage, 'tf_static', self.static_callback, static_qos, callback_group=self.group) - if spin_thread is True: + if spin_thread: self.executor = SingleThreadedExecutor() def run_func(): From 5f6f673dcd499071f784e662b9ba149be23e0b39 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:26:11 -0700 Subject: [PATCH 47/79] Error checking in rostime_converter Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 674f48000..836aa3e54 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -359,11 +359,31 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) { if(PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { PyObject *sec = pythonBorrowAttrString(obj, "sec"); + if (!sec) { + return 0; + } PyObject *nanosec = pythonBorrowAttrString(obj, "nanosec"); + if (!nanosec) { + Py_DECREF(sec); + return 0; + } builtin_interfaces::msg::Time msg; msg.sec = PyLong_AsLong(sec); + if (PyErr_Occurred()) { + Py_DECREF(sec); + Py_DECREF(nanosec); + return 0; + } msg.nanosec = PyLong_AsUnsignedLong(nanosec); + if (PyErr_Occurred()) { + Py_DECREF(sec); + Py_DECREF(nanosec); + return 0; + } *rt = fromMsg(msg); + if (!rt) { + return 0; + } return 1; } From 87ad7857e9cd0e36c78a7da7da44a7218e5039b0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:30:53 -0700 Subject: [PATCH 48/79] rostime_converter error handling fixes Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 836aa3e54..49600429b 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -381,15 +381,19 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) return 0; } *rt = fromMsg(msg); - if (!rt) { - return 0; - } return 1; } if(PyObject_HasAttrString(obj, "nanoseconds")) { PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); + if (!nanoseconds) { + return 0; + } const int64_t d = PyLong_AsLongLong(nanoseconds); + if (PyErr_Occurred()) { + Py_DECREF(nanoseconds); + return 0; + } const std::chrono::nanoseconds ns(d); *rt = tf2::TimePoint(ns); return 1; From f346d9d747ec0ce69b0348d500f6e735d9d60a5a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:34:34 -0700 Subject: [PATCH 49/79] Simpler error handling Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 49600429b..8243f016d 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -359,25 +359,11 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) { if(PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { PyObject *sec = pythonBorrowAttrString(obj, "sec"); - if (!sec) { - return 0; - } PyObject *nanosec = pythonBorrowAttrString(obj, "nanosec"); - if (!nanosec) { - Py_DECREF(sec); - return 0; - } builtin_interfaces::msg::Time msg; msg.sec = PyLong_AsLong(sec); - if (PyErr_Occurred()) { - Py_DECREF(sec); - Py_DECREF(nanosec); - return 0; - } msg.nanosec = PyLong_AsUnsignedLong(nanosec); if (PyErr_Occurred()) { - Py_DECREF(sec); - Py_DECREF(nanosec); return 0; } *rt = fromMsg(msg); @@ -386,12 +372,8 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) if(PyObject_HasAttrString(obj, "nanoseconds")) { PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); - if (!nanoseconds) { - return 0; - } const int64_t d = PyLong_AsLongLong(nanoseconds); if (PyErr_Occurred()) { - Py_DECREF(nanoseconds); return 0; } const std::chrono::nanoseconds ns(d); From 42d94f099b6bf7bd6919dea339cdd7d1a4b7b591 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:34:49 -0700 Subject: [PATCH 50/79] time -> duration Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 8243f016d..581328118 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -403,7 +403,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) return 1; } - PyErr_SetString(PyExc_TypeError, "time must have sec and nanosec, or nanoseconds."); + PyErr_SetString(PyExc_TypeError, "duration must have sec and nanosec, or nanoseconds."); return 0; } From c5b358850c54e9f6af82b95c3c1d956b6cb46788 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:37:46 -0700 Subject: [PATCH 51/79] Simple error checking May print warning if multiple exceptions raised in converter Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 581328118..086516899 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -363,22 +363,16 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) builtin_interfaces::msg::Time msg; msg.sec = PyLong_AsLong(sec); msg.nanosec = PyLong_AsUnsignedLong(nanosec); - if (PyErr_Occurred()) { - return 0; - } *rt = fromMsg(msg); - return 1; + return PyErr_Occurred() ? 0 : 1; } if(PyObject_HasAttrString(obj, "nanoseconds")) { PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); const int64_t d = PyLong_AsLongLong(nanoseconds); - if (PyErr_Occurred()) { - return 0; - } const std::chrono::nanoseconds ns(d); *rt = tf2::TimePoint(ns); - return 1; + return PyErr_Occurred() ? 0 : 1; } PyErr_SetString(PyExc_TypeError, "time must have sec and nanosec, or nanoseconds."); @@ -392,7 +386,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) PyObject *nanosec = pythonBorrowAttrString(obj, "nanosec"); *rt = std::chrono::seconds(PyLong_AsLong(sec)) + std::chrono::nanoseconds(PyLong_AsUnsignedLong(nanosec)); - return 1; + return PyErr_Occurred() ? 0 : 1; } if(PyObject_HasAttrString(obj, "nanoseconds")) { @@ -400,7 +394,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) const int64_t d = PyLong_AsLongLong(nanoseconds); const std::chrono::nanoseconds ns(d); *rt = std::chrono::duration_cast(ns); - return 1; + return PyErr_Occurred() ? 0 : 1; } PyErr_SetString(PyExc_TypeError, "duration must have sec and nanosec, or nanoseconds."); From f5d834ea1897e294c45ad8a688576f92ccb33087 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:45:49 -0700 Subject: [PATCH 52/79] error handling in getLatestCommonTime() Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 46 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 42 insertions(+), 4 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 086516899..deb8b55ef 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -513,17 +513,55 @@ static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) const tf2::TF2Error r = bc->_getLatestCommonTime(target_id, source_id, tf2_time, &error_string); if (r == tf2::TF2Error::NO_ERROR) { PyObject *rclpy_time = PyObject_GetAttrString(pModulerclpytime, "Time"); + if (!rclpy_time) { + return NULL; + } const builtin_interfaces::msg::Time time_msg = toMsg(tf2_time); PyObject *args = PyTuple_New(0); + if (!args) { + Py_DECREF(rclpy_time); + return NULL; + } PyObject *kwargs = PyDict_New(); + if (!kwargs) { + Py_DECREF(args); + Py_DECREF(rclpy_time); + return NULL; + } PyObject *seconds = Py_BuildValue("i", time_msg.sec); + if (!seconds) { + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(rclpy_time); + return NULL; + } PyObject *nanoseconds = Py_BuildValue("i", time_msg.nanosec); - PyDict_SetItemString(kwargs, "seconds", seconds); - PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds); + if (!nanoseconds) { + Py_DECREF(seconds); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(rclpy_time); + return NULL; + } + if (0 != PyDict_SetItemString(kwargs, "seconds", seconds)) { + Py_DECREF(nanoseconds); + Py_DECREF(seconds); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(rclpy_time); + return NULL; + } + Py_DECREF(seconds); + if (0 != PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds)) { + Py_DECREF(nanoseconds); + Py_DECREF(kwargs); + Py_DECREF(args); + Py_DECREF(rclpy_time); + return NULL; + } + Py_DECREF(nanoseconds); PyObject *ob = PyObject_Call(rclpy_time, args, kwargs); - Py_DECREF(nanoseconds); - Py_DECREF(seconds); Py_DECREF(kwargs); Py_DECREF(args); Py_DECREF(rclpy_time); From 020b78b196b48e4eb17668f35ce6a6c34949975c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 3 Sep 2019 17:57:41 -0700 Subject: [PATCH 53/79] spell test_tf2_py out for searchability Signed-off-by: Shane Loretz --- tf2_py/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 9abebacc3..0bc9e9056 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -31,8 +31,8 @@ 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_${PROJECT_NAME}" - RUNTIME_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/test_${PROJECT_NAME}" + 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() From d8952e60d7aba5a646cb7b81bc05b132fd2b7886 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 09:24:35 -0700 Subject: [PATCH 54/79] Static Broadcaster QoS depth 1 Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/static_transform_broadcaster.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index 81186bdbd..3e183f6fb 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -52,7 +52,7 @@ def __init__(self, node, qos=None): """ if qos is None: qos = QoSProfile( - depth=100, + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, ) From 5af17bd0635972d47ac29d091fa7f6e410c3bb83 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 11:12:52 -0700 Subject: [PATCH 55/79] Unintent docstring Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/static_transform_broadcaster.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index 3e183f6fb..1d587f544 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -43,12 +43,10 @@ class StaticTransformBroadcaster: def __init__(self, node, qos=None): """ - .. function:: __init__(node, qos=QoSProfile(depth=100)) + Constructor. - Constructor. - - :param node: The ROS2 node. - :param qos: A QoSProfile or a history depth to apply to the publisher. + :param node: The ROS2 node. + :param qos: A QoSProfile or a history depth to apply to the publisher. """ if qos is None: qos = QoSProfile( From 46eea00718fe8014ea4fb130468c5f8635e983b6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 11:16:21 -0700 Subject: [PATCH 56/79] Restore whitespace Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/static_transform_broadcaster.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py index 1d587f544..457359e36 100644 --- a/tf2_ros/src/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -36,6 +36,7 @@ from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped + class StaticTransformBroadcaster: """ :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. From 7e1c7a0e0143b18bd1a2c831e830bd24bf2fdeef Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 11:34:00 -0700 Subject: [PATCH 57/79] Return future so callers can add done callbacks Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index b22fb8cfb..2ac3c33d5 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -243,7 +243,7 @@ def _on_new_data(): self._new_data_callbacks.append(_on_new_data) - return await fut + return fut async def wait_for_transform_full_async(self, target_frame, target_time, source_frame, source_time, fixed_frame): """ @@ -274,4 +274,4 @@ def _on_new_data(): self._new_data_callbacks.append(_on_new_data) - return await fut + return fut From 5afe8300ba49ff76dbc3679b9a0e4aadc979eb62 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 14:53:46 -0700 Subject: [PATCH 58/79] wait_for_transform_async returns Future This allows callers to take advantage of done callbacks, or cancel the future if they want to implement their own timeout mechanism. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 54 +++++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 2ac3c33d5..6cefb0f86 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -27,6 +27,8 @@ # author: Wim Meeussen +import threading + import rclpy import tf2_py as tf2 import tf2_ros @@ -73,6 +75,8 @@ def __init__(self, cache_time = None, debug = True): # 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 FrameGraph.Response(frame_yaml=self.all_frames_as_yaml()) @@ -86,12 +90,17 @@ def set_transform_static(self, *args, **kwargs): self._call_new_data_callbacks() def _call_new_data_callbacks(self): - to_remove = [] - for callback in self._new_data_callbacks: - if callback(): - to_remove.append(callback) - for callback in to_remove: - self._new_data_callbacks.remove(callback) + 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 _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()): """ @@ -216,36 +225,37 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim return core_result return core_result[0] - async def wait_for_transform_async(self, target_frame, source_frame, time): + 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: True when the transform becomes available - :rtype: bool + :return: A future that becomes true when the transform is available + :rtype: rclpy.task.Future """ - if self.can_transform_core(target_frame, source_frame, time)[0]: - # Short cut, the transform is available - return True 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: - available = self.can_transform_core(target_frame, source_frame, time)[0] - if available: + if self.can_transform_core(target_frame, source_frame, time)[0]: fut.set_result(True) except BaseException as e: fut.set_exception(e) - return fut.done() self._new_data_callbacks.append(_on_new_data) + fut.add_done_callback(lambda _: self._remove_callback(_on_new_data)) return fut - async def wait_for_transform_full_async(self, target_frame, target_time, source_frame, source_time, fixed_frame): + 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. @@ -254,24 +264,24 @@ async def wait_for_transform_full_async(self, target_frame, target_time, source_ :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: True when the transform becomes available - :rtype: bool + :return: A future that becomes true when the transform is available + :rtype: 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 - return True + fut.set_result(True) + return fut fut = rclpy.task.Future() def _on_new_data(): try: - available = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] - if available: + 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) - return fut.done() self._new_data_callbacks.append(_on_new_data) + fut.add_done_callback(lambda _: self._remove_callback(_on_new_data)) return fut From c0740258441e97383272e5d1ff7f55e17be8f5b9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 14:59:34 -0700 Subject: [PATCH 59/79] Whitespace and bug fix Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 6cefb0f86..2c48fcbb5 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -235,9 +235,7 @@ def wait_for_transform_async(self, target_frame, source_frame, time): :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) @@ -267,13 +265,12 @@ def wait_for_transform_full_async(self, target_frame, target_time, source_frame, :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 - fut = rclpy.task.Future() - def _on_new_data(): try: if self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0]: From c3b48e4557b076a3417e4877f7517223a963f794 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 15:21:52 -0700 Subject: [PATCH 60/79] Test asyncronous lookup_transform methods Signed-off-by: Shane Loretz --- tf2_ros/test/py_test/test_buffer.py | 85 ++++++++++++++++++++++++++--- 1 file changed, 78 insertions(+), 7 deletions(-) diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index 14d68d084..6d23484ec 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -18,6 +18,7 @@ from tf2_ros.buffer import Buffer from geometry_msgs.msg import TransformStamped, PointStamped + class TestBuffer(unittest.TestCase): @classmethod def setUpClass(cls): @@ -27,15 +28,11 @@ def setUpClass(cls): def tearDownClass(cls): pass - def test_can_transform_valid_transform(self): - buffer = Buffer() - clock = rclpy.clock.Clock() - rclpy_time = clock.now() - + def build_transform(self, source, target, rclpy_time): transform = TransformStamped() - transform.header.frame_id = "foo" + transform.header.frame_id = source transform.header.stamp = rclpy_time.to_msg() - transform.child_frame_id = "bar" + transform.child_frame_id = target transform.transform.translation.x = 42.0 transform.transform.translation.y = -3.14 transform.transform.translation.z = 0.0 @@ -43,6 +40,13 @@ def test_can_transform_valid_transform(self): 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) @@ -54,5 +58,72 @@ def test_can_transform_valid_transform(self): 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() From cef9781b859940973dfcc47e83d6564c31cb0c1c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 15:41:48 -0700 Subject: [PATCH 61/79] Fix source/target mixup Signed-off-by: Shane Loretz --- tf2_ros/test/py_test/test_buffer.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index 6d23484ec..2bb3962ca 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -28,11 +28,11 @@ def setUpClass(cls): def tearDownClass(cls): pass - def build_transform(self, source, target, rclpy_time): + def build_transform(self, target, source, rclpy_time): transform = TransformStamped() - transform.header.frame_id = source + transform.header.frame_id = target transform.header.stamp = rclpy_time.to_msg() - transform.child_frame_id = target + transform.child_frame_id = source transform.transform.translation.x = 42.0 transform.transform.translation.y = -3.14 transform.transform.translation.z = 0.0 From a16cf154e8b1301b4d0f6c7b2cb37f12a02d5353 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 15:43:12 -0700 Subject: [PATCH 62/79] Single quotes Signed-off-by: Shane Loretz --- tf2_ros/test/py_test/test_buffer.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index 2bb3962ca..d8c8a39f8 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -48,11 +48,11 @@ def test_can_transform_valid_transform(self): rclpy_time = clock.now() transform = self.build_transform('foo', 'bar', rclpy_time) - self.assertEqual(buffer.set_transform(transform, "unittest"), None) + self.assertEqual(buffer.set_transform(transform, 'unittest'), None) - self.assertEqual(buffer.can_transform("foo", "bar", rclpy_time), True) + self.assertEqual(buffer.can_transform('foo', 'bar', rclpy_time), True) - output = buffer.lookup_transform("foo", "bar", rclpy_time) + 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) @@ -65,7 +65,7 @@ def test_await_transform_immediately_available(self): rclpy_time = clock.now() transform = self.build_transform('foo', 'bar', rclpy_time) - buffer.set_transform(transform, "unittest") + buffer.set_transform(transform, 'unittest') coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time) with self.assertRaises(StopIteration) as cm: @@ -81,7 +81,7 @@ def test_await_transform_full_immediately_available(self): rclpy_time = clock.now() transform = self.build_transform('foo', 'bar', rclpy_time) - buffer.set_transform(transform, "unittest") + buffer.set_transform(transform, 'unittest') coro = buffer.lookup_transform_full_async('foo', rclpy_time, 'bar', rclpy_time, 'foo') with self.assertRaises(StopIteration) as cm: @@ -100,7 +100,7 @@ def test_await_transform_delayed(self): coro = buffer.lookup_transform_async('foo', 'bar', rclpy_time) coro.send(None) - buffer.set_transform(transform, "unittest") + buffer.set_transform(transform, 'unittest') with self.assertRaises(StopIteration) as cm: coro.send(None) @@ -117,7 +117,7 @@ def test_await_transform_full_delayed(self): coro = buffer.lookup_transform_full_async('foo', rclpy_time, 'bar', rclpy_time, 'foo') coro.send(None) - buffer.set_transform(transform, "unittest") + buffer.set_transform(transform, 'unittest') with self.assertRaises(StopIteration) as cm: coro.send(None) From f3b726ea2520f6ee9cec1c05c8bb1e3b6c8892ca Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 16:02:37 -0700 Subject: [PATCH 63/79] Remove commented code Signed-off-by: Shane Loretz --- tf2_ros/test/py_test/test_buffer_client.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py index d50fc01c0..797ed63ea 100644 --- a/tf2_ros/test/py_test/test_buffer_client.py +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -108,7 +108,6 @@ def cancel_callback(self, request, response): def result_callback(self, request, response): bytes_goal_id = bytes(request.goal_id.uuid) - #response.status = TF2Error response.result.transform = self.result_buffer[bytes_goal_id][0] response.result.error = TF2Error( error=self.result_buffer[bytes_goal_id][1], From 3e4532f04744b7a3b329ce4e370330dad89c16cf Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 16:03:23 -0700 Subject: [PATCH 64/79] Remove commented code Signed-off-by: Shane Loretz --- tf2_ros/test/py_test/test_buffer_client.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py index 797ed63ea..53009b9d3 100644 --- a/tf2_ros/test/py_test/test_buffer_client.py +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -149,7 +149,6 @@ def setUp(self): def tearDown(self): self.spinning.set() self.spin_thread.join() - #print('teardown : ', self.spin_thread.is_alive(), self.context.ok()) return def feedback_callback(self, feedback): From 47730a1dd343b9aa17a48bec4c8e5ec64f6b87b2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 16:09:56 -0700 Subject: [PATCH 65/79] More specific node name Signed-off-by: Shane Loretz --- tf2_ros/test/py_test/test_listener_and_broadcaster.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tf2_ros/test/py_test/test_listener_and_broadcaster.py b/tf2_ros/test/py_test/test_listener_and_broadcaster.py index f445749e9..da2ff128c 100644 --- a/tf2_ros/test/py_test/test_listener_and_broadcaster.py +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -46,7 +46,7 @@ def setUpClass(cls): rclpy.init() cls.buffer = Buffer() - cls.node = rclpy.create_node('test') + cls.node = rclpy.create_node('test_broadcaster_listener') cls.broadcaster = TransformBroadcaster(cls.node) cls.static_broadcaster = StaticTransformBroadcaster(cls.node) cls.listener = TransformListener( @@ -54,12 +54,10 @@ def setUpClass(cls): cls.executor = rclpy.executors.SingleThreadedExecutor() cls.executor.add_node(cls.node) - pass @classmethod def tearDownClass(cls): rclpy.shutdown() - pass def setUp(self): self.buffer = Buffer() From 0c773837855a12b3ea31c89d4d21aeb379979ff1 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 5 Sep 2019 16:14:17 -0700 Subject: [PATCH 66/79] Remove debug and unindent __init__ Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/buffer.py | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 2c48fcbb5..eadffc08d 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -51,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) @@ -66,14 +63,6 @@ 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() From 5b3f6ea931e0cb7aeaf99095f909d97a403de0d5 Mon Sep 17 00:00:00 2001 From: vinnamkim Date: Wed, 11 Sep 2019 15:00:26 +0900 Subject: [PATCH 67/79] Update licenses Signed-off-by: vinnamkim --- tf2_py/test/__init__.py | 34 +++++++--- tf2_py/test/test_buffer_core.py | 64 +++++++++---------- tf2_ros/test/py_test/test_buffer.py | 34 +++++++--- tf2_ros/test/py_test/test_buffer_client.py | 34 +++++++--- .../py_test/test_listener_and_broadcaster.py | 34 +++++++--- 5 files changed, 129 insertions(+), 71 deletions(-) diff --git a/tf2_py/test/__init__.py b/tf2_py/test/__init__.py index 9a0eb79bd..1dcf1ff02 100644 --- a/tf2_py/test/__init__.py +++ b/tf2_py/test/__init__.py @@ -1,16 +1,32 @@ # Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# * 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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 diff --git a/tf2_py/test/test_buffer_core.py b/tf2_py/test/test_buffer_core.py index 45ac701fa..7c3234fae 100644 --- a/tf2_py/test/test_buffer_core.py +++ b/tf2_py/test/test_buffer_core.py @@ -1,38 +1,32 @@ -# *********************************************************** -# * Software License Agreement (BSD License) -# * -# * Copyright (c) 2009, Willow Garage, 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 Willow Garage, Inc. 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. -# * -# * Author: Vinnam Kim vinnam.kim@gmail.com -# *********************************************************** +# 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 diff --git a/tf2_ros/test/py_test/test_buffer.py b/tf2_ros/test/py_test/test_buffer.py index d8c8a39f8..68ff4755c 100644 --- a/tf2_ros/test/py_test/test_buffer.py +++ b/tf2_ros/test/py_test/test_buffer.py @@ -1,16 +1,32 @@ # Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# * 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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 diff --git a/tf2_ros/test/py_test/test_buffer_client.py b/tf2_ros/test/py_test/test_buffer_client.py index 53009b9d3..2c2d57337 100644 --- a/tf2_ros/test/py_test/test_buffer_client.py +++ b/tf2_ros/test/py_test/test_buffer_client.py @@ -1,16 +1,32 @@ # Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# * 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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 diff --git a/tf2_ros/test/py_test/test_listener_and_broadcaster.py b/tf2_ros/test/py_test/test_listener_and_broadcaster.py index da2ff128c..45c334ae0 100644 --- a/tf2_ros/test/py_test/test_listener_and_broadcaster.py +++ b/tf2_ros/test/py_test/test_listener_and_broadcaster.py @@ -1,16 +1,32 @@ # Copyright 2019 Open Source Robotics Foundation, Inc. +# All rights reserved. # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# * 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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 ca506ea4d211ac874ea86943ebed0370b2e4e8f3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 12:38:17 -0400 Subject: [PATCH 68/79] Only add __init__.py to build space when testing Signed-off-by: Shane Loretz --- tf2_py/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 0bc9e9056..34a8f36eb 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -24,8 +24,6 @@ if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") set(PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}") endif() -file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_tf2_py/__init__.py" "") - ament_python_install_package(${PROJECT_NAME}) function(set_properties _targetname _build_type) @@ -68,6 +66,9 @@ if(BUILD_TESTING) PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ) + + # Create importable location in build directory + file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_tf2_py/__init__.py" "") endif() set(PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") From 474acfccd5debfcfb7cf988f54ba5df39d52ef12 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:14:40 -0400 Subject: [PATCH 69/79] Remove example comments from package.xml Signed-off-by: Shane Loretz --- tf2_py/package.xml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/tf2_py/package.xml b/tf2_py/package.xml index e917e1f38..6ec31907e 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -5,19 +5,10 @@ 0.9.1 The tf2_py package - Tully Foote - - - - BSD - - - - http://ros.org/wiki/tf2_py ament_cmake From a07175f2cd3f364d42fc91fd3e45ed10e5e94cbb Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:15:31 -0400 Subject: [PATCH 70/79] Remove shebag from __init__.py Signed-off-by: Shane Loretz --- tf2_py/tf2_py/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_py/tf2_py/__init__.py b/tf2_py/tf2_py/__init__.py index 5202a0755..c9d330ff3 100644 --- a/tf2_py/tf2_py/__init__.py +++ b/tf2_py/tf2_py/__init__.py @@ -1,4 +1,3 @@ -#! /usr/bin/python #*********************************************************** #* Software License Agreement (BSD License) #* From 5afe61976dd1ae68d50b5915a2f4d6f9fce672c3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:18:02 -0400 Subject: [PATCH 71/79] Remove test_depend that are exec_depend Signed-off-by: Shane Loretz --- tf2_py/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/tf2_py/package.xml b/tf2_py/package.xml index 6ec31907e..4886ad85b 100644 --- a/tf2_py/package.xml +++ b/tf2_py/package.xml @@ -24,8 +24,6 @@ ament_lint_auto ament_cmake_pytest - geometry_msgs - rclpy ament_cmake From 75d9e2eb66b1aff474c765a16daabb68fd2a1c15 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:33:04 -0400 Subject: [PATCH 72/79] Space between if and ( in new code Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index deb8b55ef..b3207c2a6 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -357,7 +357,7 @@ static tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg) static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) { - if(PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { + if (PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { PyObject *sec = pythonBorrowAttrString(obj, "sec"); PyObject *nanosec = pythonBorrowAttrString(obj, "nanosec"); builtin_interfaces::msg::Time msg; @@ -367,7 +367,7 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) return PyErr_Occurred() ? 0 : 1; } - if(PyObject_HasAttrString(obj, "nanoseconds")) { + if (PyObject_HasAttrString(obj, "nanoseconds")) { PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); const int64_t d = PyLong_AsLongLong(nanoseconds); const std::chrono::nanoseconds ns(d); @@ -381,7 +381,7 @@ static int rostime_converter(PyObject *obj, tf2::TimePoint *rt) static int rosduration_converter(PyObject *obj, tf2::Duration *rt) { - if(PyObject_HasAttrString(obj, "sec") && PyObject_HasAttrString(obj, "nanosec")) { + 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)) + @@ -389,7 +389,7 @@ static int rosduration_converter(PyObject *obj, tf2::Duration *rt) return PyErr_Occurred() ? 0 : 1; } - if(PyObject_HasAttrString(obj, "nanoseconds")) { + if (PyObject_HasAttrString(obj, "nanoseconds")) { PyObject *nanoseconds = pythonBorrowAttrString(obj, "nanoseconds"); const int64_t d = PyLong_AsLongLong(nanoseconds); const std::chrono::nanoseconds ns(d); @@ -914,25 +914,25 @@ bool staticInit() { pModulebuiltininterfacesmsgs = pythonImport("builtin_interfaces.msg"); pModulegeometrymsgs = pythonImport("geometry_msgs.msg"); - if(pModulerclpy == NULL) + if (pModulerclpy == NULL) { printf("Cannot load rclpy module"); return false; } - if(pModulerclpytime == NULL) + if (pModulerclpytime == NULL) { printf("Cannot load rclpy.time.Time module"); return false; } - if(pModulegeometrymsgs == NULL) + if (pModulegeometrymsgs == NULL) { printf("Cannot load geometry_msgs module"); return false; } - if(pModulebuiltininterfacesmsgs == NULL) + if (pModulebuiltininterfacesmsgs == NULL) { printf("Cannot load builtin_interfaces module"); return false; From c95f0d9f20bbd646b8f85bf95379aea8fd2b3fa5 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:53:47 -0400 Subject: [PATCH 73/79] Update TfListener __init__ docstr Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/transform_listener.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 8c880e6be..170b78dfc 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -44,14 +44,13 @@ class TransformListener: """ def __init__(self, buffer, node, *, spin_thread=False, qos=None, static_qos=None): """ - .. function:: __init__(buffer, node, spin_thread=True, qos=QoSProfile(depth=100)) + Constructor. - Constructor. - - :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 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( From c0a7156f81c6fee26c32feaa6d4c45a6f7bf6e42 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:56:18 -0400 Subject: [PATCH 74/79] Remove unnecessary str() Signed-off-by: Shane Loretz --- tf2_ros/src/tf2_ros/transform_listener.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/tf2_ros/transform_listener.py b/tf2_ros/src/tf2_ros/transform_listener.py index 170b78dfc..c7bbb15a8 100644 --- a/tf2_ros/src/tf2_ros/transform_listener.py +++ b/tf2_ros/src/tf2_ros/transform_listener.py @@ -100,11 +100,11 @@ def unregister(self): self.node.destroy_subscription(self.tf_static_sub) def callback(self, data): - who = str("default_authority") + who = 'default_authority' for transform in data.transforms: self.buffer.set_transform(transform, who) def static_callback(self, data): - who = str("default_authority") + who = 'default_authority' for transform in data.transforms: self.buffer.set_transform_static(transform, who) From cfaa623e07004de987eb80a7e979643b86bfff64 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 14:58:37 -0400 Subject: [PATCH 75/79] Add -Wpedantic Signed-off-by: Shane Loretz --- tf2_py/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 34a8f36eb..4c2d42b1f 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) From e737d33b32c0faa690b0d33b67196b4914876df7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 16:03:08 -0400 Subject: [PATCH 76/79] Big cleanup block with goto Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 292 +++++++++++++++--------------------------- 1 file changed, 105 insertions(+), 187 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index b3207c2a6..2d7f3dfd4 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -60,281 +60,199 @@ struct buffer_core_t { 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 *builtin_interfaces_time = PyObject_GetAttrString(pModulebuiltininterfacesmsgs, "Time"); + builtin_interfaces_time = PyObject_GetAttrString(pModulebuiltininterfacesmsgs, "Time"); if (!builtin_interfaces_time) { - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *args = PyTuple_New(0); + args = PyTuple_New(0); if (!args) { - Py_DECREF(builtin_interfaces_time); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *kwargs = PyDict_New(); + kwargs = PyDict_New(); if (!kwargs) { - Py_DECREF(args); - Py_DECREF(builtin_interfaces_time); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *sec = Py_BuildValue("i", transform->header.stamp.sec); + sec = Py_BuildValue("i", transform->header.stamp.sec); if (!sec) { - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(builtin_interfaces_time); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *nanosec = Py_BuildValue("i", transform->header.stamp.nanosec); + nanosec = Py_BuildValue("i", transform->header.stamp.nanosec); if (!nanosec) { - Py_DECREF(sec); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(builtin_interfaces_time); - Py_DECREF(pinst); - return NULL; + goto cleanup; } if (-1 == PyDict_SetItemString(kwargs, "sec", sec)) { - Py_DECREF(sec); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(builtin_interfaces_time); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(sec); if (-1 == PyDict_SetItemString(kwargs, "nanosec", nanosec)) { - Py_DECREF(sec); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(builtin_interfaces_time); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(nanosec); - PyObject *time_obj = PyObject_Call(builtin_interfaces_time, args, kwargs); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(builtin_interfaces_time); + time_obj = PyObject_Call(builtin_interfaces_time, args, kwargs); if (!time_obj) { - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject* pheader = PyObject_GetAttrString(pinst, "header"); + pheader = PyObject_GetAttrString(pinst, "header"); if (!pheader) { - Py_DECREF(time_obj); - Py_DECREF(pinst); - return NULL; + goto cleanup; } if (-1 == PyObject_SetAttrString(pheader, "stamp", time_obj)) { - Py_DECREF(time_obj); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(time_obj); - PyObject * pframe_id = stringToPython(transform->header.frame_id); + pframe_id = stringToPython(transform->header.frame_id); if (!pframe_id) { - Py_DECREF(pinst); - return NULL; + goto cleanup; } if (-1 == PyObject_SetAttrString(pheader, "frame_id", pframe_id)) { - Py_DECREF(pframe_id); - Py_DECREF(pheader); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(pframe_id); - Py_DECREF(pheader); - PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); + ptransform = PyObject_GetAttrString(pinst, "transform"); if (!ptransform) { - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); + ptranslation = PyObject_GetAttrString(ptransform, "translation"); if (!ptranslation) { - Py_DECREF(ptransform); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); + protation = PyObject_GetAttrString(ptransform, "rotation"); if (!ptranslation) { - Py_DECREF(ptranslation); - Py_DECREF(ptransform); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(ptransform); - PyObject *child_frame_id = stringToPython(transform->child_frame_id); + child_frame_id = stringToPython(transform->child_frame_id); if (!child_frame_id) { - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } if (-1 == PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id)) { - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject * ptrans_x = PyFloat_FromDouble(transform->transform.translation.x); + ptrans_x = PyFloat_FromDouble(transform->transform.translation.x); if (!ptrans_x) { - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject * ptrans_y = PyFloat_FromDouble(transform->transform.translation.y); + ptrans_y = PyFloat_FromDouble(transform->transform.translation.y); if (!ptrans_y) { - Py_DECREF(ptrans_x); - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject * ptrans_z = PyFloat_FromDouble(transform->transform.translation.z); + ptrans_z = PyFloat_FromDouble(transform->transform.translation.z); if (!ptrans_z) { - Py_DECREF(ptrans_y); - Py_DECREF(ptrans_x); - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } if (-1 == PyObject_SetAttrString(ptranslation, "x", ptrans_x)) { - Py_DECREF(ptrans_z); - Py_DECREF(ptrans_y); - Py_DECREF(ptrans_x); - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(ptrans_x); if (-1 == PyObject_SetAttrString(ptranslation, "y", ptrans_y)) { - Py_DECREF(ptrans_z); - Py_DECREF(ptrans_y); - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(ptrans_y); if (-1 == PyObject_SetAttrString(ptranslation, "z", ptrans_z)) { - Py_DECREF(ptrans_z); - Py_DECREF(ptranslation); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(ptrans_z); - Py_DECREF(ptranslation); - PyObject *prot_x = PyFloat_FromDouble(transform->transform.rotation.x); + prot_x = PyFloat_FromDouble(transform->transform.rotation.x); if (!prot_x) { - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *prot_y = PyFloat_FromDouble(transform->transform.rotation.y); + prot_y = PyFloat_FromDouble(transform->transform.rotation.y); if (!prot_y) { - Py_DECREF(prot_x); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *prot_z = PyFloat_FromDouble(transform->transform.rotation.z); + prot_z = PyFloat_FromDouble(transform->transform.rotation.z); if (!prot_z) { - Py_DECREF(prot_y); - Py_DECREF(prot_x); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - PyObject *prot_w = PyFloat_FromDouble(transform->transform.rotation.w); + prot_w = PyFloat_FromDouble(transform->transform.rotation.w); if (!prot_w) { - Py_DECREF(prot_z); - Py_DECREF(prot_y); - Py_DECREF(prot_x); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } if (-1 == PyObject_SetAttrString(protation, "x", prot_x)) { - Py_DECREF(prot_w); - Py_DECREF(prot_z); - Py_DECREF(prot_y); - Py_DECREF(prot_x); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(prot_x); if (-1 == PyObject_SetAttrString(protation, "y", prot_y)) { - Py_DECREF(prot_w); - Py_DECREF(prot_z); - Py_DECREF(prot_y); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(prot_y); if (-1 == PyObject_SetAttrString(protation, "z", prot_z)) { - Py_DECREF(prot_w); - Py_DECREF(prot_z); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; + goto cleanup; } - Py_DECREF(prot_z); if (-1 == PyObject_SetAttrString(protation, "w", prot_w)) { - Py_DECREF(prot_w); - Py_DECREF(protation); - Py_DECREF(pinst); - return NULL; - } - Py_DECREF(prot_w); - Py_DECREF(protation); - + 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; } From b40f5eea079a6547b7f730f38541a61417732cd0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 17:27:36 -0400 Subject: [PATCH 77/79] Cleanup cleanup in getLatestCommonTime() Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 108 ++++++++++++++++++++---------------------- 1 file changed, 52 insertions(+), 56 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index 2d7f3dfd4..ab40a2aa4 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -429,65 +429,61 @@ static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) WRAP(target_id = bc->_validateFrameId("get_latest_common_time", target_frame)); WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame)); const tf2::TF2Error r = bc->_getLatestCommonTime(target_id, source_id, tf2_time, &error_string); - if (r == tf2::TF2Error::NO_ERROR) { - PyObject *rclpy_time = PyObject_GetAttrString(pModulerclpytime, "Time"); - if (!rclpy_time) { - return NULL; - } - const builtin_interfaces::msg::Time time_msg = toMsg(tf2_time); - PyObject *args = PyTuple_New(0); - if (!args) { - Py_DECREF(rclpy_time); - return NULL; - } - PyObject *kwargs = PyDict_New(); - if (!kwargs) { - Py_DECREF(args); - Py_DECREF(rclpy_time); - return NULL; - } - PyObject *seconds = Py_BuildValue("i", time_msg.sec); - if (!seconds) { - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(rclpy_time); - return NULL; - } - PyObject *nanoseconds = Py_BuildValue("i", time_msg.nanosec); - if (!nanoseconds) { - Py_DECREF(seconds); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(rclpy_time); - return NULL; - } - if (0 != PyDict_SetItemString(kwargs, "seconds", seconds)) { - Py_DECREF(nanoseconds); - Py_DECREF(seconds); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(rclpy_time); - return NULL; - } - Py_DECREF(seconds); - if (0 != PyDict_SetItemString(kwargs, "nanoseconds", nanoseconds)) { - Py_DECREF(nanoseconds); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(rclpy_time); - return NULL; - } - Py_DECREF(nanoseconds); - - PyObject *ob = PyObject_Call(rclpy_time, args, kwargs); - Py_DECREF(kwargs); - Py_DECREF(args); - Py_DECREF(rclpy_time); - return ob; - } else { + + 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) From d380c38dc10068e261c1127204f9023aa61841da Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 13 Sep 2019 17:44:26 -0400 Subject: [PATCH 78/79] Check for null before using pointers Signed-off-by: Shane Loretz --- tf2_py/src/tf2_py.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index ab40a2aa4..e4deda75f 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -573,8 +573,11 @@ 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_XDECREF((PyObject*)translation_type); + Py_DECREF((PyObject*)translation_type); int attr_check = PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && PyObject_HasAttrString(o, "z"); @@ -588,8 +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_XDECREF((PyObject*)rotation_type); + Py_DECREF((PyObject*)rotation_type); int attr_check = PyObject_HasAttrString(o, "w") && PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && From c7a9c20e0479991fde85171ba06ecfc0bbe080f9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 16 Sep 2019 19:00:12 +0000 Subject: [PATCH 79/79] Minor style fixup in new code. Signed-off-by: Chris Lalancette --- tf2_py/src/tf2_py.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2_py/src/tf2_py.cpp b/tf2_py/src/tf2_py.cpp index e4deda75f..0253980ce 100644 --- a/tf2_py/src/tf2_py.cpp +++ b/tf2_py/src/tf2_py.cpp @@ -83,19 +83,19 @@ static PyObject *transform_converter(const geometry_msgs::msg::TransformStamped* PyObject * prot_z = NULL; PyObject * prot_w = NULL; pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped"); - if(!pclass) + if (!pclass) { goto cleanup; } pargs = Py_BuildValue("()"); - if(!pargs) + if (!pargs) { goto cleanup; } pinst = PyEval_CallObject(pclass, pargs); - if(!pinst) + if (!pinst) { goto cleanup; }