diff --git a/builder/test/tests.py b/builder/test/tests.py index 2af198529..d453d5059 100755 --- a/builder/test/tests.py +++ b/builder/test/tests.py @@ -22,6 +22,7 @@ from led_msgs.srv import SetLEDs from led_msgs.msg import LEDStateArray, LEDState from aruco_pose.msg import Marker, MarkerArray, Point2D +from clover import long_callback import dynamic_reconfigure.client diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index abfaa9c33..a2bfb3dc0 100644 --- a/clover/CMakeLists.txt +++ b/clover/CMakeLists.txt @@ -53,7 +53,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED ## 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() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## diff --git a/clover/setup.py b/clover/setup.py new file mode 100644 index 000000000..afdfcd709 --- /dev/null +++ b/clover/setup.py @@ -0,0 +1,11 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['clover'], + package_dir={'': 'src'}) + +setup(**setup_args) diff --git a/clover/src/clover/__init__.py b/clover/src/clover/__init__.py new file mode 100644 index 000000000..f61781be4 --- /dev/null +++ b/clover/src/clover/__init__.py @@ -0,0 +1,35 @@ +import rospy +from threading import Thread, Event + +def long_callback(fn): + """ + Decorator fixing a rospy issue for long-running topic callbacks, primarily + for image processing. + + See: https://github.com/ros/ros_comm/issues/1901. + + Usage example: + + @long_callback + def image_callback(msg): + # perform image processing + # ... + + rospy.Subscriber('main_camera/image_raw', Image, image_callback) + """ + e = Event() + + def thread(): + while not rospy.is_shutdown(): + e.wait() + e.clear() + fn(thread.current_msg) + + thread.current_msg = None + Thread(target=thread, daemon=True).start() + + def wrapper(msg): + thread.current_msg = msg + e.set() + + return wrapper diff --git a/clover/test/basic.py b/clover/test/basic.py index 21440bd2f..088d6cfed 100755 --- a/clover/test/basic.py +++ b/clover/test/basic.py @@ -3,6 +3,7 @@ import pytest from mavros_msgs.msg import State from clover import srv +import time @pytest.fixture() def node(): @@ -60,3 +61,18 @@ def wait_print(): t.join() assert wait_print.result == 'test' + +def test_long_callback(): + from clover import long_callback + from time import sleep + + # very basic test for long_callback + @long_callback + def cb(i): + cb.counter += i + cb.counter = 0 + cb(2) + sleep(0.1) + cb(3) + sleep(1) + assert cb.counter == 5