From 437dd3caad5cc8c9ae6ce500b09026c436088447 Mon Sep 17 00:00:00 2001 From: fengkaiwen01 Date: Tue, 10 Oct 2017 11:24:13 +0800 Subject: [PATCH] add image_common, audio_common and vision_opencv, refer to ros-indigo --- ros/audio_common/.gitignore | 1 + ros/audio_common/audio_capture/.gitignore | 1 + ros/audio_common/audio_capture/CHANGELOG.rst | 83 ++ ros/audio_common/audio_capture/CMakeLists.txt | 24 + .../audio_capture/launch/capture.launch | 7 + .../launch/capture_to_file.launch | 8 + .../audio_capture/launch/capture_wave.launch | 9 + ros/audio_common/audio_capture/mainpage.dox | 21 + ros/audio_common/audio_capture/package.xml | 29 + .../audio_capture/src/audio_capture.cpp | 186 +++++ ros/audio_common/audio_common/CHANGELOG.rst | 52 ++ ros/audio_common/audio_common/CMakeLists.txt | 4 + ros/audio_common/audio_common/package.xml | 24 + ros/audio_common/audio_common_msgs/.gitignore | 1 + .../audio_common_msgs/CHANGELOG.rst | 53 ++ .../audio_common_msgs/CMakeLists.txt | 9 + .../audio_common_msgs/mainpage.dox | 11 + .../audio_common_msgs/msg/AudioData.msg | 1 + .../audio_common_msgs/package.xml | 19 + ros/audio_common/audio_play/.gitignore | 1 + ros/audio_common/audio_play/CHANGELOG.rst | 74 ++ ros/audio_common/audio_play/CMakeLists.txt | 24 + .../audio_play/launch/play.launch | 7 + ros/audio_common/audio_play/mainpage.dox | 22 + ros/audio_common/audio_play/package.xml | 30 + .../audio_play/src/audio_play.cpp | 153 ++++ ros/audio_common/sound_play/CHANGELOG.rst | 167 ++++ ros/audio_common/sound_play/CMakeLists.txt | 44 ++ ros/audio_common/sound_play/README | 41 + .../sound_play/action/SoundRequest.action | 7 + .../include/sound_play/sound_play.h | 418 ++++++++++ ros/audio_common/sound_play/mainpage.dox | 23 + .../sound_play/msg/SoundRequest.msg | 27 + ros/audio_common/sound_play/package.xml | 43 ++ ros/audio_common/sound_play/scripts/play.py | 60 ++ .../sound_play/scripts/playbuiltin.py | 65 ++ .../sound_play/scripts/playpackage.py | 60 ++ ros/audio_common/sound_play/scripts/say.py | 79 ++ ros/audio_common/sound_play/scripts/shutup.py | 59 ++ .../sound_play/scripts/soundclient_example.py | 91 +++ .../sound_play/scripts/soundplay_node.py | 446 +++++++++++ ros/audio_common/sound_play/scripts/test.py | 106 +++ .../scripts/test/test_sound_client.py | 17 + .../scripts/test_actionlib_client.py | 60 ++ ros/audio_common/sound_play/setup.py | 11 + .../sound_play/soundplay_node.launch | 8 + .../sound_play/sounds/BACKINGUP.ogg | Bin 0 -> 12861 bytes .../sound_play/sounds/NEEDS_PLUGGING.ogg | Bin 0 -> 11050 bytes .../sounds/NEEDS_PLUGGING_BADLY.ogg | Bin 0 -> 35782 bytes .../sound_play/sounds/NEEDS_UNPLUGGING.ogg | Bin 0 -> 35535 bytes .../sounds/NEEDS_UNPLUGGING_BADLY.ogg | Bin 0 -> 78198 bytes .../sound_play/sounds/say-beep.wav | Bin 0 -> 4058 bytes .../sound_play/src/sound_play/__init__.py | 33 + .../sound_play/src/sound_play/libsoundplay.py | 343 +++++++++ ros/audio_common/sound_play/test.launch | 9 + .../sound_play/test/CMakeLists.txt | 4 + ros/audio_common/sound_play/test/test.cpp | 102 +++ .../camera_calibration_parsers/CHANGELOG.rst | 76 ++ .../camera_calibration_parsers/CMakeLists.txt | 83 ++ .../camera_calibration_parsers/parse.h | 81 ++ .../camera_calibration_parsers/parse_ini.h | 96 +++ .../parse_wrapper.h | 44 ++ .../camera_calibration_parsers/parse_yml.h | 86 +++ .../camera_calibration_parsers/mainpage.dox | 15 + .../camera_calibration_parsers/package.xml | 34 + .../camera_calibration_parsers/setup.py | 11 + .../camera_calibration_parsers/__init__.py | 17 + .../src/convert.cpp | 62 ++ .../camera_calibration_parsers/src/parse.cpp | 74 ++ .../src/parse_ini.cpp | 248 ++++++ .../src/parse_wrapper.cpp | 74 ++ .../src/parse_yml.cpp | 240 ++++++ .../test/CMakeLists.txt | 3 + .../test/calib5.ini | 27 + .../test/calib8.ini | 27 + .../camera_calibration_parsers/test/parser.py | 61 ++ .../camera_info_manager/CHANGELOG.rst | 210 +++++ .../camera_info_manager/CMakeLists.txt | 37 + .../camera_info_manager/camera_info_manager.h | 243 ++++++ .../camera_info_manager/mainpage.dox | 9 + .../camera_info_manager/package.xml | 40 + .../src/camera_info_manager.cpp | 659 ++++++++++++++++ .../tests/test_calibration.yaml | 20 + .../camera_info_manager/tests/unit_test.cpp | 719 ++++++++++++++++++ .../camera_info_manager/tests/unit_test.test | 17 + ros/image_common/image_common/CHANGELOG.rst | 139 ++++ ros/image_common/image_common/CMakeLists.txt | 4 + ros/image_common/image_common/package.xml | 27 + .../image_transport/CHANGELOG.rst | 193 +++++ .../image_transport/CMakeLists.txt | 69 ++ .../image_transport/default_plugins.xml | 13 + .../include/image_transport/camera_common.h | 47 ++ .../image_transport/camera_publisher.h | 135 ++++ .../image_transport/camera_subscriber.h | 118 +++ .../include/image_transport/exception.h | 71 ++ .../include/image_transport/image_transport.h | 204 +++++ .../include/image_transport/loader_fwds.h | 56 ++ .../include/image_transport/publisher.h | 125 +++ .../image_transport/publisher_plugin.h | 150 ++++ .../include/image_transport/raw_publisher.h | 82 ++ .../include/image_transport/raw_subscriber.h | 72 ++ .../image_transport/simple_publisher_plugin.h | 240 ++++++ .../simple_subscriber_plugin.h | 141 ++++ .../single_subscriber_publisher.h | 80 ++ .../include/image_transport/subscriber.h | 111 +++ .../image_transport/subscriber_filter.h | 163 ++++ .../image_transport/subscriber_plugin.h | 141 ++++ .../include/image_transport/transport_hints.h | 94 +++ ros/image_common/image_transport/mainpage.dox | 45 ++ ros/image_common/image_transport/package.xml | 39 + .../image_transport/src/camera_common.cpp | 58 ++ .../image_transport/src/camera_publisher.cpp | 160 ++++ .../image_transport/src/camera_subscriber.cpp | 160 ++++ .../image_transport/src/image_transport.cpp | 148 ++++ .../image_transport/src/list_transports.cpp | 141 ++++ .../image_transport/src/manifest.cpp | 41 + .../image_transport/src/publisher.cpp | 203 +++++ .../image_transport/src/raw_publisher.cpp | 140 ++++ .../image_transport/src/republish.cpp | 86 +++ .../src/single_subscriber_publisher.cpp | 74 ++ .../image_transport/src/subscriber.cpp | 142 ++++ .../image_transport/tutorial/CMakeLists.txt | 43 ++ .../resized_publisher.h | 15 + .../resized_subscriber.h | 17 + .../tutorial/msg/ResizedImage.msg | 3 + .../image_transport/tutorial/package.xml | 26 + .../tutorial/resized_plugins.xml | 13 + .../image_transport/tutorial/src/manifest.cpp | 8 + .../tutorial/src/my_publisher.cpp | 23 + .../tutorial/src/my_subscriber.cpp | 28 + .../tutorial/src/resized_publisher.cpp | 37 + .../tutorial/src/resized_subscriber.cpp | 18 + ros/image_common/polled_camera/CHANGELOG.rst | 186 +++++ ros/image_common/polled_camera/CMakeLists.txt | 43 ++ .../polled_camera/publication_server.h | 136 ++++ ros/image_common/polled_camera/mainpage.dox | 48 ++ ros/image_common/polled_camera/package.xml | 33 + ros/image_common/polled_camera/src/poller.cpp | 68 ++ .../polled_camera/src/publication_server.cpp | 161 ++++ .../polled_camera/srv/GetPolledImage.srv | 22 + ros/vision_opencv/README.rst | 7 + ros/vision_opencv/cv_bridge/CHANGELOG.rst | 369 +++++++++ ros/vision_opencv/cv_bridge/CMakeLists.txt | 42 + .../cv_bridge/cmake/cv_bridge-extras.cmake.in | 12 + ros/vision_opencv/cv_bridge/doc/conf.py | 201 +++++ ros/vision_opencv/cv_bridge/doc/index.rst | 18 + ros/vision_opencv/cv_bridge/doc/mainpage.dox | 14 + .../cv_bridge/include/cv_bridge/cv_bridge.h | 429 +++++++++++ .../cv_bridge/include/cv_bridge/rgb_colors.h | 211 +++++ ros/vision_opencv/cv_bridge/package.xml | 40 + .../cv_bridge/python/CMakeLists.txt | 8 + .../cv_bridge/python/__init__.py.plain.in | 0 .../cv_bridge/python/cv_bridge/__init__.py | 8 + .../cv_bridge/python/cv_bridge/core.py | 266 +++++++ ros/vision_opencv/cv_bridge/rosdoc.yaml | 8 + ros/vision_opencv/cv_bridge/setup.py | 10 + .../cv_bridge/src/CMakeLists.txt | 56 ++ ros/vision_opencv/cv_bridge/src/boost/README | 2 + .../cv_bridge/src/boost/core/scoped_enum.hpp | 192 +++++ .../cv_bridge/src/boost/endian/conversion.hpp | 488 ++++++++++++ .../src/boost/endian/detail/intrinsic.hpp | 64 ++ .../src/boost/predef/detail/_cassert.h | 17 + .../src/boost/predef/detail/endian_compat.h | 26 + .../cv_bridge/src/boost/predef/detail/test.h | 17 + .../src/boost/predef/library/c/_prefix.h | 13 + .../src/boost/predef/library/c/gnu.h | 61 ++ .../cv_bridge/src/boost/predef/make.h | 89 +++ .../cv_bridge/src/boost/predef/os/android.h | 45 ++ .../cv_bridge/src/boost/predef/os/bsd.h | 103 +++ .../cv_bridge/src/boost/predef/os/bsd/bsdi.h | 48 ++ .../src/boost/predef/os/bsd/dragonfly.h | 50 ++ .../cv_bridge/src/boost/predef/os/bsd/free.h | 60 ++ .../cv_bridge/src/boost/predef/os/bsd/net.h | 84 ++ .../cv_bridge/src/boost/predef/os/bsd/open.h | 171 +++++ .../cv_bridge/src/boost/predef/os/ios.h | 51 ++ .../cv_bridge/src/boost/predef/os/macos.h | 65 ++ .../cv_bridge/src/boost/predef/other/endian.h | 204 +++++ .../src/boost/predef/version_number.h | 53 ++ ros/vision_opencv/cv_bridge/src/cv_bridge.cpp | 700 +++++++++++++++++ ros/vision_opencv/cv_bridge/src/module.cpp | 110 +++ ros/vision_opencv/cv_bridge/src/module.hpp | 48 ++ .../cv_bridge/src/module_opencv2.cpp | 262 +++++++ .../cv_bridge/src/module_opencv3.cpp | 367 +++++++++ ros/vision_opencv/cv_bridge/src/pycompat.hpp | 70 ++ .../cv_bridge/src/rgb_colors.cpp | 202 +++++ .../cv_bridge/test/CMakeLists.txt | 15 + .../cv_bridge/test/conversions.py | 85 +++ .../cv_bridge/test/enumerants.py | 47 ++ .../cv_bridge/test/python_bindings.py | 35 + .../cv_bridge/test/test_compression.cpp | 36 + .../cv_bridge/test/test_endian.cpp | 38 + .../cv_bridge/test/test_rgb_colors.cpp | 19 + ros/vision_opencv/cv_bridge/test/utest.cpp | 112 +++ ros/vision_opencv/cv_bridge/test/utest2.cpp | 150 ++++ .../image_geometry/CHANGELOG.rst | 320 ++++++++ .../image_geometry/CMakeLists.txt | 36 + ros/vision_opencv/image_geometry/doc/conf.py | 201 +++++ .../image_geometry/doc/index.rst | 21 + .../image_geometry/doc/mainpage.dox | 29 + .../image_geometry/pinhole_camera_model.h | 338 ++++++++ .../image_geometry/stereo_camera_model.h | 130 ++++ ros/vision_opencv/image_geometry/package.xml | 31 + ros/vision_opencv/image_geometry/rosdoc.yaml | 8 + ros/vision_opencv/image_geometry/setup.py | 10 + .../src/image_geometry/__init__.py | 2 + .../src/image_geometry/cameramodels.py | 373 +++++++++ .../src/pinhole_camera_model.cpp | 478 ++++++++++++ .../src/stereo_camera_model.cpp | 140 ++++ .../image_geometry/test/CMakeLists.txt | 4 + .../image_geometry/test/directed.py | 75 ++ .../image_geometry/test/utest.cpp | 259 +++++++ ros/vision_opencv/opencv_tests/CHANGELOG.rst | 257 +++++++ ros/vision_opencv/opencv_tests/CMakeLists.txt | 5 + .../opencv_tests/launch/pong.launch | 5 + ros/vision_opencv/opencv_tests/mainpage.dox | 119 +++ .../opencv_tests/nodes/broadcast.py | 78 ++ .../opencv_tests/nodes/rosfacedetect.py | 105 +++ .../opencv_tests/nodes/source.py | 95 +++ ros/vision_opencv/opencv_tests/package.xml | 15 + ros/vision_opencv/vision_opencv/CHANGELOG.rst | 165 ++++ .../vision_opencv/CMakeLists.txt | 4 + ros/vision_opencv/vision_opencv/package.xml | 23 + 222 files changed, 20554 insertions(+) create mode 100644 ros/audio_common/.gitignore create mode 100644 ros/audio_common/audio_capture/.gitignore create mode 100644 ros/audio_common/audio_capture/CHANGELOG.rst create mode 100644 ros/audio_common/audio_capture/CMakeLists.txt create mode 100644 ros/audio_common/audio_capture/launch/capture.launch create mode 100644 ros/audio_common/audio_capture/launch/capture_to_file.launch create mode 100644 ros/audio_common/audio_capture/launch/capture_wave.launch create mode 100644 ros/audio_common/audio_capture/mainpage.dox create mode 100644 ros/audio_common/audio_capture/package.xml create mode 100644 ros/audio_common/audio_capture/src/audio_capture.cpp create mode 100644 ros/audio_common/audio_common/CHANGELOG.rst create mode 100644 ros/audio_common/audio_common/CMakeLists.txt create mode 100644 ros/audio_common/audio_common/package.xml create mode 100644 ros/audio_common/audio_common_msgs/.gitignore create mode 100644 ros/audio_common/audio_common_msgs/CHANGELOG.rst create mode 100644 ros/audio_common/audio_common_msgs/CMakeLists.txt create mode 100644 ros/audio_common/audio_common_msgs/mainpage.dox create mode 100644 ros/audio_common/audio_common_msgs/msg/AudioData.msg create mode 100644 ros/audio_common/audio_common_msgs/package.xml create mode 100644 ros/audio_common/audio_play/.gitignore create mode 100644 ros/audio_common/audio_play/CHANGELOG.rst create mode 100644 ros/audio_common/audio_play/CMakeLists.txt create mode 100644 ros/audio_common/audio_play/launch/play.launch create mode 100644 ros/audio_common/audio_play/mainpage.dox create mode 100644 ros/audio_common/audio_play/package.xml create mode 100644 ros/audio_common/audio_play/src/audio_play.cpp create mode 100644 ros/audio_common/sound_play/CHANGELOG.rst create mode 100644 ros/audio_common/sound_play/CMakeLists.txt create mode 100644 ros/audio_common/sound_play/README create mode 100644 ros/audio_common/sound_play/action/SoundRequest.action create mode 100644 ros/audio_common/sound_play/include/sound_play/sound_play.h create mode 100644 ros/audio_common/sound_play/mainpage.dox create mode 100644 ros/audio_common/sound_play/msg/SoundRequest.msg create mode 100644 ros/audio_common/sound_play/package.xml create mode 100755 ros/audio_common/sound_play/scripts/play.py create mode 100755 ros/audio_common/sound_play/scripts/playbuiltin.py create mode 100755 ros/audio_common/sound_play/scripts/playpackage.py create mode 100755 ros/audio_common/sound_play/scripts/say.py create mode 100755 ros/audio_common/sound_play/scripts/shutup.py create mode 100755 ros/audio_common/sound_play/scripts/soundclient_example.py create mode 100755 ros/audio_common/sound_play/scripts/soundplay_node.py create mode 100755 ros/audio_common/sound_play/scripts/test.py create mode 100644 ros/audio_common/sound_play/scripts/test/test_sound_client.py create mode 100755 ros/audio_common/sound_play/scripts/test_actionlib_client.py create mode 100644 ros/audio_common/sound_play/setup.py create mode 100644 ros/audio_common/sound_play/soundplay_node.launch create mode 100644 ros/audio_common/sound_play/sounds/BACKINGUP.ogg create mode 100644 ros/audio_common/sound_play/sounds/NEEDS_PLUGGING.ogg create mode 100644 ros/audio_common/sound_play/sounds/NEEDS_PLUGGING_BADLY.ogg create mode 100644 ros/audio_common/sound_play/sounds/NEEDS_UNPLUGGING.ogg create mode 100644 ros/audio_common/sound_play/sounds/NEEDS_UNPLUGGING_BADLY.ogg create mode 100644 ros/audio_common/sound_play/sounds/say-beep.wav create mode 100644 ros/audio_common/sound_play/src/sound_play/__init__.py create mode 100644 ros/audio_common/sound_play/src/sound_play/libsoundplay.py create mode 100644 ros/audio_common/sound_play/test.launch create mode 100644 ros/audio_common/sound_play/test/CMakeLists.txt create mode 100644 ros/audio_common/sound_play/test/test.cpp create mode 100644 ros/image_common/camera_calibration_parsers/CHANGELOG.rst create mode 100644 ros/image_common/camera_calibration_parsers/CMakeLists.txt create mode 100644 ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse.h create mode 100644 ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.h create mode 100644 ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_wrapper.h create mode 100644 ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.h create mode 100644 ros/image_common/camera_calibration_parsers/mainpage.dox create mode 100644 ros/image_common/camera_calibration_parsers/package.xml create mode 100644 ros/image_common/camera_calibration_parsers/setup.py create mode 100644 ros/image_common/camera_calibration_parsers/src/camera_calibration_parsers/__init__.py create mode 100644 ros/image_common/camera_calibration_parsers/src/convert.cpp create mode 100644 ros/image_common/camera_calibration_parsers/src/parse.cpp create mode 100644 ros/image_common/camera_calibration_parsers/src/parse_ini.cpp create mode 100644 ros/image_common/camera_calibration_parsers/src/parse_wrapper.cpp create mode 100644 ros/image_common/camera_calibration_parsers/src/parse_yml.cpp create mode 100644 ros/image_common/camera_calibration_parsers/test/CMakeLists.txt create mode 100644 ros/image_common/camera_calibration_parsers/test/calib5.ini create mode 100644 ros/image_common/camera_calibration_parsers/test/calib8.ini create mode 100644 ros/image_common/camera_calibration_parsers/test/parser.py create mode 100644 ros/image_common/camera_info_manager/CHANGELOG.rst create mode 100644 ros/image_common/camera_info_manager/CMakeLists.txt create mode 100644 ros/image_common/camera_info_manager/include/camera_info_manager/camera_info_manager.h create mode 100644 ros/image_common/camera_info_manager/mainpage.dox create mode 100644 ros/image_common/camera_info_manager/package.xml create mode 100644 ros/image_common/camera_info_manager/src/camera_info_manager.cpp create mode 100644 ros/image_common/camera_info_manager/tests/test_calibration.yaml create mode 100644 ros/image_common/camera_info_manager/tests/unit_test.cpp create mode 100644 ros/image_common/camera_info_manager/tests/unit_test.test create mode 100644 ros/image_common/image_common/CHANGELOG.rst create mode 100644 ros/image_common/image_common/CMakeLists.txt create mode 100644 ros/image_common/image_common/package.xml create mode 100644 ros/image_common/image_transport/CHANGELOG.rst create mode 100644 ros/image_common/image_transport/CMakeLists.txt create mode 100644 ros/image_common/image_transport/default_plugins.xml create mode 100644 ros/image_common/image_transport/include/image_transport/camera_common.h create mode 100644 ros/image_common/image_transport/include/image_transport/camera_publisher.h create mode 100644 ros/image_common/image_transport/include/image_transport/camera_subscriber.h create mode 100644 ros/image_common/image_transport/include/image_transport/exception.h create mode 100644 ros/image_common/image_transport/include/image_transport/image_transport.h create mode 100644 ros/image_common/image_transport/include/image_transport/loader_fwds.h create mode 100644 ros/image_common/image_transport/include/image_transport/publisher.h create mode 100644 ros/image_common/image_transport/include/image_transport/publisher_plugin.h create mode 100644 ros/image_common/image_transport/include/image_transport/raw_publisher.h create mode 100644 ros/image_common/image_transport/include/image_transport/raw_subscriber.h create mode 100644 ros/image_common/image_transport/include/image_transport/simple_publisher_plugin.h create mode 100644 ros/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h create mode 100644 ros/image_common/image_transport/include/image_transport/single_subscriber_publisher.h create mode 100644 ros/image_common/image_transport/include/image_transport/subscriber.h create mode 100644 ros/image_common/image_transport/include/image_transport/subscriber_filter.h create mode 100644 ros/image_common/image_transport/include/image_transport/subscriber_plugin.h create mode 100644 ros/image_common/image_transport/include/image_transport/transport_hints.h create mode 100644 ros/image_common/image_transport/mainpage.dox create mode 100644 ros/image_common/image_transport/package.xml create mode 100644 ros/image_common/image_transport/src/camera_common.cpp create mode 100644 ros/image_common/image_transport/src/camera_publisher.cpp create mode 100644 ros/image_common/image_transport/src/camera_subscriber.cpp create mode 100644 ros/image_common/image_transport/src/image_transport.cpp create mode 100644 ros/image_common/image_transport/src/list_transports.cpp create mode 100644 ros/image_common/image_transport/src/manifest.cpp create mode 100644 ros/image_common/image_transport/src/publisher.cpp create mode 100644 ros/image_common/image_transport/src/raw_publisher.cpp create mode 100644 ros/image_common/image_transport/src/republish.cpp create mode 100644 ros/image_common/image_transport/src/single_subscriber_publisher.cpp create mode 100644 ros/image_common/image_transport/src/subscriber.cpp create mode 100644 ros/image_common/image_transport/tutorial/CMakeLists.txt create mode 100644 ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h create mode 100644 ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h create mode 100644 ros/image_common/image_transport/tutorial/msg/ResizedImage.msg create mode 100644 ros/image_common/image_transport/tutorial/package.xml create mode 100644 ros/image_common/image_transport/tutorial/resized_plugins.xml create mode 100644 ros/image_common/image_transport/tutorial/src/manifest.cpp create mode 100644 ros/image_common/image_transport/tutorial/src/my_publisher.cpp create mode 100644 ros/image_common/image_transport/tutorial/src/my_subscriber.cpp create mode 100644 ros/image_common/image_transport/tutorial/src/resized_publisher.cpp create mode 100644 ros/image_common/image_transport/tutorial/src/resized_subscriber.cpp create mode 100644 ros/image_common/polled_camera/CHANGELOG.rst create mode 100644 ros/image_common/polled_camera/CMakeLists.txt create mode 100644 ros/image_common/polled_camera/include/polled_camera/publication_server.h create mode 100644 ros/image_common/polled_camera/mainpage.dox create mode 100644 ros/image_common/polled_camera/package.xml create mode 100644 ros/image_common/polled_camera/src/poller.cpp create mode 100644 ros/image_common/polled_camera/src/publication_server.cpp create mode 100644 ros/image_common/polled_camera/srv/GetPolledImage.srv create mode 100644 ros/vision_opencv/README.rst create mode 100644 ros/vision_opencv/cv_bridge/CHANGELOG.rst create mode 100644 ros/vision_opencv/cv_bridge/CMakeLists.txt create mode 100644 ros/vision_opencv/cv_bridge/cmake/cv_bridge-extras.cmake.in create mode 100644 ros/vision_opencv/cv_bridge/doc/conf.py create mode 100644 ros/vision_opencv/cv_bridge/doc/index.rst create mode 100644 ros/vision_opencv/cv_bridge/doc/mainpage.dox create mode 100644 ros/vision_opencv/cv_bridge/include/cv_bridge/cv_bridge.h create mode 100644 ros/vision_opencv/cv_bridge/include/cv_bridge/rgb_colors.h create mode 100644 ros/vision_opencv/cv_bridge/package.xml create mode 100644 ros/vision_opencv/cv_bridge/python/CMakeLists.txt create mode 100644 ros/vision_opencv/cv_bridge/python/__init__.py.plain.in create mode 100644 ros/vision_opencv/cv_bridge/python/cv_bridge/__init__.py create mode 100644 ros/vision_opencv/cv_bridge/python/cv_bridge/core.py create mode 100644 ros/vision_opencv/cv_bridge/rosdoc.yaml create mode 100644 ros/vision_opencv/cv_bridge/setup.py create mode 100644 ros/vision_opencv/cv_bridge/src/CMakeLists.txt create mode 100644 ros/vision_opencv/cv_bridge/src/boost/README create mode 100644 ros/vision_opencv/cv_bridge/src/boost/core/scoped_enum.hpp create mode 100644 ros/vision_opencv/cv_bridge/src/boost/endian/conversion.hpp create mode 100644 ros/vision_opencv/cv_bridge/src/boost/endian/detail/intrinsic.hpp create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/detail/_cassert.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/detail/endian_compat.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/detail/test.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/library/c/_prefix.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/library/c/gnu.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/make.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/android.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/bsdi.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/dragonfly.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/free.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/net.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/open.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/ios.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/os/macos.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/other/endian.h create mode 100644 ros/vision_opencv/cv_bridge/src/boost/predef/version_number.h create mode 100644 ros/vision_opencv/cv_bridge/src/cv_bridge.cpp create mode 100644 ros/vision_opencv/cv_bridge/src/module.cpp create mode 100644 ros/vision_opencv/cv_bridge/src/module.hpp create mode 100644 ros/vision_opencv/cv_bridge/src/module_opencv2.cpp create mode 100644 ros/vision_opencv/cv_bridge/src/module_opencv3.cpp create mode 100644 ros/vision_opencv/cv_bridge/src/pycompat.hpp create mode 100644 ros/vision_opencv/cv_bridge/src/rgb_colors.cpp create mode 100644 ros/vision_opencv/cv_bridge/test/CMakeLists.txt create mode 100644 ros/vision_opencv/cv_bridge/test/conversions.py create mode 100644 ros/vision_opencv/cv_bridge/test/enumerants.py create mode 100644 ros/vision_opencv/cv_bridge/test/python_bindings.py create mode 100644 ros/vision_opencv/cv_bridge/test/test_compression.cpp create mode 100644 ros/vision_opencv/cv_bridge/test/test_endian.cpp create mode 100644 ros/vision_opencv/cv_bridge/test/test_rgb_colors.cpp create mode 100644 ros/vision_opencv/cv_bridge/test/utest.cpp create mode 100644 ros/vision_opencv/cv_bridge/test/utest2.cpp create mode 100644 ros/vision_opencv/image_geometry/CHANGELOG.rst create mode 100644 ros/vision_opencv/image_geometry/CMakeLists.txt create mode 100644 ros/vision_opencv/image_geometry/doc/conf.py create mode 100644 ros/vision_opencv/image_geometry/doc/index.rst create mode 100644 ros/vision_opencv/image_geometry/doc/mainpage.dox create mode 100644 ros/vision_opencv/image_geometry/include/image_geometry/pinhole_camera_model.h create mode 100644 ros/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h create mode 100644 ros/vision_opencv/image_geometry/package.xml create mode 100644 ros/vision_opencv/image_geometry/rosdoc.yaml create mode 100644 ros/vision_opencv/image_geometry/setup.py create mode 100644 ros/vision_opencv/image_geometry/src/image_geometry/__init__.py create mode 100644 ros/vision_opencv/image_geometry/src/image_geometry/cameramodels.py create mode 100644 ros/vision_opencv/image_geometry/src/pinhole_camera_model.cpp create mode 100644 ros/vision_opencv/image_geometry/src/stereo_camera_model.cpp create mode 100644 ros/vision_opencv/image_geometry/test/CMakeLists.txt create mode 100644 ros/vision_opencv/image_geometry/test/directed.py create mode 100644 ros/vision_opencv/image_geometry/test/utest.cpp create mode 100644 ros/vision_opencv/opencv_tests/CHANGELOG.rst create mode 100644 ros/vision_opencv/opencv_tests/CMakeLists.txt create mode 100644 ros/vision_opencv/opencv_tests/launch/pong.launch create mode 100644 ros/vision_opencv/opencv_tests/mainpage.dox create mode 100755 ros/vision_opencv/opencv_tests/nodes/broadcast.py create mode 100755 ros/vision_opencv/opencv_tests/nodes/rosfacedetect.py create mode 100755 ros/vision_opencv/opencv_tests/nodes/source.py create mode 100644 ros/vision_opencv/opencv_tests/package.xml create mode 100644 ros/vision_opencv/vision_opencv/CHANGELOG.rst create mode 100644 ros/vision_opencv/vision_opencv/CMakeLists.txt create mode 100644 ros/vision_opencv/vision_opencv/package.xml diff --git a/ros/audio_common/.gitignore b/ros/audio_common/.gitignore new file mode 100644 index 0000000..0d20b64 --- /dev/null +++ b/ros/audio_common/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/ros/audio_common/audio_capture/.gitignore b/ros/audio_common/audio_capture/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/ros/audio_common/audio_capture/.gitignore @@ -0,0 +1 @@ +build diff --git a/ros/audio_common/audio_capture/CHANGELOG.rst b/ros/audio_common/audio_capture/CHANGELOG.rst new file mode 100644 index 0000000..8d0100c --- /dev/null +++ b/ros/audio_common/audio_capture/CHANGELOG.rst @@ -0,0 +1,83 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package audio_capture +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.12 (2016-02-29) +------------------- + +0.2.11 (2016-02-16) +------------------- + +0.2.10 (2016-01-21) +------------------- + +0.2.9 (2015-12-02) +------------------ +* [audio_capture] add error handler +* [audio_capture] add option to publish captured audio data as wav format +* Fixed memory leak (see `#18 `_). +* Removed trailing whitespace. +* Contributors: Felix Duvallet, Furushchev + +0.2.8 (2015-10-02) +------------------ +* Update maintainer email +* Contributors: trainman419 + +0.2.7 (2014-07-25) +------------------ +* audio_capture.cpp has to wait for generated AudioData headers +* Contributors: v4hn + +0.2.6 (2014-02-26) +------------------ +* audio_capture and play _require\_ gstreamer, it's not optional +* Contributors: v4hn + +0.2.5 (2014-01-23) +------------------ +* "0.2.5" +* Contributors: trainman419 + +0.2.4 (2013-09-10) +------------------ +* Update CMakeLists.txt +* audio_capture: install launchfiles +* Contributors: David Gossow + +0.2.3 (2013-07-15) +------------------ +* Fix install rule for audio_capture. +* Contributors: Austin Hendrix + +0.2.2 (2013-04-10) +------------------ + +0.2.1 (2013-04-08 13:59) +------------------------ + +0.2.0 (2013-04-08 13:49) +------------------------ +* Finish catkinizing audio_common. +* Catkinize audio_play. +* Catkinize audio_capture. +* Fix typo in package.xml +* Versions and more URLs. +* Convert manifests to package.xml +* Convert audio_capture manifest to package.xml +* Ditch old makefiles. +* Updates manifest +* Updated manifests for rodep2 +* oneiric build fixes, bump version to 0.1.6 +* Removed redundant thread::thread +* Added a rosdep.yaml file +* Fixed to use audio_common_msgs +* Added ability to use different festival voices +* Updated documentation +* Added ability to capture to file +* Fixed ignore files +* Added hgignore files +* Audio_capture and audio_play working +* Making separate audio_capture and audio_play packages +* Moved audio_transport to audio_capture +* Contributors: Austin Hendrix, Brian Gerkey, Nate Koenig, nkoenig diff --git a/ros/audio_common/audio_capture/CMakeLists.txt b/ros/audio_common/audio_capture/CMakeLists.txt new file mode 100644 index 0000000..939335a --- /dev/null +++ b/ros/audio_common/audio_capture/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(audio_capture) + +find_package(catkin REQUIRED COMPONENTS roscpp audio_common_msgs) + +find_package(PkgConfig) +pkg_check_modules(GST gstreamer-0.10 REQUIRED) + +find_package(Boost REQUIRED COMPONENTS thread) + +include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GST_INCLUDE_DIRS}) + +catkin_package() + +add_executable(audio_capture src/audio_capture.cpp) +target_link_libraries(audio_capture ${catkin_LIBRARIES} ${GST_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(audio_capture ${catkin_EXPORTED_TARGETS}) + +install(TARGETS audio_capture + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/ros/audio_common/audio_capture/launch/capture.launch b/ros/audio_common/audio_capture/launch/capture.launch new file mode 100644 index 0000000..b409784 --- /dev/null +++ b/ros/audio_common/audio_capture/launch/capture.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ros/audio_common/audio_capture/launch/capture_to_file.launch b/ros/audio_common/audio_capture/launch/capture_to_file.launch new file mode 100644 index 0000000..eeb02aa --- /dev/null +++ b/ros/audio_common/audio_capture/launch/capture_to_file.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ros/audio_common/audio_capture/launch/capture_wave.launch b/ros/audio_common/audio_capture/launch/capture_wave.launch new file mode 100644 index 0000000..43c621d --- /dev/null +++ b/ros/audio_common/audio_capture/launch/capture_wave.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/ros/audio_common/audio_capture/mainpage.dox b/ros/audio_common/audio_capture/mainpage.dox new file mode 100644 index 0000000..4733f76 --- /dev/null +++ b/ros/audio_common/audio_capture/mainpage.dox @@ -0,0 +1,21 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b audio_capture is a package that records audio from a microphone and makes it available to other ROS nodes. + +\section codeapi Code API + + + + +*/ diff --git a/ros/audio_common/audio_capture/package.xml b/ros/audio_common/audio_capture/package.xml new file mode 100644 index 0000000..57d92c0 --- /dev/null +++ b/ros/audio_common/audio_capture/package.xml @@ -0,0 +1,29 @@ + + audio_capture + 0.2.12 + + Transports audio from a source to a destination. Audio sources can come + from a microphone or file. The destination can play the audio or save it + to an mp3 file. + + Austin Hendrix + Nate Koenig + BSD + http://ros.org/wiki/audio_capture + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + roscpp + audio_common_msgs + libgstreamer0.10-dev + libgstreamer-plugins-base0.10-dev + + roscpp + audio_common_msgs + libgstreamer0.10-0 + libgstreamer-plugins-base0.10-0 + gstreamer0.10-plugins-ugly + gstreamer0.10-plugins-good + diff --git a/ros/audio_common/audio_capture/src/audio_capture.cpp b/ros/audio_common/audio_capture/src/audio_capture.cpp new file mode 100644 index 0000000..b8b3c2e --- /dev/null +++ b/ros/audio_common/audio_capture/src/audio_capture.cpp @@ -0,0 +1,186 @@ +#include +#include +#include +#include + +#include + +#include "audio_common_msgs/AudioData.h" + +namespace audio_transport +{ + class RosGstCapture + { + public: + RosGstCapture() + { + _bitrate = 192; + + std::string dst_type; + + // Need to encoding or publish raw wave data + ros::param::param("~format", _format, "mp3"); + + // The bitrate at which to encode the audio + ros::param::param("~bitrate", _bitrate, 192); + + // only available for raw data + ros::param::param("~channels", _channels, 1); + ros::param::param("~depth", _depth, 16); + ros::param::param("~sample_rate", _sample_rate, 16000); + + // The destination of the audio + ros::param::param("~dst", dst_type, "appsink"); + + // The source of the audio + //ros::param::param("~src", source_type, "alsasrc"); + + _pub = _nh.advertise("audio", 10, true); + + _loop = g_main_loop_new(NULL, false); + _pipeline = gst_pipeline_new("ros_pipeline"); + _bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline)); + gst_bus_add_signal_watch(_bus); + g_signal_connect(_bus, "message::error", + G_CALLBACK(onMessage), this); + g_object_unref(_bus); + + // We create the sink first, just for convenience + if (dst_type == "appsink") + { + _sink = gst_element_factory_make("appsink", "sink"); + g_object_set(G_OBJECT(_sink), "emit-signals", true, NULL); + g_object_set(G_OBJECT(_sink), "max-buffers", 100, NULL); + g_signal_connect( G_OBJECT(_sink), "new-buffer", + G_CALLBACK(onNewBuffer), this); + } + else + { + printf("file sink\n"); + _sink = gst_element_factory_make("filesink", "sink"); + g_object_set( G_OBJECT(_sink), "location", dst_type.c_str(), NULL); + } + + _source = gst_element_factory_make("alsasrc", "source"); + _convert = gst_element_factory_make("audioconvert", "convert"); + + gboolean link_ok; + + if (_format == "mp3"){ + _encode = gst_element_factory_make("lame", "encoder"); + g_object_set( G_OBJECT(_encode), "preset", 1001, NULL); + g_object_set( G_OBJECT(_encode), "bitrate", _bitrate, NULL); + + gst_bin_add_many( GST_BIN(_pipeline), _source, _convert, _encode, _sink, NULL); + link_ok = gst_element_link_many(_source, _convert, _encode, _sink, NULL); + } else if (_format == "wave") { + GstCaps *caps; + caps = gst_caps_new_simple("audio/x-raw-int", + "channels", G_TYPE_INT, _channels, + "width", G_TYPE_INT, _depth, + "depth", G_TYPE_INT, _depth, + "rate", G_TYPE_INT, _sample_rate, + "signed", G_TYPE_BOOLEAN, TRUE, + NULL); + + g_object_set( G_OBJECT(_sink), "caps", caps, NULL); + gst_caps_unref(caps); + gst_bin_add_many( GST_BIN(_pipeline), _source, _sink, NULL); + link_ok = gst_element_link_many( _source, _sink, NULL); + } else { + ROS_ERROR_STREAM("format must be \"wave\" or \"mp3\""); + exitOnMainThread(1); + } + /*} + else + { + _sleep_time = 10000; + _source = gst_element_factory_make("filesrc", "source"); + g_object_set(G_OBJECT(_source), "location", source_type.c_str(), NULL); + + gst_bin_add_many( GST_BIN(_pipeline), _source, _sink, NULL); + gst_element_link_many(_source, _sink, NULL); + } + */ + + if (!link_ok) { + ROS_ERROR_STREAM("Unsupported media type."); + exitOnMainThread(1); + } + + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + + _gst_thread = boost::thread( boost::bind(g_main_loop_run, _loop) ); + } + + ~RosGstCapture() + { + g_main_loop_quit(_loop); + gst_element_set_state(_pipeline, GST_STATE_NULL); + gst_object_unref(_pipeline); + g_main_loop_unref(_loop); + } + + void exitOnMainThread(int code) + { + exit(code); + } + + void publish( const audio_common_msgs::AudioData &msg ) + { + _pub.publish(msg); + } + + static GstFlowReturn onNewBuffer (GstAppSink *appsink, gpointer userData) + { + RosGstCapture *server = reinterpret_cast(userData); + + GstBuffer *buffer; + g_signal_emit_by_name(appsink, "pull-buffer", &buffer); + + audio_common_msgs::AudioData msg; + msg.data.resize( buffer->size ); + memcpy( &msg.data[0], buffer->data, buffer->size); + + server->publish(msg); + + return GST_FLOW_OK; + } + + static gboolean onMessage (GstBus *bus, GstMessage *message, gpointer userData) + { + RosGstCapture *server = reinterpret_cast(userData); + GError *err; + gchar *debug; + + gst_message_parse_error(message, &err, &debug); + ROS_ERROR_STREAM("gstreamer: " << err->message); + g_error_free(err); + g_free(debug); + g_main_loop_quit(server->_loop); + server->exitOnMainThread(1); + return FALSE; + } + + private: + ros::NodeHandle _nh; + ros::Publisher _pub; + + boost::thread _gst_thread; + + GstElement *_pipeline, *_source, *_sink, *_convert, *_encode; + GstBus *_bus; + int _bitrate, _channels, _depth, _sample_rate; + GMainLoop *_loop; + std::string _format; + }; +} + +int main (int argc, char **argv) +{ + ros::init(argc, argv, "audio_capture"); + gst_init(&argc, &argv); + + audio_transport::RosGstCapture server; + ros::spin(); +} diff --git a/ros/audio_common/audio_common/CHANGELOG.rst b/ros/audio_common/audio_common/CHANGELOG.rst new file mode 100644 index 0000000..29267c2 --- /dev/null +++ b/ros/audio_common/audio_common/CHANGELOG.rst @@ -0,0 +1,52 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package audio_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.12 (2016-02-29) +------------------- + +0.2.11 (2016-02-16) +------------------- + +0.2.10 (2016-01-21) +------------------- + +0.2.9 (2015-12-02) +------------------ + +0.2.8 (2015-10-02) +------------------ +* Update maintainer email +* Contributors: trainman419 + +0.2.7 (2014-07-25) +------------------ + +0.2.6 (2014-02-26) +------------------ + +0.2.5 (2014-01-23) +------------------ +* "0.2.5" +* Contributors: trainman419 + +0.2.4 (2013-09-10) +------------------ + +0.2.3 (2013-07-15) +------------------ + +0.2.2 (2013-04-10) +------------------ + +0.2.1 (2013-04-08 13:59) +------------------------ +* Fix metapackage for REP 127. +* Contributors: Austin Hendrix + +0.2.0 (2013-04-08 13:49) +------------------------ +* Versions and more URLs. +* Convert stack.xml to metapackage package.xml +* Start catkinizing. +* Contributors: Austin Hendrix diff --git a/ros/audio_common/audio_common/CMakeLists.txt b/ros/audio_common/audio_common/CMakeLists.txt new file mode 100644 index 0000000..d0f5151 --- /dev/null +++ b/ros/audio_common/audio_common/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(audio_common) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/ros/audio_common/audio_common/package.xml b/ros/audio_common/audio_common/package.xml new file mode 100644 index 0000000..d6a2bc5 --- /dev/null +++ b/ros/audio_common/audio_common/package.xml @@ -0,0 +1,24 @@ + + audio_common + 0.2.12 + + Common code for working with audio in ROS + + Austin Hendrix + Blaise Gassend + BSD + http://ros.org/wiki/audio_common + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + audio_capture + audio_common_msgs + audio_play + sound_play + + + + + diff --git a/ros/audio_common/audio_common_msgs/.gitignore b/ros/audio_common/audio_common_msgs/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/ros/audio_common/audio_common_msgs/.gitignore @@ -0,0 +1 @@ +build diff --git a/ros/audio_common/audio_common_msgs/CHANGELOG.rst b/ros/audio_common/audio_common_msgs/CHANGELOG.rst new file mode 100644 index 0000000..3f1b7f4 --- /dev/null +++ b/ros/audio_common/audio_common_msgs/CHANGELOG.rst @@ -0,0 +1,53 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package audio_common_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.12 (2016-02-29) +------------------- + +0.2.11 (2016-02-16) +------------------- + +0.2.10 (2016-01-21) +------------------- + +0.2.9 (2015-12-02) +------------------ + +0.2.8 (2015-10-02) +------------------ +* Update maintainer email +* Contributors: trainman419 + +0.2.7 (2014-07-25) +------------------ + +0.2.6 (2014-02-26) +------------------ + +0.2.5 (2014-01-23) +------------------ +* "0.2.5" +* Contributors: trainman419 + +0.2.4 (2013-09-10) +------------------ + +0.2.3 (2013-07-15) +------------------ + +0.2.2 (2013-04-10) +------------------ + +0.2.1 (2013-04-08 13:59) +------------------------ + +0.2.0 (2013-04-08 13:49) +------------------------ +* Catkinize audio_common_msgs. +* Versions and more URLs. +* Convert manifests to package.xml +* Ditch old makefiles. +* Fixed audio_msgs names to audio_common_msgs +* Renamed audio_msgs to audio_common_msgs +* Contributors: Austin Hendrix, Nate Koenig diff --git a/ros/audio_common/audio_common_msgs/CMakeLists.txt b/ros/audio_common/audio_common_msgs/CMakeLists.txt new file mode 100644 index 0000000..eb8ff56 --- /dev/null +++ b/ros/audio_common/audio_common_msgs/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(audio_common_msgs) + +find_package(catkin REQUIRED COMPONENTS message_generation) +add_message_files(DIRECTORY msg FILES AudioData.msg) +generate_messages() + +catkin_package(CATKIN_DEPENDS message_runtime) diff --git a/ros/audio_common/audio_common_msgs/mainpage.dox b/ros/audio_common/audio_common_msgs/mainpage.dox new file mode 100644 index 0000000..24249e4 --- /dev/null +++ b/ros/audio_common/audio_common_msgs/mainpage.dox @@ -0,0 +1,11 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b audio_common_msgs contain messages for transimitting audio via ROS. + + +\section codeapi Code API + + +*/ diff --git a/ros/audio_common/audio_common_msgs/msg/AudioData.msg b/ros/audio_common/audio_common_msgs/msg/AudioData.msg new file mode 100644 index 0000000..dd2331b --- /dev/null +++ b/ros/audio_common/audio_common_msgs/msg/AudioData.msg @@ -0,0 +1 @@ +uint8[] data diff --git a/ros/audio_common/audio_common_msgs/package.xml b/ros/audio_common/audio_common_msgs/package.xml new file mode 100644 index 0000000..c9bf75d --- /dev/null +++ b/ros/audio_common/audio_common_msgs/package.xml @@ -0,0 +1,19 @@ + + audio_common_msgs + 0.2.12 + + Messages for transmitting audio via ROS + + Austin Hendrix + Nate Koenig + BSD + http://ros.org/wiki/audio_common_msgs + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + message_generation + + message_runtime + diff --git a/ros/audio_common/audio_play/.gitignore b/ros/audio_common/audio_play/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/ros/audio_common/audio_play/.gitignore @@ -0,0 +1 @@ +build diff --git a/ros/audio_common/audio_play/CHANGELOG.rst b/ros/audio_common/audio_play/CHANGELOG.rst new file mode 100644 index 0000000..174effe --- /dev/null +++ b/ros/audio_common/audio_play/CHANGELOG.rst @@ -0,0 +1,74 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package audio_play +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.12 (2016-02-29) +------------------- + +0.2.11 (2016-02-16) +------------------- + +0.2.10 (2016-01-21) +------------------- + +0.2.9 (2015-12-02) +------------------ + +0.2.8 (2015-10-02) +------------------ +* Changed message level to warning +* Fixed underflow. + Before the sink buffer underflows the pipeline is paused. When data is received again the pipeline is set to playing again. +* Change audio sink to autoaudiosink +* Update maintainer email +* Contributors: Benny, Hans Gaiser, trainman419 + +0.2.7 (2014-07-25) +------------------ + +0.2.6 (2014-02-26) +------------------ +* audio_capture and play _require\_ gstreamer, it's not optional +* Contributors: v4hn + +0.2.5 (2014-01-23) +------------------ +* "0.2.5" +* Contributors: trainman419 + +0.2.4 (2013-09-10) +------------------ + +0.2.3 (2013-07-15) +------------------ +* Fix dependencies and install rules. +* Contributors: Austin Hendrix + +0.2.2 (2013-04-10) +------------------ + +0.2.1 (2013-04-08 13:59) +------------------------ + +0.2.0 (2013-04-08 13:49) +------------------------ +* Finish catkinizing audio_common. +* Catkinize audio_play. +* Fix typo in package.xml +* Versions and more URLs. +* Convert manifests to package.xml +* Ditch old makefiles. +* Updates manifest +* Updated manifests for rodep2 +* oneiric build fixes, bump version to 0.1.6 +* Removed another duplicate thread::thread +* Added a rosdep.yaml file +* Fixed to use audio_common_msgs +* Added ability to use different festival voices +* Updated documentation +* Update to audio_play +* Fixed ignore files +* Added hgignore files +* Audio_capture and audio_play working +* Making separate audio_capture and audio_play packages +* Contributors: Austin Hendrix, Brian Gerkey, Nate Koenig, nkoenig diff --git a/ros/audio_common/audio_play/CMakeLists.txt b/ros/audio_common/audio_play/CMakeLists.txt new file mode 100644 index 0000000..f3c09bc --- /dev/null +++ b/ros/audio_common/audio_play/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(audio_play) + +find_package(catkin REQUIRED COMPONENTS roscpp audio_common_msgs) + +find_package(PkgConfig) +pkg_check_modules(GST gstreamer-0.10 REQUIRED) + +find_package(Boost REQUIRED COMPONENTS thread) + +include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GST_INCLUDE_DIRS}) + +catkin_package() + +add_executable(audio_play src/audio_play.cpp) +target_link_libraries(audio_play ${catkin_LIBRARIES} ${GST_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(audio_play ${catkin_EXPORTED_TARGETS}) + +install(TARGETS audio_play + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/ros/audio_common/audio_play/launch/play.launch b/ros/audio_common/audio_play/launch/play.launch new file mode 100644 index 0000000..9b97169 --- /dev/null +++ b/ros/audio_common/audio_play/launch/play.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ros/audio_common/audio_play/mainpage.dox b/ros/audio_common/audio_play/mainpage.dox new file mode 100644 index 0000000..5c05407 --- /dev/null +++ b/ros/audio_common/audio_play/mainpage.dox @@ -0,0 +1,22 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b audio_play is a package that listens to a node that produces audio_msgs, and plays them through a connected speaker. + + +\section codeapi Code API + + + + +*/ diff --git a/ros/audio_common/audio_play/package.xml b/ros/audio_common/audio_play/package.xml new file mode 100644 index 0000000..83ce785 --- /dev/null +++ b/ros/audio_common/audio_play/package.xml @@ -0,0 +1,30 @@ + + audio_play + 0.2.12 + + Outputs audio to a speaker from a source node. + + Austin Hendrix + Nate Koenig + BSD + http://ros.org/wiki/audio_play + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + roscpp + audio_common_msgs + libgstreamer0.10-dev + libgstreamer-plugins-base0.10-dev + + roscpp + audio_common_msgs + libgstreamer0.10-0 + libgstreamer-plugins-base0.10-0 + gstreamer0.10-plugins-ugly + gstreamer0.10-plugins-good + + + + diff --git a/ros/audio_common/audio_play/src/audio_play.cpp b/ros/audio_common/audio_play/src/audio_play.cpp new file mode 100644 index 0000000..a72b2cf --- /dev/null +++ b/ros/audio_common/audio_play/src/audio_play.cpp @@ -0,0 +1,153 @@ +#include +#include +#include +#include +#include + +#include "audio_common_msgs/AudioData.h" + +namespace audio_transport +{ + class RosGstPlay + { + public: + RosGstPlay() + { + GstPad *audiopad; + + std::string dst_type; + + // The destination of the audio + ros::param::param("~dst", dst_type, "alsasink"); + + _sub = _nh.subscribe("audio", 10, &RosGstPlay::onAudio, this); + + _loop = g_main_loop_new(NULL, false); + + _pipeline = gst_pipeline_new("app_pipeline"); + _source = gst_element_factory_make("appsrc", "app_source"); + gst_bin_add( GST_BIN(_pipeline), _source); + + g_signal_connect(_source, "need-data", G_CALLBACK(cb_need_data),this); + + //_playbin = gst_element_factory_make("playbin2", "uri_play"); + //g_object_set( G_OBJECT(_playbin), "uri", "file:///home/test/test.mp3", NULL); + if (dst_type == "alsasink") + { + _decoder = gst_element_factory_make("decodebin", "decoder"); + g_signal_connect(_decoder, "new-decoded-pad", G_CALLBACK(cb_newpad),this); + gst_bin_add( GST_BIN(_pipeline), _decoder); + gst_element_link(_source, _decoder); + + _audio = gst_bin_new("audiobin"); + _convert = gst_element_factory_make("audioconvert", "convert"); + audiopad = gst_element_get_static_pad(_convert, "sink"); + _sink = gst_element_factory_make("autoaudiosink", "sink"); + gst_bin_add_many( GST_BIN(_audio), _convert, _sink, NULL); + gst_element_link(_convert, _sink); + gst_element_add_pad(_audio, gst_ghost_pad_new("sink", audiopad)); + gst_object_unref(audiopad); + + gst_bin_add(GST_BIN(_pipeline), _audio); + + } + else + { + _sink = gst_element_factory_make("filesink", "sink"); + g_object_set( G_OBJECT(_sink), "location", dst_type.c_str(), NULL); + gst_bin_add(GST_BIN(_pipeline), _sink); + gst_element_link(_source, _sink); + } + + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + //gst_element_set_state(GST_ELEMENT(_playbin), GST_STATE_PLAYING); + + _gst_thread = boost::thread( boost::bind(g_main_loop_run, _loop) ); + + _paused = false; + } + + private: + + void onAudio(const audio_common_msgs::AudioDataConstPtr &msg) + { + if(_paused) + { + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + _paused = false; + } + + GstBuffer *buffer = gst_buffer_new_and_alloc(msg->data.size()); + memcpy(buffer->data, &msg->data[0], msg->data.size()); + + GstFlowReturn ret; + + g_signal_emit_by_name(_source, "push-buffer", buffer, &ret); + } + + static void cb_newpad (GstElement *decodebin, GstPad *pad, + gboolean last, gpointer data) + { + RosGstPlay *client = reinterpret_cast(data); + + GstCaps *caps; + GstStructure *str; + GstPad *audiopad; + + /* only link once */ + audiopad = gst_element_get_static_pad (client->_audio, "sink"); + if (GST_PAD_IS_LINKED (audiopad)) + { + g_object_unref (audiopad); + return; + } + + /* check media type */ + caps = gst_pad_get_caps (pad); + str = gst_caps_get_structure (caps, 0); + if (!g_strrstr (gst_structure_get_name (str), "audio")) { + gst_caps_unref (caps); + gst_object_unref (audiopad); + return; + } + + gst_caps_unref (caps); + + /* link'n'play */ + gst_pad_link (pad, audiopad); + + g_object_unref (audiopad); + } + + static void cb_need_data (GstElement *appsrc, + guint unused_size, + gpointer user_data) + { + ROS_WARN("need-data signal emitted! Pausing the pipeline"); + RosGstPlay *client = reinterpret_cast(user_data); + gst_element_set_state(GST_ELEMENT(client->_pipeline), GST_STATE_PAUSED); + client->_paused = true; + } + + ros::NodeHandle _nh; + ros::Subscriber _sub; + boost::thread _gst_thread; + + GstElement *_pipeline, *_source, *_sink, *_decoder, *_convert, *_audio; + GstElement *_playbin; + GMainLoop *_loop; + + bool _paused; + }; +} + + +int main (int argc, char **argv) +{ + ros::init(argc, argv, "audio_play"); + gst_init(&argc, &argv); + + audio_transport::RosGstPlay client; + + ros::spin(); +} diff --git a/ros/audio_common/sound_play/CHANGELOG.rst b/ros/audio_common/sound_play/CHANGELOG.rst new file mode 100644 index 0000000..b1221bb --- /dev/null +++ b/ros/audio_common/sound_play/CHANGELOG.rst @@ -0,0 +1,167 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package sound_play +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.12 (2016-02-29) +------------------- +* remove chance of uninitialised variable being called in a subscriber callback. +* Contributors: Daniel Stonier + +0.2.11 (2016-02-16) +------------------- +* Fix bug in say.py. Fixes `#72 `_ +* Contributors: trainman419 + +0.2.10 (2016-01-21) +------------------- +* Issue: The error checks for missing publisher/action client in sendMsg were inverted. + The non-blocking brach tested the action client while the blocking branch + tested the publisher. + Fix: Inverted the blocking boolean for both branchs. +* sound_play: Fix build with -DCATKIN_ENABLE_TESTING=OFF. + https://bugs.gentoo.org/show_bug.cgi?id=567466 +* Contributors: Alexis Ballier, Neowizard + +0.2.9 (2015-12-02) +------------------ +* [soundplay_node] fix resources not being released on dict cleanup + This was resulting in the number of sink inputs reaching the maximum threshold, + (32 on ubuntu 14.04 with pulseaudio 4.0) after which no more sounds could be + played by the node. It would only happen if the rate of sounds being played was + slower than the dictionary cleanup. +* depend on actionlib. +* Introduce unit test to ensure soundclient is started correctly. +* Example of using the explicit blocking parameter to override the class setting. +* SoundClient can also explicitly specify whether or not to block while playing the sound. + Each play/repeat/say/... method can take an option blocking=True|False argument (using \*\*kwargs), which over-rides the class-wide setting. + Conflicts: + sound_play/src/sound_play/libsoundplay.py +* do both in same script. +* Added script showing the various blocking/non-blocking ways of using SoundClient. +* removed trailing whitespace only + Conflicts: + sound_play/scripts/say.py +* loginfo -> logdebug. +* Enable blocking calls inside libsoundplay's SoundClient. + This makes use of the actionlib interface provided by soundplay_node, by ensuring SoundClient receives a response before returning. + Turn this on by: SoundClient(blocking=true). + Conflicts: + sound_play/src/sound_play/libsoundplay.py +* Use new-style python classes (inherits from object). + Conflicts: + sound_play/src/sound_play/libsoundplay.py +* removed trailing whitespace. + Conflicts: + sound_play/src/sound_play/libsoundplay.py +* Revert "Set the volume in each of the sound_play actionlib tests." + This reverts commit 55ab08c882809fc6d21affb849a7dac9f1901867. + Indigo-devel does not have the volume API +* Set the volume in each of the sound_play actionlib tests. + This makes the script actually play the sounds it requests. +* Specify queue size explicitly. + Removed warning message printed each time soundplay_node was started. +* remove trailing whitespace only. +* Fix wiki links +* Contributors: David V. Lu, Felix Duvallet, Michal Staniaszek, trainman419 + +0.2.8 (2015-10-02) +------------------ +* Fix test target name collision. Fixes `#49 `_ +* sound_play: remove some raw prints cluttering output +* sound_play: added queue_size to SoundClient init + Should prevent warning being displayed whenever the client is created. + Fixes issue `#43 `_ +* add simple-actionlib functionality to sound_play +* sound_play: Added functions to play files relative to a package path +* Update maintainer email +* Contributors: Matthias Nieuwenhuisen, Michal Staniaszek, aginika, trainman419 + +0.2.7 (2014-07-25) +------------------ + +0.2.6 (2014-02-26) +------------------ +* Fix path resolution in python soundplay lib. +* now importing roslib. closes `#33 `_ +* Contributors: Piyush Khandelwal, trainman419 + +0.2.5 (2014-01-23) +------------------ +* "0.2.5" +* Install sounds. Fixes `#29 `_. +* install sound_play.h and export include folder +* Contributors: ahendrix, trainman419, v4hn + +0.2.4 (2013-09-10) +------------------ +* Fix cmake ordering. +* Contributors: Austin Hendrix + +0.2.3 (2013-07-15) +------------------ +* Fix python. +* Contributors: Austin Hendrix + +0.2.2 (2013-04-10) +------------------ +* Actually add proper dependency on message generation. +* Reorder CMakeLists.txt. +* Contributors: Austin Hendrix + +0.2.1 (2013-04-08 13:59) +------------------------ + +0.2.0 (2013-04-08 13:49) +------------------------ +* Finish catkinizing audio_common. +* Start catkinizing sound_play. +* Fix typo in package.xml +* Versions and more URLs. +* Convert manifests to package.xml +* Ditch old makefiles. +* Use festival default voice from libsoundplay. +* Set myself as the maintainer. +* Fix filehandle leak and add debug statements. +* Updates manifest +* Updated manifests for rodep2 +* Fixed sound_play +* Added test wave +* Cleaned up the test script +* Added default voice to say command +* Updated the gstreamer rosdeps +* Removed comment +* Added diagnostic_msgs to sound_play +* Added a rosdep.yaml file +* Added ability to use different festival voices +* Added exit(1) when import of pygame fails. This makes the error message easier to notice. +* Added Ubuntu platform tags to manifest +* Added a link to the troubleshooting wiki page in the diagnostic message as requested by `#4070 `_. +* Took out the deprecated API. +* Sound play now publishes header timestamp in message. `#3822 `_ +* Cleaned up temp file generation when doing text to speach. Now uses the tempfile module. +* Adding missing export of headers for sound_play C++ API +* Changing node name for sound play diagnostics, `#3599 `_ +* Added test.launch to run sound server and a test client. +* Remove use of deprecated rosbuild macros +* Replaced review tag with standardized message +* Updated review status +* Added a launch file to start soundplay_node.py +* Made the sound_play client libraries be more explicit about what to do when the node is not running. +* Updated manifest description +* Updated copyright year +* fixed XML typo +* updated package description +* Added a copyright message. +* Removed debugging message from sound_play node. +* Added tests for new sound_play python API and fixed a few bugs. +* Fixed missing self arguments in sound_play libsoundplay.py +* Upgraded the python sound_play API +* Converted non-camelCase methods to camelCase in sound_play C++ API +* Changed Lock to RLock to fix `#2801 `_ +* Made the deprecation of SoundHandle into a warning. +* Added debug messages +* Updated soundplay_node to publish diagnostics and increased the number of active channels. +* Added diagnostic_msgs dependency to sound_play +* sound_play: Renamed SoundHandle to SoundClient. Added Sound-centric C++ API. Changed byte to int8 in msg file. Updated documentation. +* migration part 1 +* Contributors: Austin Hendrix, Nate Koenig, blaise, blaisegassend, eitan, gerkey, kwc, nkoenig, watts, wheeler diff --git a/ros/audio_common/sound_play/CMakeLists.txt b/ros/audio_common/sound_play/CMakeLists.txt new file mode 100644 index 0000000..3f9604c --- /dev/null +++ b/ros/audio_common/sound_play/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(sound_play) + +find_package(catkin REQUIRED COMPONENTS message_generation roscpp actionlib_msgs) + +add_action_files(DIRECTORY action FILES SoundRequest.action) +add_message_files(DIRECTORY msg FILES SoundRequest.msg) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +catkin_python_setup() + +generate_messages(DEPENDENCIES actionlib_msgs) + +catkin_package(CATKIN_DEPENDS message_runtime actionlib_msgs + INCLUDE_DIRS include) + +if(CATKIN_ENABLE_TESTING) + catkin_add_nosetests(scripts/test) + + add_subdirectory(test) +endif() + +install(PROGRAMS + scripts/playbuiltin.py + scripts/play.py + scripts/say.py + scripts/shutup.py + scripts/soundplay_node.py + scripts/test.py + scripts/test_actionlib_client.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(FILES + soundplay_node.launch + test.launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY sounds + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/ros/audio_common/sound_play/README b/ros/audio_common/sound_play/README new file mode 100644 index 0000000..6d62de8 --- /dev/null +++ b/ros/audio_common/sound_play/README @@ -0,0 +1,41 @@ +Dependencies +------------ +python-pygame +festival +festvox-don +alsa-base +alsa-tools + +Checking that the speaker/sound card is recognized by the kernel +---------------------------------------------------------------- + +cat /proc/asound/cards + +Your card should be in the list. Make note of the number in front of the +card, it will be used to tell alsa where to play sound from. + +If your sound device does not show up, your kernel may not support it, or +the module may not be loaded. For usb speakers, you may want to try: +modprobe snd-usb-audio + +(not sure if this list is exhaustive) + +Telling alsa which sound card/speaker to use +-------------------------------------------- + +Run (replace 75 with the number of the sound device to use): +asoundconf set-default-card 75 + +This will create .asoundrc.asoundconf in your home directory. +To make alsa use these settings, add the following line to ~/.asoundrc +include ".asoundrc.asoundconf" + +To set this default to all users, copy this to the system-wide alsa +configuration file: +mv ~/.asoundrc.asoundconf /etc/asound.conf + +Getting started +--------------- + +Start the sound play node, and have a look at the scripts in the scripts +directory that exercise the node's functionality. diff --git a/ros/audio_common/sound_play/action/SoundRequest.action b/ros/audio_common/sound_play/action/SoundRequest.action new file mode 100644 index 0000000..49ef24a --- /dev/null +++ b/ros/audio_common/sound_play/action/SoundRequest.action @@ -0,0 +1,7 @@ +SoundRequest sound_request +--- +bool playing +time stamp +--- +bool playing +time stamp \ No newline at end of file diff --git a/ros/audio_common/sound_play/include/sound_play/sound_play.h b/ros/audio_common/sound_play/include/sound_play/sound_play.h new file mode 100644 index 0000000..2514918 --- /dev/null +++ b/ros/audio_common/sound_play/include/sound_play/sound_play.h @@ -0,0 +1,418 @@ +/* + *********************************************************** + * 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 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. + *********************************************************** + */ + +#ifndef __SOUND_PLAY__SOUND_PLAY__H__ +#define __SOUND_PLAY__SOUND_PLAY__H__ + +#include +#include +#include +#include +#include + +namespace sound_play +{ + +/** + * \brief Class that publishes messages to the sound_play node. + * + * This class is a helper class for communicating with the sound_play node + * via the \ref sound_play::SoundRequest message. It has two ways of being used: + * + * - It can create Sound classes that represent a particular sound which + * can be played, repeated or stopped. + * + * - It provides methods for each way in which the sound_play::SoundRequest + * message can be invoked. + */ + +class SoundClient +{ +public: + class Sound + { + friend class SoundClient; + private: + int snd_; + std::string arg_; + std::string arg2_; + SoundClient *client_; + + Sound(SoundClient *sc, int snd, const std::string &arg, const std::string arg2 = std::string()) + { + client_ = sc; + snd_ = snd; + arg_ = arg; + arg2_ = arg2; + } + + public: + /** + * \brief Play the Sound. + * + * This method causes the Sound to be played once. + */ + + void play() + { + client_->sendMsg(snd_, SoundRequest::PLAY_ONCE, arg_, arg2_); + } + + /** + * \brief Play the Sound repeatedly. + * + * This method causes the Sound to be played repeatedly until stop() is + * called. + */ + + void repeat() + { + client_->sendMsg(snd_, SoundRequest::PLAY_START, arg_, arg2_); + } + + /** + * \brief Stop Sound playback. + * + * This method causes the Sound to stop playing. + */ + + void stop() + { + client_->sendMsg(snd_, SoundRequest::PLAY_STOP, arg_, arg2_); + } + }; + +/** \brief Create a SoundClient that publishes on the given topic + * + * Creates a SoundClient that publishes to the given topic relative to the + * given NodeHandle. + * + * \param nh Node handle to use when creating the topic. + * + * \param topic Topic to publish to. + */ + + SoundClient(ros::NodeHandle &nh, const std::string &topic) + { + init(nh, topic); + } + +/** \brief Create a SoundClient with the default topic + * + * Creates a SoundClient that publishes to "robotsound". + */ + + SoundClient() + { + init(ros::NodeHandle(), "robotsound"); + } + +/** + * \brief Create a voice Sound. + * + * Creates a Sound corresponding to saying the indicated text. + * + * \param s Text to say + */ + + Sound voiceSound(const std::string &s) + { + return Sound(this, SoundRequest::SAY, s); + } + +/** + * \brief Create a wave Sound. + * + * Creates a Sound corresponding to indicated file. + * + * \param s File to play. Should be an absolute path that exists on the + * machine running the sound_play node. + */ + + Sound waveSound(const std::string &s) + { + return Sound(this, SoundRequest::PLAY_FILE, s); + } + +/** + * \brief Create a wave Sound from a package. + * + * Creates a Sound corresponding to indicated file. + * + * \param p Package containing the sound file. + * \param s Filename of the WAV or OGG file. Must be an path relative to the package valid + * on the computer on which the sound_play node is running + */ + + Sound waveSoundFromPkg(const std::string &p, const std::string &s) + { + return Sound(this, SoundRequest::PLAY_FILE, s, p); + } + +/** + * \brief Create a builtin Sound. + * + * Creates a Sound corresponding to indicated builtin wave. + * + * \param id Identifier of the sound to play. + */ + + Sound builtinSound(int id) + { + return Sound(this, id, ""); + } + +/** \brief Say a string + * + * Send a string to be said by the sound_node. The vocalization can be + * stopped using stopSaying or stopAll. + * + * \param s String to say + */ + + void say(const std::string &s, const std::string &voice="voice_kal_diphone") + { + sendMsg(SoundRequest::SAY, SoundRequest::PLAY_ONCE, s, voice); + } + +/** \brief Say a string repeatedly + * + * The string is said repeatedly until stopSaying or stopAll is used. + * + * \param s String to say repeatedly + */ + + void repeat(const std::string &s) + { + sendMsg(SoundRequest::SAY, SoundRequest::PLAY_START, s); + } + +/** \brief Stop saying a string + * + * Stops saying a string that was previously started by say or repeat. The + * argument indicates which string to stop saying. + * + * \param s Same string as in the say or repeat command + */ + + void stopSaying(const std::string &s) + { + sendMsg(SoundRequest::SAY, SoundRequest::PLAY_STOP, s); + } + +/** \brief Plays a WAV or OGG file + * + * Plays a WAV or OGG file once. The playback can be stopped by stopWave or + * stopAll. + * + * \param s Filename of the WAV or OGG file. Must be an absolute path valid + * on the computer on which the sound_play node is running + */ + + void playWave(const std::string &s) + { + sendMsg(SoundRequest::PLAY_FILE, SoundRequest::PLAY_ONCE, s); + } + +/** \brief Plays a WAV or OGG file repeatedly + * + * Plays a WAV or OGG file repeatedly until stopWave or stopAll is used. + * + * \param s Filename of the WAV or OGG file. Must be an absolute path valid + * on the computer on which the sound_play node is running. + */ + + void startWave(const std::string &s) + { + sendMsg(SoundRequest::PLAY_FILE, SoundRequest::PLAY_START, s); + } + +/** \brief Stop playing a WAV or OGG file + * + * Stops playing a file that was previously started by playWave or + * startWave. + * + * \param s Same string as in the playWave or startWave command + */ + + void stopWave(const std::string &s) + { + sendMsg(SoundRequest::PLAY_FILE, SoundRequest::PLAY_STOP, s); + } + +/** \brief Plays a WAV or OGG file from a package + * + * Plays a WAV or OGG file once. The playback can be stopped by stopWaveFromPkg or + * stopAll. + * + * \param p Package name containing the sound file. + * \param s Filename of the WAV or OGG file. Must be an path relative to the package valid + * on the computer on which the sound_play node is running + */ + + void playWaveFromPkg(const std::string &p, const std::string &s) + { + sendMsg(SoundRequest::PLAY_FILE, SoundRequest::PLAY_ONCE, s, p); + } + +/** \brief Plays a WAV or OGG file repeatedly + * + * Plays a WAV or OGG file repeatedly until stopWaveFromPkg or stopAll is used. + * + * \param p Package name containing the sound file. + * \param s Filename of the WAV or OGG file. Must be an path relative to the package valid + * on the computer on which the sound_play node is running + */ + + void startWaveFromPkg(const std::string &p, const std::string &s) + { + sendMsg(SoundRequest::PLAY_FILE, SoundRequest::PLAY_START, s, p); + } + +/** \brief Stop playing a WAV or OGG file + * + * Stops playing a file that was previously started by playWaveFromPkg or + * startWaveFromPkg. + * + * \param p Package name containing the sound file. + * \param s Filename of the WAV or OGG file. Must be an path relative to the package valid + * on the computer on which the sound_play node is running + */ + + void stopWaveFromPkg(const std::string &p, const std::string &s) + { + sendMsg(SoundRequest::PLAY_FILE, SoundRequest::PLAY_STOP, s, p); + } + +/** \brief Play a buildin sound + * + * Starts playing one of the built-in sounds. built-ing sounds are documented + * in \ref SoundRequest.msg. Playback can be stopped by stopAll. + * + * \param sound Identifier of the sound to play. + */ + + void play(int sound) + { + sendMsg(sound, SoundRequest::PLAY_ONCE); + } + +/** \brief Play a buildin sound repeatedly + * + * Starts playing one of the built-in sounds repeatedly until stop or stopAll + * is used. Built-in sounds are documented in \ref SoundRequest.msg. + * + * \param sound Identifier of the sound to play. + */ + + void start(int sound) + { + sendMsg(sound, SoundRequest::PLAY_START); + } + +/** \brief Stop playing a built-in sound + * + * Stops playing a built-in sound started with play or start. + * + * \param sound Same sound that was used to start playback. + */ + + void stop(int sound) + { + sendMsg(sound, SoundRequest::PLAY_STOP); + } + +/** \brief Stop all currently playing sounds + * + * This method stops all speech, wave file, and built-in sound playback. + */ + + void stopAll() + { + stop(SoundRequest::ALL); + } + + /** \brief Turns warning messages on or off. + * + * If a message is sent when no node is subscribed to the topic, a + * warning message is printed. This method can be used to enable or + * disable warnings. + * + * \param state True to turn off messages, false to turn them on. + */ + + void setQuiet(bool state) + { + quiet_ = state; + } + +private: + void init(ros::NodeHandle nh, const std::string &topic) + { + nh_ = nh; + pub_ = nh.advertise(topic, 5); + quiet_ = false; + } + + void sendMsg(int snd, int cmd, const std::string &s = "", const std::string &arg2 = "") + { + boost::mutex::scoped_lock lock(mutex_); + + if (!nh_.ok()) + return; + + SoundRequest msg; + msg.sound = snd; + msg.command = cmd; + msg.arg = s; + msg.arg2 = arg2; + pub_.publish(msg); + + if (pub_.getNumSubscribers() == 0 && !quiet_) + ROS_WARN("Sound command issued, but no node is subscribed to the topic. Perhaps you forgot to run soundplay_node.py"); + } + + bool quiet_; + ros::NodeHandle nh_; + ros::Publisher pub_; + boost::mutex mutex_; +}; + +typedef SoundClient::Sound Sound; + +}; + +#endif diff --git a/ros/audio_common/sound_play/mainpage.dox b/ros/audio_common/sound_play/mainpage.dox new file mode 100644 index 0000000..61ca855 --- /dev/null +++ b/ros/audio_common/sound_play/mainpage.dox @@ -0,0 +1,23 @@ +/** + +\mainpage +\htmlinclude manifest.html + +The \b sound_play package provides a way to say strings, +play WAV or OGG files and to play builtin sounds. Documentation for the +package can be found here at http://www.ros.org/wiki/sound_play + +Multiple sounds can be played concurrently (up to 4 currently because of +limitations in pygame). + +Python and C++ client classes are provide for ease of use: + +- sound_play::SoundClient and sound_play::SoundClient::Sound for C++ +- libsoundplay::SoundClient and libsoundplay::SoundClient::Sound for Python + +Example uses are in: + +- test.cpp +- test.py + +*/ diff --git a/ros/audio_common/sound_play/msg/SoundRequest.msg b/ros/audio_common/sound_play/msg/SoundRequest.msg new file mode 100644 index 0000000..8f20b92 --- /dev/null +++ b/ros/audio_common/sound_play/msg/SoundRequest.msg @@ -0,0 +1,27 @@ +# IMPORTANT: You should never have to generate this message yourself. +# Use the sound_play::SoundClient C++ helper or the +# sound_play.libsoundplay.SoundClient Python helper. + +# Sounds +int8 BACKINGUP = 1 +int8 NEEDS_UNPLUGGING = 2 +int8 NEEDS_PLUGGING = 3 +int8 NEEDS_UNPLUGGING_BADLY = 4 +int8 NEEDS_PLUGGING_BADLY = 5 + +# Sound identifiers that have special meaning +int8 ALL = -1 # Only legal with PLAY_STOP +int8 PLAY_FILE = -2 +int8 SAY = -3 + +int8 sound # Selects which sound to play (see above) + +# Commands +int8 PLAY_STOP = 0 # Stop this sound from playing +int8 PLAY_ONCE = 1 # Play the sound once +int8 PLAY_START = 2 # Play the sound in a loop until a stop request occurs + +int8 command # Indicates what to do with the sound + +string arg # file name or text to say +string arg2 # other arguments diff --git a/ros/audio_common/sound_play/package.xml b/ros/audio_common/sound_play/package.xml new file mode 100644 index 0000000..7170c57 --- /dev/null +++ b/ros/audio_common/sound_play/package.xml @@ -0,0 +1,43 @@ + + sound_play + 0.2.12 + + sound_play provides a ROS node that translates commands on a ROS topic (robotsound) into sounds. The node supports built-in sounds, playing OGG/WAV files, and doing speech synthesis via festival. C++ and Python bindings allow this node to be used without understanding the details of the message format, allowing faster development and resilience to message format changes. + + Austin Hendrix + Blaise Gassend + BSD + http://ros.org/wiki/sound_play + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + roscpp + roslib + actionlib_msgs + actionlib + audio_common_msgs + diagnostic_msgs + libgstreamer0.10-dev + libgstreamer-plugins-base0.10-dev + message_generation + + roscpp + roslib + actionlib_msgs + audio_common_msgs + diagnostic_msgs + libgstreamer0.10-0 + libgstreamer-plugins-base0.10-0 + gstreamer0.10-plugins-ugly + gstreamer0.10-plugins-good + rospy + festival + python-gst + message_runtime + + + + + diff --git a/ros/audio_common/sound_play/scripts/play.py b/ros/audio_common/sound_play/scripts/play.py new file mode 100755 index 0000000..cac0e24 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/play.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Blaise Gassend + + +import sys + +if __name__ == '__main__': + if len(sys.argv) != 2 or sys.argv[1] == '--help': + print 'Usage: %s sound_to_play.(ogg|wav)'%sys.argv[0] + print + print 'Plays an .OGG or .WAV file. The path to the file should be absolute, and be valid on the computer on which sound_play is running.' + exit(1) + + # Import after printing usage for speed. + import rospy + from sound_play.msg import SoundRequest + from sound_play.libsoundplay import SoundClient + + rospy.init_node('play', anonymous = True) + soundhandle = SoundClient() + + rospy.sleep(1) + print 'Playing "%s".'%sys.argv[1] + soundhandle.playWave(sys.argv[1]) + rospy.sleep(1) diff --git a/ros/audio_common/sound_play/scripts/playbuiltin.py b/ros/audio_common/sound_play/scripts/playbuiltin.py new file mode 100755 index 0000000..d34e302 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/playbuiltin.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Blaise Gassend + +import sys + +if __name__ == '__main__': + if len(sys.argv) != 2 or sys.argv[1] == '--help': + print 'Usage: %s '%sys.argv[0] + print + print 'Plays one of the built-in sounds based on its integer ID. Look at the sound_play/SoundRequest message definition for IDs.' + exit(1) + + # Import here so that usage is fast. + import rospy + from sound_play.msg import SoundRequest + from sound_play.libsoundplay import SoundClient + + rospy.init_node('play', anonymous = True) + + soundhandle = SoundClient() + + rospy.sleep(1) + + num = int(sys.argv[1]) + + print 'Playing sound %i.'%num + + soundhandle.play(num) + + rospy.sleep(1) diff --git a/ros/audio_common/sound_play/scripts/playpackage.py b/ros/audio_common/sound_play/scripts/playpackage.py new file mode 100755 index 0000000..5b9c80e --- /dev/null +++ b/ros/audio_common/sound_play/scripts/playpackage.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Matthias Nieuwenhuisen, Blaise Gassend + + +import sys + +if __name__ == '__main__': + if len(sys.argv) != 3 or sys.argv[1] == '--help': + print 'Usage: %s package sound_to_play.(ogg|wav)'%sys.argv[0] + print + print 'Plays an .OGG or .WAV file. The path to the file should be relative to the package, and be valid on the computer on which sound_play is running.' + exit(1) + + # Import after printing usage for speed. + import rospy + from sound_play.msg import SoundRequest + from sound_play.libsoundplay import SoundClient + + rospy.init_node('play', anonymous = True) + soundhandle = SoundClient() + + rospy.sleep(1) + print 'Playing "%s" from pkg "%s".'%(sys.argv[2], sys.argv[1]) + soundhandle.playWaveFromPkg(sys.argv[1], sys.argv[2]) + rospy.sleep(1) diff --git a/ros/audio_common/sound_play/scripts/say.py b/ros/audio_common/sound_play/scripts/say.py new file mode 100755 index 0000000..6a42881 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/say.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Blaise Gassend + + +import sys + +if __name__ == '__main__': + if len(sys.argv) > 1 and sys.argv[1] == '--help': + print 'Usage: %s \'String to say.\''%sys.argv[0] + print ' %s < file_to_say.txt'%sys.argv[0] + print + print 'Says a string. For a string on the command line, you must use quotes as' + print 'appropriate. For a string on standard input, the command will wait for' + print 'EOF before saying anything.' + exit(-1) + + # Import after printing usage for speed. + import rospy + from sound_play.msg import SoundRequest + from sound_play.libsoundplay import SoundClient + + if len(sys.argv) == 1: + print 'Awaiting something to say on standard input.' + + # Ordered this way to minimize wait time. + rospy.init_node('say', anonymous = True) + soundhandle = SoundClient() + rospy.sleep(1) + + voice = 'voice_kal_diphone' + + if len(sys.argv) == 1: + s = sys.stdin.read() + else: + s = sys.argv[1] + + if len(sys.argv) > 2: + voice = sys.argv[2] + + print 'Saying: %s' % s + print 'Voice: %s' % voice + + soundhandle.say(s, voice) + rospy.sleep(1) diff --git a/ros/audio_common/sound_play/scripts/shutup.py b/ros/audio_common/sound_play/scripts/shutup.py new file mode 100755 index 0000000..6aee7f8 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/shutup.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Blaise Gassend + +import rospy +from sound_play.msg import SoundRequest +from sound_play.libsoundplay import SoundClient + +if __name__ == '__main__': + rospy.init_node('shutup', anonymous = True) + + soundhandle = SoundClient() + rospy.sleep(0.5) # let ROS get started... + + print "Sending stopAll commande every 100 ms." + print "Note: This will not prevent a node that is continuing to issue commands" + print "from producing sound." + print "Press Ctrl+C to exit." + + while not rospy.is_shutdown(): + soundhandle.stopAll() + try: + rospy.sleep(.1) + except: + pass diff --git a/ros/audio_common/sound_play/scripts/soundclient_example.py b/ros/audio_common/sound_play/scripts/soundclient_example.py new file mode 100755 index 0000000..0f4a7b4 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/soundclient_example.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python + +""" +Simple example showing how to use the SoundClient provided by libsoundplay, +in blocking, non-blocking, and explicit usage. +""" + +import rospy +from sound_play.libsoundplay import SoundClient +from sound_play.msg import SoundRequest + + +def play_explicit(): + rospy.loginfo('Example: SoundClient play methods can take in an explicit' + ' blocking parameter') + soundhandle = SoundClient() # blocking = False by default + rospy.sleep(0.5) # Ensure publisher connection is successful. + + sound_beep = soundhandle.waveSound("say-beep.wav", volume=0.5) + # Play the same sound twice, once blocking and once not. The first call is + # blocking (explicitly specified). + sound_beep.play(blocking=True) + # This call is not blocking (uses the SoundClient's setting). + sound_beep.play() + rospy.sleep(0.5) # Let sound complete. + + # Play a blocking sound. + soundhandle.play(SoundRequest.NEEDS_UNPLUGGING, blocking=True) + + # Create a new SoundClient where the default behavior *is* to block. + soundhandle = SoundClient(blocking=True) + soundhandle.say('Say-ing stuff while block-ing') + soundhandle.say('Say-ing stuff without block-ing', blocking=False) + rospy.sleep(1) + + +def play_blocking(): + """ + Play various sounds, blocking until each is completed before going to the + next. + """ + rospy.loginfo('Example: Playing sounds in *blocking* mode.') + soundhandle = SoundClient(blocking=True) + + rospy.loginfo('Playing say-beep at full volume.') + soundhandle.playWave('say-beep.wav') + + rospy.loginfo('Playing say-beep at volume 0.3.') + soundhandle.playWave('say-beep.wav', volume=0.3) + + rospy.loginfo('Playing sound for NEEDS_PLUGGING.') + soundhandle.play(SoundRequest.NEEDS_PLUGGING) + + rospy.loginfo('Speaking some long string.') + soundhandle.say('It was the best of times, it was the worst of times.') + + +def play_nonblocking(): + """ + Play the same sounds with manual pauses between them. + """ + rospy.loginfo('Example: Playing sounds in *non-blocking* mode.') + # NOTE: you must sleep at the beginning to let the SoundClient publisher + # establish a connection to the soundplay_node. + soundhandle = SoundClient(blocking=False) + rospy.sleep(1) + + # In the non-blocking version you need to sleep between calls. + rospy.loginfo('Playing say-beep at full volume.') + soundhandle.playWave('say-beep.wav') + rospy.sleep(1) + + rospy.loginfo('Playing say-beep at volume 0.3.') + soundhandle.playWave('say-beep.wav', volume=0.3) + rospy.sleep(1) + + rospy.loginfo('Playing sound for NEEDS_PLUGGING.') + soundhandle.play(SoundRequest.NEEDS_PLUGGING) + rospy.sleep(1) + + rospy.loginfo('Speaking some long string.') + soundhandle.say('It was the best of times, it was the worst of times.') + # Note we will return before the string has finished playing. + + +if __name__ == '__main__': + rospy.init_node('soundclient_example', anonymous=False) + play_explicit() + play_blocking() + play_nonblocking() + rospy.loginfo('Finished') diff --git a/ros/audio_common/sound_play/scripts/soundplay_node.py b/ros/audio_common/sound_play/scripts/soundplay_node.py new file mode 100755 index 0000000..3c6a86b --- /dev/null +++ b/ros/audio_common/sound_play/scripts/soundplay_node.py @@ -0,0 +1,446 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Blaise Gassend + +import roslib +import rospy +import threading +import os +import logging +import sys +import traceback +import tempfile +from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray +from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestResult, SoundRequestFeedback +import actionlib + +try: + import pygst + pygst.require('0.10') + import gst + import gobject +except: + str=""" +************************************************************** +Error opening pygst. Is gstreamer installed? (sudo apt-get install python-gst0.10 +************************************************************** +""" + rospy.logfatal(str) + print str + exit(1) + +def sleep(t): + try: + rospy.sleep(t) + except: + pass + + +class soundtype: + STOPPED = 0 + LOOPING = 1 + COUNTING = 2 + + def __init__(self, file, volume = 1.0): + self.lock = threading.RLock() + self.state = self.STOPPED + self.sound = gst.element_factory_make("playbin","player") + if (":" in file): + uri = file + elif os.path.isfile(file): + uri = "file://" + os.path.abspath(file) + else: + rospy.logerr('Error: URI is invalid: %s'%file) + + self.uri = uri + self.volume = volume + self.sound.set_property('uri', uri) + self.sound.set_property("volume",volume) + self.staleness = 1 + self.file = file + + self.bus = self.sound.get_bus() + self.bus.add_signal_watch() + self.bus.connect("message", self.on_stream_end) + + def on_stream_end(self, bus, message): + if message.type == gst.MESSAGE_EOS: + self.state = self.STOPPED + + def __del__(self): + # stop our GST object so that it gets garbage-collected + self.stop() + + def update(self): + self.bus.poll(gst.MESSAGE_ERROR, 10) + + def loop(self): + self.lock.acquire() + try: + self.staleness = 0 + if self.state == self.COUNTING: + self.stop() + + if self.state == self.STOPPED: + self.sound.seek_simple(gst.FORMAT_TIME, gst.SEEK_FLAG_FLUSH, 0) + self.sound.set_state(gst.STATE_PLAYING) + self.state = self.LOOPING + finally: + self.lock.release() + + def stop(self): + if self.state != self.STOPPED: + self.lock.acquire() + try: + self.sound.set_state(gst.STATE_NULL) + self.state = self.STOPPED + finally: + self.lock.release() + + def single(self): + self.lock.acquire() + try: + rospy.logdebug("Playing %s"%self.uri) + self.staleness = 0 + if self.state == self.LOOPING: + self.stop() + + self.sound.seek_simple(gst.FORMAT_TIME, gst.SEEK_FLAG_FLUSH, 0) + self.sound.set_state(gst.STATE_PLAYING) + self.state = self.COUNTING + finally: + self.lock.release() + + def command(self, cmd): + if cmd == SoundRequest.PLAY_STOP: + self.stop() + elif cmd == SoundRequest.PLAY_ONCE: + self.single() + elif cmd == SoundRequest.PLAY_START: + self.loop() + + def get_staleness(self): + self.lock.acquire() + position = 0 + duration = 0 + try: + position = self.sound.query_position(gst.FORMAT_TIME)[0] + duration = self.sound.query_duration(gst.FORMAT_TIME)[0] + except Exception, e: + position = 0 + duration = 0 + finally: + self.lock.release() + + if position != duration: + self.staleness = 0 + else: + self.staleness = self.staleness + 1 + return self.staleness + + def get_playing(self): + return self.state == self.COUNTING + +class soundplay: + _feedback = SoundRequestFeedback() + _result = SoundRequestResult() + + def stopdict(self,dict): + for sound in dict.values(): + sound.stop() + + def stopall(self): + self.stopdict(self.builtinsounds) + self.stopdict(self.filesounds) + self.stopdict(self.voicesounds) + + def select_sound(self, data): + if data.sound == SoundRequest.PLAY_FILE: + if not data.arg2: + if not data.arg in self.filesounds.keys(): + rospy.logdebug('command for uncached wave: "%s"'%data.arg) + try: + self.filesounds[data.arg] = soundtype(data.arg) + except: + rospy.logerr('Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'%data.arg) + return + else: + rospy.logdebug('command for cached wave: "%s"'%data.arg) + sound = self.filesounds[data.arg] + else: + absfilename = os.path.join(roslib.packages.get_pkg_dir(data.arg2), data.arg) + if not absfilename in self.filesounds.keys(): + rospy.logdebug('command for uncached wave: "%s"'%absfilename) + try: + self.filesounds[absfilename] = soundtype(absfilename) + except: + rospy.logerr('Error setting up to play "%s" from package "%s". Does this file exist on the machine on which sound_play is running?'%(data.arg, data.arg2)) + return + else: + rospy.logdebug('command for cached wave: "%s"'%absfilename) + sound = self.filesounds[absfilename] + elif data.sound == SoundRequest.SAY: + if not data.arg in self.voicesounds.keys(): + rospy.logdebug('command for uncached text: "%s"' % data.arg) + txtfile = tempfile.NamedTemporaryFile(prefix='sound_play', suffix='.txt') + (wavfile,wavfilename) = tempfile.mkstemp(prefix='sound_play', suffix='.wav') + txtfilename=txtfile.name + os.close(wavfile) + voice = data.arg2 + try: + txtfile.write(data.arg) + txtfile.flush() + os.system("text2wave -eval '("+voice+")' "+txtfilename+" -o "+wavfilename) + try: + if os.stat(wavfilename).st_size == 0: + raise OSError # So we hit the same catch block + except OSError: + rospy.logerr('Sound synthesis failed. Is festival installed? Is a festival voice installed? Try running "rosdep satisfy sound_play|sh". Refer to http://wiki.ros.org/sound_play/Troubleshooting') + return + self.voicesounds[data.arg] = soundtype(wavfilename) + finally: + txtfile.close() + else: + rospy.logdebug('command for cached text: "%s"'%data.arg) + sound = self.voicesounds[data.arg] + else: + rospy.logdebug('command for builtin wave: %i'%data.sound) + if not data.sound in self.builtinsounds: + params = self.builtinsoundparams[data.sound] + self.builtinsounds[data.sound] = soundtype(params[0], params[1]) + sound = self.builtinsounds[data.sound] + if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP: + # This sound isn't counted in active_sounds + rospy.logdebug("activating %i %s"%(data.sound,data.arg)) + self.active_sounds = self.active_sounds + 1 + sound.staleness = 0 + # if self.active_sounds > self.num_channels: + # mixer.set_num_channels(self.active_sounds) + # self.num_channels = self.active_sounds + return sound + + def callback(self,data): + if not self.initialized: + return + self.mutex.acquire() + # Force only one sound at a time + self.stopall() + try: + if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP: + self.stopall() + else: + sound = self.select_sound(data) + sound.command(data.command) + except Exception, e: + rospy.logerr('Exception in callback: %s'%str(e)) + rospy.loginfo(traceback.format_exc()) + finally: + self.mutex.release() + rospy.logdebug("done callback") + + # Purge sounds that haven't been played in a while. + def cleanupdict(self, dict): + purgelist = [] + for (key,sound) in dict.iteritems(): + try: + staleness = sound.get_staleness() + except Exception, e: + rospy.logerr('Exception in cleanupdict for sound (%s): %s'%(str(key),str(e))) + staleness = 100 # Something is wrong. Let's purge and try again. + #print "%s %i"%(key, staleness) + if staleness >= 10: + purgelist.append(key) + if staleness == 0: # Sound is playing + self.active_sounds = self.active_sounds + 1 + for key in purgelist: + rospy.logdebug('Purging %s from cache'%key) + dict[key].stop() # clean up resources + del dict[key] + + def cleanup(self): + self.mutex.acquire() + try: + self.active_sounds = 0 + self.cleanupdict(self.filesounds) + self.cleanupdict(self.voicesounds) + self.cleanupdict(self.builtinsounds) + except: + rospy.loginfo('Exception in cleanup: %s'%sys.exc_info()[0]) + finally: + self.mutex.release() + + def diagnostics(self, state): + try: + da = DiagnosticArray() + ds = DiagnosticStatus() + ds.name = rospy.get_caller_id().lstrip('/') + ": Node State" + if state == 0: + ds.level = DiagnosticStatus.OK + ds.message = "%i sounds playing"%self.active_sounds + ds.values.append(KeyValue("Active sounds", str(self.active_sounds))) + ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels))) + ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds)))) + ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds)))) + ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds)))) + elif state == 1: + ds.level = DiagnosticStatus.WARN + ds.message = "Sound device not open yet." + else: + ds.level = DiagnosticStatus.ERROR + ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting" + da.status.append(ds) + da.header.stamp = rospy.get_rostime() + self.diagnostic_pub.publish(da) + except Exception, e: + rospy.loginfo('Exception in diagnostics: %s'%str(e)) + + def execute_cb(self, data): + data = data.sound_request + if not self.initialized: + return + self.mutex.acquire() + # Force only one sound at a time + self.stopall() + try: + if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP: + self.stopall() + else: + sound = self.select_sound(data) + sound.command(data.command) + + r = rospy.Rate(1) + start_time = rospy.get_rostime() + success = True + while sound.get_playing(): + sound.update() + if self._as.is_preempt_requested(): + rospy.loginfo('sound_play action: Preempted') + sound.stop() + self._as.set_preempted() + success = False + break + + self._feedback.playing = sound.get_playing() + self._feedback.stamp = rospy.get_rostime() - start_time + self._as.publish_feedback(self._feedback) + r.sleep() + + if success: + self._result.playing = self._feedback.playing + self._result.stamp = self._feedback.stamp + rospy.loginfo('sound_play action: Succeeded') + self._as.set_succeeded(self._result) + + except Exception, e: + rospy.logerr('Exception in actionlib callback: %s'%str(e)) + rospy.loginfo(traceback.format_exc()) + finally: + self.mutex.release() + rospy.logdebug("done actionlib callback") + + def __init__(self): + rospy.init_node('sound_play') + self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1) + rootdir = os.path.join(roslib.packages.get_pkg_dir('sound_play'),'sounds') + + self.builtinsoundparams = { + SoundRequest.BACKINGUP : (os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1), + SoundRequest.NEEDS_UNPLUGGING : (os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1), + SoundRequest.NEEDS_PLUGGING : (os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1), + SoundRequest.NEEDS_UNPLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1), + SoundRequest.NEEDS_PLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1), + } + + self.no_error = True + self.initialized = False + self.active_sounds = 0 + + self.mutex = threading.Lock() + sub = rospy.Subscriber("robotsound", SoundRequest, self.callback) + self._as = actionlib.SimpleActionServer('sound_play', SoundRequestAction, execute_cb=self.execute_cb, auto_start = False) + self._as.start() + + self.mutex.acquire() + self.sleep(0.5) # For ros startup race condition + self.diagnostics(1) + + while not rospy.is_shutdown(): + while not rospy.is_shutdown(): + self.init_vars() + self.no_error = True + self.initialized = True + self.mutex.release() + try: + self.idle_loop() + # Returns after inactive period to test device availability + #print "Exiting idle" + except: + rospy.loginfo('Exception in idle_loop: %s'%sys.exc_info()[0]) + finally: + self.mutex.acquire() + + self.diagnostics(2) + self.mutex.release() + + def init_vars(self): + self.num_channels = 10 + self.builtinsounds = {} + self.filesounds = {} + self.voicesounds = {} + self.hotlist = [] + if not self.initialized: + rospy.loginfo('sound_play node is ready to play sound') + + def sleep(self, duration): + try: + rospy.sleep(duration) + except rospy.exceptions.ROSInterruptException: + pass + + def idle_loop(self): + self.last_activity_time = rospy.get_time() + while (rospy.get_time() - self.last_activity_time < 10 or + len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \ + and not rospy.is_shutdown(): + #print "idle_loop" + self.diagnostics(0) + self.sleep(1) + self.cleanup() + #print "idle_exiting" + +if __name__ == '__main__': + soundplay() diff --git a/ros/audio_common/sound_play/scripts/test.py b/ros/audio_common/sound_play/scripts/test.py new file mode 100755 index 0000000..e864b40 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/test.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python + +#*********************************************************** +#* 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 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. +#*********************************************************** + +# Author: Blaise Gassend + +import rospy, os, sys +from sound_play.msg import SoundRequest + +from sound_play.libsoundplay import SoundClient + +def sleep(t): + try: + rospy.sleep(t) + except: + pass + +if __name__ == '__main__': + rospy.init_node('soundplay_test', anonymous = True) + soundhandle = SoundClient() + + rospy.sleep(1) + + soundhandle.stopAll() + + print "This script will run continuously until you hit CTRL+C, testing various sound_node sound types." + + print + #print 'Try to play wave files that do not exist.' + #soundhandle.playWave('17') + #soundhandle.playWave('dummy') + + #print 'say' + #soundhandle.say('Hello world!') + #sleep(3) + # + print 'wave' + soundhandle.playWave('say-beep.wav') + sleep(2) + + print 'plugging' + soundhandle.play(SoundRequest.NEEDS_PLUGGING) + sleep(2) + + print 'unplugging' + soundhandle.play(SoundRequest.NEEDS_UNPLUGGING) + sleep(2) + + print 'plugging badly' + soundhandle.play(SoundRequest.NEEDS_PLUGGING_BADLY) + sleep(2) + + print 'unplugging badly' + soundhandle.play(SoundRequest.NEEDS_UNPLUGGING_BADLY) + sleep(2) + + s1 = soundhandle.builtinSound(SoundRequest.NEEDS_UNPLUGGING_BADLY) + s2 = soundhandle.waveSound("say-beep.wav") + s3 = soundhandle.voiceSound("Testing the new A P I") + + print "New API start voice" + s3.repeat() + sleep(3) + + print "New API wave" + s2.play() + sleep(2) + + print "New API builtin" + s1.play() + sleep(2) + + print "New API stop" + s3.stop() diff --git a/ros/audio_common/sound_play/scripts/test/test_sound_client.py b/ros/audio_common/sound_play/scripts/test/test_sound_client.py new file mode 100644 index 0000000..89c5678 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/test/test_sound_client.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python + +import unittest + +import rospy +import rostest +from sound_play.libsoundplay import SoundClient + +class TestCase(unittest.TestCase): + def test_soundclient_constructor(self): + s = SoundClient() + self.assertIsNotNone(s) + +if __name__ == '__main__': + rostest.rosrun('sound_play', 'test_sound_client', TestCase) + +__author__ = 'Felix Duvallet' diff --git a/ros/audio_common/sound_play/scripts/test_actionlib_client.py b/ros/audio_common/sound_play/scripts/test_actionlib_client.py new file mode 100755 index 0000000..38eb5f6 --- /dev/null +++ b/ros/audio_common/sound_play/scripts/test_actionlib_client.py @@ -0,0 +1,60 @@ +#! /usr/bin/env python + +import roslib; roslib.load_manifest('sound_play') +import rospy +import actionlib +from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestGoal + +import os + +def sound_play_client(): + client = actionlib.SimpleActionClient('sound_play', SoundRequestAction) + + client.wait_for_server() + + print "Need Unplugging" + goal = SoundRequestGoal() + goal.sound_request.sound = SoundRequest.NEEDS_UNPLUGGING + goal.sound_request.command = SoundRequest.PLAY_ONCE + + client.send_goal(goal) + client.wait_for_result() + print client.get_result() + print "End Need Unplugging" + print + + print "Need Plugging" + goal = SoundRequestGoal() + goal.sound_request.sound = SoundRequest.NEEDS_PLUGGING + goal.sound_request.command = SoundRequest.PLAY_ONCE + client.send_goal(goal) + client.wait_for_result() + print client.get_result() + print "End Need Plugging" + print + + print "Say" + goal = SoundRequestGoal() + goal.sound_request.sound = SoundRequest.SAY + goal.sound_request.command = SoundRequest.PLAY_ONCE + goal.sound_request.arg = "Testing the actionlib interface A P I" + client.send_goal(goal) + client.wait_for_result() + print client.get_result() + print "End Say" + print + + print "Wav" + goal = SoundRequestGoal() + goal.sound_request.sound = SoundRequest.PLAY_FILE + goal.sound_request.command = SoundRequest.PLAY_ONCE + goal.sound_request.arg = os.path.join(roslib.packages.get_pkg_dir('sound_play'),'sounds') + "/say-beep.wav" + client.send_goal(goal) + client.wait_for_result() + print client.get_result() + print "End wav" + print + +if __name__ == '__main__': + rospy.init_node('soundplay_client_test') + sound_play_client() diff --git a/ros/audio_common/sound_play/setup.py b/ros/audio_common/sound_play/setup.py new file mode 100644 index 0000000..e335d64 --- /dev/null +++ b/ros/audio_common/sound_play/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['sound_play'], + package_dir={'': 'src'} + ) + +setup(**d) diff --git a/ros/audio_common/sound_play/soundplay_node.launch b/ros/audio_common/sound_play/soundplay_node.launch new file mode 100644 index 0000000..43df691 --- /dev/null +++ b/ros/audio_common/sound_play/soundplay_node.launch @@ -0,0 +1,8 @@ + + + + + diff --git a/ros/audio_common/sound_play/sounds/BACKINGUP.ogg b/ros/audio_common/sound_play/sounds/BACKINGUP.ogg new file mode 100644 index 0000000000000000000000000000000000000000..4bc6db541a87dcc6be24ee464531ab7bd6cb5d1a GIT binary patch literal 12861 zcmaia2Urx(vhOTQ&RMdHl9r%kCFz2oumoXY$tY>bISL4_2uPMJQI;G8Bxi7y90VjM z0Ru=>6a)#v8}NVbIq$yr-P>PpZ%~|da7sUuC1*uKmh!c+=~!4XI1oc007~F z__%sl+IyZgK(H-me_-;3oc}dK?w*bOuXHx@jAENgut+O%`hTT9=wC*hAl=a3&F;!w z4_la%y`}y+dzczbTvYU$sI<8F88-;g-_CHQTlx@y6l@XGfOTO<;?Mv<3jpRkaOy-G zHF!Z%j(}fklImHlOEe-iDWZ$TDq5`jUj!c5fK1t0-?$s%7{8EEW^3OSis^bhet1WQKkpE zU@TnKT%6|tvD}Mc3q%$M2SMAb`+^0Hlc4-(MD|c&~r%UYvw-%+4(wP z;9Qi5S*>Z6NYB`Z!wXYAKvsX31p~0)GJ!ksjA~Vk163UF-t*l3DXhaF^2A&F2o|;pjD$X?WKc~m%cmWr}on@2afF+fP zvp81^7G%qkLwbwyuOzTTc+MAC`bhfDI~nTqWn2kk#@L$(y$t@QlGKcI3px$4eg%P^p)Z!Mz9u< zl)+!ur1zKKKd>mdmpSxC<|8;FVm8CEiOdYF9av|6dq;NG_TS-y0PSWm&C;C}3=51U zwq)ck zs2LO=5I!r)I)-F7Dm!RzQHp^m7M22098NX`-t??rd+4~ijC&~HUF3A-0|Q6Gw3-g9z6fyIe0OSTc9^p3v2HDU8BN#P@!Hb z7yi=#0DzC9R6pNGbWkD_D5(jQ2wG3!fA$!VIw7e!CJ8Dw830%T;4*0B5ZXwOI3*Va zLsGF79-;+#&FkRn23XKUe9h5suC;=vAjVCd@*J5E*ZwpP;mamh7 z+a4jH0{~dDUa?LVk0G@_obr$uSyoc741Yo5s2W@(g+&6+TS#y9IWuomtuH@`MLjYr zpWZ5>92^9YfIo_ykvJv@f)M~LDN*F`&?M9ld?gOmFSg8qLdq0n>JG_}a)$Ja5obN2 zg%@z>4#9gkLIxF=GoOqo3IG5M4*`El0+QA$04+bTCuHVk{cG7X|Fo?HzK9 zOVb5POG`@Yol7gqO>$RDb4&ZHZA&Z4`m0~PtZ>=@sTIYg)xxEv!Y`}Kj!K0$>WfR8 z%loS%FzT4 zt8rlm+q!pzZdzQAEwD`oXIfgGQTlSD+C`|NY(q&09cY$}S-fUubirluTCB-bOm69V zv*18GC`fF++m)k5l*4RJ8YE;@ms12<_3o1mJI-~xAIW~+^UixkN!IfW#0KOH8w~x3 z9Bv^3>~#o0%>9*7msYR~=0%HIhvn;u5p#QLA-gz|w2=JHZ6ZD3`@dFh=^)ZQX<2?IT(BxR0TjM6YaA_; zG-?U5N@ZI(UcheAe_!AKZ1YPym4?!{E-g zkTbVc3`Y&A+0%Lq$_SDv4a)GBcxWiW;pjm{Qr@~D892OaQihbbfLUEoq)Xos4hL5u z0>&NZZcD5g(FGx=G9m+%K`-uxf%^_IYXQrX5te*LE80s1ETCNq7_C6vvUpM}39?!Z zDGIFZoYP2>AqP02zx#+er6r|U7|csgT#znA1Ib_V7}RLF zHwcA-u0dmm^4e(xX?f_RxH>Ng1)K8DRAV6k*aRoEdA^VE0&ye&f+Y`>D`QJ*IViKr zAqnn`93j+#-I-6Ql}MSuX@K{tK^d^1RRRfMyqEI?EDDlF&j={w8VqcM7kHf!l2GIT zAm|20M3p+$p26`raL*(F{Lc1Fqzp+;(rtJ*EGYp6iUXHa;z0@Ou1o)nkYv0G1EJw0 zwt|HCPLMA9_eZB}R z3{+#ZSQw*aq}bQYTcq%=R4~RVWH}2xz?utjP3?{dZvU?;6?v zXAvb}oJFS(-mSY?*h&9J<+th2FRh&Z3DQ&lUj9eS{_p7hf2x@{mV=P{&jLWaCMN+- zN!a!8T!*t!2%oKymKqH7I#R(I6LW@y!B_J`K=b6>Ldpp6f+&H*LHK|kon(wm^F*Np zc?(j`wvKUQqzK5HgtuT4#GhT@DFeT1HESp};O()0Smo z`J*V%KUu8GKnF5*1gGvO0S(1?~(j+{#E0Rc9FC4gt;Zw=E>@zuIVJzy5_52&k`r`z2{BokNQ> z2W*3Lf*=z(uknI4vcCb`IT%Ce!B8#<>_~hjhD`6)D-M1>K0dLlqOC3XidXe@)y)l!czknXT}y34ZR4h- zw=ZT{tAtQCt-UQwG34svx0k|Z{B~p34wm^YKOXHKb86{t0+J#W3%ug4vC((lv0~|* z9=TvRVxq|TpU4FxZ$-=)2@P3ygi1rK;cXct!J|1s)57wmc-->xl6F(V>U;g)LBzFO z&IzxdTqdcy0qNxRCW)(#02d{O!+q#ful;LCgAx=Xpb; zK2Qfv>1+hX1kvC!Uy~4Pxy$Mp)UA{_)4jMkXk(@8@5`9s;H8X`2Zeo;Yq z-Rj~ylsBY#!|z-Fz3g#!wqjgr%EQNjrX9~@TB%NqZ?!Bh5K$ejQuROgrM)=eR^ytp zc)TxJ&UuhDmBHRZSEGTQVvAG5zrgZCLWpV|;c_S8uWr?btGhv#H_`;de4 z6=(DQPGu0zn6ct#>o7%kh_>DAgoOc2q=2v!BIx|eI5si5hAYAHm!xxCvl z$;I27cE&z#@5^oC1YJK<-Nx_x0T2X4JK6U~pc4e!4{c1CHZ6H>+M?fkom9X${xMRi zCEeOKFg}$KF|##UDnQN%+*qYrn*-WA4{Z1PWYx=KHY-fcR$!H}30IVE`JzdtrK3_ewg zEds)?heNeBXH_VWJj}l7iUN@2q^p(r#DHw`m^PU=-`yYAG|8XGlPpFD`&`lep3Qh8 zI!dru!2F}5iOkZQV+rs`Cum+hmxU*zfgH|Gc05+R<|sQ*aGN+7YW8svx!mj(rAE<)tMC%3rgGB=+(aN*+S zPbbrQx3q8)qIcY_#e$3Zg5eg$QMrSxJZNqusjY8O+ym z*p*Aj054|70+GC=d;q%?ETp~MR0RB*ls+bmbEDoYmDzak@^T&}=C->Ud&CS^O|F-S z+R8J6&{=ETPr?0Zf2T@EYA1P(N~TKJ7UFSgVRU`2L1}s9z0N2N`)(I^e>)sx;kU}B zJ=W`syn>YDCf_%kM=8hTOS))0#Gim`2?}6GpujZH7pVtSr)_+mc~x^ub7#Hv5z8y} z214$wQRP>{Ip&Gvuyt%>YD#eeR-m*B4Xt#C(@&r~k@2eejuAO_Pp&LtEwI_2c?DDe z6}R!g;wuGJJOx@JKLwo^;q8i|bp{Xsys6WDlcrBC{Ys@uq=HD7xAc)RZ2Pj3N_Z{# zZGW)r3-j4=DRoK#@)v6=D;)&p-1~EPS=x;~w|Zj{btSODqs6Ii80pZK1K`CP?|c!{w&Kd19QA!~ z?(x}=6(p|eBoC>U%I{FjM8mpcyB?zY#MQQfF+zap`w{C?t& zNIa}~W9e;kokx$w5dtAMq^z@6+`y>kwIPGjp}9lIVL&9nx0q!7Y4y)~xHf=8FY}FQ zi)`K3$&ECd{xxGdrGn0vwGJi*5S_9$ae&%wb6|v5gGYA1@OOB|bWmo`Z$IO@Ar(RU z3inl`^eE0`24G>P;EyxJ+E%{pmgh-WoK%?)NA;#vLZ!Qkn^<@sh`oi>An_U5@maRp9b{{^cS&6(!^C zcUN*GH4aT=R9ng1svV=vosF8PWtd}$A~{>8$sxdt_0WMiU~};Jt6AYq8)l`7 zkXC)5xeaRoD3Nsj0bo$bWOV_aJFw;0$9M3d2e&&$za6^YwvwoNe~z6apPyTvPDJnE z+Ph#SdJ3T|%1~1(fB=>rY^TPAm(d~U^0HlH4No6bD9L9z#q1#=gdS6S59TDS?c039 zlUMpFuOP@R9#E3eHChbozLf!fKaV#b8Ii={stJkNnD(@isDpyn`-JS3)wy!JlmvoD zxFf@^`asAQ$Zfuk5gXd>{kYOq;!_;6x#B@-13A71oZo=oCzFO3xFH2*&@s-bu4Rs}biMxGCu~_fz z>v57-6mPvByA;^7PfTWIE2oO%DzM-|Ob-+b(Zo6+fD>|?jSuwO`Y-OA{IGegt2CzOGM$&C^ME=O+D%0c<6o0 z8&#j34XI!GK4uOkchjXDy#KLKtbg^PkG51M@IW&q%)bTB*fyYYLtlN)a+{GRo$AtW zzX;X{5w53uSAly+QauyQm2yX0SE-78-XNrKEf6XHST0cmaF6~@)C$nb!uW`^V+b7! zSXE!v*Vqg#yry^}zUQ*Sb)2N^z1;b9p=SGv2D1Nymopb{E^mr|N%7H13Vjk1Z)}|K zob4%~PtTF7LVPVDTDwZ;`Fq%J&tpd>!@A!^?ID35X-A^f7>G)!mx?fr%s@C5yy)h9 z3`D#3GI4}Jh9E8L$2{}$xz0h1(ek_vpKDxPOoEyq<%Z%$`YC*~!<#m+p|0c^gNuU|R73Zk8vRM`wTMdHaD ziS}R$OMVw&)IdHOB||(E;Jm42!n0!L~~nblE< zoz{n|3M+>RF8NCnaRan>igc-gMPe$QdYQRZ3)9(dj;{-Gi|Jm*voJb#q@$+kk{(qE-l08Q=0w#n{R{Tkqb`EVX!n5FkF}j(O!Xv~nMhO* zkj8%}YlP6sB6zX0fNE(>F1~1La&PQ{*`MP9$@J$l25pt3y=VvteG-97XIt90Tm*Rz zA@+K~>h4`Oj*fSSE&=O zGLpiY90_5fqJbHHvkr;a=TOQ^);4bWj<=lT+|tTn`)LeOcF(_jEF-k?`APSQMqH!D z3Ah2m`~$(Z{0}gAT9`G<4CzDeS{EikV?hSs3RgB0<7}*2*nGL3;p&m zBL++TYGH5%)L zy8df9!T!Da!y$Hv!f8}J9+x>c#LbWl+D>7*H0 z1z!;g0}Yl$)xfR+;F4OW51gFJ_|0ryG;)2wjyUa>s=NoKz3uKvG)u7b+RB&gN}m@( z8AcsvcOg~)0l-uQ(i~k%AI|YpG5gF<<>oqwh4U68-nBJpVHmfM4EZ}ozNa-bjr&9R z=JxHO2(qEFzCmcl`$L*$lJK7jc~fz>O?}uV50mI#+IYynWp(dNR6tc=LPyp%^JYlq zWPl*H?mr#GZuG+qbKBaj9$W7FR67#*lWP$6rF|d?(o0au0BzI?PSXRzTss_Bj^iy8 zd3V0`NX|Mv^HQlGH-^LLSF1IW?Yn<2pg&4HZcxs}UR5DjBgFxJzS-&`1-R>1ZVAl< z_)U^^yXxAH7b-2Ml(WQT-Mi@5{d`d*kK$;)l8)_w^BNGsDhfnAGO%ffK^~9*=FIt) zcjMm06&1$Rg{V_FdQKKf%vs`o7hty7i9X!wtjz9RHb1q`BiEQD>PfX>w-J=}7i+**WfO6k)#6JL zw747EN|MZ-Wtq>l2+b8U2OdhG2moP;1-|%rNJjuH_vJ zSvVkCzBBT8;o^RF_JiFC8@0Ng4-QNMz}I9fm0JMh0wM)*N(qxCv*`lv{rUAMDCjEl z!2Hxj!+TCLs79r&X&?T`P+P>zs!isqB#LocgYLU|pe4@8)|r3+KP#*|iwY*tc}t&i zC3)T>nnwi43ci1dGP+YQB0AgPpRd4`d?GEnLQRiD#ZU`8j`l%Gz^t; zTf4je%6L^{=hAq{wQ@!TsyvXle7s~JV9X$X8~EpuL)G~chx11cP_UZCGnsIqq~KKr z_(q_rzOn}2i*KuKtZsc(S5Y2xpt7bu0t5yPJ&Vt7pN&d%eAa?~Q%N~;DhT3=ng$3V z-n;^E#ZKUiFO_4;JzE>)@4`=f>|4y2NjAr-2y=B+)>Db&96v%C5x z!MCJN6a!wtCx8rByQ>_jMQ|xsH1&4e9R?ejx@t?^$2aV3Y8^7oBp(btn34wM+#HJo zooGnWL@-gsC6c-g;@7|p3u~iq_UTP?C!<$u7vzC^YeZ}6+QFL_haWAht>rW_r`TR| zZ|G=fw|@K#;8=o4d%?W0HIO)XUkYUDv*>L1SUl$7fzT;~<6-tjV|S#Xhc19+()^)p zjr_n_N=r|$fb4Lnu9W2jXRm8u39rVTa zz#KiyP<@iZ0JzJ zpIxK?Pq{EJ@A7s!w;(wHL-ZYAyI}oomWxqt_Y+qDK-Dg*tev|^{#oRm19tmPU>t;P z(+3j)7#2?LY;G7^I*#;>5 zI!NQ$$xc$4R&HqrJAp7aa@}$oouqbsaPtPe!7bTmWKh0143k6wBJ8B{g~GM&UY1EZ%Mi8DHS13Eel{-cDsi^>RXAvuYBT%+ok z;Y+_~)NDKeOXLqC!naTDPk%YT&!o(GdwDQe`%cSp2lTPtXGiE~8WR%-d~({eVBpZA zh3{*be5M9d+l4VohDWzsrll76V-_od$RoSBOf_sOu2Q#B%2BFf}j#$(Ph@R z*JosTh=Cp42-vZ`-lU7_QJ?Th$Tf-kao!EMjg^8#4 z>Vw06Or_QKe}v#xZrrm-PjP_#81T6NLtQ_8FT;xM6#6o>Td87|`eGvV=gD_=;%i#| zBpL5NeGnvJpE5l(+q08{mTSMl-FmKXBa=}s@Atf~ZM+%-G)qGkt}}hgYn=?=*^|;! zckKE=u2lee0d(@!wWEWXpwAwO?+o317V%>pvA*}kL{juuGv?+n#gkkSl@O+x5>6d~ zi&cHkXLVInzI3Ebv|m@l#rX$^-Iw}VCGpi#EClyc{<4rxc4!`?u;pp8_(1K7e|l3- zFX?@wsyG4OOuAaL(#?Ev9Q8nbdhLRWt4WmPk>9@+{>` z`uffrGk5Z*DU>d190Aco2qg=##^- z#@!a(lk~e4cF}V{(TR14P~qTVrN>%Xdkl2K?=abdzR&#|*nt-U-iJ06^-ah{K^*Fs+X}J+hd}&TCOU;ld4DPZZG%R zs=)+Os`s<-rRO*C$KuW64>r>@?mo*MBJ+-;-kol{V@ZCyIOn%og~KZL0qMH%+hn)- zURa&TGUaZw7R`t4mKUvPm*?YX`n#2N96Z7Ey@Jn1YL$jQMurMj$R=c98b@acbJYBy zl6BXHa!h2iY=eLBDz`bR5daI*yDpB<5yqbGSiY8>c5kc@WVryPW|=G=`RP-Ub4J;| zD?eY)!p+m2rS*ec)vjYFq(4=Ja6w(jax$j{13^2L;r$5#VrttKtC)6P8{LnIl5-aN z_3x5ZIHjYiX;@goxM%B{3ySwB-uKZi}_13<2fp)36-Y?$0s5eC=T* zF)76o6jr{fGLVl^5yNXuY}_uotzf&g`DaWodU%R!f`G{+Elu2Adye=1iH+FkmD*>z z-ou>Bz|ZZ60UuZ_QBCc`4)2Hnj@=eJx7373t4gVykF2Tq7hZp+S9?k87WIJa@#IF) zeM6k&)E2Z8qScVj z9Qt1RPB@gTB8n5kT|VWMIKeZvRneWlft-7m4=KHei=Wc3ypg&h>}0qva`)!#`lXwL z0>wh9r+^JYEtx$!!7)20cbU-CUgv^$!{&$DnHG;%JXrz@CL(6vOWGX2D|9fHAv5U6 z7ieCkBBS0>@HDlzD$E+}e-=t#4U8dqY$p4=3`xxNRCTQvoqtTs9!e1riNZs<4mO>T z-}ZV)y+_B4w~Ud@R;6r&K`t#0vjUXgceoI1nokthL%gZCsd?4yFL>^9 zP9`>{tqzS5H+xA~9NvuC^3`M2wJSP7x^nriGcKsZ&E{U**Dp-fIZavSA_+~BST3e3 z*n=R;kt*`@KL{`bZshoIlfnbPCnt7^v<78_VqEb-)*o~b2bn*>(9~=-k6&iEn*n!R*Bf-ol368L`QutNsro9HbKq@x}GD>m<)X% zNehPWul)z_NED72=Q{-D-kOHL7@$f^@PEY@kk799ZNF`DyH1Y`nk*$=nb7Zh@P<)` zM|4ZS9&aprkykG7BQe4ip6d0H3nEmX6P0l3!`?b2uC>+*#h-PJ%O$xLujb!?K4JDX`u zOkdiO!D3f!L@1_CJdCo?X??YEv*3Riv=L3< zkF_UPJ_?G$o6LA0KH_hb_~cA+32zGfW*l*QQ%i2n;q7Icd6sxGkCp{BiW}E?pIOCk z2UUt-vRCdq4@GM|xoPTZXX3BIZfSNk<7b3|z9nr=2+apRK^HrF0xyOe$DM4kJh+^- zl83s9_q5pvfg3ZspTg~Iq6x_zW9}PBn%`O>;KE8$9Z*r&VBj}A7hQfY@|!+4ZK!Db z=|X*5Ytpn!CZSRRWxR;=+YLJnx&I zq`}98WNcVHhMg#cmdY@^@v1Iq21184#?U}riB2;n2+-bV`U~|%fC%x~nTuHH*;7hH zLM_v%aTRfpOOTu3ybn!;^Hb`qqb-V-kh^*}@}+f(QT^Am4lh8o>3qK!yeX~g=D!*n zeM0J-ZDIT;nmjAYa5ZX=O-4dbeb!_)Aqp09qb#x#8+j5hkJ{upF(9^&v)W*NSYyVL zy2t03G#nqxltl!zH)VdxY&EryL*>b~8eQFeN286QXUB96A~KF9LVFCZ*nHbqi8=LQ zcimsGZHeDeuXkif?wfX5t0os+EmfbNZwo{BH4MzZB*&Et#x>Tw{K-7+WNBXZsJt$_ zoM<{cafL?eT7;n$2DY<@FtjO~9H(ZEsmEyu?a47malh zDZxK&&$A@XP#@Mgm&*1Fcl0~F&2oT`!8fd&Pv|Mj2jS>$#jm>aLv5hEpf2rP7dAfxXDYQL!QTv2D^TN8~GE zvtw!g>y#I_rjm+FKgjYr2?p^AuV(pPj%Y8B)8_KJ5p2tHFM{KjbRz*Gan3>za~USE zwk#x=U<9#RFKUs;T9>Pp zDoZ4IL4Q9ikgV~1SogHFmG{jbmKLHSb@{F*&GGj1H#%=zt-in0_GQxr6X0U4?DdS4 zuUYQhZR4BtTORqiR_`{R8BsFQz{{U|3IpwQUS2Je4#9=*_W0e!j2p@5#~(zXpDh zY^CY;bO$Y6sG_Y@r|t?Q4*IiFaJd(9-HkoL{p_d z$hdgtO6G3dmUU~jNX+s9?l@g~aW?hELlg0|A84Hjw2QK+>F?t=bh*2o0_7L-rZ<8^ z0>q~I$6HymD4%us#)XxR_Yexqq;8UYUVZFc;1jIvS|E`^F;f-sdZFYe(}riAL~HSH zRUJ~zODontOb&=Qf51NqvpYn8udADUDS2>M*;FlS#h1h+;!d}z6tG0|y+V@h??VJa zp!MGm5zfA62rS_Vh1Rzf1w~}O<4T9JNj7P%Qsc^Tg})4iY;b%BFUALbs7#8=o?bKZ zxVhK-jXGN)uT!Bqged+6d0n0FDNO`+k2`nAl)OF-n$_X(bB>2FGy7?s58v0h(`A@q zhi#djoeTJ`&Pe?5WYzSx_>&N;(l)Ezpa-;RX(JY4##V=?ACyi5u2TqpVcIg|Et}Bo zPJUx?S(;O9nJ2I3UJj3Qp;0Ss^%%EM+KI!^!q_K4*{A^hEeh|p&{vsc?N1rES>F>q z`Zlh|tmzse$c9tWoq1;LsLx1p@=YPt>Eq7Ll1CL=?KgrBKh-(4#C^CapQp*ECRlnJ zVk8_$T=&qQC|&X@9I}gmo9%s{cJK^ziy{LCbyB}yzHw8peMt1_&}K2=y@X&n!LgkOpKyT9*sqS) s9dIJThRze>i&PQ`w*OGJzW96{-6+Ex95bhF9`cwn34Xxb1GxeGA39aQg#Z8m literal 0 HcmV?d00001 diff --git a/ros/audio_common/sound_play/sounds/NEEDS_PLUGGING.ogg b/ros/audio_common/sound_play/sounds/NEEDS_PLUGGING.ogg new file mode 100644 index 0000000000000000000000000000000000000000..0a15885c383ce1a0d03dafbc95aa011a87811655 GIT binary patch literal 11050 zcmaiZ2Urx(vhOTQjw@N>l39WT$w6|?3rhx(j3kMIh%8Y+1SDt4NX{7%lq?`1IS7bI zPKtnn@&^2$d(OM>efQ4S+uPICUDaLntD5Q=9R~+}00a2vQo{*1yK1`HCeuS0AinON z)=pkm9T3Het3U912)X`if#_V#{IBb3<`u;#Vy_v-d-;D|gV4W>=s~)%(*ws_I-U-2 z7bk1OYxZz8xFA2jD8GoH;1xFr*5APhc?ClVfD85rXu^BZWAP{efB}FND}p%DUJX%{ zl*j3xmZW;s>gA70ON#2Hwuuqw`&R`QvZVk3OaRS^7hCXE?f$5^12spiXRf%Dwpa-q zCt7bs9PjN9b{n_ihGJ*8Q3M+-qc|opLOa8u=Ze*NQ=Npe6neg~+C4B=e6nObq z82Q@G`1;KF2AZUWo3#X*w1k_@hFhG5-=mKBruzsf1I$+5i2^?e=B`#QqwApis%a&_f8WfcEE#oi>v z?td?`jsskPEU3$V54wI2RwX2Bzb6mQwT1@((5A9To?cHO6)&NFFDbC7cq3KdprK# zwOY_1Tk1Sq1;W3QzyXnYU*H*I8Q(|5i8I!7OUW}oc1Iaxa6|e**JLc?>#J6Buoab* z$xrR9F zhXvpBTvZkCMkRJ?-oX?I1wa(@2!kko-uN}gt4dtexu|j4l)ESisg5Z6>rtRl;kNN| z*f+18BqGgo2vGuh#eW*^koCh2`tkoLACBnsNZG(LF378YTN-76GV*mbN%dK14!4-~ zS)K`8o_T(kD&oHb>%Tk)0G%f6+9#8&V+8uMLbc`b{u=mSp5sB@|AKDt1*=9KtJVb1 z$TuOK??U5bN_s+C`V>%MP-z==DQ44?X>-`<-a^f#gi2*pyw!gvi^tXbP3QYfZkNcqxJpo9;L@Yg?cMd z{igu{pevqG<2sJ$A$g~e!c$0Il!5gB>@gs9N=WOY5UAK>0JsSNETECYV9}oO@@~?` zxB_2UvGT;R^LnVmN?^AM#fg}`6eYU||xqMs#)| zsZCTRI0?W3e*}4B@e~kQasaRKw|dDu(K+%d1>=Kx);ivO1o!GM@6fit{p_ou*f1 zZIwfHHJMfAJ52`!?|63F9+Xv8mDlZFQQIna`aE{p_#YP|-WQi|x0SZFx$U%h3^%#b z6_jmva=m|BGgNo5)5h1}!=#7uSxT~4GXnSFdiP`6-xYDx9%(C*GIydfj6+7~Js6dNkyJb-e6Dqf5(O9#Y=lNyZ zZCvj=K|unGeeQJa{B#xzA|N5Vu96_oX5bX>gY!b4$GODXiBCZ_AzptRhz-aUHpqr# z^O(3X&|(k(n`whwAI8-S_l6<2;e`eQ*i2s9>b-PHy6X3tk|w$O;0Cd316hfr>W+w{ z2_#Nlk*>OPUeYwuxwd%HdJkSmsqVy^G+}zbv~SWh=7tSSaHG(6R1L4F;fesV+Q=;= zmRBz-B-c>qH5>+kTreT0T3Se|uU6U!vMMU1)KC{_A~$mrn5F|~6q1^$A^OPmU3vR5 zSo+`~tHCSb*D}`Al0a7Jp$Y2hmB|Un;jF|fR-v)#YKW!;bxm+p>gu)ONK+Sq=`5s+ zFt{p^Rf4(;Z=b1|W}^?xl;C<*m5qkZ_bdBA;cP|J8tS!;l+JDfUuo2xDzAi>_EMU~ zg2MHkg^Q=}UlGh)rKdBz)GMbakkvsjIOSfhjFM9hlF#j3pVeMLw@ z;sXHJ127}1(X;gmd65goObo#PDrTySJxTR80LQr?ThF63n(_J_S z4Hscw7znFAQw8{pjNw05hT>eSY)BfK6N6!B5?EQagnK@U2ko(UHkbQ%X z16<e6TNq7;EJ*7!y*uyg<+t1@)Xp?IoUvzAP^vYK#xu`RZsUq zBDvU#Qm#VBq^UYD$Qy^PXc~DRlp=v!)G&D^YF17aQ8aD(j2Cn{b@0|YMmPcq4(Xk> zt`I1kK!W~BZBqd{kf}4cbZ1^fL`nTgN z*S2N?lWqBHZC@P>kf1Gcuh7Dzs1Bm)3L{Kmpc(%5g~k6@A53E7FSI~FZTuaVq$__7 zE!;e?4?z!tjPtt12Db42CUDna3?l_oxg?PD-@YVLP{wt7mxR1RB&eDz!ZlQ{TX;-Y z>bf4x^mp!6E(k&tdKHCPHdzaXm|cWc-$HdA2h;&HApm&H-rqq?55Rk~!csKC;yq#3 zw{9S##Z_wURB!fDPbCQyk?SL!H?yo`^mMXq)CJyEBpIr6z6hD{tZ4Kd2G5XiT!0_O zbURoULQXvZ3JSwS0J$|?V*=)wm|sW*@d@$!!@MS(pa3jvfbd16F(D~gS#Z9!1Ym4# zHGqRAeEz)Q8NsGt7na4M4?GNx$rRZOA&*CglDYYPCemr?#*5+0L_XO85wCGTkviT%BpA^R@n0elkJ*(RpH|P zJ*r?{xBniAWySxJua3l5V&XSY_Owb$mui>RmlBu8O5(HgpO+`!e;Di+x}$VyaH*Lh z?VUd!e_la79AC^+UiO3vlb6oOd0gysx8d+X?DswRjGIGadeqc(n(VDknzDCf0Y|!l zb=5HbVPNU5yD8T*FpSt{1oN%dzi<(+qK;8rV|JF`7soYCRN3GJ$Sz|Fy|@t2R;rbwsxYZo;NLAqkg%hsC7)Btu!JV?@sO)Ex%dm=3>ExgYzeA z9TBU(*rzMF82kpo#~BzR&=ep5LsCFC1)v>1XvXRy9y`JYI$p1IEdUcx*)`K1sKN|c z8&9XbJ@#)};?0HA!VFp z1@xEdnJl1#YZSUcpc>ERYe)(&%;X+Zc`SEW6`&Mq(It9FoEywebGGOHT3qT-eLt8= zCxihKCW{8o?k`D`{a5im-J_1DZ`AJ&awK9#!(g)Ke54Ba{QU6XppidBS{R*!zGUR) z@cNhZ4LSRi7%Xv1Z8BJr#X-tS7(kZOMkGi06GScmh{2*2S0Nu#<*gyLP(XJqXimZk zC+&=srwgc|ntY|2!2^lHsWwxMFM?13!Hf4T-oqPt*!|ho8xF-VV%Gr=vcm`c8!TUT zSJ~;^nh^JHsMG%uT}h`1GT6iix=6xB&8WYJ^_Re^YYyd)81a{|{YG$xpPK3tp_4

Ytq*el{dTuufD`+@73+wG-GzCyZOhdN8 znuRJTor;yPeYkcz)saIOpeA_51t+P&24cWIzWgcjbrCsVG84S7_m3++su> zK=7)?A#SARH^?U%A#NJ#EM@^#G#XzDEq_MLL(tQHZZ>V_O(8s`-p9!m#9~Eq%XvD(8eepG)&mHl2 z>f3RU!v#;|hUk^g>dU>-Mk1b& zPo^u=q|2D+4sW(;CaoOCbm|!pC(kzeV~|T1u8tgkSP#b`rg)E?2e^*xc5fO>Caz*H zVi+4ta;Sf^kfBjtVqSaRjH>s?zzQ&%uajmOv=AODtcIMItdz>eB9 za~QADjFija+ov37weM1iWFjJTl!g4k+#z&S7yk6&-;%tKOeL4^h%vRp(lAx=(ZbWmF?YW1>?WR`1nLC8fb@Loxm|ndW9mgg z{OX+eF^|=&nvyfO;H9jsIhjQI=d;ypVV<}y=};T?_!9!%3_el(m?Oh}e!Jh{Eia&Q-05B);7Dm-$kMISr4f2p zwmy?S;l<+HW?x!D(LvOTERXzgK%o|{f6rsgPpeA)lf`m^35i*lb$Z_^lJtj9VzgP6 zF(}TMCF&LNSb?M*VJ7N(Ed3!VeXeU?^^Wm4Wt}k^DaAgA?45fQh%o4EmW)Q`E87h6 z3(w5dldX81t|O;*&bv3^4lV6EPaN3D8HEA!JMT@-<;q+!n~Vr$TWRm^{xYpJG?R%P z)p&WpSk=qPQrB71F@#Xf#(22bv?$=}bk4nCMiyr;Qo}&`(Dd{|-zr|;$2yV^r z+5B2&FD)5QWL?%FRVFr|o#G^v&m#nimqn zsX_EjGNI?|_BpMDcGOGm!_NGTNsp) z+r;@}t#jiE+`ykx9o;G+-5yo>i%U~MTlt>|?8~zbs=1_tqKaJ=!pF|c)yLf{ z1^dB#;2LfJK%X5hro%4+)j3p_a^_qchV54`zS9EU$V*HCa_qX z_R0HE=h>%G-jwfCzob6)|4y2^SYXE=$1D(7EWgx~vsh*)D=r+KWAi7wk-l^0=#`YM z0fBNui1lMsp(Aaiq<>H}@%ON}hj49HN~(noPHsdK-bA#U&YB=iOcz>la7WqY_H=k= zx2rw{MvKg5Wo6W&BDVG_J09gJjeVYIykX*0e(&_iVD@PBF@f%kOx1g(2Hm=q1`SmS zNfujPV@({o3>m#C%+Pdyyx^o;o)rRKJ`XqSMm@+np`A*{HkjQiki)77c?v7dva%R8 z`C;;YoCB52>92pzt1VpVE)qr0bM3x4u6o3&{gVCTf0vax_x{_kec$@J^ETH{3f73UpZ^Mx8RkZZBA$c^%G4o3!S$iPw=&lI~#Yl$bD#Ov}FfwvNGDyGL{yQ{?t68tS3%1gE{0?D6z6T08p@Z~Yt4Zpdx2!Xt3;|pg`#snIV zQYqHF3Q8gtSZ_isvclVx`07bi_Y#@}WgOn^({OuOI2n;x#kpnTF&OTaO9lh-YYcul z#Fbe|8@-&)alw@fH~-w;qQbt@6Oi)2or6)ZL+2L#6RGq1oO=bGarjS8eXzedZeT}( zPcRmL&1IkI^?+|7RWaTz2e`Oj=JvE6nL0cT=?(is@tG*$CC;#+2(l8Em$~1|`=m*; zLBXMWDnOk#U8G-BI5Dm$1mi7~FIIS9nZF0Q&HGrVqAPyuR`Xgs%MPw53Zn*#d+SSa zw72=d1$=w8`#5_=W+}&KTO&b6$*d*mXO6-9*qhwdd#K7nygSNt&07!*T#6mG@JQ@e zTr5W0Y}x|7jgKS}A3b`UyYwfM@3Qm9^26IrQ3^Wg611-?vFn)Xmu22LzGnDSR>pO2 zhg;Z@Ft`XcocB@6rgrH|+F~mH#1N&eRyYnI6?*uqtwsL*>5-Yl*dXS!A6m2TlzU9&H=ZizQvSI0E+lOAmrkuZcHHgb zPj81kHzmqUpJoX3JtAp8(nd8^85Pf(m{@~y7I5RLU6Xf=8sx-ZG^6&JpL3+upc^07i#DTSl>(UU zaz2*xH=i5y`u8;uGt}! zo`}3NzNYEn$Hu{>3q}#f^T6Zblg(Y7=D1VaJ=Z4IjnKJR;m;b8^C$UXOb3P^2&pXj zRqY6@(~M(rhtyZCFK5E3XtNlXQ>Gb@HUe8No$vxIAN25-VRJuKC9Hi$t>U(0*Znep zidW8n2xndlAKRzZ6Z5w@(=DL`OWXcfY}}NhMgi`45sRM{F$6aZh~Hr?bQ6DVqJGua z+k0Z(r}=x43x*$KdY?2N#`nB#E9h|Z|fG%T8M$P=Bxq4k`oJHdT1<<7g8r(Xirq!!W7uJewfYOQ7vC58#t*6LvqOopCm7&|tJ@Z$p=rrf~n3|Lu-1Fp~)3yfv;9g#oD*tQGci=ehS zdoqg0T|=qHBgyQupLTwC9*yuX79Sh&kRG1z@-GP!9p413>PW^8AOobbBZs#}h->6K z34jqx-8FJJb;T?n27g5wBzq}HYIL}#2fN3Yq-VrmA?R}X#LDf}ginDEWwa#Es>tD_ zjrmit!3=4++p;XcIw{bb(_oZ#8^2}~y(tYjp|SFwTL`j$MB0TXrdCTBF;)Khw`3eu zu(J@EE-~DKVZ}Zm;CuNTdRST97^5Koo=L)(CEhY_>&g-Gd<7eBSJ3Lxkrr@Pq=9e`1jd&r(EQu98?`f*X$0H$#|Gh6|$#^@|> zbRG~zC*yG-YMy2)pnom_i~1|^fGQ0-$w5xg-gq1uuZMmN-DC>=_K?6pF+hx%s>bH9iOk`Rg(0AXkDrw;Wj^{^# zP+;N-4gi-1aDJ102M>XoS#r>`0J$}6%uqMGTa477I545M9;18ZN9c8H>z}_5QLld- z=+TV%kZwPhk71I<%yq@@(nUvO!2vf0M8jm>2YGRrg*HYlP%(xK8%)nXPuRRG94(5E zEU2+mR*;Cr9Qi8ybqn1BStObKO84X;aYt|;k00qHl%CPG$FWMF{f@^nsOd*`0a znhAE7IWOG`P*=kOq86`)s>Kqh!;JqDXyOgN=@GyH-1&^997DS2AKgIb?cfFTJhGeF zU=06Q`8>o<7L#w|nM{iH5Zo90X~yg3*Qe$O(wtJlJ&dTGg%?Zsdmm%ZK6Gee=SRk3 ziUTdY4^r^g(7AXjyN@QNf~Cn)!Z@pBcYl&T`JhdLkv0$AH0<>B!T+xGjr@K(Hqct5@#AA%zJyCclr@NgA1p zIW<{K?O}ybO3avQ-swrKP#MXo_ffj_HK!`sXW!S-ZQ{}IQLo$)E!_P%1v$8%X>ga( zN@b@G7T_YHWGxyJX1URGI;GI&9w7DR^GAm|&uFGYK9#+{v$ET%hH`?;Ogmpv zil}uHaqwUOk7he8_0bTk-w=Sh;9f;Es&jWx5Zb=sp92=&2X3iuG8e7pelzw+GP&QY z-RJwQ8)s{A%ZG2)BHHO>M(L~B8si?6`aENrsNk;ru3)HhtT@0_hDdW@T#xV|HMtEQ zRPPc^teXkyYoio*HrZTjDMB&7=Mz8tslC-NEvftT+edB48>i1fk<8Pnl1XV#@*G*R zk5x|!wA&iX-Xxg z1<6d2lM6#3j`l+%9RAWmk?n=r(whngsR5>sp-)Q8AIbn)z&`jE7C+uF`R;h!xI^D3 z0ALWkl#vuaPU^9hxcubb^k>T;V?SnSuv&#fkwuz(v{imYCAtyGR#dP401XO$9 z9c9V(ZBzPPr?hLAzcoY&U;4cHr?GkRWljJ&*Y{4>w_l8kZ!K?3`{F~Rv)eJLGeuZ) zlwF?nk&)naokD?JsH-1xU1Q+_U7qJ*TRA^<^{-+Zuj;9h z)z+iz&9shK28$@egyI$=BjN!knt9Cu`ygXZZ<{)M4>1mrS%F@jL<&n;7nYkj6z;@Z z(u+b)cOHw`-q2R4uE&mCPMdm$8kOg@PRCz5n=n{p5JJ=|zL)aBzB}xj8^|M)#OZJ`cyQp>)B*xA?bS8(Xn(C3hFRu{B+Abc@R%e7#O=YrpeRWZs{V zWh72+bdGd&F)0*t>XF)Ko?I8eQu$b;@NlNDi`nl$3t)1llzQumtm@b_w~fZIS@-K8 zD@ff*>AN$ZTDzaV1NBICcuZJw!+l*ag;gyh^ zySUPrCC>Lt-Q2_2k$VBD^N1mesu#){$p_w|!jtvf7GdOb#Zs-4wpf1O{DtW?ls9MP z*}XW%8HSRUmtOmh8Sa_zooNLXwM$dzYAviB+g>*9f1Fy)Kv}xTx1=6blCBlI?GvMx z*6Hs{A_|o{d`Bu-n=!~4hJPADmr9cNH+h>5;cs|SzZ?N85ZO6hM?sHjD;iqkE{pN$E4 zICo=4bGx!B8bY?MBY$2tDcYv`G{3-OEZ|W2M*LZ(lH@pKrC?I^jZXcFW5&L{*}|@rL z$TW7?6FnZ1q~WTX{NzTFOumbv=Bc8L9?eDqIVer94gj)ZIpM$+CF}cBZGwg)-~~ea zd*SjA1ry3elZ_M zvvO8^*pn|`WFr-YYb03P^#w5{=!QNqO`osS%Wsv(2N;y;X8b;5!f19-0s+84Rc+aG zwL60P7vJkgcc*hCJBLnx!80=pejf=7`^%i~M`$!9l9%@h90=dWZ^UFt{vNocE*VIY z@qCA{==O_5TJH<@IHtT0VJW%1C(3L~3(hIw&4DE>{y!d+eo<3g?qIyjj?UGb{wH_4bUmdF*|?7XheN(o8tIO6`VXOas#8pkNGR#))Nu;T|>k}U7mr- z+&@Va&KF-gUtbVX2`!0Ou=hY08aLAYk782g+U2OD%3AG!5zI~{^H|Sdbfk$8eUt(D zSW^@aM_sk_`>xOv4~e@SW7_UNzb|PlnLMG>dNUaK`&hp7W5(cdBGuDhTK44u^OkIW z>vJ@fVVcb?s2owOn%&RTxd_#CtGhqhp`SEYs%u!WM&!>+8{aNBP1{fgvInN~-(Z>B z%v9hSE>5x{+8uKf$%>T-rWB{!#c}5?SiZEqAb&nvB!mwOpKfpFplC%*5ZsD3y?J(1zm*K5S)0S4~Dj6K@SQ1HFlA_;x#^B3W1)Fpu0Xpp-n=jB}0LB>Y)a$B3 zro>L&06I$KfKSl*$HwZV==jBh^*?%Qq9$Ly^N|?I!)IzC{3KqL`3~GBlp3}}mygN5 z2tH*CQTzGLEs&9s^t6EO8EvnO=3wgXPIp5~%dZ0Gh($ literal 0 HcmV?d00001 diff --git a/ros/audio_common/sound_play/sounds/NEEDS_PLUGGING_BADLY.ogg b/ros/audio_common/sound_play/sounds/NEEDS_PLUGGING_BADLY.ogg new file mode 100644 index 0000000000000000000000000000000000000000..d336822e814007b8cbb3af0509317f145ebb2e3e GIT binary patch literal 35782 zcmeFZcUaQj+c^Hhy#oa@)IzjU#L`kTTWH|cM1c^q6w?$}IVvj?afCR^nVFX2M(`~u z+OX0Tvm9v(nq@X@XM30Wds*+#cs{@9`aZwwdamDp-*X*ao;~hy&VAqK9JlS+;{(V6 zf4hJab2uWzLI4#o#nK%u zVGW(2fa3yy0Ao|F{16w@>VgWBgyI6{g^7@N-N*rq{5xG)4_uSI$;j(~C}VU-P0W-_!0zU1g5lOOuu%2X*t7+pg}GQfg4+ zWhMV3v4iWMmHz!v{XGvfI2H$`CA*;xDhthXOlyi`z^4983k|TqX#!5Ua2FB0PozIE zY`pCQj-+Efyq4%g_Mm_+-hcc4;IaMjWBU_*i_`qhC;FaG^BYg||C$!4oBp@|mN@lw z!T+8*8UmCXtuK0P%EO>|kJj_#Q_}@{(*!Av;zR%QeU!9{{{#RZ}X8D>+P|d#$r9jKu|0| zQ<<1)%epBSb$S8jV>7~`8cp%tK>;}J^b`Y zX9oH7$_w?yr+;HGVsOc05SOMmPf5y84VA#u-v-l`JS)}eSrQ1A z3%gySTz`iK*7Y4Kd0KY?4u&!cPOlVSApdFaZ?dT0SvuIc{vlW+W8!r91yz4A^nG{f zn*r5@wErX@JczfzaowIWHaam!i2*mPG0a~YcU&vK+_KlSYH4bn1RUQ|6)1;QZddNq~dJx(=)M+ z>z%`^#WK2Cb%D35Y4snD0+EW|eZpR8$!{h}FOIosS_^W;za#FQ@hzCa@W1(6@AXOA z%RxW9#@uJ^CJLFdeSf6y$@sf%Y5wE!Q)7u!W7)eFr~fBm{gZP5kZDqX^JD=d%kp|@ zil>9>9|Zq{bN0cn=jz|cHFi5^>~R}6@WE=^N2_5SBFW0b2jM@Ah#x@)``Gx8SO?Cd*k)=h{@*xffkiOua~?bADg29b%1v?}+2$CzdcFJp4xwKPAIpwm|M%?KI7z=WE6#El<+U`B5pRmy7)i(e{zmfj4|jz&LPGa z{~PCYSn6*Dx#=9vZW;yw8b@6G?+5^ZjuRSgzt<6xx4G2YTIy|1A#eIWa}1a( zwelFT0s&hH080R1x~gXMkd`*%#QF^TP!*SMeaQ~H7p0XuvW}fFa&(z7^y!Bxh>M3@ z(%_?y=uCrHa%Q%0x3j%@Ymc|#lR9N^*;4^3mH=AUA^INj`flJ66KM9a0kc`AFK{8-MFYm^@P=;Gs?G8!u}}<0_X%V;|y5Xc_HIj=RfEg z9LPI&of_5QRHod zaCu>J;PwK7U2y^IHleOKEw9jCR{(7cbUd^zdXm61;ezrL|K>hi-$w!o?%_l6Klzwe z^elsCQc}aexC-p(*6%F}6z)Px{+Vp$0S8xMa5eT{T=Fzyp#?s_Q5`HfV&wsr98=_l zmekbq!JP#EyP!1Qe=%nvjSucFatnnh{$qu~tT3?N#YUVw`A>XFIiLUXVxw6f018ld z3WI%>9xTMp;Q-)42LwQOsMy`8JEx-OasC|uN1ej}9FP{W0$nYuHG;Gq)G7kq9h?n! zdwXL|tJNH^%L~2jovW+7*SMNj7kMih7UsFuqpQ5VO%(x_fi3u>sidU~6o3z)v=>ys z=wJWL*umBEX=$vsVYMzO-qgc`7G#`)JWEhc_H?~b45qED-VK&3&G)tg)3iW=BMvN5 zRp9OLyfhY+Kgz%&5CH!N+S~|F3vKdma3g?@2Mg#rvl0ZiOI}UFnU$d2%FL_!lTxzr z*5A-iIP*7*WBfsEEC6bG-vW;6`Fu4nHUa`FZ9(4U^Iu(SU4Z{seBj*$;J>=q0m3~# zc**&N?Hi{K0NeHg0JbSGJ~f{w>Hs%7xrO~Q)l$V2l)01)tDp|frfDbsEBQlMU>z3p zZwTK=WK{Qnsmr-uv0R6utyO&;%?n!Q$FT!zR|T8uiU4XCJsShwd` zJ~uANb9F^HWUV(@URXru3LQ9>D%Dk^^axAjt@X+kRRwgeY3m~pL0}p<75WZ!!|DQh zrg^LG=?Fs*YvEv8&{|h9mV2ws{9s_K~;1?Qq=}& zx4;{$mj?FnM|K7Qq6YTyJHr)JS>X<qMpmRF6tC>x4tZoz2#n=Ykcz9G{%50odZ5Psjmk>KX=JL_B_#VP!}8 zUU%D-Wi|j{&-rag_*+5)^?T5eu&@g~hu7&qa{Hkf90}U80%d}&k!ML1(~khXL*veu z&a+m@?--K!D?6w-K-n~XJVe#j_F?+U`|ncqO3Qu7FY#a#0I(Ez04{BJ01SqA>hbe; z-vCwE5<^qV_4cse*&Bdw0O0J+;&4Kr+S+a0jCUY7I=i}idVvFg{N5$;07OOQk21Ot zS5f(6^!mtQ9?fC+)rY^25Vt>({Sa2ewCacG^mG*t`W#*lcI- z>gnw2?68I4s9|uu$EESZWB;J%wl5(fjt-U$5xj?dpb(bavXi_b1b`iVccS`DTG@5NIr6 zkQAGGkX}Q~7L*b4S#Mh#86ESq-ny`&`MlEV&;H+oQzVQ#?+2;Hhc+4=@Y`>8-wSCHfb8l%M^0wN>AI z^$kbOU5~Hd1*Wpq6V59)H9>_#PBY(JfaBM@=mo(2VEqx@P@ufew#IS@=m`E9;I{lJ z+7Z{Ig>&o&oiR94=#5eQ!iX5@NEu}aIMulTcRrCUs25wmN)@zAt{Z;monp- z6e*g^GeHqyozw*_=5$ju}}3!^o%M(T(7@sGFk zzxSrbiEp%GVtgI*=E_Zz0o4g(o_?v@(nlLdcU5fdVP7!{ou@<#7rWf1LuUlxC+1$CNBx-4C zVM=d%+v9Lnc2-;t#g8H^QCh9U=Q_eVusoB4MN>&8a^sG}Cgb6T1EnZMvR}A-$EPU8 z9A|AcO1Pc{t7C9&Lekwr?c<7Ob90x*e}2py={y^5l5uK?1GL%$j|%$t#r7Wb%vt>S z+gr=I1KTfxsI;dLD?v^}1_Ohq^#I_^>q(7$eRnjkzkCrV&HwjE?A`Iw;_+g_8#MX> zNWCi_bl@CyUF4J&7!5As2acnteu{pFCHHq~9^N3oL0&ve6X>+io3vn(5?vWtqYLgu z($TTL?=qbb^JOnKpo0@CD4yRngY*r8TF9aOJyIKw+}LOKGxSY`YwgMQfuyTQN%E-c zsylqb3cm|q#T6NBwsl<{oARr^7lIjo(^fKdZIWN#Tz~e?k1tQn*6oe@R(~4++}`X0 z<|x3@*OOL1hD*oZeMW7&?*ErJ`|@eWEhQgOS9U&rasCzMrm~cMj*3LU>l?jNFueo0 zMhu3sQ|d6xasyPyabZdTn$Oqm*0l^Ebn_9C01+06r7aedJ6l{}*)lFxR-vYbSyip9 z%P{AvVASeNvW%8u7*e*FUp{{IecJ|U&f?Uh=(pCzPo3Y>(}do`9sK^8#c-iYD6yfu zE^j(gE8!zb#KrUSn_;0&KoFX%(TZ zS0H)ggiwF6_rOS#ON1Bl(lQFJs%`no@zQ{@%?9hCnd`N6&>;{f_YPbSICxe?@qX+V zY>lvj5xxgiQ&v2#ZexSBDXLRsqc~{@pNMa}?!gt0$@r1bAY2_sm(V1m_NPHQuv9!j z9vi`Ret6h;dB^u!)5KMrZHreU`aUFG+hcp#ZE{(2s1-YVoSymNtE4nUd2e6j6vnOz zsbIt%@8bR}+j+%zqHK%e(5=5()-Pl-@(g3Y{0IV}Pq2i!>3QIA3b0Xf`<7*xRs>ndHDQQ9 zZQ0(I^x`6>j!%N);VrEusB9K3oMPX75hDk%zV6i9)oOV(p-00IJZTs=KqltluL7x9I*% zb?x#h$-Nu*x_dr6nA~}Y?r_M@jT`AwDu3c;Crp%*GJYmsPRnyS2u0A9>+Vkv+!I}C zGcJ#@si7$7bXizRF*RWm$@SRQ8a_RX?<3)JA(|Wxi=#r8HO@;R5f;&0RTvHT6r_*U`MO+iG zSK81^FS3Y47V>B!W-)eStX~}c3%P+J=8@uQEBSm_l0$7DHlg&yT04Y20?4ue%z>g; zpFiZj4fC39F8tK@`Ox>?uU2()cU*rY-o!P;JILTSnT8D{cwqsXN!YhjUB9}dyPnoR zY@DTKx);50LIUbIW{zIrWDOdklB;V(6_CWF%~u9EHKx|g@t zCbq;y%b!gZj-3_rDZ^|bGg8XxbX360(+yCHeo!w`K`7w7dI+1L7FmB428(Dx5>Fr_ z0TwxL1Atb3m$Ue4M<1YZ@(D3@=5|tAIfi*2w;B_1a?VTvLE2MxXsw%ZuQfOiCZUPP z99O|QDs$NiWeROSR|E{#TLSG-4|@i07tVK$oqry$zHWa}!tAA_PJv}zQruor$!fE{;*~A`bp(c^H-`a zYbi@YQXL{K%9_h#>0wHQo2o-xgI)p1=4+J*fNY!!23+A45n! z`KM0ghk*la>d|fZ`qlUXtoCTszOMVn`Ai~Q{!{vsuJ-oYZ+o7bzSitJmXNUR{jDLN zgJz$pyH5-e?7(9L0auJECn@6WaB`gn3^u)M=oSxY!!Ivo+;F}095vVFQcayUEfTb3 zHa^@^N@fy|0zxV{$|@zOc45_#&4yfEA&aA=aXQ-GE8e7Z$uQO~Vl=(0zHu&hz7&S3 zTH>pxReSM=(_E}0=S0Y~vhxYhKEuKS3g`0#Kpm`wE#VOZ_UTLaOtg7{RNDrHXr#2M zzP@xliQ2`0Y}UBHqUr6@d0)*;#e-L+n2HN6CqPR@O}0cf$k1izBK=0#yih`flwL*` zs*R1MaMRK>8B)F~{P;k=%GLS3;m{c-)H z)w4QFM(y5P>2E_My*S%jF3VWmQSvkoq?JfHJfWM(hZ=Qop>ibg4TUbckf%a}WUJ8F zG!Y31hkF4UnFE;u-r4I@RmZ;c2Z|DMDW5)mJN;?sOhPylOAIIhZCflviY^gcG>J{1 zylH8|D6`?!)N`!2#?)0K{AkTld|;aUSTQ0o{p!BEs8$Vw7nxpSZGIq=v=iK-3ZAWRZrZg>h3PO)1pw2e#bQG&gnUX zT1ez|oMyAny>shk&EWkLISFW1`2t1%X4ic0)kJ-YN zFv3!*)3~x#Q+zqTW5eEV9Mm$hNsSHxO6fOTettJd8d5Vhf8!~fuA2Rovn5n94|$-i zwa(mldN*AY06J+w3Ynp>zJ^ICrSHYX0320-ASpt#fvof{g|YdkpEz$Ey8_v-l41`L z4lKi~XIcW~=D;=wFMO7=9Nh>&QPc&W5<3~N7CvW#uY1w_UAbTE$(!C^H&o0{%>Oeq z*>|C3qQ1$m3a~ z*)raA)Md`DZ~zc^H;DDphh91rOtZzVLRplqL&Gvsp1MpZywP8-xU^OUfWf!FQ@J%RUHiUdvAQdq{J20%ckq901bTIQM!<6#h_5c zbi`J`TKM%sF<-1=Vn+rZMD6C>gzRdk~VgA|0^G~;D)o*`K-juiH>->(zW52kT z@n%vJqb2>YB>-N)(M@^o6|cgZpl4e%ECYyp4vcLQ!(UO!*-GoKnF1|4YhQLYzKu0i z`sft%vr+bnylMP%;wnkNoO9HaH2SOjDM$4oaOI{qHLtT&(Cc#Sq!SV44F_E4c3CP) z*)kP5!O^?^w8tBdqMq(5uekrRZ8FVz@NsS6oFeL_fl4_d6|5bq23V3TH@k&ij%0{* zvpEGs6MHK@#v;_K+vF4bRfrC+Z4)uw40>ipacp1(A^I>^V>^1|VQ|raC8ukJf{QVV zVnl232`7-Oc%eh4e3=Jj6nB=KO2mBby9Odq~ECZmS$byBg>U1bd)Arf4e{$HHPlp~P|)%??)` zm+t^r;{>Wms)D^(hffZhXK030pN#||%MGB2iEtups2-K_sdTc?x35jEeA^56OUl5v z__e>(n;d!|hcF7&K6wUk%IWsb5TzJ7)fY>gSM2|-z?#v*5uzSlfrL8K?fKdxqT50v z0@{E{AE-mP;e-oNK`6{=u|i>)#|OsuUu^y3SM`uLqE$2R9`TdIw-4b5m8={sXx->|Y+DnLh^v#8 zbFnN@j|!T4Bnz_p$CW~Tp#1K3&+*x|D@(qa>6Gu$)_nGOv1yX_Wvhq14;A@6CVZ6u zbzo`xBz#9f?rI~YIHx;rzG<189K2+D-o$8^qsH@`bCd54=tIPo+|wyW$H?9J7IxG1 zDwQ2d$%@^-vD{ErzyFI3Gqc17X;x!yj_bZ?NTbte6m>J^`0fF?y81{HrYJp4s{utW zBw_GuIupsqB6(FnUh1!!3YOtFP+|*NuO(HDUQ~3O#0dPc##rk20 zl;<(Y;bb!(DOor;Gw0nF<#JiY0JG^%@M5R}+>T?}iX|+C?I= zdGv>ix=!DnT?A;TQ3;gcWD~CE-GD$TKTKWJ0zECvEPf89fQup^*TH{2R89iz`O>Bk z4>FiUn;99{@!;ZRYv4@(q{0+jby+Op;}@?iqtS2fPovn;+|=V2xCw_%dSJ{WJ7qQE zTJ2&jIfj;8y*JG98ro5<$^zV8fjSC~rm>;HkQo#mosvd5Z64WijZd)__|e)J^yc9e zlrFd>VylPyX#uJxAi$KV%gp5PDFR&zL4WOx$;8F8_!cz=ARhoxr*&|t5V9h1B^upm zv5HgshQ5`oFK{R84YsS8-nh~M- zw@xkoFl7J~-OYaC^kPTxtU5Uov%F1j>2rbmTE6V!1;j@cqdiFZUd*{q1_WvS`56#+Nk_weh= zMYlKK$$dDt%xaS+PZv+HLbiTL5bRs`(B}Lrj2zs8W+~$$Q#e#m@W^!4mS=zy=UOHE z*!<3KLW7KD>S$D@U<_x#}OJ91y{ASC69T18X{2}tnz&hjkPi6Qr=pt4@?)_|19(qdN>>RS^^zdnh{b;7ts_5 z(nnr5)XK%?iY~T@s%1hz;rR6*H=FBfPp%+7U|$bPeBwJg;+BuuyP6e%sZaz=RE>#} zgSdy<6m>m`)#vaZUXf)0kL1<#RJ9ing&Ef2sURSL^}1B(I$7?eTi$J%Iu6ssiSmHUJ%N@X-W*}f7xddYK@v0lt8 z#QTO#2Z9dIpQUD(+pl@*_ZMf^%mux>Kj&MR3YvMAN#6eP*q3}H$;WTEuellaRIuW> z{`wf_LI>S$eeZ5se{#mD1q(dOn#~0lYZ(eE(pAh9Id)qSq%BRI@DVw8vsR=*E4G~X zU?)j90YO@DF?4d;&6wNZMhYP*5R4{lX!LfRmJ!=rPKk19mE0*yegG*>g^M=(d}vd^ z{!4FTr%RJ>oqFrK!|Bo0`CSNz9*|XMmpy;DeW!Qc?Ck7Cqj#%WL5I&M$&b&=Ow^uQ z51z(|;Lq(Ij1@gxq1sR0vt0RTl;Jx)pyg2bNZ60hZL4}~rGBS9NB+7RZ@*btuH#w< z3IOQNQ0OxkSkbxf)9*eFGw`*K&u*c|#vL#xdWl;h@RQ%oc4mH^lj`LiPRFhKxV;_u zcD7-i@mGxmd|8Pr3%y>$8>IX!&cs&n+O$tRG>iIPE907I#B`Yc7vxt_?jE0KV{Uhs zHqS;cNwA4OdbwrB^6oMfU;}LpsAJg|T-Px|ll|BKJj6%J^Oc}R)QIqoJS7Pmzu_DJ zKRNQkTI}D53I6#$v+u>Hl8IH@bI;9wYopiNXwMRqFCYPyB?ev2)8&n$q$YL<`OwH@ zfXNB;z-ef#6Zi$x%W>-X&s%_gy7JGg6T7DRj1nqiWp?9xQh)u}x%7r!SsOiC%pTgY zTe(3I;GoR7)-wc{{4n2L}&3wY!Hgd(ZdG%0# zXr}Zg@oY_^PspzFQ^Wi5q+0Df)oc3!Ae@lh`K_IVlJJMQGGHiKR`DDNmt3F`DmhAU zvfva0G(YGc|1L=jonN+nYu>u^|0QzIwR(jdy*Drv0ytWl&>HCQY|!+FAO^i#Bh(8h zw%KrW)|aL*6#D(G@~@nSW_(fh^}qV`Z=8-jdi@KEtx3h}RG^*+~DIBNm;YzVQIYKdx)(Xx0_OdY`D@DQ1l@PflBCJZ6R`29+!1AzNx zbxqUA#Iy8>v(wCU&;JtJStvM-Hab?S0iYQI9^W3v$g7T1@!D@zr~`Y&$CJ;hA8DuW z^H%(PH^S`$b^pY6Gf#f`(bJJG_iuV0#>;$=s`mCYkm2x@p}SSno;bdz1>nsAB^%43 zP+Z5^&kvUr#BAC+UUy)NU*5VIlP8DLn@Deuwi-uIyoa>nRDe(wbjM?zKvxc%2P4IC zK+P3{Qjlj6&FsV+&jua^0)!W(Bk%|II9ky~iu(EAm*ePf+mXhL`lgHfR|gIsdL~9E z&{izN=LaAafI7LV)T%0&98ZTxrZj~hVAk%74~AOPE4d~pz)Rh&3l&;qiUSQR@}t}Y zYoDl(+Xix`HLaME_7c;!@3kRLACyqb@Lpa}B;ch3MMAY<v59liBuB>M=8CU2 z@iDR=&c=e5@UE3VFm_fLB|(Vk)?UhowTa{T?WA)h)f$G|Z`}G6xb1`E4fDzsa_p2` zP|-JHD3FKwd}h?G(|T5XML_V}%L=qKQa?YMDP31a5vuVn)C2dZxvO?JUg>MJI)6ck z@$cP{c+x*?Z)`K&N9ltCXsPd}D-zA9WKw)x0Io;|UR@xOB`704TPwCu2MQ~*BazzbkA9C+8t2_$nmtcBj)s#UMZ zWz&pgD1yBiW0($Y3MCM6oY3PDfWpbn-H4KbWnJ30m+er>r-a%hX}CGhE02SNDeqMv z8J^?I9!<*OO!&(7>kCoN$goAV+4!^J(DKN+jGqs~qOC zvH}K!0I$>i^0GhA&$XW^n~XZY{0?pY?X6dL?o2YPZM5%d0{|i%lF9Rdkl;j-EP#@! zLKW)@d?@%76Fw)D0Kr#c8N)OSJJdbKh1ePQ$=Q3f%QMKnvhY1G-#qJi8M%l2ii+Om z1?nVN9vhV&k8s~wiLg|&Q7cZb;Ek~AP(8cUZ52S&tcjafd*qe7noh`la&#|lq|Tqs=4LHd^2e?%n(D;zttpTe>T(F_ z`x|(ka!5Eu8(R3*(=Z|6C`(mM3n&Nl_gT8EswNDx9V?mHTrHAV4IKZkno$nlggW3+ z3zI0BqeSDOa4AqlbwLIlxYA{=nQtz3J*Ru=qd`rla$%wt2&9UhocF{#Y1ii<`v5+|#CO%_uaqrFg`+Hi zuUrg(&j60>+ec+7rtm~Ekm6W84uqecUCynhUyZ#p#BU`XE}P<|rCX~bfKl#e|83&g z;dDik^k2b5R`x~+U~-DJjex$UUB}g;AV?HVz6lm3kD|25g;EUcV1A-YF}t+l>)50? zW}ITn{$XE(Lx;ub;C@XBUtA`wXxkp{9COR-GT;`fqz>|gb3PPij7zSUjY36a1cF3J zp?Z-CM8R|NXw*zbT6IQ-SaQH)Lm3ymeKt`-6q&7_O?iC%=mUmnU0|5hQC{ARHW6xV2bXerSkBv1t0p`)sd2xHUns7;*=iqQ zuaI5ybYdjn%Dg>+V5GtpiiHwZIY;GD^ng;aBG;UX2q%ec2(B68pYtlPJM(@`Dr;>Y zbA$skuMf`S-bmk?NfP7$#;Z0L!#zWX6fn0RW}zSK!cz(7CU<%)`q*d;Ovi7Z8=X-& zlPPViyZrvx-UHM{+4PF9cgp@VAu*#uc-7+=S<5svd7Ok*M#k<9P!3s`<7;w+fEqC) zSh1fG7>4M&=ploRD23b5>u5URSeuG;6puj}!eUTFFCYa=SLkMRbH_s!n0@!JaDcYuE|zmf#x&t)SdOwaf0#iX9Xy|3GOG4%PD1p zMC?Uu!gA~OzkhP=n7`E76ZL?|h zlaa!Vq9OP6Z$az!ABD=J<^PyKIs^-I05JucKo<18GmiwS%bq{K+|X#TZk^3WqO&XB z)x&A#_MmiV-NyB=dwV64k)@@P?`xjcbiI2ny)tRsqdwSr&?j~z@ZcxrRFwAPNo|O> zjT0kBe}$!vk}_jII>0+C12ifF5=MA*_#SR{7&k0vV!Rj`p=qh1w>8oOCs;BbnxQGv zaChs5w$LeXMKU3I>CVhSEN^QEoj}+dD>}nlb@gNQ7YC2CTv`JYrYK?()qJTdB>l`; zg={XToF)P|h+%$!h}J3x>}XrBejz+fdg;<7tw%O{Fn`@#+5IHs>MM=rOY~QML&Cv3 zp^e%fn8E{MxwnJVC-?rkRd&O}qh9XLcWOfO1?hKBK+qO)zqMLVa{Wy1E zLvx|k!X1%6Fx@EvPuI35-Y47M(r^B<-gLbX4)a2+G`U`;2@fq(W(k%axq?trL^WwL)u~+jH*`n|te_f8QRQxy__h$RK4)X&r?`vrgC&t=B zc~P;vW$kaB6#y7}!o{NIt`^RCeI2Jv+B4WAyWe8j?dk3}Vx?$}W3vN`YZ(5P8YGFa z;dfap(Q638H&=P2a%s^=UGUmT6NDTDCwUPhbN~V&wXi$!1}YcB#g{S59Rj{W=4Feg zeeatU0QmjU_#Z_lkGE{wDOghYbj`k%;Mo!tFW`kLLRhBqbD3o~=O^#E4Q-r{TDC}M z6_O@9o#SfIGsvO`2cC%m+~$USrnm@1o&0I05I zvq`>&52CKi2V3WOzkEI4rv^E&VW~;OzF(jvWYkCVW^d|%J-Jf8gf)Dj%3m&z%X zYcv5RC>ur*z#hJnRJ*8UoKwilW9MOzG!cVNBk4&lmN2n&D>WpH3=6L|39cVK;BaB} zms78)2teI2MHKt2tktIts|2kiQA9xI{fJ?%ZOrl%3?wIGYW}CQX#Pqx&@|=Z&Ar;9 zZ{foF`5i~gG|uv_y(*W~{A8??^zzg82hpv+><;2dZ7Z}FOr>Z?A+hSrnelQWOg>c5 z#o^n7*5#zB?$D|5;iblDArY77u4D{f8RKnoRvS2o1`n{osrW+dRb8~Unv7vT7Cw$0 zm8iqAA;6BPkqvFnFrLIC2JQE14n*A|mU6$@eS@@2gYtg~gu^a;1XJRL!eJ0Ij!f1n z?B8b`mUBg3AM3Jm{knO#5v>Dt#%0s@ey#S{up{QvA>&>h74YPUrrB_FOp3?RN!4kL zU0(&EHa+yE=?L=9$R`}i95YpXgYvJSfr`bN7)^kM)W(z9un^T|UZ53PiA>6>d9!UB zRoqsk3GlXRl!mlbE&7}&L~j`PZ1%f-?8g=A34@5XGZRd-wa#ds5g1@QL986d$f@y0rYPq#Cg$ltspCGz(pk$JzrBZfji9u_?n-$7@G!>Ry2svS>2w2nXg-A1{ z+9P+(kF1PJ+@YhRA+{10$|0c=yTiZ^2lawA)0tCG+b*RdO@ezDUmb}43enC`S}#`) zffhoQcNMQ;^S0Vk%u(62&q;X*Sc@ISvsA-UNau%nRH^}`X&#IKw+5NUZmQvmGqDX< z$w!*|1P<;SpHY+QzlD623vE@HMgb-e*6QqF-*nXY?hHO%r`rmL+tHZ8qj=$Dfn*W| zBC?KIw3n*UKouBKt(5?MyD%U0rJYyJUp5*1D_B;@q%f8sLAjTniO8ZiljTX-SPYi; z9F$Xt#99+aB2k!OKExygw)R3=n}y=VI)(ekZRS&s|G0T-byz!b`MaYn-4yc8C!Nv)&0!u+sKr4Ic$-rcr^EFTxAg+&G^F~!Jfha*T0a?B9?-cN;DKHSMZLj<8bS0DSn{)}Py!4_L3HI;ju_r~PoZw{f>A_lw5;yz*#pjR zp9P6{Tx+_Bhfq)W+7}6Lj78dHZWn zUu*&bDsArWMs(f2=H1;A5=IwQ(~D_soh=sbW{i4WH5M~lLW4+J*@!YOtaH>eM#nZT zZEtxIb%ij%NVmIVEWES1Br4i)!^yVw&nzMjb(c3l-+AM9`CZFNXT5U?U#p(`@ePv_ z%fO7YSQ3;D`5q;;^FyJVxm<#G`0lBv+8)g?yItwu5(#|=rdVxFRNzc#drtVzuZ?%U z&E@JDdGX%cHO@?33VHm~DJlzpvf3%y&bCU$wkah=*K!~BnzP(qEI1pM@FAtb5p^;=X&!O_QHvuiQ&~6;PIGtTHtvrDtj=L)(_`NMIOp@-p5% z?V2v`y>eRR8~M}`ZzJ$7se&J57#leDll*1uh9c{_%!E$?HT9)??hTH7I%r89IO|eu z`*{DEn(LV&=A8Srdq0wMS>_1vdXMhfwHs_PS?<5z_yTphHH`5)G@_6;Gvn?j?j$z^ z*hjiIv^FuC>6S2enA!t0#W05?;r!3LPmnkl;{o7{Dc-%*!Toi|YrMNQ^!IH+76OiQ z%(AYNfls8*U)z`tUD~%88j`@~$%NyaFgj6)s?klas|N`TF>D`v3Pq$NBo6M}9DWk_g>MA!zV~w+8?7z!6OhV3z`c|GHGL8!n4EpBg@7cAi*?U>g zQx|k+{;c0Is#4=g>A~h!>Vm->DCaW>0^+a<7knl&T(R{S`@+@jLZ;X*5)xsMId^t! zQ$bFe>SLMc`Fz6U))%y5i=(WCdq4O>7A%;i=1L}6V`PDl^uWn$aD0nh3l~CJIS~xM zyX#56``07io&A>i5Rz9_j?ZEDgj{^q=QQ!77^Bv{o8*HGE-1_*``uZvhf7T5v=mZr zxOb!`pIQ*N(^gm0=m@s#V|0qjn+1{5M$h zlU5(3#isin?8^PIvG|Ki>~ImHd&jCrA01*Jx2#z;+&l5n#%~2?-ujVI+vQU_0f3>p zN`;(rzgPjh1%6~%qAZ&|&Rw$WrI0hGZge+# z*B)n-oA>@Ren|&nt=T76?#+$28x#IAp0K_9lOH?{At>q|j`(y(?#^4ghT5{B&j+j_ z_k(}kYi&)X+e5%<#NM6IW-2Nkx$Y%{mlJZI#O;L!9V0WkaUK2L{WwPuANX!M#8PRL zaYzXUutP3L7{C5e{?6GPs?z7o16|iX$94yl z=uho83T@(uu;@=8O&y2@54ew0zE<4+`PW*V)Sdeef4On6<&(p1S8yCE0p z+S}|aW6z!)t+zv2>0Dc#I`Vj@@BTU5m_zti*45dhWDG1}i|cdL*?V@TA zJFoU0y4k_|izw|g=1!-XQwF=tj|?Y$Yx_$ZxDwnRc_W7V%J9|vl|fm)$gk&a5w7<9 z(sOffUqx(Ma%pd5QE7d*z5=-2>(jCfRE*ZHwFF-@0TQI5s9DECb3>V&ND^wL1Y~B* zut@cCCQKd(i2pVF(_9#B=(qdZSl7gCV)U1(bt~yp$e+=A;OzsI*2vr0P{WzIzsBb_ zy~+( zse;5T=;A~DpIzI3eBa|@XtLDE8WLP?lVV}uy<(r5s5m91IK`-z&5{&@x)F;Mp3PE0 zrVwbQFeX#t_<}fwqWZ_PzEUCF}8?5f!3NpJ!^jowXcLk z=ykb%n3|ZKSe1L>*jA#^Nz*Iy?}g;MLm14?aDw5d>?1}GsY<4oe4Ci}{qi+9jE397 z&0gMjOH2T^taTiNp}~gafMZ9|I%xOoebx^XC9?jIe6ig9;CuH&pNdm1ja>~M%ua9U zJAZk&KV131UrBwXdYWVToK)vghu7$*99n-TsC~cb(IccSI}`4%p*XdP*1! z1}KuI{&|3;yC0bu7BpTj!D}4$>$*5#BNBWF6F|{0V@yO)%n^#9IatVn zn0tNvXuFs($S0x`zj7CKgF>n!!KlaET<8lES&&J@@o^1j?H3Lrj2&Dt8^92&oJ~Ey zA|uCM&#Q}{TGKUV?%(2phnHK_S*zki0{?jh zkE5v&(Lk4Sz<0`}-nmcLBG##PnTA145#(oHovLER zrEu)6xmwvQzs%-3Ak>NrV8-Vv&BiA#D6y{ft$n)=+J0Is4-;-(#~^kNXbR z9&z|7z2B2CC+M1+x{iD1v$&CF+>tnH!tj!&rY1&R{Hr2HU>&85PO(CXm@qieQg{?2Q4Bu&;4+CKCTW-};y9;&JThLscx{TZ9sq-7W4JLh(NEknCa0bi&?e=F z27` z%&wm~-jei8v}KTdI$t2Uu>*G(gY^uujJlipImMjKPO{ zGL#XEE+#+)6QK-U8;&lsa-hZpZwf4(yxg`lcj=R)!Na?9iYJ#vlPxbEj^2dXbaTEG zd>UtYY0Le(N5QGZvNtDg8cOA^omZ`jLGQH-Oa;v}pzC6YQPjigOtHv%M%n}!X<<|j zDA{1B6c37<`)QBU0zW>TkFBZJtWjHG60>R8=r8rYD;7vS0kesvaAx}4l|_CE?^ap} zEad|Hi0s&HU-Xbnnm9^@8OB9eZeGbc4+A>n^gu0)1FGe4paZ3sX#${Wcv6-a)xzem znG`X#`t1d#<|duSyzOq*H*7=I(JD0hVmDadV40gsQJe}8`=j}u+3(v*Yl-W>C4ElH zxwSJ+_cVBW>EGjr-Q2dQBu^R^7Ss6Tk%w|sM((%k-L?O`=85PAjhwuivI&}$86*w} z=TjmTM7m5928#~w>}irf-6TjxSTkj-7igzhK_z&oLz0)8d(0*nzTX!F&tivWj~A`a zzO^p1grc)_*_!kqr)lvv8-v4AUMP_e0l~ZahHsL44n+XyC~zmilf1xZdX$ERi~rED zzZ9~5X(XPJ3cJuvDr@#G2RFxR%EA$Il;%nIOnJ+Xq?OoIN z!6E9=iJSJgLK^nii{5&@THC}xV|)v1$N$sSyT>#6|9||~#vJC*mTgYedq!d?3YAWi zZ7PQ>+gg*FiP%sHy^~l@%^W&Q4y6gRS#21V6fvog)8>?PAeGLAPWWAYKfl}W`~Cdf zZryIL%OCRE>vCPM=i~W!++Tl}UF3wvlcneuW(rHjK+~K!G-Qj6spK3$%LdFCnh*q< z3IRYDlRio@c$-FQqJCQuz9HjScI7N2IWk6!!`%$F+s5#p`d1h%%;g{O`w(JQBHp@ z37sN-*T-=XFjW7cC)fzkTy}V%Dva91hsj=Q4nYBaakE>c1`-1PX3G4akz983;@S>u zsb!dJh7LHLKYBIk55*Su4hVy{<-KP%?^B%~UES@p)H`kk(5|fk8p2%w3-^tun$i0J zg<~c2_juX>i=>b0#^J>z(3sYAYqKCYa}m$R4Yk+}tV*6n`%pj9NEfqE?@Oa@S>tEK zLN79WdPiMtrKMf8Dcw&4pgWLNfKXK-B6E>6N@P(ovK!AA4D3Ve`wu6;2h=tni~yzB zR|*>Y3qv(l5&Gr|w=OUIzeAVUmHv4w=<92sGsg+fL6IVZ7$QtI4qL0nM{o3v0YL_u zs$vROh~D`dqG-G?n*+WJ#`WK8+igbIyhT4Y9aun5#fKeM+pesBv@#+Y1+<8pip6{e zlet>p9$e!_B-EJE90lQG(SYc#7NgQb0%tB}LZ0^l@8e$tB}^TWK0W)ib7bm%^N)9* zn75YRDjFWmYR@o$uoSfBXF!TVKrTU3vrOoHZpk7B*Dj{PO`%1|0u!-fx+cNh4x<6) z03uG_JVM6aTAXi}Q@!}*Gt(1eac?XDcf@PQ?O^8B8o*iC;w>z!0Sm)rZ~eq#66#CR zFi|DWPgWQKe$Dyl?&2Xpc@^9O++n-d8eQn$bbE93@5-I_ZE?b{zdUztiMkS7UAx89 z)<)-6s2Lz58*s9DZ;0JxE4K%8sV#fYI56dCY2YRvKbx+}rPFv}+d+cH9nK*8$jqkH zESU5Lvvy!)c7a8(`=nI8FI_+HhF2KSXhW7V2@)IT;+`7SeJwE7)M`dUDaA07M9aig z3}yLL>IwY;+$dKc7)$>GZ}~JcR#rYR@xQ|)ph?#n0G0w9J(FLgJjRKb(s$JZw#=ep zGgJ+eC>O&h=e5mb7*a4*nr;3YQW@98Zj9+Kh>Mp5%1j(brpk(*NkgUjnnq)~I{<`I zB!b8dC%fWC`nz=4XwB^G=(VHIes%TL8)o@pzbrQZuEia#tA8?U|14*q{C|f@yx6fZ z9UdHEggnoN9i` zf`4zfWoB0gkGwIAj7U;T$*G!$JK7ZB-B_s*DR|*D18&jbT1TlRK3Qy_)plubw@u>B zTh-lHH9nt*0ujtJL77osEG#qH%h0B^Z!rI?Jlj?{Mc=kd$Ng&y0ub;i5TZj%e%#lE zZn9Rv&ZOh0eKb(5`eH~S1v{BE3Od0ENE75~;gtm2e}xb2F^d{Iem=aIyexl_pFawA zVv&}qz9!E^A{1GU*nF}0=S$m+>q&jxN%!9{Izr!m-kNVGDxN&c{)tT%6JG>lgXCa9g(zFGoKnY$4 zPw%^(M%Ju>i({EYgu6r>))k)p2GB6;wylB0yaOI9v$cTtaf7-opDNbd{2TS*e}^Mr zUIDr9Ybq9K1Y2SVB!~es%NE-3DU-Qkod<94&Hwf%P-%9j7r0f_CX=}uW9j4j0&0K% z`tZ0lDvIG-8&ZE`^lrra>0=wzWJ92lGcY)y#}|9(xuK=4I%ZrHN{0*@uBu7(oIhcW2(;_(-D03v(a5~951PgH>FIi9hjO+qVVraR{sRun zekct%)vn{%1`7fJswx@gC(9k7&%01wExH$R1X>){ZPO3DPgGe1%_(=@T|D}6=i-I0 z_a=-Fe;JAA2(S*T832z~DR0Fk)60E}!~(4VRCY1w`r;cBDtEb~D|hjjt0@4L3!@e> z!SrNwT0{Bnd#`VP30wULJ$*TPHvd&?N#Lf#mEl?%08gcs)wvi1cUKh4T`+#M?stHr z0gbEU28!4SnufflY2i)E^&J|OQG*?GD>~0b%TpT@V;@INCmp(cK69z&blo!>4^vkk z4S>%kIwXsj225(UTx=p$Xek9W2QrtYA>!+DN`6-<=uKg5E!k` zx_~+gtP+RMP495Ez?O*sEYD>4O(XfQWrK4d{G5xI z7Z+b9$_893m^ec0fqB>3l?MN^$8vu2i zVq3`>UWYUQvJ(HZGhlxA0njE|_+)4LiO|Iidr)toy8-u%k5$%OS zk0t@_3|6?VrYac&X-d{f`g&$~-?eH*%_nb{KdZ}ecZTNOPM>yMTXNIic(Vp85VW2F z^4Q{TzM0sS$7VAa$Qm0R5u1VvUi`9n_R9Rs(*-dJj)v4eKO=vcR(dR$E+U~b#l^VG zQuw{4r*#2V7hfoT7dxbYI?qo5&$R&p3PzJeS8D;^bH?qD*WOF)T3aVP{)y4qv;X&` zcF|$Gq0#(!l2-DV>@*ZgsAM?u-Z+Yxq(nqfvaR>|k(2uruDF-E^?pEmluEc;Sh4p) zrqzM5|GId3TVbJ@*L;67TnBIwlKq;J!%!8AL@s(T*NXzfCNb;=CV&?!MgTRO$eQEb zb90OIt=Ko23fG~#VM$4bS__?a&&Fv+o9r6{CqI-oJ?)BT8BEXIdD*^}d++LZu`b^7r5B|vNT$Ao|ZWc174!8yf zos=?=G?s#Nmk8_Q-CkU{;NQ@wqpnE6n((B(=R-|rZmL$y4W>_vH)*tlsE$c+-%`U8`HG>V0r9 z{^E@B)S!a(MQ~4c3s^#cOC?sYmFz4^L}Wyy1O0u3v^I`y5?71zQZ&`D!^N=@jM~5h zO`|%4y<`D{3cd)k*m6L}m0v7k7xmn&+DcR|q}Zi1t5K@&cYFbgK^VFzB0vS>myE^M z8`(ktnHG(@SX{rZf(F<$@oA(OcD5SF9%6}U1OUL55BF-)Aa-gW7qHF&{{tGzvK9xB zH7S*7n3QeLV-v?;p?RY$O_~!krua=CLsj9S?ba2BEW!&&&dgyQD8c8`ACe;++yE)~ zf!|jjzY2r%$cS@j|9gIqJ6qOegFKanF7elIo^unoPsJsj?fx1LX2bD`NG)=Sdh^Pe=*|0Hl;{C)Ns( z@P!&A3A-k`U;nlHhf5cl*p>{$q&Hl)Clw7EO;}3jN;bpHBn;gM{vii3Wms92iPQ-T}Re(n^p`F z4W@VpIhV?@wdicLs`!f~DDc%L4-`Ama-VecPH-XSu*NXQ0kEqO7Ha}83kT?twK%nX zo19+%$63fb=zFT*T?@cW&Pz_SWs3)(8kH(O_yDJnG&AX%&s`HPd3j!5pI`Xulg-|p z=O9ZekM~&lKDzj0CUet9y5gAwRW`l#+#1`xFGp9*{a~o#>-_3u-!%#z-(T6-)P!7D z95w*zqMZ7Z97@&fnEBHAK!}EMQkK5d*jS^-c(_q)Z1y6%M({%anNglQE~?D@$o3DYT=74h%<#t|Lhw3`vlQ7O}%w9b9~$sA$Zp)33I zb0JX=K;DLIYWvKs-<@%RVF9}pl~aiiat%`m`(Ml-ZoR;J}f z1HFKbA3xqDuS|ILdgZ}7BtcHDY)+=+U>~h#RURnLu_NPW*TtFuf+bBK!gjh(C zacC|&!kC#QX{Sv0u9nS}u|7}<-T)%A`)V&%I3z$z>4?n6>KK5>3un|JYJLT!@Bvqx zpc@ZH&XFa27`Rhr%A-VAF?7}a{Ook_jy>pE|32pFu$mv%EjFXce@JL!QCk(3DTrYM z)!q|Da~>PF=j^|O36yG_>Cahcevp8@$>Q!YWo!8{o-?wj2f_Y?d0}%n8{@ zJ5a>aAAk#jEYq{IZKuFlsl*fnjLvXg*)pKGWlRlL4_nd*^NdMXfPSl0s`D| z3^NcIszmhj&n6Ck9P_ANfuE_xhoPMSEWEL-4by_rh-4!&nQh@gIGM5%$ z(gk>I!t2?=2K=Fb&Cg<>Sx|7=b^dS9hox@1LdmxI{@+(1^I%_up>2-lje@t2Y2Q<3nq!c|vzr&V6KVufRs!jmBNoS$0bm-?Ma*#Gy`fnwQ9cG3 zC&S!{GJ3a2Ec8G5*HiSq8Y@r;RXPGSe3Y~Ah6HJ{# z8sDl1LhR6tsncSp>MS%fJ>W^QH9MbjfEU<2g8VxCkgP!|Vrn?scbPjKOetM;{ zWk)Guo#Ua8)c;IT1mUxQbhMm}4hUFBP%d41&E3q#16Hb=0WyVXP4`iiOdEO@3rvvE zsHmO&^;?zHoT_v&KCBXtt#x>^+6!imity_aU<^!9@&Urq2blxoPd}fkTGvg@)6m0o z0qM8R&h-e8Qkzz?CY5&rUnkYOcg=3l@nJ*i(*5Ea%bq92oHMe8A9Em{JD^y3FCTm#6u(AJKE`*xkCNo9~rxV59FA&gU>cMPnJn`GSL-%Y-koogTPp*o2pXDL!l5d6 zoCy26L$WA8q6x}~$cER^Pj4T$S;i*Pz=*Vco;~8@{*NfCj1nu&S z)j4n~JRLD7n?(tBqp!cky%l)$%`;~329y8xZQ*M zU#zOfni+fgNrsE zrhHrK5Cu`IJn&S)O{~gY3rUY1R5f^BHCNqsgf)ov-Rn;a&nSX0W5sem%pk@isk}^?R zhHZKpv(8zS9}7kxoTR%}dwZw^x^7Y=KyX=ndCSvl{8!1%)o&(gx1KpY?z!+Px(xjQ zx4uH)JzaQRQkC<`muZ~~oB3JS@GWPhk(R+gvOBff6;K~{}kH;EI# z%h<}>LCNA1Xn&cbaz|f&b~00R6<;QRFLG}^9sKKgPclrxaMw4crCIqH_e~}CU95Y< z&^UMKkfxS97Xp-o`8An1!B^hBu;Sy2y_T^IT7Hp3-RhCZ9DV$a`1O+}GyKw@ZTCv2 z2`L)8CVmNy#uN@0l!p%;pyhTJO@*&-89MBIFSxj<|G|&tEgNv-vcnIIO6rtc$Xv*u zyWe+R}4M{rA-dAlQl@f+sDSx&)tf2=BY3!7TAe zV^)mD<*?rmzO9Y^ci?$Pb8VQ=JH)B5a#h^$%m&Wo_@rS2*ncu80C*<(ujlY5l zfAn|I@&`}Wxj4J7bzQfaNL;&Nt0!p#nFxY72%f|Z#I4?x`6cuEf97@OP3AS%Y*_7< z7?MylNw(QyeenMqXzjQxV0gD%TmWy_RYsXkPOJX@AJcK^W^C9xiW36X{SJkSv0>9H zRT(_6xahuWrfzCM^*TDD{fKBg&=Rb?|M1ZEhn|CsQkFJ0E8LJYAl7SW*@6{MajM zp5|kFkoyluKA_61zu=UtX~yKtNNC%xzSi1@@J@XFN^Tk6UAMZbOQ7f zt^PH8SC?K@8hwyuZ5@ZTw#FD4S?NKHk|vY#K!kq4v3NW;OSn}>9xI=l*c4|5Q|eLkB%XgYn^ zI>r67QSz&#y7><;=W@6KiQ6alWA0o0nSQP@H(w9^zUbj2*4y_lvg|BsrvA@(=>vPm zwl+s7!2!fRxJZw0Q*KKd8nwS0RQe%mCQ9)+-V@lpxZ~i09j{LgOsTr{XRslO{J?*7 z2ssZOp~X5_o29;M&PRW}Fw2oH{P`$0ep8ahaE$8@-SvxmIaOY{#_H`4YVzNo9Z#Er zT)2(4S{ZF(0%7TcrFD+g&ey-^pPbq8JGy1G^q}wad%*Wahm6H!|1SMqSIo|2DZa0>ptL`cfG&FqJHa~cDrA^`D!?0sBcS4>< z?0>Mk>Dh(@rmKe`s;YJ#R$m{qO{vU+`{r;vzH3Qap*?|JX(@5OvD{RxYm1v&!dC>@ zc5SJxvCLcZVW~aL>|q=hFkParNmvDeI8k!pRRcF>R(sa#&(9aFSBsflVc@2A^J_?g z=~j(&Y_R2-SC6-tIcl1kXDphHE3@9RoDcYA2FX1>P&4USVpl-Gajunr9pzFSL}bs6 zcRfxtj#k6rvQA@ywjz$(S+xV~!6&>8CVvr;xrI=Q7S*cB1l)au%6dhcq; zP>21%K&acR)ivJ7Ce`QNt&UnRg8?iVMvre&`PTMYTqhHe(qv5}U8+qMh0qE^A9e-Q z%>3M6rupsvju+_^o3<-JkCRpLddQ*WkZ+@KHM)fwd&TCe?LZ|^D{dCQgk&)fO+?qm z&MBG;m9c%iNKJEl#U#lShv@NJ+c{)Q{6#Vu0Lp(-l1%cvP(=f#bmi!M4s&)xP>6mC zA!9Wfi13Xap?|&jqgORI=URR{%I>GvpSPT(q}ihKAZB&z^3X*#Om};+*h9fL5c(7KcWWahvV%S|*%5@7ez zX2wbN8{GsmDJbQ)Z~}*K`1b83)HqGbdXgpo{yzX(Sv1>-s-+ zZUq`4pccaPgk3EC~#F;UO&U(4{S( zZ>4%MuJ_}FtJZKH}P z*Rx*n?^Hfr3(y`%t21alKIdvxm-#SaXY4TR>BIq#==!|x)5l^Qwrc>cuC>z` zZx2T)q|60OSwUR|I1|BAwE>~qmO;(tVfk=ZJdftg=L;Dnq-6kLCg>=h?3pjwJFbTP zuq#YD?;$sh3H+7xUGnrckd*fA)YsJjFN0p&BiUBweR|EBYisz(b@qj@m(k}zw8GQ&IXV8#JMa0f++7=^vA!;= z_ev*X628i_0Hq*WTRh)~Ge?DkPz$d>DcTaRj4JP$3BUrrC>G zN|4h7rlp_7k9T?LHpECs6B`10@&axwinqObF-q~~&VDbmZv~u4k%cXkA)nA(!;gdp z;N?`JpdMHiWc9l6f4r}JcN98>Ns1t;WYyI>n>+m^f}s5am&&9T>^u_&nv#X-4yzIS z8kk*NmiOlHJFs{7uE6ZZpBt}`jwA=i%9J2}g&>wiqo7-$+!`X0sF?*YHMnAcCAQHa zcflFx8gaI|7+QQ=A8Y++@7bp#QQs=B2$Vv0HUKTzDf>}kx)CiW=*yRjPNZdk!C{z} zypiCO_TFbR{Q6)vBf6%aM0uFl=Q_jUT$#S(=~AvK1Nd(KI_Y z%3XQ;P^#O4tw-M^spl^2NW2ntiDA%JKylUkWI9n^@RaPaZu6C)M?AlM#MO}~ARN_J~)^iqDcoUU)YJ}|W; zIW|`??)m=6i9q?}2F*zaZC%{5IBmJ@^XFI)U+^>ob>#l@Hjv@{w+-OeClfV@Ez*mW zZ26EG#@w##FT$;MnjVm^)}Iu*vcgsO{rnv?y^zgr3>~$6Ao9CsrrQ+zSA=m$jmhXe z#WdcgSO`)-Kyq+xG1#p>n7B(WaACkAD7#|*{Z2#7XI3M&?8ikFeY=exG}7U3y2}kr z@a!}Nk5`m4>_GuIli{M2iCqOjXm{TH5&$7dXVz&_X^}O8@bGX@ReFE-Kv_f73`Q#U zh(O3J&^@>lA~zBQAr-U&yiX(&r3TM$A_M?EbLsyAOR`jRWigB71Fv3azLcB&*HwJD z4}J3-)4wTiDkJ_S+9Q!|9?Wr;Bp6Z6-Lwu2VA;E8FO_+)nKmcF0=$whAyy| zJ4*A|{2&uddJstoXSheo*dqu*;TBsE{~|1=*bDdvv0G0&3c49If*&bprDUS~AGEu( z8KIjGsd_PSM?0HlUkjnCzgeG=^j&T%xr~8=szDry964V7E0kiws|afW z!~(YR!TrzXZzsDA(@5PSIttch)auxGF6GHk}zd5s>R&Ucv-nj|tbi$3itapArrIsK98xQ$k~ z2J}gZBinZM(XxlOMXrde>Uh1Sy{X9}ZQw+nGSAWRVWHsflapMD0~tv)WPtv$bxx56 z=JGnGkD@z2JjjfbQW|7i7GwrG<+ABDLN>+4W@l$e=gwLo7||@jV=|QzMdFc>!z1|x z-!6Rn@FDVULxc0${p|Zi37g}Vdfts)gPo8#lkMrC_s(9-2Hle{_7&ZTIQL3T!%ZZN zyq2`r9ku&7QEwAnje=H!M_A`F&X~*?@Kt6~;ILiHaeD2TBZxc_JFW>z@ zx8N4fZqO$a4L*|eK;qqaZF~O*Dc2h4OP=_!{7&XxJ_=F^dompOukk&EE%e_>^*JPV z*@J5!c7jiHX(d}~8B53Q4sCl~GJfID^iK@n&GfgbJNhok5^WmalaIUA;XiL)(blC6 z)q&r~0R+7>*j6|kIDc$K{ef#eI&MuNTZ||K=K+?VGyM-hbSnz+JR0~9_7;Kszr!Uu z1Ybi?5`_cTi*+6^%&8rV4IEpkvdZ6#yU1@+U)4$t4ZytmWfpx9m1TK#Fjsl=Zu+7{ zuvcx`*kvbAfHpb^5*b9Nit9iGy_*&v=YRZ=UQTGW;CfGW!Mn_WgU)qX z+ZxaADuV%xEWQbsLGm^tp47@#GHC=-3DO;v)ggfL3{{{dPAwjAi=;t@1c3)1#$~%S z+AY_-->P66@2(0AHux;iJ0Uyt&o2M%p+4tJhpKG2cDn)$Oeq)`@)(}&S%`aWbHUOH z1E#cE{zuMqEasa9XDOMLBo$ME!?!Z^xEKs3JuNLr3CE}r%{5U$K^5txy1D#}WPGwq zEm+}jj026Izkl9=TzEfV{?61RLAMPKZliOykQeCOq}v_z z1Zf5cn7gx^A5Ak`>|(+S+`U0}Vd<#7h>ipSGU7vb4Jtq`e?q1<;2nLzCjZF;OLe~M zMvN)qR?*GMUB-zxF|7!rj}?5ku;Lvy*T1?G_%Zu?#r&Kea5wX~MMcDy;2TcMGigS+ zndQ@I*Q%7K8d`7K(|!0)7t9*?ojY^p%r<^U)0q@Y$e}9*U}D(FsN8YY-Q(nTWB0g6OwZ7iIwmCo zx?gzb;M(^oH!>1pG`fe}9K>*-Q5TD`XE}>-obZ8tLlHq&b1`7N2L#m9?cm9Dv=el| zj6|7ao0QpbcqRy#6pv+KSN>dl*!Nb^?FC49{{_5r3nUJ;)+90jgLjfZUvdw1;D z6vws)x5-)TBJ5>5zab|4ex17u5QUWJ{;BP?rqd1w`hXw9>3%dp}~q`K8e{SQ=cOa&wEPE!!Ro+I;|)LN2a=#xY1A zBGJ8SwVxgM>TCt=Wlf#VeQdUqN{DB;*swwB0AEa^TCzzkP?m-X%wX;ONG)7XjAbI7 z1RfWno0x;DB8&=C2|>|XAaBXM*EPSvpDwEPt;D*TFhG6K%25PXwr~1QTu4uUmDX(o zb}q7MDa)EGYkGIKI&-GhZHWx&xNyV_;FrZHh?UIMFntQzqgEqz3+A%GfsL}SRfq@h zW&tU_sKTqwk1q6X*sA65d{bA}zRbA+69g%XA&K$b-x=`}1w1R5O&2x1`G=FL4%nZ4 z_u2c^#1Eu-Kg*hDZrtx9aa)S-SY7AY-(6uOcal50iPa)NDse)8c`}kq$}*(*oL>g)jlbml z@JvwfG5Qh22Dq3nB+1Ykt?US;8TWi>$0q4|X$=F#t2{sAPmCafoe$C*J&fPC>v8sX zmz}!zoCq=bheztd(fytpFU6FP1r#=G1J90))R_3}oH_Y?W>e?fQ}0i);GGjV@=x;U z{F>i~S6yrotQvx7d$QLCVdD-G+m8vFQ5XGpfqi_SQQg|wDH{U8pgs{!5%OkKfMl@= z#$P(T&@R`t03*yz4NXy;F# z$75+N;;S4mYvj=pn~A2W8!n`H;Mq>mZr8j+twWvuwikdBwMnp} zj)3W;HlYwz3SoI`Q4)@h`zreEUSh40G^0< z?KHx}VriC$)&28_b$kf0n4wymBBnrWiq@{!Z`2mXIoF8fvZY{e9qrOutke-2*-p7~=L!;gnD#maHS^D$ zfyzC`Lmw+{^m4_D2|&)W2p_!H;_T0V z-|m0AO5p3PrCFo3o?hx4wZ`m;1Ao~mnr*guPU~U4mk&<>V`KZksbc`$B|8fr%l@1f z`bWRZr4jMpb5K3*Nl?Wwk0pNRqCb1SUcfyMJ$LH%pY1~Y4;p%M;OpE=8!rW_g_}OrvCN6u``%Zkj z-5R)LXt-;WZ@k-jd!5o3c|%p?(C!2E$by~3R{^tQQW*uSjQPG4i*@iCX2=T2gVpQq6<<@~kLj+t zJNOf}bhVuVGB!4*3sPN_IJzo`U~_Rc|JoZIv+j{|%DOHQrr1N{{Em|GQ;wf@MFjdS zEA0#WY1w`Z6JXr;a96qSC9I8(M3H6VgIKk4m9BNb z!e}4Rz@7~@lOy$8oGZm(1Zwz~WjK*$z)E4F{XlcQxwi|LojY;=7%&$7rQ>M(qfZA0 zQxcaOj}|_t{4mWCD|PXv=+TsB(qfLIR+Ae527eSn z{}-zO45JQOxo5EJ$jKk`D-jVx?0fMc9;1t2&Oau|!ZT-gnNQ_nZ-zqw8**hbx_1Y9 zG8CQ!MwmD0!1X(Dti8yg|FM`MEuNnxcE3 z4T;QkqoW!6fM1rAkgO0x+yif49(tOw_8zX@WEk_|+k$E!mELgj*dNKKnCdfaaB2L4 znIAhx--cSAnSy9O3<4E{Y|Z4C(A3AJV2}o*3n_x*F;*<&A;Y)-3g|BPVG(Pg|uk{VdjKwo3nKMKl{bv$toMOhyYT#uSOUu z0O8aa+T8P8lPC{WZZ5&HkSIMjSj`?qgGv1Oy@aW%=MT%e<5kn_Hy7r=svajVD`<7# z;CHmmLT>$yP|%ccJb>ZSGKCB<`sYZOTJRV#d^*i0->PqR>h^9}HWD%xKRBX#CjMr6 zcD>cZsj7NO18riJ)zK}{YC2Z{v|q%6b%LA&v#-1=bY(8mW{=2(P>~r-Mvf7%NL>OF z@v`NO(U91wYwWZF&i3&_HSCFFB%Xe=Vw&0+9{5?m_gPh?t%hS;5bJOMg+^2ffzb7W-k9g3fY4PxApx@5HPF`==R14LYi$U6W?sv zXYMTZ?secwXqx&fO+b$wJqwD-hLr^VeDrCP=+5tK^E-81+aUq;&YrDxkMN13k<(TO z&V>K>)l-q$0ef|63Isq9(P%CaZHZ?Dzi|P0t!w?4#XgR9^@0_0UW`>8c>D7R_w4H4 z@c+I7BM0Y#^#Q(CBO37vvD^YT_++=c01hS6#)esTi9)dqbOt01*;B`4(re z8yW;%3DcLCC%sP!;5S*-KDD@DS-$R?&NUpyG87a1@-KM?$ZW_~a^%&k9UYZwHM=BT z#Thc?kQ_{e79sjpd(;sHN|*S^1Hy>ii_mX^H5JI?(5 zVdm!k(l~mvOfIKn7D;X3uC=x719{;2ROzL$dXt!3Ej7ZfR?L!0#qi|AM@etp2J3|< zx8IFRJ)C!(md_`*(9w>qwg-%#9Q8H4`128yg4WE$v-SZqTf+!mqrY$8NSylU8@>F1 zSMmEl+;?;KI<01Qm`qYPl-CO7!{~y;iTkn!E&{ulo&C`{%k;u zCZxc`lV5X34M$f&qprwKEMwpLY_G$Dw1jQ%J7?An12*P+hdh~^_Zy$-z3}z;i4FgK zd3@Kxf|NWHaMIdfA=u-dwd+5yQ~!wf`T@w^XTq`|c1o-qNB8zFh^UnfYPr}uL{w4q zm0|-?emY(q#HDuQP1S-u&#mB8;42fO3GELsfgdu4q$*`#L>7J3rYbVa$$Q*tzJ80nqsWH?} zzDKRif3)o4Ar~W_hHqYjF19M@!hn5})*qu9b>d4*; zT>ke{t)+oqYn35K=l`9d-Adn*RAM}^L{_@cYy3IZ`t@@{J4oBmZ2!Bqj_*H2`1E2V zL5e*7iK3wsY=!|?>w1%9H4j0R*x1RzTP~ylM3qS3ZV%F~rF>=dg_J@r{>aXz>Zx8s zdW`_7!B=MX^uZNhZ)2kd`&Dgg>wHD-op;)f7VYD$&>RSSbZf)zhnJY+k3RTXO$?u9 zT(}wCdGv^4+sFMnNehhwe%c@Y`1Ezx@7rb}?>{cT-ZSM341Lw0pT$)2O07WO*VAcb zI{pjGkkx+kL#6@kWz(X@kT*eIByW3bGKf4Oczd^Nf?P3hEvNLg~D1~@nKtMk!A7h*i{^0ikQBLlN$;}*V zznE|W8}K{r*5t~98+z?8u5Yq>c6xuOMdy`FE&28sJlElC$TPzU_v^$Aj7HGj*WxLO z%jQ+LBJV9aUVx{yO$l*;w~8{~-PMWydae z^s(@_D}sCChMPZysJgA1K5d*YsBo&^cEII?rq)<27um~<@x3DQ_nw;V1!c_9m8uW;4UliA@~ literal 0 HcmV?d00001 diff --git a/ros/audio_common/sound_play/sounds/NEEDS_UNPLUGGING.ogg b/ros/audio_common/sound_play/sounds/NEEDS_UNPLUGGING.ogg new file mode 100644 index 0000000000000000000000000000000000000000..1596c734f4937bba42674f2f0cadddd12212c2ab GIT binary patch literal 35535 zcmeFYc~nzL_b6H?6CnglFkqPU5C%gC7!VKgOxlRxtbGWB0fGV| z0xAX&3(s8j zckR7v*YY)Mf&mWv6RvFiTX#B|!yl-EWFn9jhVXQ%&RVg0wt9nkXWmj4dZ zEg>wm>f?uSy4{!m7VzJCGQ%oJ;*;a-m#3_Olj3EeU;BrBU`HNr0nf$Jak@K*_^q;y zz#|j_WGu(QAMQa13qk;(0U))s(c2sAV^dRH$=*_0>^&Xp;pLPT=kze6ln%ZBdcjW7 z1^^HM%GTap{nRJn+QK!=xw}(T3*(o$9)-1X11A=0cfGTYT36dzo4D?pjTKopjEGGX zAl~L#jG*b4OktkQ#9q5hQ~J!09!#Nd)G9f}u@BDhcbq)3cbVIyx_YXufB#I3NTIX%QKeBANi!}RlY>M#UU77I%r+f=cPs~#<^!e?eT;6U^=NMP+EmQ7K$Pi<_%@%RJn zamU)%_P22RTc}^F00^rx9aQn_PoD5>`P``2;Q>EdTxL zLcQ{zRdtL5!<{+e19MN;l^*!If=Snm9%E~1bET|DRW?|@;hGP%yyjbMi~a$v#=09= zp3$>|dc8)GSiSfg~hIy^|-r1>p^!BP8`fQHO&fo3j zTbCgUGMZbHD*rlj7=TRLS1kV5>nkdM!{Q@FxtuHPlLL=|;j3!c9?L>#hW2T>_#xjBSw*~$e<*a91Dm1-ZXzABt8E}Jp^*5*GzdH@h^bB+g2sVfq zGDsUXj0tv$7|I3oFUnD| zcRzC7ZQ)a78CGPyrDRP`d0kn>WL*{Izfn$H?$OfRql(;Tirws-lK7nR`qNcOSB}3t z^S^!mO*uG`Kior^6It2_nVSwESfDr&sj?|89)zg<1EX?7@k$t*Ndsewk9;{xa z+*M%V<@4G+cz{M|D;@O7VvIZrPcYjc&dYC%_U7BQ_Xe5&tk%NV9vM^`0liC*X#n2T z506pabnqDl0SxHs%<0_%3z{J(WaeUIrsuch|JfTG$SIJ3J^yrr4KyT%hMfKx3mZi< z-xG1dc9Q?|@IM(V&>33(A7dd-0f;!@|9qtX+r$5b!2cEk7>YxB*w0WN-{T_LdR zF;WEkOgW)ZVN>p#js0g%NDSR)lxq$Di#o6k_BW@^v)yOZY8N=osL;Be5&v&V5Wrio zf(op%_jJLt&VS)G_90XLGi$YnG81s_!|)_ zMbTlHeBoUb0|7K9tU?QbqL?kS|L5KOf6x7I141YpfL&D*!(`Kb-0r9_q=>5{4FduA z&Bh1=ny!qS{yGyUD?=@|Q?wPJ5y3<%U|3X6IJL@IP>Ts=`-pD%zdf##uW$m{tBXXS#>_`r|rS z&0(hitmZ9ZRa|*(T|KswAYUuqOpBPMn9i%mb{E`qrI3hQ`(n^|tlgR8T)g;O+DKJj z|KiMX+kSuvP`CHRgpnquQ!QKoCNvO$HDtHT>K3v|K*tnd7`L#10>cGmq_0B_HA-JV zsf_d&c$-HD1#xU@C<2bvz97DL&5I|A*>1nkZ;h=Oq}0nkiLFIUA}4BS3NwOcCYVsdLj z(UEUCX_cXxjFzWPX>U~6q%N}x2jo#ZFIQl{W8;mY=g;MB=aLQfwxUo!T%He zArxza(*Ds(Q)~Gz{pJ79*jRXCV_PU-UdzHGVVU{!8!QhQ>?X}JZqWy1 zZEV%~3Ww*G#YMip1_I?mw$;9p@Z3g$!hu|KWF$Pv!SMP*t;!?C;kh>LkFW*8^029d zuhKEEDGtxGYiAxvGRIae5zC93@7spuVN*%u`kuz}ntPau`JU_pE2m2n8=Y-_-r(1K zGd!xH&AM7yG@aMbHp(gva_nUUW3lo~UhfBHRD;8_@V)w7A7=;KVU-gxT(F4Fuj>uR z5Jx*knQwQqc*8X!Ko+|K#EQwEYZPJ_S*<;5Hu`m8PGdSGhI+lz7&eVpK}VPyFf@ZR zr!}!7!xE^^4t4cK@Bz#miaAfdLS(MPk&zrDh}d&OJeL7Kh6CtICkC3 z<{x**EoC_RBVTD2)6i&I&8?&1$CN2oe0&-`yo{f!j*R(enYT&&VqpFiAKycAvJI&& zOUbDz-CH8)FAbu~Y#maS4oesQEEZg1Vk}z@0G{qRlgvQU)2YA}@h-ptiVoH6q9=mb zn^$X8$^Nc$54ixqS9~=j{3D?OFl|Vf-i54tgVsZZ1GF4P5A0BB%;wbM(H>&w2%EPjDtT>xMTCNS1^8?abaWsjdv zeFWNcBXb*vg?#$gVi)ic0Pi4Fp@@6x>gK)#5eU7!ef^h-un&N~?ve%|Kexh`6?$TPs_OT?Fi zFU6rdZg|CR$c~MA-in$`23;LY8ar&bx>J|iUK?+bP`_?PYj{X{Z)8VyrM8^BGNdhh zZCPrDZFpm-o!?EM7xm@XSRymK(ttp3f9>i5qBm{|zfShu|MEaG=hWH8xr>YR*Pe;csyS6*(j^5YMs?WWwO?kR^i)ucP@FO-ybVaPs){829eT-(+J8Z`Y12$;sRIyx5CuAL1qA zJ*?bnS@bwkqazaZ=;nSmqYEO0&XuZqbpY95&oO3U#>PBIwtW4 zsi=8L=EzNAcqCvzas#G>s^~GVI@<)pXvl2PfP-N|q1zEz9$gl6me!nB2TQ0Bs;Xkt zMm!BUA$u9>>$4uRcOoIxK~?G^oeOEkmc0$!Aq&e_o%srZAqw)ZK$bH=I1v?D<-%W1o{!n`f=Z4w2xqla3z`N0-x1*AtFaHV=sN*vP>bX&y5w~ z{q#*`h(1vSQO-JLvdL&D(w(l#6q9w*`303$@SMLhwb7$5$1qZCDM zt&zvU^_gU7W!Y^TG#6KodIaAvrVITgTtXo?vVgW5K%}Ip;|F#%sz)WlPEIZqMI$=Z zOZhA1nFiF`w(#i(MD9Y!Wc(cO_C0MAo1?uC4v#Ze-IPeC|Mk>w2km zEa2VDQ`#Bd`*jw$st`7oD{qfeS(MPug!v6O?e#niP{+ZSj@-Xe3EwTCbx_I=B`zuQ zObX$h1X})1FX45=aUuYMigQ=s;?fcsIx0{l=fEltI-*m*nP5Sh@8|Dp3^md{6Z3j> zaVk_nIp%!B%aA&#>SP}~bH2&=GLBrh&!7QSB#4p_u5A-!d7UMaQdpGn^dXK&!4EEk z%;$&T5|u18fkA_#odGO#mhyus?B{Q0eylLE3oM`RUs86?C@Jq;o+3#7uuZeEqN*ZmJ?DEdlZ?V1uzyknU zx$8;|A>l7;{-(~g5sa91+jPqY20%l=?DIcyymtKvsT!%kK%*@V^*dLsO9IxlYY&Nh zJ>*a}o}*H$O5f?m*#!9IWN-B^+Esf1kcGP1HL?A>Ziu>YE;umCS=|U4qMr{jan{m2 zW`P0DN#2aEqtH_8&|Y$;n*6-kfR?Uk5+Gi*F+Ks1wH&=Dex%#3o$Sn9+wcf4!m3=A zIRa(qnLs{5|5r2-&h^N)`J`kOIfKE1EEa1hD1Se{Ip*S;Os8<;5+8OqU~FzinScOm zq=O0Rs9k+s1}AqVp2GyuGpXdMdw!hJ%{W-of=b`4qntUgSTSR_ z>dX}B;6;#r9GbmgoZx7+V9q$+D8ct>t;wG7>qd39jw8kHe#9_*Squ)GLx(vxajt~y zv7;Xu^LA3G`*1cgZts9Uj+YvfHfput6@hFXx3d9PW}FCRarI!yCz+x7WqbQ0YY<-1k#0f=i>ZU9Xqra zVJO>Hr?NL&EFuIEhA1+j1v*jy+Np7E0$84_px>8X`(;?1YGo1C09rF2wfSi4MdH0@ zSY|)-Y3;|EPe8CH>DA}O4~6VUuUz=wSLw8896%p^cVDdh?kX%?TeRp07C*U#&h&x| z7mQP74}wh~)yCF6^(M1uz8Gs3YiNpO(PopS`i`}m@ zT(1F4EX38IMaYPndEl^KJqUyp)HKZ??UB_w%|3&O>jYQjy{0Yk!bwF;=nbbmDGLcj zIE8Uw$%%tUIH5gP&ODQuKTPM=IOIEfNplPM}lWa7}o1bI~k)f}L-nv9!ybAXv76dJTTZg#FrZ`U1T z0mD{R1kG3kYkYVvwMa#tD=~j zh8;F9=Ivx2bP9`OGIu{i#=@?89p_tO9CPQ1g>8J=Umcb$l(V-Y#%fo64lA;_dCLUT zW}Dh1NvTLvS}pkzNY0FF&bN*ede>X-l3tCtKK?7i1~3v%+;#6OR=0-pX&VuY3_{$CzMU*b4EsUjCB*+QNO^iPdAe8$Lchdvf{6 z>TPRWmf+vaI&y#D1zG7YBPf@D$BYtI%U4WDT@$ z1~=h^|9i!n`3y^+?q4dDQ}Nky%N>p4Q5p4x#bjcQ+JG=*t%{?Da=M{qZT#9#%j<12n&4h$oZD{a*`s_> zf3NxGz(vJx)4W2QGNp6c&j-D5nL%`4w<$sy^=jk8`}PiJxBW2oSJI4ocgG#8mn7kM zJcZtV^Bhj7sI@+h9%#T1As8-J91C^sYU!d>-H9=@DcIm1ycGG9-mva`7-j;XTmGGp z_R=qWGTSt>xBqjiTHpE>?$Y!1V;BLLRmSEn8#myOgxragROYj@y22Z^TM6l=#;+>A zKkC$f;=0U>(Hwf#QLA`$-T?=jDPBpeA)A&X>0y@S@_i& zcq-SxP9mTuE=;mL;~CO(&#MACP7@N2-6>)Z_zG|pJGxI-3L&^5JxJlgiW zr#ds;A^ST_BhkzNU>O%m&i4G}!_CV{xl9ME-KBfQI6XaoZZF6*%+K@&(Io^$Imj&C z^Mx~N!k43y>~*gsM>`aP!OK=PX=3w3+_L`H=fhrZKAD`>VQN1!?s>$ELjB&%#XF-~ zvJdQB7E3`81e0sybQLF?m@0?a^>-E-ah@we_{WH3y3|7$CDMewNb^IbYs$(>*r*%s z<=7${1tyzBJBiE#c?=vv$fnlalu!($^n$0cS$B~mI3}ImAV_Gus5VSGW}vaIQko?Q z+C)w_QG|OZC}nqPKJy;yJWM#$H`gC9w~B!{*C3y-lsorpR!JBObjp# z-wtVrk5Y5zqA<9>X`Gp2JJa*Wc&6c!doc@k&$?`L#t)bC^BfGi6Sf$G&huz#`elP< zR3A@fPKd1+4|MzEG$Ak%`2OjSovefIrgsj{>L%k*mov~o>Viz6ETokp=js)^EYa*f zm3|x8KaP33EOg28-w&QjM&;T+ku)?9ZzD(a7Jo>&%|GwnMs*DcC3+9E-P_#M63Fm0 zw|w<(mCXRd6Iq$0*RvW<_0!{Td>Ps!0^$6m0eQD3l>iW%)D>^pz4gLMIf)R}aE2kY zF!UrCS6A^YoA8&qvOHK^#sU_3Ey6J1iVAr`KMN+cJ>jyPm)mR?MWh|bttaD9JP<}J z%`%s$fQ*d~F015e-H%3>?&P!ay2kGO)uLBZe*u@U6K}lLT-{&8oo82us@K z!VRYj;8(b*_Ji-1{jmJYAFD^_<13bqyRzOZqryEW4q^f%>$v}1Lv;8n_L~3{++EU} zT)d}{C7LsQIXUODBTyNVwUH+*I`|oo@ALLEYZEw#(Q04zAMHD%G6zj=ZZ+cSTmiys z4W~XICRJ)XhH8@Uq<$=_G0nzoS{cnb=X=73oumX{c)pdi z^h`9_6PnrOW$y@Lnt}7+U9zE;er=y1lf0_$Qxjpr(ZC$cyrd#P;Tn4o)IE52%Q4$X z0CWrF1i%l^7NEq;eJ%U4nL;M~vW)^GupAbMJ+-ODsUCDD(r}DHD-Ob=L^@50o7tOg z8FtS1vF{VC<-w%nhIi2v`QWY`gu$jkvcD96pC?p!b~Y|*>x zMdNYTwX7K5+>&JNRlvXV1moOb>Ke7>{sA2{$Mm4l0skSFXQ@TLrX`Pp=|^ev=g$W) zhYb2+)7?*e+P;b)n8kPthcD0A(VUBuI~>1W_v^3z>ER=vIy6X4*b-NgSq4OW8RIT` z6&t)}c7TF~aC6)1(O-O}0HM8Y@mhd3u7rk0XkJhDhCJ4mh9&UcNAIm6N#bdJOGL&m zoDvbhYpI8X;+{ljnF*8MSRcMJI<#?2Rg1$Z__U$QTs2AHdNYH}`}x$XQA# zGi~OYedpmDB!(SJQ!uL)uk@I8qjGKOPa%^#$=OQlzKH$Zw>hpf0U$~P8!_v}O)1hq zbKlc9Bcf=70ngCDi6k0-F>rH*z)1Ml{du8Vc5n2pRw`x~`tQG{y;|!6!Sadjg%#uM zSBWF^F#Q_xP2UgoCY~|}gHpmg;b^R*zP%=T*}2W%FSYk`^Yr&u zmho|*3MvDlPi3GSD1M3kAb0-l-E2T!-^+gQf}?V< z^Sx27hHgpg>%6zr8A5xSvX5xjQK)&aj5kC&`vpxGq~itV+EXnQ5Gk>c(E~MxEqGdf zcQ@=WwWR&1`{V>EufFuS937DI`l4_#4M}NReh)ZtS7(oaNcyz8dDbo*j|#}Fs<>vd zOuc{j*w0>rQ@Bny$iN&=fp0trxYN<{=2w%&t2!J(*j|9qG;_fRe|KHH+U)AFb;_3( zYY4h(vwwlBV1gLvk(sH(dl&4q!z12T87q@nW3XzGC*oj34t8dUcpqqXJ?kTc2s6(p zel;Xcp%lJ*t@%u!%ocI%RdVvf8E-UjLS$#3Ufqq<1@BiCL6O0WbT8USt_cCGDz30uKB20RpgfbrBcM;>3^}ln%F(tX~?!05JL+5;XYhMc-GVcDH`L zmMktglgF$fIz0syE5>PW9Ta)1MMucDDxQWc5VY^h*|oZ$H7_%9>DO)Q0cLt2*ngV9 z4K!TeHLFU{@j|GKkdXjjLv(G_7$+?QXlBPk`Ih<{HYnlZaY5tX&`bgm*p;%9KC$uH z#S4x_YMZY$)S-c?`HnLkNAB*5b+LXKsdTmENiTZ#WSq;?g?TW1Ir{M@!=kJgo4m|r z;dCRPMq?Zc6tPF@bV`6z6*)9(H^C54k}~i;@JwxlMo|u$z zq)C+Pd>b0WJSZo*jF4c&Q)~$Z4GN_=nJJr+-z+2O6;e|kqW(3@g9+BQckgib6>miO zLvE08J8}8R1#P-`h{}YY{PrgSb$2@rO5WckU>vLaTuxcac59uu5=4ZNs#*3?neAM~ zxQlYU%l!pO`ocs{k@je8T_Fat(%t7jk^tPP6S#Pvqwo>QADr7V5MaM*QYHrAT+>|LshHH z;*3g9*wkmOPVMSB=VnbzYkg>K7V)rB+K~xp(R=nq5}R-5j^%-a`bHU{3ywRJSsN>x z?M3X^rg)SmZ84BV+2XET@eV2S#}|bfknqnn&?6eMPde9rCW#b{N0r*dUV1|1 z%`D8ag_lc_K{d(NAD_v=4dGd&RXWk`%NBmAe9V9efrw){4&`=6XN!(b*kt*i8SvSe z7K6KdB|f8zI5wt$$}fqFht-cX)lb^}J;uE!!L< z&AvXf?bP5Q?2Jr+r$x!SQsGI6F2ZkV=XY5_v~FAONL*uWKiTy78^Tp$F_w4p05wiof_EABH4gy@eO3XQb} zA1SMRe8%^FTF|g7^w?ZCYq`C4zv$(ME#TGple840wyO}{>zLo@b4KeKfQt0T-yd+& zq4};|J40>A$S+uS#H#pE2PiW%E_4b-cm<#<+rZ;k?vq@#GhZnplVEAQIhmEyJKEt@ z*}R?=<};?pld>%=TEe?|?1YZ(cA8zyMI3p8PB;}MH3fRF#A9|{hc_%?Md&i`axKnE zh4j#N-$Q3Tl~SEh&t=m3ZxI0E%(evBtjWAi>Hhsi>n`Wwae}h&ZpMjLQ>DVydg@Py&t!9OK-5@#5Ad_&@+@Xc& zo$h>@c9oR<9tV51s@=qXHj2hdMjlW$tLPcJh+TPW(~Wv^J=rkWEqNCyvy(i z2c^-QaO6S$ED6;(uRY>mRy}kS9^>V1B38B5oLN5>BdQB0v=St zCnxXM6_JQOOzG0qtHu6zl8&E<)nj1bx|-p9xukv7N9e5X@3kIr0kOW5^Ezj}a`~$m zYCGx`ba1DmIc@zN@gef}5Wu!iJaAnCS+d729XvU#ZDoxN49KVeJ!ROxq%txw8S0LW zDeg#X!c2%reuj;QZnZCwhV4g+_w&eu`D)rvvX~{s>(0N<$K~wboGzGe7Cg*MX?sD@ zov3E@FFG0H!0|&`A&1L0F0V0p<7J5hk93o8lV#xJ!e{FJ`#2S4 zw=tLeuQCP)umSomiIuXerJDuj+=?P!o~MH;Vd3S!>#`4fK!1?UnA`3x1=ZJt&artx zqURH+eyveg^b><9Hqr{S&5m2F+Yzaq{<}wF!l}eXWZH|zG}HrlQsmyGfFPZsDpchF z_kynQmG4G+^F>52z{5#wOKRG^5G6#1wi@S!((|PHnV8}On{CY!utieSbja4F0M$sd zvIiMbJX4G3($_+#;*L8rH{n|+Q}3Sj?t3V~0LL>UqcqRoKmJ0+yA#N3<+`?br8CP3 zth#pZNay2bmrDFy_1>4OT^{)OngON+;(0HbWhPi^W<&cMe^&R|k&B)8mr|x?U}%L= zp)l>t(zdl1Ft3&=*5kKk0{JYIkajfg+QP0?Z!(AN&kkUO8i2W6cndaCf4QX7M**&G z2QBhr@@i^Pr%eyH<{iTzDu>!W5!_Me^yZuavzyQTy)&0u7~jf%d+*%$EBAtZ&LX5` z_MolNB{aNA59|vha$5u{De^Wn^$v_PR`~Crd&j#7UaRv_{(Pw?fS^Dvny$BjEh?o` zG@hvjfi-O)B*U&Fd8-{@)f@Imc0G4m#)ErFbm7}bdWfk6^ZTIoRSIqpdhrv-c+sLy zM<3C%MdS>q&>R>Jer#N>LluCg&V5~x5Sb_1IHwn6T_n>$2?R_p+pB^_gg$1vgTv1E z6IPM<@4R%0|9E8_z~+vHX%`Xx4-kXo|uO8Kgt#QFyA$Rg)5lC(Y(`+ZaMrU%DXn+!7$1i z0*H?YqO~kW9rb~m!CrOAi-CKiRK?^v=MgngrC8C=Zp{v%&jQ_upl8Gk2efV+LOTps zJvOrn;sP&^C$W29EZtxpP8e|zB`-Czm1papn6udmJoQTMr9NtMMA@;<;UZEhs*@*w z*qdFjgpsdl+Dt(6ok1k54qbUHKx?(BgKL*Xo~Tbk(C42(v=K%byjF3`K(q)MYQp2& zJ?Y6kV;dbgmK;=GBWxwq{30L_eD2c5T;%pBj^L>TIT{5W$4D+%ePt?P(-Kbs1Jyv% zUmNH^C1P*@tp+d{sz|sOBA;2lD{phAIkZ#DtoQhix6+V98ROk&pYt|PR%?mpHLd)J z2dHrNilZ|52>|8^$I1LI6K^zrX6EFMo?AIH-L_NqV&`lC4Tp~u*u3HF9AEJnQhkp@ zmycQ=J6(n0vSUb_CU=pNtqO%+1ygmcGCkjCdQ9rK4vNDwAZN}ya5v3?u1CyegJ0HO zxmh0HyjK?#jGak{I;KsQVb&er_#u8eGc|LV$z5hj_eJ?N3N|XAm1<1TrI8C%P4%iu zLK#mG?P4Y*Nhj)AN_b;MesU41DcXl3A^@83bdE-ZI!K>rc}3wM zpo{p^bO297IHroEk^n(^9+0Jg9-<^Vk#v@p{QaQF)Kpx0iErsWkCbnA<}ug|R?oV+ z{?-z2kMGYHHyucx+m^FqZPCew*IVygT=(d!bqTYr23!ZEIWY8QAec6R4CuW{-E~Yb zXg_~GhIztmA^;}gvoUvc1bf(ag-iDXs|$V|iK zGy-IXGh`Z6V@^~W*c3I20!XA$^vy&oINSa<0L^xQEZ(a(K4T-u01(2q@Fx?!E-by4 zZ{a$02a>5?FN{M*IF{`YL(x4r|dfy+#cPzcrX=2T&hWpyS7{NA3!<)NHa7Q z1jO?GwynFxvPXk@LCP}B*;2zQFal}_Gihyh&UPWl8zG>`J*4MqR|*j8A@ynY$cHTq zQT66t?|C5?IP&QPXk91}OLraS--k@I!PXPH2 zkl|AID5z7;3@+GlF>|58)*lnjm}{Bhd{Sdn)FmGV=pWo9J>iFn)n=o7EzNno-saG` zRtdZQrB@dHT(z`mGY9Aq&(5>+wZeFU%pfW@7IU(W&gvr|#RSoxKvlQMW-Rf^a|48$ zcZlPaVWYW(t3`S)v#fIV+I&*>KV0eJ+EXwCO=j~sy~UG-q|6lGkI^UoIAmhCh!|?M zg$=SjqfTb|>VasJ@|)ed!;%MLCLYNj470yi)P`nj9dVx&X}~qcTp-n|Wzag;jOk87 z+PO;YZsi{^jx>I!fOG~S%qFbcKePS{v;Sy6^T)9LPh{UYKZ0G|*=sQ1%-rL~yd?PR zgy8j*Uis2J6($#h3VlcPzD(}|n8>y6jv-ItZU6!wy$oJx{3x@O!ke}%Vy?};I5-i< zo(ri?=TaSB+EM@-APaNjdWj?n&^z7gUskLwv0+A+&g+9LHm)R?QD~f*|Z zfE5*Dck4YtfCH`rCvAfA8Us;m18FT1a;Iu$`VvqBR`2RWhq&fhc=8!RdXw>{C)R^& z9XSxr2+R_k+`#Yc00%!lt%p>l*)6`6fBQLM7uoplf5mkxM}I2hTsOD;KwRSfL0xhD zrVnRPpq&W9gVi5eA2xRI>cifS|7eo1^o45S6x&`I@Al$Hwb^F?;Gr4L&W{xs%U-Zp z=ZWl%9U|9m@6MUEkiJ2X<>)XCDI`EGAeC#SPJ=!fM7 zGn7jpu2bQU3X(f&HFHx)W)1Ub&8Zw?vo$B3I_fH$AO=|qb?j?a7A9R<>I_r~3i;jJ z_vapY4^+&TcU}~uz3eKcw2j;eq}oi1O0zSP!8+m1Q~tHrn%ukg1SG)2;Q)DMJW-`p z!y*COaE6IIHcKjaoE|uuadN%Fsl)X9mC`N^;I(JFKq+$M7?Cvt22cV@^T$i{(VeXj z%v%yia?RL%>c;}5+4A8l_ef6=Nw>)P`7a)gy=BN)*2q2d349!MQ>jA6X!VLyBS2%ipF|ivlPQ*|xo-NJ{kB=}u zgBe=U-_t6`ymz0RsX6A|-rQ;nXwMft3BE9exlg8%=DnTQ!iK%*MJrq9W~Qi9cjfh8 zQLNbXXK`(jqo3ec@2KsRG7ldE$sY@RGo8Z+6!gSN!QDnZI%KIDHRc&|<4i>Sp>P5h zWkQ{jSu{tErW@|b7IBH+M{gXes%HUpF&rw=e`_e7vOdRC49V|Gh4CI<`35+tUmNmB zuu&E^882LTa0WO#$w z9|o$gQw7Sb@-L(Ok?alEp{YP+knYIG8S(!mz8kGp%xtr<+>8f-d!+*pk6u5Q!HABn z?ZKQKRV1M$I8Fz?K7eLbyU)tGb{7#eZ!>yb=ZJ}DB+r|cF9Q)SOWkwfqs&wwi7rnN z;ALIo-CCf)MqBwe??mBa{TGe$8-oT{<`ZV8GIEzt*FVBn~3a8?-Rm8xw zW7z_S_S$2RWTY*|31&An*p#5>+QocQh? zf6mRnAB-;=p;04oJ@Z@CWVcLt$Cbz_frAhHx7SNnQBC38>y(&CqK64Ek5S6=wOroDgg z9TR6{tO?Sky^Qj~_NFoOBHCgFg_$I%q~WPvBpR7E4Ai1QEEB!oawUWj~sJxG&=`Ut$o(g$xsdRFv3 zH!f5%=4g3jh{aRvhH;X)zFpT#iRy9lso7RtGq*3RykAs2RYCmoF%^7%I#4v{XGNS@ z+n;w`GV6379NhBOet|ml^1Clv72*Fj#?;OQJI(4p0mzHXu;Kk-r*z_ic2{>cCfuz z6%gTBxjwMs@(uLn8{pj#vm~RGWD)gDilIgb!K8%)9f!n&5{E#*_x6$zxk;V?COEf;6l%+>SW}_o8zhF*BKgSU(d|&}jNMZV!0Ecy?eCVKhm1nSe?}+)ca8 zdvFQh`%)mSAA`vOCMD^Br#9bNgb>x%I6l&_cF~Hz4$ZA9+Gt0iB;;N>wKn_3vK(P3 zfp_8QK=lbX!Sm;z8G(zoXFMHz)`8ONrjcoyTTk$==G?WwG(Szsd`sG9Jjx3|osMc~ zZPsN$wPj_P1*V*)kLnf$r|d`-13w&6;QXqM@2yJ|8B?kHd$H?PX(px|KFa_gBdVd5 zX(13b&&JwqE$tbERR1aJ)fRTWcPzVBtvxllWDYCDM$WwZ>3(TM>7P)b|Cl@KR3Ka#O zceZ}^n{oO=sdx`b_XBeo^qJH)wC1Yr8;RY!83`9V;?N~8;}mVDo_wd)otB3J2VO2F zKEPDV{(u?CBdC-DD9Xu0BigU$@7FHPDf7YZH4vSO;a=&QVus(HPimEU`~B1+C#DBYf{}H%M2-EFFY9IHPR1eOZAtDhGh_VCkTRA%G@~$=F?M^Js#hNSma$ z(~@Pim{-AF>N9@pR=jK^=+naJKMM}*Jk!+=#j?O`t&oXhKj_<>=rR6Lb>ZQvk*+yA zqI|ZAcYMy+y6Zdal0)&b6Khz1OdDFRmIA{(XTZk4IrQ#fch0f;0sF7SB?!=;X#V(W z?Kmj*2GY0X60_i?34qc2cVT|5E(L)2oA+YqnFSRdzeiO- zw9h2nhjY41caG})P*SY`fsZ-BhP?0(Q!aR>+&zl^@T0HE6Gd9x)AkbbWIcjx9T zG4>5~`YH+$gaMQnLrwIfOKp+g^#Qz*QRd3@ygeHzXq|!50a{zVwrtB@ArT10RAI<1tyQGQn%mH3VvFHQnrJr1 zzKQ8?Hz+4ita+d-@(}^kG6${EUL!n$N=?ahPtXuD0@;!`Vpg3H-~fV1UL^(jIsJIG zBJPU3G707#cLMO~lr(F5Kx>y}(1jqIhAAgNLefH?yZqyZF+43Df3uQcCwM^qU|ydo z8@sSF4|BZWd^MJEzF1=^_#amc(3RoY&rK)r1{Y=ElHXX7`wg2|jGw$ud~cWb`oyKo zGvzg|aSw^qil+k`ejFP53;B>8zW!?2+#hF|Xs7?8`%d@E^l+FszFOtgNA`}uqukQ! zM~9SLK}f_mO9~M?L@d|F_&O+We7nS1heiX8IqT*fx=9oGh<1teE*v@Jeue>{f;$J| z7wo;gs_a06N7`i+Kj{%Jp%9}F$KXjNy|ev@h=bjHiP2>$cE3Ah#604;J;K0DEoxkX zd0#kz=jb+*z7($@?$QB#OpFH}6;2G2WV%zCy-du;jLd2y#+Av5KJ<%@xzyYtL63bePOTy9 zd-c-N&z{LP(|NRM+Y6f0=ESJE{m)wy!t>Ye;#+}=-U0mwJ#6j8>$KH>_Jz;i^Enoe z8=sl5bSDOzRscmd0JA$P?m4%~{l zhP3U}iG5;fuMdQCWEX=VfF7jg21lJ<1Mv#}x*liNPb2kv>(~Uh@XmFJ>eEx{d7Cf4 zv=E4V7rW`9)RM5)awUa8HH5=Gh{#D{v{(US$KOIF1k)PqLV)rn8$ksuz~hF7xdxKi z6eW?2E7eC7p=Se(X-2vp88){|phgnpIDwEHt*LOC(2n)EFIUB)qGl#gfzLt`;6StxVw2e=UJ^dg?&)BT)i@kc^w{r!b~syxxuI4l4Gop(7Eq#&k2BLtuR9d!uBs zJlOEWC4F!Ww>92xCZntU_2?H3_~*%N16aF=3w?L0E`VNH0%Q1h0JsrAc@kgXb%D*N5vVFsy z?qA#5MEC{&r4z5Fg#?k#L6&OWrrHmGKDAiM!NjY)LJU+K1}LzaX8!QJI`>354mjyD z#e+-_;83G6miw)Z?A%z^zXtwWe;dZ+bsrbc-`TnG=SlyIqk}D(4*TLuq6MI&2K8*1 ztuSv}2f)~x^rJ70_`o0-u(r~7sgCgdBE?8ymIiRWQ>i4jgB1<3f5>Vk6x1QCP-Gwq zIZEv~ytNSa+jWYOaM5Xv_h0oZb;7ouIW;me?OlpKcka1dgb`xV&oYg!L}fMS{)x5i z#q|RNR7|HqTyStru31@REXsv|_3q}Jz05}d#I2{4xDI3NN1Zd07!>6t=YWK&hMyIT zDl4QfsUJG=c=CpB-(*{+iBn!ui5Bt(LdSZI|1^T8FOmi4waPfb7NJCuN2Wo6CE7|^ z9fasVJOXnFp>#aS_k@$NKF}bp`fNfk3%o1;`9*~xQQWIw!nb)O-b4ssHkHs>m|gl$_#$a#!5wABnr(w^_<9^M;An>uv)AdHm z6Hi;)iV+M*{WgSox$RtV$4>Cc`5Ym#=;vRdcnve)x}(Wj)8TznGzlPfcT;THH|%I! zL8=L-74h@t270evK*KX*&-Q~}V=gJU8ziw#h78bwnkqxhkm-PF0H=?o2SrS&sG!O? z%}mmd%++bPAV1WKB~XN5H|4PDG*@yKBMAXW?ovJK^p5sXDm>SQ1!Bc*;}dP((-G;Fyv?7>G#0`|FbcYMhjzp~@`=WU6@%Ae=mC z^a|I{IMehoMov-o!$}-9h;CQaiTvg!Q`MIdXxaixm~1YdcSGHbWJbU>v9)?Z zfh9wQTb#UfIGB=6+5CROF2M=Ci=8-^+I>A%?*6XMdg{-&sx}KYtfzf$KRxc^``sbN zayUX^pDcxNtd}Y;qmtq}j(KQT+bo(WV--l$-`YfW!7~Ts>jAddy44C;@Qo%9*Da#8 z53O)AH?Ar!DS-_aLyjU>rVLt1AphsU549fHP&jL&+70XxAHlnfOp<8ntqE5oKs;B0 z<6_l&d`#1S<%Xw3QHT8W)Qn}q$&MyPBd^Q9zpXDienNECidg(s81$g2(Hls6H3BLo zhcgcjVzM!8fE=Fdarn*_ZvjBPxQK?J7pJ4{EFysPcP)Z&`v*8V#e)tZhSe5*{j<}H z`44e?gurB*<3aCuR(SQ74>F3oGkF@2xnZ_iSwMQI0)~spMaDi1wU79W;M*b0J5Cp$ zSD=X`*9ceBtHo4H}Dw)1%g^|8Q9S zA!@4j59z1>>zHaLRJfx$H?3*!eJ1)gN^7kz5N2B}KpliAf?OMf8(rLMd}?chJ&{M} zI%?$Fy5pOlXm2ex7RxHl)Z{c$H6<-H>#wVw-g_|%EAjTFl{4!P_DGYlZ(pAMbmgkz z=BxoXkGP-z!@Jju4!HiUo4tJ9=*(Le`pQ()R<51|EzoIlY6KF$;@rF4L0!XA(Ac>Z zq$E?5wHe1-RXK$C?oy$;vb`iMM_rYtOy3W{19zwK)PwyDlwQwOapoj@mcc#QN#Fvy zyAD%+o{7jL)UkTPy0Oi|X*Qyh^lVYB9 zx7+r6KL?xcQ2QP?wysiIK6f}h^cMBAcH1X~_Hy6gM%tFjzw?}tsM*v znx*YRuu?$2OPE}Y6D_2OnbqDOT=4S5meXY96nbDoBiVK#SH5s4I#>h9(oK+r56BxW zFZBXNcbAJdY=(A&k0=Pf^>{&U;C@>mSn(G+KQ#0+aUtL`HXc{GriBAcp}uofe|{K1 zr;dt~<64DLa*YQ|nXZUc^7yWZ)F@^+U-zhM-vH`>sseFss=Fd4S7cZUvNnWt4c*(% z9C+LBz{fL~$tUA?5df58BPoH1k{q2IKuath4erDPM!|m<$&V2mS!NJVef2!KnE!bpY&i25)0lupvAuXje z?6obZRLuzgZzQMbG$vc4>FaX&Mu6QAX=of826gCcV)-0d#L34Lt`EX91%l6^rR<9l zS^%6JxkX+fFd&18BUUfC2ML3>Zre6iKuVNOGP%A#TJhx0J!5HKTc0L&;-p$2@=#Td z@Z{`}z2<}sHNDFb2~qBs=OlgFp%=&MY$OB*a>pFDTohRmYO*9?s3Hh$a2)C*9|vf( z5={RQYEZkPrJ2^G3CMX$G$o!RGfj@^7Fyb3k5WTszyU<1-M=;{El_G26oDXzFq~8; zZvTolPX04J4<+_4i))!T^I{VJ-ZAOvi%h+|*W&y=+AX74FqIq6`V(bx2~^ z8qD4op^8`Vd{E&%Uu3~ABLH~!m~l1WKmxC*?YVhF&o%9eQRa#<5}br$aQk|NK1_l`s7 zG?#U^XaNB)tjFPiKI5w6x9RyAhmdf)dG>#vyZzyT|EI+u=iV(@SlgR;h?nma z#FULX`g22N5j0&wDiJFThgtC?dqtma!*LZ-5x0jjsUW zoU4_X3pIf%z7*neCK(kql4&?n5+xbm7|=+TD_id$({T=#Y6H1925RH`j~O03_?LC+ z7%_*eC8u8SNWsg_bhGZ0^y=r|J@#$RyIb>qpE$I<{X2X8k$~UD=<7QXig2C2Ufvlc zO)@Ky`w3+{P_@y}WgjJ4f7h_vzpcw<9xCyAIT3+L2f0sIPgnMk>ra zowx4$f4?{}aIabMpr`g%_)EpRpUs#&hGrM}O|oci4I0_LbPzDyMJ9V$uzjsz;g()4fvt~I#_%hz-$-IwnQVP1SpJG0$B3?~H$ zsEIW5n`@j!szBmdEBTZud(SoA>K%oY5DMVw{2$vmwo~fRJQ;b%$P*J2s zI`)M#Z>LIdSbx6c8HrzCmFnxj1th<{(%Q-s&7S4B*LH%?$Nt{$=UVyZ zVZzeQ`;JZSSh9JZYvP|RU1x)te|ZwkBC`vORz(p-vjI+g`Agal`z=D5=Jg`VXNAY3 z^RqV<{6FShv<@HqymNP&)N9zfyZB$S0cV8=HMqVWU;+mmrSRO70oC(%4;y)-Ykg@x z5Lfgsjus8C5|6yH9vk~|PH^M~hz-@;b^dGp9%)b@u27{YxWM(3rUDfFB;28D@8D|# z=fDP&l1*Mwkh1@H%XU&dSUshOr3iPapE*A)62}oTF<;S4(z$tyx^C`9oSdAj&?*)a zDrv#%E)5eLT~1@zGhwk;&0^}AV;)aFc@m!}qM&k}G6SKD)>>tjv%Gla57hED>V1{h z+m5*oxG&gxf%N8(kB1tS6rHop!9X3&%x9K=vA_TMTHfVb){JnsGmYxLWT~05JPkMwPofiPvmVi z#y`2R(?vf1^=HjX|4-rZ-)IcywJQ!8Z{&XR!hs7HrWSpw$-;wX=Lgpuq?rTE2z+G0 z#U@_kh_#1W8VLq}BAhZnoxjKrPiS@|@)A{)Zs7r{+UGe^t^|>T&j8LB%T4v~pSRp2 zQZ2c|C5gDJo=6Dq$j?so=c9Hj{8EoS_Nr8{V5lK!7FM9ff*VGSK*8NN(Nniw_gj)s z0r!_`Zrg(ux3ykU_faY$^7gJwQk^r6r~h~K#7cW-x(%O=INvIye4X25IX}@__CLKS z0w$A`3}csr{-p-NkJDJp&l;)@{H(BdH=wGVkSApWDBfJ$^@hie^^az7F$zCuHS z1}JO^$m58DlzTP;VP3SgUpK_M1XqN$Eit`hlLE8~kUB+5^zR=}+q&O%Qfm z&js`n-0@(SI$9N{mc;~;X6No^U$(u}HzxMm9}E47#8@Dk6b9x2K@YFDR)zij(zjJFWe$3GtqT?`86C^e&!vKzsYd+g)Lmy?)Pv_;A74@8-{(|GB~UqG3s? zi)Oy#Wg@~~1;5HH&7Tirx1-yL`Bw^nJ?%m6^?RwjFpq6Gw(%fqn0@;CY-~3PXK|k# zuiNvN!kiEUv^Q=OA3v@j1x!I~IO74kg>$}%0uGxH9roXUG@p;leq*VK|7gTc`+SzG zr6Ss(f|;#s=-E5qZ49Ip>CazAR;dD@+QXk)>s*H)1Qn8bVsnuBo{5hI@+;zs5j_&M zH7iLNBPo|C4vGMfXGC9Ua)ZUHmlIf3ugP9cfACRF8vaC0u@1=5WXEfKFhlk-&=-ik z@a9~M)&2D9fwf0N2RB^=LI%oB)4>6+pX)9?#&6(`W!=XR0 zNRdY6@6WA?0-Ms=@^u&cm_M%{)OzV>YV24$ws>i2*|UytikInEJj}eahOxC{ zMCLj|B?+qwo%mZP?@f2iHK+==yZ%?2FWIE?)hlE<1;_bfc5V&b%W$YBD|1+iOm z@UPzvL+ETb%s!I1b7MN5dupLUlBYf}v)k>t2*Y8`d(k(etz<%r2}Uth4C1H$d|@oJ z!$x0z8%??gnxb^E$-hA`zM5*b$aaP5fyTr~8wYeB^m8~MMK9bUCs2~Tqx{ds&gaW< z<#$}F?x6@SMUu52pAtxHbfG64+)hKvUWI40wv0L5SdSM!o-BJ8QTXHJ9Mh*g$9uxR zn?fY(-M6`ih)%@uf0gE2R$lvg&6i~o^I^mdc3uA+yF%TS)P0rHu^ki(BOb%fJ+o9p zwWahroV)_KX*qxkoxu?9D}E2uO%dNI2l3yVR@`0)ffv1^ZHaOpPU;gSYHp0>p zcyLJ|1QF;1IpI2eb-Aws={nwxl)x8~u^4xNrshT7=9exR$l8%!>R)*!8jou0=+c(K6a0Aclvi#9^G zQAa_)-kR>xL>L8dvGnS}ODqBrMNp>`&XDL$OrtzG$Y3f`$$9yPHZl0KbYQEI1r)DS z)ib0OM{2~x>HO7mljMd{1C+UdI1PKEO%$rFUUDhNa8dj21#kcUWasA>Z$Nn8zw72- zbhXuwBd+&iX_@ZJ4h7wD&LZiDH8i+0;TfG6*gWI;V=~0?L3vml{HiQ5f?2z?v~G8$ zO~DKB@z^|<+^Yoe1u|Z(w%l=L^n`+(s)3Aki=uf70Hvf>4Js>zY6OP?dA4ZD8EgBY zH;90O$pE+X9nt~K>4oKgS_0s$R|@TdK1ASC9{Qqf9_%?%Kzi{Sbu*A|uyicZs%}-W zHxO##Z@kSwpOKVoQFNyaXj6RUXC)U>dRdUF|Ynzi-y;=h<47+y`}{CRSIDEN-@UE{*;# z@BO-{7ieeuM`C^QltqD))|gc9HcqSza16Lc6Y_t?JsoaVwCOF`-I{MzN%{R;tDifM zc+U6zY|RC|bd9u468z$|+7eP?^Udx2Ap$uy!-*#tFU*{k)Ot%4fj8KIfD2zQ#OT=+ zYyzTRoR>EXF*yIQ`-yKP)6#aoy^OHYg0S~O)yFQ5tE21Z2|Fdh0;H-sd(92zj?i*@ z`-`RKgS5ZUSRoJvdIc8{lQkf)H){(bb^}jjqe;zVK&LiBBLmYrKF5b^T$Alv86ZFv zwoqJrlgSXF>_4Rrn5w!4I2F0Gs6)C2Gn#kve7{AGmISesn`AyTu|&V3a#&Eh4Qn%Y>XxncB)KSH)!sI3#PidwKZ*)@WI(1F=Q&Y ze7&i;0^N^#jK|6ti_{9>#$)pgK}xIdi?ag+vs?m}T5Nt&8^^`LK2Y3BEBlT9htrOO z(jKMV+!Ygr;rz(^aSlbv$F;!fEeATF!UC8r8eJJ4*99j6Vp?rCXiZ=};BI#arNomk z#3GPRjzELae%cto_BYuZRN9cqGWbLkFJR1$;xHGGuZikNX$aGS>@;{y?|Hg2S?&i+ zmPA}bej@IpJ8dkDg_3P{Lu@4D$$iXvIkPN(Wxig=7<9d4;NC{?%hFLlU+tjml$H={ zgtzNw?uz-B8QP19tu03}cEz8pSuo)}J^$Y1HnVK4>YeXLoUchVzHH1<8Mq$dm%r)e z1l&`S+E$x?1vmGGxZ8TRb@Eiy$m$LTix$DfVJh8Il)TqE!i+jONU*_SLGHRoZi-z! z<(mMC9VeX7%~)d}F<7u%WqIiQCbJhZq@aAus*aa8XBVd1dO)?#){h9}E&H_+#3n#2 zqev}HiogCN)CzHBwsTMwpmE1zW>QBkRYrK@%9MQLuDx|c%-Gr)M$Hk{B6yZf{J<6{ z*H5wdQ$4N)N9N)&sqN`abZ5L=+&9jZrjwL45=Ss)KUR0o$|^4!(Tp7QG$nwAd*#H8 z2v5No|Bty?{r{K?xcY#-;iU6Co!`39Kw<?``%xd z zk}DC`h9LX#@{=yXYs3MAto6p%0{-RAcxiMX=0HgvaO~;zKu?F^v9aLF)U?Wp6r`UpUKeLgRjLxFIOZqR?jCXo~$c&A9=r0x<1 zE5t2*^{$tp$-99Z8Zo#B`}sFhwnsXoPz6N}0EcuhD8OkV{q&&(Y^M$u8tElGLt&<~ zl!bB;Cb*V`Im7>zuK&%d&Z@q!S|Fod3|$kX-T>}__+W1S;Dpci@%^?(F%((hK6M?< zH28Rtw!hNB3VnfX7tDoXS>LhJhuv2C4ca%Zka8>Y_vU96~F z6>(j-Ds>*{0^GRo)E9-rY#=Y#+3Xi$1q)T4jLFnkvlR?&(h#mm$+9;yA##9dgftoN zW}hMnBUnWyKhN*Q11TIV&23Ec@v-kI6~pKNBo}0N6fYDe9=~6!sC?XBt5K4~1q=ot z6G+qGlLIZBy_>SOl0o#~sjb(6t#D_c_cPhvYV}qj`D85!hWpJf6acGjPJ-_KDhd$m zS%3(EAM<`Oxv|eSm6?o-Zosglq5x^h&h75Qo`9%_^;7-xo)PO0lblxDVMU}AB{@KY zJNq4e>O7+`$Bp-8Uc4hVbz!5Yvob!{%!bRGwY-O@?x z;dDySpMzW{95eKZqbg>!wPsYEP~}c#nW%L@y;COivI9O+18wjD!IpN zJEp|al0-IEd^P^iUsKonPvae>{x7a%BcXdA8GGlOBwrqxmTQF%*17`%TG4cwiaJzWktmJ-Z7~oRCdn-doABaK%0pe{}U}2tnBbq?K1DO{L zr3Yc6bE!di^cCzHftz0Dn~h~2N6X9zkm(A}CMu5^I7z-0GpKi0e{%EKP39bKYddHoXzieRXWg$Os_w(%I7oq+kr++gony zna+NBYWzGVX9!Fov5uw2jTFw32^wdaPwc?3B$ynk#_MX09f6aGAal$YB=IS{Jf7ILD#n*C4+p*Qg8nz53V{u^nYc7up8yaob1*03N7?c$2~|&BQp#JK$g}Z zt(SN8YWWgCCIa6@XRb9GaG(;$0=Zt>lLfFmtglxO$F#-aa01m>l;MTpz$>VInx@12 z;?ux{w7-VjDF6WMIshWoBrWb>^dL74Fx|*fE0IO?=jDXZRIE>fo3!dPorYKz%C&aJ zE6fd7N;KJiL;S7bzJH4aM>@G z&LzLAnzY8-i8Yd6tQg{{u-Q~4jdP7Dfm%LJGmaH&&#qD9)@lc>tq8@7(=1HXR-0V7 z@j9OkfPr2fm+D;a-X-kC*&t|W_y8vo0oI1&w$^VDsOCPt$96T+Wcdd45Dpj%NV*1! zJCe|i-lh#11_5hN;8#llOc>Xm^W3RU2 z9KZ_fsdDly>!t2isY` zp8RH=ZKJAD0TuVh_eEm$Bvt83UvH03gJVYjytutBj6x%_pgVH9bU5Z$Av`^mGfLYH zR=(=8QW=L#0B!J8fD8X~b57>ju=km%qlqkWL>-f;I&yy3TukBH(Xy`P44+Rgvb?Py1tX^bMd}2|hToevcoa)2?6C?hCSUN%m zQmQTIp=iUGivnDdULf2f=-8u@Anu%G@z6=aIEy*HY*T*YwOe%09mW?NQD_;J}I27o55?U*_!nHenU+c&uWGACMgvlz(rL%D?AjPNL6z+2=Mbv#5+< zKJK^l?vK@if5L2p!z!~SMU9`i5)|;?Yy7)^jYX0Zklo>lKl3aV9g-|6?Mj+m4*jch z@kSK|0?SAHANeEAkQ}e_QEzTg0P`uS?_dPL&VTrH2Lq5EG4R(0?3r;KSp!O)vx*Er}oHF!m!jy2D|9D&#?Tr*Pf-qzr& zdYHqU&5Jol5prbe!LK0jE4*sJqsBY+(pIFRJW?KJNjk0utu1Qy3RyS}0f^o?AJB?6 zNDF1;fBb6^>Gtw^%j686r|s-YTczf2&t&oy*y8BG)P9-@5mZM<`WXNDj!%|vlA2Eq z*_yF0=oRREMUh6)XAAEu1ApoUl@tj*qmv%o{>lvkAS=yYC)0WUr^^S7b3ec81QKGm z1Gd+U{wu9}#;YIssqghb?_Fe%CA)flvdZMDl2O~t^^0cdN6%K7QO{wX`wB(r?MiLi zjs>20?j6;&gj5z6^PFGUY_;eD3?+S(NEC&c;^l=EqcTHT`>?Xe9D8zyCWx+2=Kzo7 zO2xaHo&?ZX*%i^wf=x><`J}p%aAf65b)eoNrC0i{KHnjFhL)%X15K{J@7+!S7?bl9 zqSn3$%&q7_m_TuL&((*v3v(Hm10I)uv6=AATG!J@A#?eu0+CB_cr3kkZug{zh*YkU zXZ|-SHB0J}Pn#q67))WS=8%wR^LO}x3|F%2G7(|LeU4<)t-c1cj^FG0|03vlbhRYW;A z5(wrR74Q)gvw~Xse?=#hGE0+NZ|%Aegw#TE(ZSJeikIO5`p$%Xa)kA&mYqKR9gzA8 zT-J=KMJhI1I~1=*1Iz})!qStQ9WP`MLn=pL^QM(EI`~Q|hxB~xTSw=S#blf)LeQE; zW*|x-m!oJ@OLNH2#jY$%76Cy^dcV<{iWCYHkQ}M~uF6tF%2BIS7*}#^+=EALlEH>= zc8jO2=RHv&NkWu$ROcoAt)FH$y1iL-a+=0p+kBJfxv2LS71J|!EZCH$Z{e8;4@v}@MtkC8#^;PlU^8ZMP1Mp|VkJ-T%@VJ1J>hEr95PpN- zBXl=kM=kKK+`0G4Wxs7KZzkR{37-a_xb)<)ftG^gzDZ%7MCXFXz=paARg! z8}4v}+I!f>PLM0BiN-+7*>jlMOPa#z_{{(O1$Z2eH_T6PT+Bc=z&o@SJjjxjYQU_T zlP(lMbJc|)bs`s$6C`&2H`FxDc7-`vbHhy{n1`QZten?RaS*@Ck~6E5m3#F7)}@~R ztQDZZ*WM}!V~Qw^l-rDGWn1Yzs)LA3;BpgfmG0wf;;+8yhts4kzZtK(x1eRgv}$)m zApRwqw?xc#ESQqkD1FMT+3ZqLlS2JG#h^p4HnQZW;+alVD>YDa&f;iImrrTdrJa7? zUwm%CabO$?d7ZoU`?@pNUP}5}pD^+}<^~Pzt&bdK$2Cl-At#0nkK}Wi*PHi?A)RnI z(B-&t1heXz6VBD`!bv-`l36ERDu-3ShOU3X6l)#zE{%@Hxw1jyJ;13!0CWG*qzVio z%%Ixb`;xo`jM2+~hgkyyAgbXU_sZDs2~jyYKBCcC368Y~_IoEbz!#=m(T4I+F45`y z8jTo^kLn7{UT4A#fkUhsve0U=eT)CR&_5P$@eB`8*15g4!6JS_F~+5rP?VY}!Cq6g zas4!!#e|_lUM0+=2;#N3z8b$S#rYKy>~0(VqSh}v^yjuMZv^)*nOso)OYjcgwe?-g zrf`TxD#x7y>gEsc$(b01tI)ju@;r3nT;833kx2Pk_rmrl;;RN@5G{Pf^zg%8Qn8#mygWXh?eQ5>*@XCE_{AK{Xbs~ifZm$ z13RGHT3n`*6(1i{6NebNsV=ja^_Ierspu_nLFyv&L<%X2s5-FFA=;Qg>vFXp!3&yv z$xKmktD685y9gG@fMx1#T@`U&TidZYC^Y^z5^aymV0nV`yVzU1)}IJGK-dHcZfGzG zt}Zu(T^GGC%l$;bM@1=Hxd4!zZj8Td^zpa-c8{UD1`$qw-b#Ese@(^x&BF$g?^P9H zM-Dz~{rYOA49*gE%<-A=Qq*3yR*Tj?IeiV=WV1`I6}WbA>f2tswSS#X;atL(*;>Tx73nb)->7LIJ6YJqA9R6n#Offort7n< zUO;5!CkrioeMahRXmlW0hSr-T_yPd$U|^DC$sD_MZ#b~;Y&^rH153j64k@6qiqc5E zYUOHJb^;XNE6&AnfvBe_$SE7li3XhvOlkp-##%Inn4_mkSX`z;nDVvJAXrE@497_8 zRMdrf7VW=^G^)&h=a#rcp?MvKJi|95>|S_5Q&0-s0kCxsFa(exEuA|?(sJ67ez7simCx2;F5)Y=WH_##i=%keIX(r z)~Fs>jF>PGkWP&TgvOnES&jBwXVPkrj6%3;KZXPCKU-9K;Yxye=j-(=ApLHaCKA-#Q83Q>8W)ftDMCG z3~Q`X)p-h*uXK2+!4MP=@ygeL z2{Wu(3GKmm-odJwUcY0z-5({fv>OY(9W*tS)P!K~w_Deqkt@;^N{RP+(m~TMjU|n)Sz$QXcIV7X5fX#f zI{>$Q#*a&Cs*O}2@LxS-QD1#j8E~W>k*Yw~aRp<+iaClTxdS4#J!}hbGB|^`Hb?~3 zP2f(CgP+xFwVZxEz~CObV@GwhA|VVB7%*T;W&Bf7UD;~)$zzIQNQotVoNUoJaWOus zSRa={YV6=25HcQQ&!qhtOTYM|&q14lBy@Y|2eYq62ThZ^W%&%)9N0%@t#t= z-_^dGS8txrP+*a(j^inh(z>UW9@gLGMW6AAl__7AF!psO>`gEz3PyDzyVhw6w3C|K zjuE7OHQMd^1+rmdBw9Gn0zp3_Dwy6eoQi`O27EXH2C@v(oNES@+R}C)PCoj;-)7{n zh!_mE0qaMafi|rlD4@H}Krai1fbdaHJz}+#UAI$wR3P>wC~{| zFMdT=Kz2mA8qS85b48RbUNGp_mrVcEuz0rs^!u%+Fm%q0YhucDnVgH8#6up>c}zIXGvUXo@q+??Zb^<8rJVJy}$WgP1e16{!tRy*M^AXz-AUib&v`uj?5q90tzPh}RS;{06^$ga z+OxUCaPJXp12Es@#?2{#bS!*oF0ANL(_Tg}UL)8%$Wkb1qL9x z!KnbEbgxi&%QFlnyhwTWNe3PU&3nP$vEOniKr$!dc1) zc=I_JVqf6shQR=?){XW&A_0LDtr4~?eHYnJ3%T)M11Kz(?zBC@&LN;6fD(R>qB4gG zZ=l1}Wilz8uE=#ZEwQ=+zE)CBk%Mr|5W=g_Z3nB-TBhd1B2I-I>`VW+ZQuhjMvH;3 z!>8qIYU4Bo;dtijJZgF3zIT4RT0vv1x+=b3e_rRtk_3krs;g2AKK!P)!$|OEo7N^; zxs1Hr^XACfGlM~$Y4i3!zkJ5#Z;!#@W0n8>nFOD{`%8A)!_CZ#xmiymd(7FOtUKX1 zHe+p~Ql}1Mm}qnld~|A;vbOXPdME{-x3;*)s=7F?1jaIE63)t4L+c0YQ7NzcmWm+BEN7l{`Sxpw`XJxKIm?rxdkmcm6s*v=tYRRY4QPK- zw^{*#D=oF?gRe~qC52ECQ3`8aAYpI`z#`TtuU+%Ush)FYUm^{*wdv2{8=bOMj3Qc! zh+O7_un}cbT0pM~{E5=c#=}|39sMoeomHU7C%v?~u?UTM6L=@jy5N<7KF4`FLs7N}wjwA_#zmTsr)YsDhE9`jJ~4|xjx{zIu9OfE844KH?!JPqL6Rv5_~)_$ zpiZ?l((S>QFlCsv9++&9B3r|-*h$w!8)aK#=(pv3A<2V|);jZZh}^w^>aah}L(C%c z<()&k@Y9!tA35`?82wCAu$Af+GfrE5KGE(T&m)`D-8gB62)JGSw(NUdD-!n6hx_(f zz`Jm^occ58o6Z%>B$%qYzh#_zv+z3W&4WmtTIDkm?q5S~KdUxZ?x}wjnNfxt_A>JD zN0{_h%BE26!$e%Nwg#V%cf66u5%*Lw)@&~>(PV=$j5lqPLA#oH+u3MMfc(2x*M z0-OfO;Yk_&=*G%#gWSa$<^=Akfwx>Mr^b&rdw)z7RdpDZYP*I%&irf3YIdkJ-r+aK zJHp#12+#xO$ugglo?ZS2xoSfcj!R0)kSo(@_d=_l6mo-Z4RO z7{Vs>E|tTpoA2CPs;+H829mzTBnCn2dZgfB?GA-(d@5_8xNvyXUw0G}@didI_P+s5{Tr=$o1d4Xz@s|ZMY$Ooz@bE&##0Ugw5^z zLRR!%l=X-Zlm>;;=(^) zTqJGNLK_w5wn!=~X}2mI#OtrR{Wl6KzdGOnC6AE<#)C=|Y}fHZ-wc>KkNka(-~}1(iz>C0S~Wyj>E0 zVC$#_^Ho*5uWj#xvPWyd1!vE>8&;ypexb~O*%Wk?lbk!A>%qFi zVd12;kx@p;pw>jw+_J+Ja`sUoH0Q{MH>Fx+j@U8G*4S1Re$!Tw3FszO#BN)yI!Y}m zxO@{xdc71_w>lec*MaenJ@i5;tz+;!gCOd*lPfQH${Fmm7Cn)t-?93Pkvr}%!U@-D zEKM!_8wE2HAL>{6{5^0YzeL(yS)}2$`^ER>5`0Za+1dH5b7pt|y>Xb{Jca*$ zh`NnKH0n1SM8y=8C@*bm8fpslXz=$?Z|!S$ulCV48|my87<+q|RLEG)RFh&~v!=$a xNycBqM%lXelFZF=7)?t4O*T~r8Jch#PxC5@7;vjtRAhNmYB)0DrpAB3{|9#@mMQ=M literal 0 HcmV?d00001 diff --git a/ros/audio_common/sound_play/sounds/NEEDS_UNPLUGGING_BADLY.ogg b/ros/audio_common/sound_play/sounds/NEEDS_UNPLUGGING_BADLY.ogg new file mode 100644 index 0000000000000000000000000000000000000000..028196f3a69db238065500ed74b2018ab27ce799 GIT binary patch literal 78198 zcmeFZc{o+;+cS+{K@QaLz7&j|!F%=ydJZHFn%9XM-H=k&3V`(Yrj^O;NY;5l^hX5{?Sh!Ocn6_MB z06svVzOlmAa9889{JlnN3-VngcNfev3i2~9sE1@*H2>p7S%<0uuz@j>$tiv68g*^b zV)ZdOaiU3)p0)=l=}fQQNit_Y8ip*}U%!9JvTMc$TzP*EP84VtofLt$a<=b;5-KQJxFNnjgeoc!42B!%#wg_Bn1+BGOS8wRgdT;8b zGU0}m(`+9PUp!U?%v~LJYxSyIt2g==q|9&J=--$!zdI%1b4rkU>fgUF8y|d@{D!Aa z5g^i=wEeMhiSEdfN0Ule$*EN&gi26C$>ix8=astd505$&S=115uwiLyooQ>G+;9~j zu!`gmMHcS=fBnM!w}<`DKRTkt1Zc$N#aOM2vHDKl`WNF&rH3)x0!UNZ+w?-5^^6tP z7gtQh-;9-xm-5}OesEX(2N5J>2cRvqF2rhGLTq@Ob;M1WyJFg{6&|+`6%vg6pO1uL ze!+?8>qWK~N>|+W z!?ylyg-^?m3-B%@zi7DQ-&~cEl>I0B z&`7tSZuRC}iPXj%sbGQ5ex0q_EB{c~Dza!X-lx4e$JwnSN$9OHrfg;6@XRTIWd1N0 z|Kl{w%D=dHUtXrkWuqfkEL%+_r>b+;CimBz<2%S%AQziXKrY^V^y%&uhbFl!DLdLV zZ){nxbZs2l(GyZ%ngOW6Q_H!RI& zTTS!z@twPRssD~ucaEh5bgz1FYvY4kn-`5p{ZGdFr{n+%P4ciz<_Bk6Tr5uVw3qos z;D1O?tl;7ntxH?l-RgUe+G7RReCQuIf+=n{5-&F(ItOH|*x> zu-gq`^N(%xZ}?kaeu>S4TdV#}awH<6J1OU}OCI}Qk|Q$8d1RZT=bESQpJ%vs`{Ink zik-V(RFv@kLvkWA4-{k`NY8who@11;Ju;)P@_0$~R@xC1*yQKK@X0oZ|HV zO>)E*S`H{pb*3JF{hH&n8;H=AP9y%E0081#xtYUy#LL_4ruT%K-e$f&)BevAgKuwI zdvsVMVz&Wk0QBwK-?>gfIXHJxhFt{L^}N=F1(RPCmoCUo%hhvseXTS53ZH$npv^T! z(D`Uy)Tk9cS(~dvUF^&nn!RGe~a zs#qx9@ek=h1N+`OUVEzIewkcr#of}^lOq4=2?DhiU+l)pE|M3|&isR~5s(3QmHjd| zlB39}9m$brt%EB6d*B!VhL8pZR4&v1gsId;8NfC#)F1r|m%K0nm@xm{(g1m3YqkIT zZvMZQ{>Oj>V+4>^;-?m))k@}s_|ti&vVs2K!HOSA3kXSNQUaZ(wSk(nk#(p6@Hx;M zBWP{6_Ug0UUKUx>5^FlM!*NR(Izbt_)_AM5``Oz4GS~mYc^hSxM3QJ;5bYcZ+HE4- zOcjI?Ep9;RgZQEC_9tsgj55)Gs=OR)+WMEHm%wATZ@`Yne%qgAlo%E6uRJjh2f99N zqR_dEDEw!!wTA$;Flys~5rt1PBt2AC9+JhH9o8OL^A@KhqHupjC0Y`CxZ=%K0VC2S zWtC`MNJ%B%fLq(bm`H4Q;*crJ=hw4_sIv0qi9=&s0UfB@+rs>%dL>VFrU1QS0w^(e z@}=@RuBJ!h00>6b=|Vd4LUxdwMVVZPvORBakh{H$PN=uHiE$av-o#*=x1CGbK5t7m zv$E~p9Gz`>ZWYu%Z*OA`Fr#eo&ySmuTE&L<=bv01rCJW{Az(W5*K`%q^{&<_*@~m!^dCeUoXmVAT0b_h;TlBEh) zKqn-~fi3NlO9D?;Zqzv^njWW9Di+1K+m6{~17MdvoJjcFLqn5XT>xl9FRHfNpsQQ5^YQb6FCe3&p<`?@$xdnb^%VF5;Ns1sr$;=soji3qZSUyp z;^yutL;#3kD@g;v<^BprCC^;$udD|>Zn*t0uoAyEW|eE42BAHqHe@#BFf?PxZfMGo z-_U}gl|y@no((}y*!Q=li`+o^x#iC_+s7L)|L}!}d)egVxZs;j_u)SY)?IVml-Cae z|NJRsE!%h^zE{Z!$yOIo<8nm}}mb8Gela<-#tH5i@7|n#&njsmge< zm6YUBQ#pC@m54L@<=J7Eermcp?g|}T8riI4W8ZM`^HQ24G__aPOmp#e6L(CA*q-|= z_-x3zJH^>8I@Cr|9Pv7Un3!u~T0yj@Z1^{vbQb80^#b zh0zUjr6aWx%60vu*h#PYDdkOnyevfM6W)RMBTb`V`G)*V;xpnqFL#Jq{&veW16sX&o zaF|behyCN)gw7(}Y?EnQgvKiKD7B!<6Lu;$)*Z^bZAEq|DxAx+JkqaqZ@%zZr$Wnz zxWp$@woZyn-l&~+wN&fG+nyWk&TsEHZhTgG@A2)&-=D-%t;Rz}ybJ<%e|8N0F4gvU zA!UE+w2|)RWAy0t3EX!-CGzu>rt4h4W!o$N???hGb zX40K0_WSd{h#98cbH|3@Q=J+zdnrMcUVY%eM6>r?ZtVrXIjzbO8uORbat`WJWKfc@ z`~78SB{g4Pr6g`vrC(W;vSayVJ3Ia(-=J@D3NHi<6%*mZPadce`sfDf>&a2p@0z9N zC-J0e)XV8y?q2p;O9nh%BajFnu>=zu|Xuv|$rKW{*2b>J>NE`QDyaFj)sN87>77y7U_f zxz)gYHlC5Cddb$pkp~M+tQIoD2O$&Vo3c#Y2){>xYn9fYdbc3{c6%;gO2AUf5(&J` zjO=Feo41=LqPta>;-9>9Z%w&cijLIXTAwU1EOd1`Yiq%^|Ko6Ji@fi%zGGki>?5zm zsa)a3X>bFig_h7-MfP-~+@2F%RvivYbW287&mhCVy&9dc6 zi%T9PZQ~Jz0_B5_ZpPus!DQ}8nS92YekE1TiL*#(buIN^t8*EtZ-z=0Eov@P6FvQP z9rTw!cUj4Q8Px#GT6 zKQK{7T{ZaWz9;2NXG($55)xD<&vBWvd2X%lrx{E51h>3YaM!$KhC!LZK>SKM8EeP= zr7t`Ed%7D;G?Gln**ZM;2*<3)_neaj25rHN(B3bwT|d(+aH>ezkUceH?$~;6MnXYt z^yC*dF(D3x4#vxl!mD}~5y&HyQ=JHY$b8$ddS3I@Ri_sf+-3tFrOrVZDGQE|jlMUkFJbU9^Tmf2r>4gaNd)+J{9}?h-v_EybW+2-<^}`@hg(=k zcrW7JS{b0nDFfvLqUSA+sw~BAF~((?G?5(tT4D44iPL8f0_P?<-FDou$|k9F=Y%p5 zoYMGqROWSFPJRigA;j!>#a-?XZG=VAidJCJFeK#?PEb{n47Oh zR^z%r4b?~&rGC3o=3Nxa=QKItWOVdF8Aq24XYX<&1+SRYkr~Sz;%ZfrP5E5o!pR*r z7S^E7RcbabfgoC}E3a)FG0W-6NH5MvbJ&|QXeR8rJmU)M%JMrIL83a%x|!i8FJ5$= zZaOUy$fXbMwR)XSYuwq*QZ_7kuXK(}f%c^o$KMPd#V&4<2l-IWH z9}Oa4enZTPXLhn7dZC$V)T*P-SxQ#2=3DgvsBaTvih#Pa4XBYako_P>35*g4jQeQ< z)DoVTT)nKh1%rO(uO(NmcIwOXL?Q@uewn*yiM2=|peS|I&x;9Vp;1Z2!Bs^bPL4uf zPS&kTzHXi|mB_eAF}`dsYkA$kaxdS2dr@;w?>JW=HaVys(B2TchGWlfc_QXBroydC zUgZ|6!lqsIBf|x~%Mquf?m(IS1Z*##GDB=Gt`|%FSRi0Q+VMV&8~Eqa*DBFUy61zJ zdML%QMVwkaZ=sW_*H#}zba2zx{5bRbZ^@}^0fbD@$f^5h=#oYR$mW`N{4vrW1iZgh!W5lRCy7=U(9} zFLfroIf$z+OD(N-Yj50q=j=neIOBelZ-ONmK^Led)X3e?y&G7n!yn0P6WZK{>Meb% zr!Jf~i%X@O^X(Evf?PEtmcBXj`TgF%I>AR>Mqi6$HJZnn4b&VA0)Yz2!~%StCwSxU}Xp!q6bDn;8CkWCuS zeVYuV`nPu+_G_r2P2=HmmY!6>bZ7U4fxh_YWOC!pL|8Sa-f@IX1f)DQxd zPi^r9^WGB9IGcy|VuD;d(y(Rgq*N8@@ZdrX$OTGmQ@0|d$Lf)AbR#g?vS7Yo5qb&M z+h!0TINGD|sT42>d7#PzsFhI(H)i@R)8tF3WL1nlqCRMHR(HST`wo+DP zIag$4lIZ8Isjngwc-?wbKS4Di)sF=X2f|A}E`O|*hBTC|jI8gxQZI*Q6ry`hIB2!o zBH7^Ry@Us?SO_#&m@cir>t6pJFml;zKenNsXHBM;bS-rj*aJ8nah}Lhho1Y+^G(Ra zO!fI|oTRy)2X(`Fr;l2w@wU)6>n*H|?Q%Wzw79?}Sh7ndp{wXFthF#`WQ1H$tK;n=bD+gn?KfQ;H{7?Ao-Qrye~ z?Q*`Sj5d&!IV+~>(YT;ECx9~ojOd!pO9(FT4qZq%q0UHytC@F&E(1}8kNgxtKznB; zDS?YrwNJ-72QT0&$z{#}j-&Zse#D$t7J;}BzA*p9D+h-iclRI}?WbxK-LY;lLyi%d zs~x`+JG$>E0u!^TVs3aKCS*YT&h*B?7DM-PH z3LCVROp^yF*A4m2Spc+M-O?*U;H6t{U&xZ1z4*W^1r=CJwfJExV|j{YI@F~9h`0;MZ}?`$bs4w`C>r-PB{ z=07HmbzHadjPf_-XyXpcL+`DO`yM~Hce(o;pothJ_VtXf7iE|h6&2`RifP2~gg=UB zaUkNB4QfK=ZWvv@403>`&814;EKw9nUFn1sQn%exoNz zJw3UBfbE(e%wUccQ;0#)nvx#EQ?&_(yB=>P=J&ynW!J27Z+a6U1&(E$6|{i%5eD0MAWbZVu{utDeO=`%Nc38CQ1zUJ4CR-i3+ z^8RDk ziKAlqmdHIveN6xg3;easU-5wPyYX`2k!#ja4h>Z)dSGUfbJSgr1&zG)olYSHxFn6V zxpT~#F`}YalU-OXjB2%azZnoMfqvnZDh#{Euk?iz@LPEI&Hy*zg6_O(Aj`|CE;&rM z$xW8W{^^jLxM`v!N`UHSTzw@0=E0jE$!RqC1fBhO5Sh~Nu&I<-J$GsU{MXW8jp|RW zMw(9%W@1Iyjq=_kv*_Md`=;qlvbyXYi*iP%mEByNuwkR}T)xG33A+zRV7~$LDeVP_ zk3?QaVS(u-d)VN4cMLQU5X~rFK0v^u7X%|c4Ptd!z2T(afRv{js4c4pmbA-)wu|yC zz7`#q@Ps_K37VUh-)T7RP9kT#d$2)=pqgr@q22tAV9I1*+&eo|!gH=n3q?6Q1oW$} zDlRS6viKRwLnFY*)CV{|ZJ6FMnI@P|(@+~xWxLD~%u^j#BqGz4b~Tp(w>RbZ&}RMv zEoiROI35V%K25uGLJsKWA~nQ4B=6nrh1n&c!>g$%I;Ymmk>WyVdUOd&3T<9eei=vs zKN1}#iaA#Jt`uj?ofo+&*K-|D78tAN-?`_!5|$RTmFw0T?YB-HLUDQKt*#A)W}l)3t>Z zEMUKGsoxz3Oo_wNonuK+Vu@Nv*BucFf`%SNh6kc^?Oh}vZie3rnnBQDus-?)h>Uo; z{Z+@5P~E($^Wj(kw|wptFCb|DMFA5SCB8UhJgc09ywN7=Af{0*fuITphTtI7bsRx1 zgTeQpK}M7Jwx0)N9Ph<12omSZJpB^6GvebkYSH+Nam5e+t6ugqa&z-oQZl zaf`XQe~FpY6@Y~`4-O*rnL~4?BS)C3sX+}>4zT6K`iBLb*th6QwDjE*cbySag1!N+ z-Z9f-$8i0?$nM9XH8achVsXsNJ==!d60okH8PwlA?T>nd8}Q%;?vWC&JEnm!scVF& zAWNP9ic^EOotx`afdKb@e;;h!b%76Ba5XgKDa3%#@`w($-g|3_A=V6xoT(|G*#gRH zESDj~lUJe8l7&6(nOq|fg<3ia_#CI3?amH%0-@9FtM;-&puJEH0(YOyT+VqLus0B> zvms}xF@-#M0#BLmSrB1|g9hVNg*35g@X42-+@*t~4BlUO(L`Lym3i72yVLLEVs78T zy^Uv;Q1D6kf->~CH)gaP;1T*b8_suHfcc&|neFEmcMx^+US#7r;lfTX`rxmI&=y12V*w)vBd?x7ESB5!Bc5_oLrih0GqJ_d z4ExueHXmzQ-`**CKY8k2A*zH97c|IZxz|nP6HqyaoFo)VC|MtWFwsOrnKZLkX3xff zT)wx*=6w8l)e@`KTpRp(@;)Z+Ktw04Tu6C2OLzwo9OP%pN{kKt;LXm_=Wsy0J+CH> za6rA)@^m(3#OO4rOVZ(%ix2ZenNVq?iEgwg@Lq#t@7+!-0{C7*^-m-XcTS`cmfB?o zns`ytJUfh}YF$I1L|qQ%m?rP}IG>wIL86~za|cb35hzP=9{>1aW!N<6v?0L0;cJJh zN9V#fXZLbv?Fii08P?~P*439)H}PV+MB@|iWdznASTK9GESm2Dc`)DDD~$vQg5JG5 z;3Er0*L&(ij3p7@CMBHe3ZrA}Yk(sBlD`4jV9SjeBk_TkHc^7($62H{L-J5P zFl`@9F#qMY#0ZO%6r6QYn1bg9BNvg3PsnZQi?*^@$>YsbyRW}}{R4?g88-L-NX${j zMP=TjTgxJMRb7=3EBV8`ujVZ55Wsoq4MQA+uE_s>b@>@=BC#{A7I>+DT|25A-yF1P zK=A3d{v~NxYSK`Tj6_;njeUeqUC|?xSiPWQlcozsFlQ+rd#~9Zo6V45WRuCmOKGzw zPKaOi#gAclOx{{MpauLa9Ti?BfXxZQh%pMxzL%>-CILY$VhngBwO7m&cOi;=Oz?RV|l1PqAM*Z3hO6zV6p^c7NL~(=2_z!cy{J_lo3ePdL@GhT4h;^Gf8*o#r5mh#l`r7 zowh(xgi4|`sS;7nvy?Mo0OI91G>F6ffX$~%AMR4=RbPyd^)U&p-+H&b#=fB@Yx4Z3 zV_wh3VzR62-7C-j1T3}nEZxk#h-DxYKY81w%*8vN$f~J*I}Znc8ZiZ*X;Vu{d?Shu zibM;>G?eIJcu%d^IT`i;h32HX095>fel1)o5FU-%it34paF)d`%_rkLIATL35J1gt zEW3vaUCoV9L zZAUmf-7)uKK*)tL1~29~G)>a8_{zd={}FQw_S_S=TDK=kRy8X}VT z;W+mJ?`w!e=B35;ctpp_+pQ)XAokdP&&DTy)_MVAQ)DyC#t@ypQj13?8#aRfMG*+xRX*1oaYXrJ5APh+B9vQclxTa86h&U+@TtRw-}s)v%3n~kx) z$H+~$_RqYs0|(KPn)cUFRvyN=kYirr`rAGm7BV2%oWC13h_AtssIk2$1O>QicW|2& zDz&d)t5p%G<7U;^CHU8unc#5h{-Q)Z9!&N->MCSO@xAh`q=235UO_N~9pA(moRl29 z_HayqB5Q1FXR|S}Z~2;|M1yKw;9lgW%pa53G!3~)_c?rm-MgTS+w$6xq=85H<5r!=gjn-)$!Bq!1FoWFJa2Df`5Xk(bg z6Md8s(CP12^Pv3q5fHzkyc)#2dN;4VrvdvFwb{%ge+^jPGKOH}8*dZ(rxL2jz=zFX z7-QF)c%IOg<}VOkAWWqLU$1X^y--Uk7jidU+Y>zz`o7e!k%5=NApVkvAa1`;fSiR5 za0X%sFnu=2Wk~(gU8pF<qks@? z7>Sr~izrY*{!TdAhZclUmVllp=PCdp)u}lFoB(hQui8s-VC0^I}u+auB>7Q@&Lnm zvR`u`RSUHY>Z_cGKgw6&4b!}MO#XNr0}sD-5_8{Kd9?&LekE7EH4^Yq2}L0vz{Fc2 z(!hprwI0+rl&Ef@lP?IE3EPpYsk})9nuep1qc}L_zZN)}OC{O)-a={Q;vz*EJT=1NBg}7TR8^~H_!;EUQKqb#o?;g+9qU+v{@cXp!Pl8B4zWE{P0{pk% zE5;M>ahDoKU?&NX2l@qogA)?=;p*pW594Hy?>vO(IBndIKrer@bQ&_Pds4U<(?QSX zU_R&2Q+ZUEN-M0{XJnZ~%S=?+DOWl-qpM^|hj~uVl12)@yYE5>A^o}nh4Gj9T(eLB z#VkO=O8cWf1Inn3ugX4hv)%nYLdfmi3C9l)Jz zE;s;Lh!jm6DPr=`NB9>tR9+I2}0sBJ{TLcz4-Lr;4ap1*v4)0-p&^B#p@oA6 z`Em0C20cvPUnrmL=T$uJ%)Xlw_2z6aEPbx#)FeAb%PtKQL4PmZTz@fU2~a(I)-40^ z#g)fcaQ=uDfeT{2eewh|e{6#5fc?iK%HpK0;5q3&Xh)OfuR)s&UapL$?DKPkO80UX zac?T&^653%x``$0_l?N?#wnK2IOZ=t6l>Az;IH z9~wA5SC4zEvN_;$mq1&|&z)p=R%ySQ0`z7KH9Ay$TP_h?!H-SnTe}`6BGRKm8-GTU z!zRSYBXZQEDRpwd=n54=hRRriHRy+5s_}O~^%HvSJ}M5b-e22!KtrhQ0R~4e)5h5` zfm<^1X9EZKM|hFEavpR1k`^$ayF?B-pRcjhFA<^bddo{^ zOnCdGvk4~}sACGiFqJO_EQSy(Az%?WQ)Vg}!G$lLbT;VB+vHsFyi0cQ>4K^3>pKG# z-1V}y?Y5KC{h{s2{v9ELpAr^AS5V=cr)oVAF=zIXn*wjO3O+E$!DveaET`%OZxC(*Yf?|M27aP(mTU}~Yvkt3 zfCKUVg8;clqp8&Sq0fP!aHP6V5_yQRh00iRy;F_~mU!7keuGl?cfB0Zaf z`X#S&ynNAESd1W~KxZ;Ya^5&1gVenJq`E12`}b_+dr>W;a(15GfC-;igQf2jl<$=o ztvSP%8_xe?dhKs>5mJa|ymA%~;^!3Klt$ zCv!3Vrw=_b%y2M}iz$GuuO_OC=B`W~jXac;ih9j}mPa>64x<;)dc+j$WaT+4l?>BvvUZbIz$SAct z*5SD`{7Vqoi>pU>#vs4c+%J|xku_IVLk`JI+UktXS&f@6wzQA6Wf=iA!GaBJO4$l& z!xL0*b(HWJcQtTYj6^mo7$MY2+wX_O#Bz22c}n1@>NA4JB#LxJN3F-$Hv1!>^UmJA z`iH5NL{gaZ+YgO9!p1FqHB4aJgIW1Y*%I`P87xA+z}KBi?|xo476sogwf&fM-Xuw3 zZ+i6_cWf-|oTG;mk*+#yf^(=@ohAnqK>r988ehGCH3!W$U3Z9sO6a}C8bcC;q}5%+ znXK$Jsg;SkkXq~js*Xf{k{V4AMAf~@38Zebfj1^Wl1#~+W)WBfYC^xPrfkl%J3~V z6dx&Gjk+2Xw=kV?l@X@EsSEGfY z!CAs@gn5H3f3bMkDp#}s)4$%_0yQ6=PGP}jZHReI!YL>0IdH=SGmOUDSa9j%3yZuWSmJTqy+0Srdag#= zF>(~QL7@~8+o?JQvD{g>@Bjw$mR%#c6h?+#EY!TCwyKgMP_G+6b}5N#N4*Or3O1QQ zil{fmD1gjg*4P`pa;lf35?^3m_OZXP6TMG*tM>F*uZsDh$e_JnqdzSk+~R~9hv5z! z`!ILL2?-wvMz24hguTkbCXbnOQeR*Zz>3=}tq~B2y#28doAQ`@AI&(xcdXL#F(JX4 z`FI-GBIs(atHjPMo9D0?DVplvl>$b{p1Ylqm$?1XV8^F#KJ~>p7?nJ>(E*VtCiiHt zSPDKo!(Y@9&dy*IAM-+dU*l|7oU(!MFN{T9~FY{Hk;0wn^wIgZQ8w_FB1}&|=nK?K)&K?NTj74M;}+mHb|+|V6rbI6=20pk_vr`O+VobMesg@p zb^TlrZ~ghyNf~y|dXxvEqtkGy2I6xhIl1NonMi)8_P4l?bpG#}I1&aAnXT4H-INdL zUJG9>c4Px3T5$!L!=-s1`qm=-KW<~oM29t>u&KGB3>SiUkza(oHac^xZrvu>_d4X|WvImkx?v>{5xtEqlDsi+17&x;{!AkM<;fMlcX^cTL>O}6l5)KLpwVMX!J33+7JZFD z;_s;R|4t5J6PW&TtuzS6s6fs&WC=xCks92`JyL34s6D#@K3#>oa)IPBLm)&-d;S(Clpr8W8by7=NS|qpG_87)OA~NOrlJU^Ra1mJ4S*4P zkR~VJI<2NFVt7X1_INWT>F|8%NXa~(Z^m;Mj*H=i z2sc`zOX%C!w0_B8j`CpeN<}?>;h57lzpa^VaDTR_C~KwS#i0MPHKTskaT4CKFzW)n zdi|?$?_dY?u2zn*Iiq=CI*7Y^ky&EB$x}LG2+`;mE5co@>G*?9pWRHzY1L$~>SG*0 zZK5fiE7i)$la(8h+wcR@ubv#oQi1bV_dJ$^h`XoIrFnga1-_W}xgHPj+zpp5({Q;I z`i~2cZ;#b>B7VMHKePvVqV4|TiAqLrJk)f`mnscl_ufVqK|R; zlqLutN#2jyQi*43(>@-b&MLQ_i(Nr^A*2KEaxo(+7jdc6Jtu$hOm53)1Nmj7g9&0J zaLJF;gMB9B0ts=#{@1!kM$In6;lXd8*}~Cy1@o6TJ4iF^8Qu2h%IhTWAU8T1LlC?{;R~D3%~_F1HyIt5O)?*nKsam-a8daERbxnBj@k2Gz2!zgFYwfa_%G0Q zKnkXpZa^?Qid>~-_)fImU}*s5Vx1Wj3{isGzTSc>?jaQA6|oNPE35=2n6KB6hm3uyESiYi_9M?B5m?An-lyWoY>IGu|=+VZfOLbqLA#ZJ``_@a@YOz(Zb+ysPjC!Bj#x*aKNNgpU) z&=qbtO#~pF_p)o7%m*9EKs7%HAEzoxNH3bJ_~+q9cf6yCp5~;`1RnZ4&0k;Rw=_(f z=|4 zz-ylLT8Rd`^My$!<>)W%NAd%ikb&Bh$@rB?Am}_S!$XoukU+D?&caDedN_HYG&IB+ z`Z_d#J0zYC4Ut1wi2*rGI{J1t?ebf7s6F?v+nclQB|&vd?mQNbe>ZC1-lB`g)AX_q z{41Q02GMX83n3ak?#2{tNzVKD`Sh84fkKnzQtxvoZ>*uZ}lX8g}@x&en|tD zcX2JFJ~kbFx0QaVZ>iQ>Du}M8#XFR6fp7XMb6NauZ0X`$lwpIDcexyBoBI+KLGFjt zkd0_sjBeeR&ME;x%liNpp9_BdpCwuN&Wj6i_0ghM6XzbLFdzHl>mW;r($OsALYW8= zdD!!`^i(F1|G5}RMOnIgAdd+990fj@8+_X8IJ=X0M38x!fyE**KV%H=NivYJd3r~ zI^o;E>>qZ=kfBe+@v#;6+D!%8P6$ghXJeYNW<$G#nB6CTF5m;3y0pPiqCJmn#((m` z30(7P1qQ%&it7+CD(^|CF_+;LbQdl0K@Mv9tcAAu45*$4DSn9K8sr#tHB}Iv!QN2p z3CRwEC&%B7y8mda%}P6Qtd+9d)_|F#($;6K@k(8G=E=3qWW;}2XmH=w!J;v@;I7S) zq!`ZM@r5rn`+;ugUrO{g$XrRhvzo1M>!yg>n-862V}bU9=jV|6cXJsr?kLlj&Rsw? zisOi&-=H7TH}b7y+#yE$XU}wHxP10KLH`_x1v&t~yl6yutl@$1?I7Z$9RHz43?>~l zxL?w7XU^K|CWu7S?TZAM(mgn|2`3X97_81>a~vydE1eipxA(~UhXhqBLbQ&;ZT%1` zx>`<;<1#+yx-*cYEFaSQkaRx!NT~b`1&BUH{A)QGRyQ2gTNCL?@F`p+6ET7a(rg1$dqhpIuBMVl_Y zBPnPIH&;c#{HAr;Bdj+*PTh{+mvpV!ic1iVlgzV@Q<%N^8Viy&&BS6t_8TWlR_dF)a?QRoHE&1Me6pep zJ@UkC*gbGfR)$%yyw4NTM^DoRr1W<)M7Rs4*)WkjqlYa7k>r%@!yFny0JfELn~1`LT?+Qylv z3c%@RTkWaB;@wC~+pQr#zy0*sArbes#{;h{HEVKlQTD_6jT;{woYpESPXFxe@H|@< zWfXEg*WuWBxLo%?VIjEzliL#|tmQm-9Dx&B-ads7302Y?Mc8@Z>xRAf*m-RZkxasy zde0nuD&74P*$>z&r{0vomTwB490Eqf8{5#cmoeg%Hx+w%v1zvY9S&h$B#c37MET${N748}) z5Yx+zX9C}o91uxU`#m6wRu#*ZEA~2%ZcEdY_3Qrp0}@&4((v|?__a70Ko)txS4&E7qDw+S# zaEW3ICiR#9rU3+d;_}12cHpSBpkg+H*6;vVis8oh8r)*O~p|;Z7)2Qe>f6+ z9*c98`p5s6U&@k%5KIN9RGGoXfgAeX)<|R@nA%UKc=5XQ-4e@%mMc*4llP{4@^_2? zxVdU6zKdwyj&m@&S}?y3gAVzpxj1E(_4PM%3LhA9H!dwS%$$8N`E0tpMJ1~Fykb6#C z$B#cMZ$@IK?7(i^N2iZ_%X&T%Oa~!$&=eJiD=-r=m*}22v1AU}%lbn5V-9`>e!i)E zE&6PV+P+#zl971+F!2JeHskN|@UF5S4KdP?e-U^0yz9Hjg4yASpAXba0>s%basF-T z+t9UZ`&fi@so9L$R`hbzIIG!fPrJ%4FS)N+b1yifmi2AI4@-5-2^G%dK`%vr4<8oT9Q3BxpZy8OT4HZ3q^G#=$@924{rNc zso=%*u)nSmLq4B=hZF%-HAWE#A$8}g5nJE|21is;Ycp5&>@*Wm6s47)P9~<4#Vqg< zDmm7LFZAVbXqP+dcJsJADt9NMl9`7ma1whHsDPSeq#W;9b!;D+9Z~X0`EzQDNl}pF zr>ie7zctP~ocU&bcIdx!{qWt}4Y4DkfPFu%2fupk)d9m*41!C!o*Kzy*KGs0$ zDrM-wZKSy|M+j~s$~UX~{3g3ckoAMSBuOE^UCRs7$J8OS`p?#p(DJTmx<2$x!F7lQ zmbfbvb02%NrGbv$aL^dpb4oU*romG4fi>=u)!%WzA^5i*OpLFh2ltJEBul5yyBIm+A40(auoQxB{(74MAH9WJ#8H>vMMe<18 z@_MXHinQip&M-3q6ClVz<>wX#<#|fT#dxSZZI$~rk3{JeD@=-?usjkY5OH2k_^VjA z(id9T>D2cNr$3OyqYxN+tUnyDxPu`$>mx^m%|C|Q=>iGQyLTI|VFMkyQtdNFaj{gGV(?4@b4ZIB(YlidXV1xDdDXlrOW)L?+eUIp2%TCO8u= zhL@+4jD1wBAvckN(4874CAh{EJWUf_xi;S=Y*p~Vz3a{3XD;*~FDfxA*DahR`8Xr6 zTJ|@Bad?RSw!d@VN`u%Zd%~>;qm(Cdw4R%WbbeZJ`1?KG!Kkm}=ITFEL{$7*Y@R&@ z*WtB1cJpBR<98mwJZfZ#L#B|An$$phI)0oX!`xw;%PhM?YOzP#;&S#o0mIrFqtn|A zY6`jC!~an2!RHrZeTEIo&i^r98hU1(;SGvks;_8L)+TdS9i6G zB`TSEQxY;1e9TZn$oj{2IaBuLNp}078PHx*#Ni<+`WI;QbX)uw zWGI3j_oLet&j0k83%RX}*wDs?r>#iW=h~_=5HDUop#t?%?YI0EH1=DLA(qdljhez1 z!1)u`8R@CLH}AhwzbXlwn-^DtNRkSS`Y|<<#Eo`VpG1g=(sRMU0du`?mrc>1)*o$* ze8gXX-3Tl3HKU)lLgH>s(2n8(M||@79T%ikN!p4~MdSe6#gg!S{KZZrwrE{I3B&{A z%R;s}e_aWSXx9TM;_;y|N+dek@zO~~i6oe-5p3{@{Dc7=@oE-UvRk=K2?&8t>Rg&& z(-(yMEu80JuZ8#C8v5^kh0p+#I@)&jN=eYilcZZ2_5V=yCh$=G-~afzcNSx8V<+O07^0Ahs4gKDMTn3lDGH%fTIgCT zq*AH0X{e-9QBo-yg=mpVQo^)oQ_`MPerMjF_viol{(C&+nz`%iyk2K{p65CDivpid zk_~G*17v`qL-E669&j(8bqB&``1wF>eT@z~TlF4>ClZ3CU#)A} zpL{c~@{nn6&Y7%_dM0lyhb?;&6Y|Y*IOd1yI_4u?@PF(vzhf4VV|aHd0TFiN2)Mb^=;fqKvEy2CdpKix=1#p>z!hAm2>QAX=;s z(aj~UdyEFqlfNEB<;dLp8gmo_i>!lNjB%>(=xxJMif!MF7D0r>10m!l$(uITqG zWMt53Z*e($G|2+!;%%P)R*F=}MixgDu@KZ7aE8w#R6h@X9DFnQWv~~^M84q9Zza4| zi-Y|zJmtiov2{2RJsop!Aqp;GhCzica%@saL`;8`y-bXD2FQA)QVX;Fj@Rkn6Ty;Z z2ZzL9+yI{Wi~1W5!;dK91s6Lk58r$KqAK1KIKM4OahC1y5VG#9D#bIyybmd>nPI3ef%MP+|g3WI^Dbqo1bMoh6_^!FV zEI12?rL=4;55(lYy^AraN{J$d;i68})O#i3Q^G89DlBm@mHRscWgZ7| zSS*&2Tom+elryRVK3U*nrYl~2*Zzv$%bvRUaf*LsngO|Hm@HT^(+a3BUp?TZ*}Dmt zE?7ZGZmj#I1g#I$U}*q>#|2a?kW#l_V7_TCJFBADU7u4^HblsFYU#$5z>>f?yBV8>;Yg(x`^FFkzpDst#xLehVCj0((Hk={ks$6y6$R-QGKNX` z_(t4b95ZPm3iOS%F9|0`L-k$^`EaK1#6QH>g}6iD(y=lOwrT;wGNjmcRVoyMcX85T z9$qNyVwoE5vOUxU6H*!bd?^Hq!&Z2>E_wTpVnfVD2BG*xA)pp*grS?bi0@ zO3~)GvvAv6Zq@9H1R>6=lB?HD+348(Xg9_u$T;8g#0~5-(iX(`1R@@pvHeq$JU-y} zQ!#G13M`uz`QyQ=x|bgoqGdUoynwDr1&R9H3=xQ#OLtue4NXqM@WWZg=&)>wI$I|q zB$57G&@7O}Ta?AdAa3^Dt8BH^ahk?qc<`lY>!VJI-i#UC&Y5>t+4?$q z0o0y;C6eO4l!C1^1*B{=Jmc0d8)WfZtd2xRAMY9|`k?HIia>B{!Y4Jb4o3Fo(m+EH z?ge`K7@X24jG~HgG55yILX&%E3a$%rb%9V9W3%ypuVHx26%6wsA$TF<=Dw% z1qz(Lwt~;YDj}`OxaKtOj|UZCF25py4d-tc;P=gmNhZMe;PXs;%aLb>&L7C@r{@ni zKg2C@&aao!RWb>K+nYaJzG}%g+?1HbHhQk*d*ddb3QNNk0y%GgGbVOJY_9`QOXLaRzlfv0= zucWfWH_BIA6-@bKK5NIQEXOm_^n|Z+=+{YA(0pP68THPA4Od=NPlPtMLAxsN7&1dn z{Q`QmE%fCO)?Nw_Tg-u&J`s4jk%^^nHW}=#iE(NJpQ}k@xzw?ZD8s&U>+d24((L7k!(?Vd}W-wtqT3 zgCQaEiVYk8Qc|9Q$4ev!Ug9sct~z-Xi`#5l9R5Se^}qq@*k!y$5V#mpXn-0o-9`!@ z*lQP|Jui0Hv*lW70^1xFhik;~)Oj=iYN8`SS}a@ZX|k6CBy&;zbw3!wCo{;vCv#*@#ref* zc(3&EVR)izYt9y-N?|tKIqXMhaB{!7X#&T2YK)q*vxdK{0>5uk|&L~pfu-`BN|6deguqe(dQt8p_)2R-)$*UpBc*o+Y90KQWqWFOYGLaU}S zVg!0IjaGA1q(t+^!+Pg&==P0z?S`&cL8uB2&HD}77rOY9vTUx1>K&!HqF9`hlaI-l3uL$tj6Hor8N?|!lh3m_ad^GzU&;8X=YT96 zV0srlGsV9eOwe_NwrFz(t#iq?RFW?YAG%f+&iAx%#6YHctwkz0R+$Y%bHIjC7>X?e znXLH{qq9UHNEk=*CuZfUQInMaaptc)a?PU$Npw#AUA;#i8)4vrqR%@FrIa zs|}B7{-+Y)Umf22u4*RKssLRz0;Kr{r7_Z0R(Z;}cw0h5Iz^NA1fz1GQXN`y<)uX` zk;9)x!_Lw!9m%x~DGKTrjUY4WD!mM@6N>%SUfRg23+z;nBI$vo!$3_eL-zwiZ9{aS zd(mi|T!ys^f=TD9W%}E*yGO$bn2R6h9>GUVEtBsAnTCnfUSVaYtdv zm?6M;#@-u8`+(v-8+kv1E_;hEAm>QIsd0Fye>~v%J^yCmG!a)JRTOxu5x0j+l^8yP zn@3{k=$sVDuw0A6ioDbmfo-?xoy){H;d7d{V6{E$DriVcxW5DskIek~;{56hGLYKH zu2pp7)=xX!6sJW$Yqjd;;0_}BYxd#n~LWXe>pMSxk7ymMqL<%zqeWSE0nEeO9oHb!@<&ji-vjn_@g9DyY$aCNC-Z zsT8*JjQbCbIknw;0_r!6`eTK)(Ut;FK&p%jCeKvm>R{A(#=Zo=kNH~Ua zet=P79@hO86`K<@Gw$Y2uqHZ%2Z0%TQJ!{`E+TgJkwp-KIIP|;MnY%}R2WK152eI$ zTf=dYGgsDzs)Oq0wH)ZL<^cM|rOWg1_AYJG!BwcXBETH0@lTC*M8$e|B}$GuIw29I zP6fE(Ea{)3h%T$rU`!wSW15RYk>zP?kJLkeJ%K72avZ<+n?7W&L4r-at0G8P_t$rg z#_t{y`!O=0&yE-4A8IW)X3`G#!zcviX4xkx0fLSGxRB`UXZ?|6;J2ZHWWYFM`Z3^% z`8ocEM|RFe^CCTUJGuJ7#wj?GVDf_Od{`tyY8r-c+S-Psp<^U<3((f!x)30;vVbSQ z-^Js@95_~TP6G=&-9H$jie)!;Vpa`W%Iacd3%d1cF8;4^^6o-Br8MjV=I0SvJT{re zoPd?e_+V8UXonIu?L&yxy~juQjy`Y-Yd%5itd=*z7``KMAPQ%nZJtF)nE4;%hmB2T zGTuxFf|g*N0gV*7H1^jNvRdAK+9bDAzm#qJfBmf9ZuDikMa<%oUzRQpPVwI+-}pb= z&SUxe*s$krB@ce;|3V11aSurKK&184+Di}KtXPDZn0y}On8|W-o`zQhE7Tad80g+Y z$OMkW4g}{mDolDW=Yfv7th$@<7#JTSQ|d7t84B`n6lq);XD|oz=7Bp_KI{&DkHbG{ z;2OGT-m&wFaRmJ^$SFPy20}siPn^eGaeoXB&`#ed!kcF6^`rWe=oDq+N2!yF3~@kS zRNqR(VsEvoa{TN_u@!28;wvGtg;RfBMwEE-O$}FG!;20h{C?4IGX$-mF40|qA>rn+ zRJ^)Hp)T&`dV4!#OhS-ey|u%h89}JlrRk3CJv0AW4#xwa&@d>>!CB@iCcKS3lwT8E zjmr?z=+{)X*#U)n^d%&9s`BwftphT|P?Bh@K=9eL&SgaPh`_BKFKGh8b!3ZovHrX+ zbW84%#YXBS zeGhWZe_oySvd8I%VwCE7-1|Sxf@CU8y3@=In4Ku?vV*XC31(V;=|P^8CUl0!fW$Ph zjqUBol8%)l6qc@GLa(Jvm1W%~&=NvhpF;8uDW{`PmmaJs?H-sI0+?xWUl(l-#hXWR znA@v9=|Hmvb|1nFf(-Rec&U!`_ozJml?X~gg}co7OY}1`cc1hmEq4@!fv{Rl)luV{p*!ITUGQHkX?I!IzKrK?T)8~;)MYg_nLeWbO6b=PiO0%<>HLZX^9lt$dnI^ zVm^${w~$G-H6Ysp{k-r)N|({oq_R6?-wX0N@5FW#^=cw2don9;!6xK~&~6DvIyF$S zbOK58V1fHt3*bsl%oxfAYo^IMgO`-_nm({0sldKnm1#aCt*wyz1cl!EZ>^=pX!nKVNVg`#t1V z=RT}*uC~~^R)vUOFWfRFp$@^$9}I@<-&(xDI)0fNe3k=gq8eOz zxJ?<_brT~APk%MI;33{6-BQ9~y!zZjM}b($K#|GA=9;F@+)1^iCe$-#1m3svX=&I<8GL8C)`bnm?OGuCbj}cySjZvAN((P6>xr}g>0JutEOeByxWn_bl~IUP z>#kGU=yD}~LlMN?_ML1@x!^(T<_|@^QqZokk?!EJ}z&%>Nr{OnY z$82yGc>KJD+xDX}aeK#=`7`m{R#j(6kUE>2Eyh3Az5g;s1(?0R;a5X1J1RgV)D)dF2a?|Y$p^IoGj{2u9Q>`bJmfW2*kHgF%$N#SGLtOeJsSS~7-(LpSpW?T zi}jDB83l!LrcBD8)EczJB{ zMPlgJD>z{UztQF9iI>!lz_Scltdz6-KVVE@@ONTC@|BbLEtvdDTphY?IUdQI`1Quy zBB#}|RUPOt_zx)y;Na0mZ#_^;1wwgB)yH+a@VXgET2A;daPriox%ew5B3X8qM&~B* zfuh=6alFNuTTUWT-SG~F6T2ckpFDeYFK zgjp2XnFrW)9vMzZ#=gwf!AU=1+aoF0={ z#WfS2YBekOnf^Ksr4l(eFA}~@ho#(R*0@O`a-@kq+I^OsVo1(KQ9M&uJx}x)*;wOD z30YN7|II?k$#*pY<4Fi$(HuBH;b3?rMDs!VHa<@XIfQ%?E)DUv7Rni;SKcHf{{sUgk(OS`6vXW%@X>dA%EPD2o>t;{q>!NaMCk2_kz$aoFT0K900# zHSWZ-EyXJdnx8JkUySD%RQB{6f3yj-jsVN(kjuy8fwA~Gqsaj_tnl&1JOd@71r~Ii18Wd==6$Dv;em(Sk@KGgEjE} zDr8v>1YnjwM7Ob9%Bqo)=_ zlF+jN4<^6&O!d|TO~HZ$E)cZFS?d6t#5xz831>~V$Afc5Vo2p5CTKR)g^a+*<^38W zWPFsR@_px7{xe?LcEpxlP*dgLitUG&w~Mh%5Kv4x^;$r^P<# zZoPRQGRz+Z+w)WvPm~g6hGL=S*W*A_%epq=MRQM$#_u5_9dj#%55Ej>fR;=zXCXo| zKYAXjIm+YwbW|uB@kfu9Y7dn~@$}^ImmP7-qwsvNjGj?B_l_xUiCKTll3ZMF;wC@3 zUN@$o1lHB6aAMA#HyI6vUv%gx zhQoGkJ~|ogkE^cjJXV%GS-eb)ox_N)7)LVM0fdf#k#%p$U8&~#cj@s4?H6mg3+CI_ zq~BjDLt?Ym*;KC9wL~tJ_+M{N1;i_Mt^~=k+v-3mkK;hB??*ML;5eB`cA|zKOSjA> z`s$VBK~K6Rbd?D-rK;qjYgnQpxnojFXscl-w}A~C6v)Ns@hwY{V#UVm>l}s+{k=1{ zuxZy@V}QC^t_P)v23=L72{?T@2cO+s&k}e7R+A;V?D27?IJQhs(FYa|&4ODv8s4LN zrm_%)9`i`n7(oZgIlADi`+Xf7<=5E}BPAr-b5{Kxi|SImdn=j+mr+<21EVt6uf(M( zh(4pQOp1Zp2DGxhzjthl+;9eJ7 z6I?)gS~nS7HQ)AgZo=|EtLC$3mL-aFZwZMBD#wpTzB_8zP1LpnrZo-Ny6zk zhZoMBYp9PGowwRQ2A}5k=?V-kyXFU0RM#AAjJ+6BZ-pPjgu$q%hw+@d8QlZi*M2!jK=ts~?0pyAqnuD$q7U zGhPY;Wm|#3!`0p&uO9R8P~rAits4`T7WkOv`PMt1`gvh^D2uauB7AyZMU%&5Lh|AgLr( zPhRn&AZ{2E?2;LSkvbUJ!S!k{nQF(z?m-^ds^<}#$(VUrh;s~n3}H6_%5fo%#<|{m z*kF%#2^@{XsdIs&Kmya@zZsQX!hteeXkxbt)Z0ZdhKR~%%zqjRG(4Ou1Iip$OtHkT z8@ncO2*^oD5aN(Ew|TK}^@txnaYe^R@N&dA9MioH(@{RmBW)XW+P#0ju>!Pye{5ud zLDtJp7!MEOG~`w zHRF0f?XqO30{9bjql=GB#y8u%2|~cuGjYQcy1VOvkT1!3{0l2K=(6!#4R6pGG2n%h zMrTJ7BZSmxTD>;N-FM5e^eg39>$v6fjWO;!jc6W7EyRh8mUq7V?8dhmWmh(LxSmp5 z<(VU!2G@iCIxbQ@!|&KO8l!odkSTa1(#odnL?0`HK7e_si8C+>a%ZAE7#49Mt`L)u ztej)W+jv>40nv>12{%HbH$xfTr0wTKU#m0G7sN><8_#TsOWTgWs~e#o=db~# z?anORL%gHi4qFj<=VYUKB}HvV zkOT&y$5=Qu>i&vxe$lq!BYkkzjQLnP6Z|-;{&PQC@lG&8dgGtMVq_SNAIkw2^eAEe zFOX>FHn7l*`&qF+@>_>EINqVxKGoB@@rT!vp3cdI!5Ygx^3@Db+QfgPoeXg50T&X> z)iOqyA?4zzCrA_InHyh;8Le_mF!_t4U{dTSlw?XK9IufU!VAada^BT~G)rgPA;JD{z&Oskl(*EutF7(9dsLks3 zIZtB{=!tAPii2J0@s+^1d{Wl*R26-PBK0*-8|kO zsS@6g`EeuBisNiC&u@x5x68A+5vlrYqw2N2=O|q0N5k6(PlcU{;-i*;lE3kf(uO&{ zLxo`=GI_C8!w!WQGA<33N^$s_c_0THcl9p8X<|KFt4hK`hSLUY^2Y?;A!W3&S^LHe z14hcr^fCgV0>kggCQQlRR5Alj=zj3RE}N35X$R41^S*4;V*$Jq#g;dU(QY+Yu-R-k z=NAXCh?{TmTEmsLhODMjSv(y@acC>0qnFkvvpzJA4CZdEB~U%p{pHV!HT|Yp#p~h& zU~CP0Tz-APpT*>-B|-y!*ng95=!CE!?|T8(KG`cl`nYM(H@ig++PMbp-0+2b&@!Ka zCA963EZyR7PKdeV6g=W~qDSXT47uoCvxO+9wru#5D#_pITpc?w(F9D8P`p0oAOU3; z$3V`fpWO(z5KQtE72q_-l;Qv!`Q9tWp>x`g8ajOA!D4e7`XYaq$lRgl57ysf0af+< zwt*9f3s(A?AluOV(E(F2)=M}}ltJN^9~a<57O(F(j<(?Dtvy@V1RVMK;%*VjE{Et! zzy;-u1bFNQC3}-8LI1){{3io3WU@<{v1OLpcjQ>y9eg@?f3SM++F;pW!{D6t8;vmq z+t5|`oB(RtOaywEN1d?^Ktqd8r>iDIGJ175yc(JMUzvs>mF~7Q7AP2u8nw%F6=o42 za9gIMN|nzmB;!FsbMU!ka`HvE8MklS_ZB1zBSdiIoU~HlO?bn)mJdAv2>M=mb({ca zP=2y66_JL$O&VDJ*c^US5${!LpiBm4IHXOi4K_^tz^*?YqzGxw`b`E6Rz_{i)>U+OdThW3E_tWitmGIi<7K#LYMA`r&kF zt!)IKz=_c}b^W5{EPBDE?;uWoIhTaXm>T+S z;LuJ>J}<-(&RBr9Lm7 zU(#=9Okucr>xaG+n8lSREw~I#@7{|UB&ZL+Hv^|cc;HMt^2Cm*ld<Xzs`O0p8S+7gBNR@r-%~=kpS|{gHBJ3|3CYEPo3#Z&}SKoE&4FL;3 z>Tc91x;?69)a5nBUC=-N?%Zj_D}=)SC0BlV|BC~{B6C8rfE9OrE%dGIFXzAmR_F6I z@(?7M8~}6Fk$Fe1XS7?U0VZbMISlJRA$-!@2-^i27F(y2G^+m3>U#(#YCj3jN8m%d zF29KyZ}O^Pm_-?*XMv)Qn;aUBS5?PyxeV&zmk|yO3Hy45Y~V3lKgzKM0?wHq*A3kP zn7&OERBNxc$L~_5;EDtLHoMJsgbW-~>Ys8~Xt4-k|H_2Uk=Lt4>=0Nc(t% z9dm!|98?wpPwrXJV?quaT2J0)qU;|r-%M~%3KDKF0=F-1|dCN6Zp5g?M#BMIne z;4(b8at8bH(w>oNsVCHNzT|G=NYr@x94ejA+uiIJkaLohjKte*QxF3u^fBjVg0AX2 z$H!BvhjjV=bbX)jmh<%YZmYT5(3O5SvnG~0O3G9^8`L$yqtsAPYFHZgY=f1y!MXh1GfaRYB)@OYJAaqWgoFaK z?#2s5$@GTd`a~c_}szI?lV*3)$0W-}!YwhevFh?t z;S4z1ylSO)8l0x(ZPqnz)F)_vL3C-avpk4a2cN@Ok{P{%GeJNxhuTj}O=0PN+yMNS zQEqI45d@x_4QP@1meGo^gi}UX^ByW_2|y}!S7QjN{1#{ICqEx-pn47Q!wmJ@ZqXbT z6qsv0_{1Wumxa%_{qfQ-xvrEGpy32c8W7Qf1LVm<4diQf`&vu`kI~ZG*5%hcF zL=DORqOwDq*4X0f4Hs}=ozpoM)Yp!ZhaV4>+O*=DCrGXz1Wb)+kGXgUyJ^4jl(0<( zs_(AYpVsCxG{jWKb*o!i8%olTH_I^@W~&z<1LrcUh9z8VZH4P5VNx6_)}I!W90{mh zd&y{}Cl2GT`Ad^cSQL|Pk+gI&rvXlMC-=ts`iSBA@YM>sS15i=hg;Zp9!b88b(FKp z5@JjDI=|G7itmVfZ@!&5BlU4A^!~kkxkFt9ET%V2w?bvV_{P=5=8h*g9hrvD)2~lZ zJ%XYZf9BuqT!y78DBO+jXYKl=r+iZ|1n7GKVG5drrra#$8CFX5cFj6bL+dml=%qN? z(kf$?_scS~53?uRF?^lQIjO0}yc3tK#`%M|aL8Uq53X0i5n$^su;Gz*?1Ioob2c2Y zp?|OL(FCz`x|!`QLgJEdx|Q%@T)L9egi>pz-E(6jK`?IKVI4FNRe{!&i0<(tU>I8= z@J*H*e7ct>SkEHL7;kfH?i?uFE~??WMAEmY_YZY0u?42PONpH~@0JoI4o=LR6PD^-;LdDSbhoXO|1q^ zfx7-9mDCXVfwb+A6C&T=?SCPy%mr=dTumhH`t6O&5@JeX+|BQw_zo;`!}`}L0k#l+ zv$?m^kHcYXC%)Hpb|!-~3T6k$dK#aoef55hJSF7Ywd^G=D2zO>mbFo^*CqrnE&LSJ zX{ntj^E_);w)WS~RsM>bftue}9kd9w!nh~CHTB&2Co;$!W?%08bo>u0ACh_b?c{_B zJjO+`ch!Qbr7CKPBc8X8Z1s1BC(So2wsIF_=PhxY0&PNSv*E#0g=D$)f^N$!jzWUY zthucjIwEUo{Ol~9NUGrO5;KOwYa$gvF>Iprf4&&Kt9!kAW{`S-eMbNPeZhqOTnv%cZClgDVg_vb z+Qy0>-6o{sGn|i8V0~^SnGHK{RX@L8HSLipelCtSQ&)T`YiJWV=h5F^ zQ71=YrKo72E4slAqqv3NK8dkYTI00W-;xw`3JP4$Z5+?ln&TGy56+%wwgdy8<~0WY zWGAniqZ%D6@soV85vjl5V-`nAl5M0e*Pd3yG>x~m87-U0&4n>zKx8?n3e81wP;1Kt z?}p#+3Xvk%I**`tns9AFeEYG!MIy2_SA1A+%wT{uf2r+gbVz@#{jwG-hhRam9_dEV zXPc7l4dc;--?~U&bsv5q^Fw(`k;Sx}uR~_fM!mpZIM;pH1etLwwVpF@bKHsZEl3ZU z|5q}`aD?VmycMZY-K*q$go=c)yWw-!-3avj82<+WJbQHX2RGFl&>C`XMQ5UiKpbWN z`tSWBCUT1H$Gzv&#e($0tGU7c>V#X(<?Is$I?_+&tik1y z6qY^5A@b45rT7-i=J}O({3La|CC9&rfJH>sf70-Co@PC|at*swC*|#|dBTbm_w3if zHH5Ws*pDi;G@PeN=cfZ!Yp`8NL87Y1kn~~`@><;*pPs5k`ySCaiFgL z!Sgh7Vq;zHGVOh>tB$FSxj7-*Chs@-{OL|1zL_lJ>%c{i|I=%d1$}ROKpI~Gi93eF z({{pI;i-kBk*(4tCuy-HF3y$rOl|MteyvGZD1CWX3o;&G4OeHtR%5qkJ2E)&TC>E5C1Loq6ZoKkpEo)$R)PjhPi_)7?n4Yw!}`bXW~1qvV#51*)pSjCcbangv!?-Xqtow) zA4KVF@v+vuttx?W_nJ#y>$fz|V$-ww{p^mTU*Ji!b@y8 zJ#9XfQut}BlFn`B>?X!#0^^!6i_3OqhwM^QC9tKYMjN`N-n_iDfP4dyWlqYjWh3%d zn5Gy_{JLz84TzAcLc1DQUhwZG6#o_v|M};D@z=Gx8b(wa4y)}e?(Y9!>h!fMJZIbr zm1EZE!~Cx`bbfh00UsBgz(QKCfgZEYR1H*y5~fk=aA6c9A9oqlLMk|)Cv^QjVC6M> zw2jPJ2>FRG^Z9;vlp%ynoQR$NK;m!`lY0$ohoH4;NR0Z50U+hkTg1Wcz;Dt}XTr7j zf3w4Z?frcP!H`CKF0V9J=J2S7ecdJ;ny%@oNS?&u6eRiuJk4`42fBLD(iBMVtM1cr zz8rm9F>oxfc7Ymtz0QUMXKST1M_{-)`;ym*;2#|+2o|uU^S3!PNf124kmJas$N!=a z#xhNTqk`yzOo*Q6SN&8!J6JJTJ9q_ghl_)igC&DigJ&9Z;1mcd|E@g;V|j6%pO2d& zu|vP5zr%~rU|U>Ys--9vTiArfyY2BxCHMn|H`)6NNkK`}r-@qNL4OS|d#i}A{}{7} z4NUpNAMdr&6I>Sk2|sEi$zYxZzA>q1|o!8|w zsuPEQ_-62~a=&{)uL!f621&QOl5o+)X zTLoGB5X>eeiPAPMgeKZS+~qYAQcmrF17|6>Pwu@LJG;z1Q|{adcUu`|@NMQ3tn&Eg zo39AJ=6&G6o5%i2Y@mDZEyfY89@Hl&X39-re?HQ?9v&(%;xUD#8FRbD-h$zDOWZ0# z3^@xzkte%t+vzVWNvr^F-Z?qbnM=Yhl_C`3{BH(+64p;{XKW@COmI*kK)gP8*n`ci- z6Mm!|EKM>k{)|P-tFq$b2gJ)hgwl3tqc)nmmWz-jm4&()!IT zFH%Ak23;#qqR21dXEf#0h>fLu#dKrIkhMP3Xn*c!ABxW!RyMXwkT&+%x?R!fagRuf zCU;G)doDH-sB5MY>;N`<15@KsLOuEDlX$e@!3T`c1ff<_!q?kS2^-fctz}E(lOwwy zcGvj+aFwz4@?EHXT4g3dXMA!?ced7A*3FG?!$qn5?@Jn1w}x9m*tNU5FF>x?TYm0PMUC`gB7*}n^NRVrzcPqr1u*ujLgam<%MbFC!Cn`TYM#vD#&U|bdCzPAU zaQ1KKLq?;aw}xIIlLU4(rThw9u`yG6=}e9cF+((Ou<*jRp|fG5&FGmi4+@FPhN`X?85zea!!v|L`zQRVQ9mzDlT^piR6uxNl`T)w-mNsHkysVZK9 zG7+>aw+-&7=S%Qdt=-P({rr9!!?R{AsX@Q|3l%D-q@@sR_Ov@xLsHHfJ_nK~QM-<#`!&;M!3O}!xI$^aJXPFy0Q>9_SySIDL(f2BhYSbAJ|mhXpch8&#!CF2QJvPDR7kM^trar2)M@8Dy6wZ zVB&T*Vai=lvHwZnA7xHxAg49qLcOJ2!U$yz`<@qC%X$`{x4e5q_3^&Pt~dXO)*z^Q zsDT4N9xrA1iNR)D37WhxR_e#7oQ?&Nu{zUkQmSO%%+(&=_QN3}xM=R4K-mD%$sc2X zy_JVcfx|GARhRX)JTNZ0L6-?BSekIbf^&u2*L<@yW+n`00t}BCX)~$b@3xH+7h|V{ zCoezEKzL4k@#J|e8mQu0Hm2P^T?!tJ1h>w_Fa_9WE_oifnL;O|WBv(GpN7QLADsc{hEnRnT-z0mr?5wCP4jhuiy;4yU?vSYci( z3T(jiVw9CB!Bu^CezvK&Krdt2aZ=~SM%5l$rdiAs`x2Imb*QQtO4P6u`R2LJz?kJY z^HTK7tdVs{Z`Ds_Yw^_e5`LR}%~z@ZkQb;EU0R0uu`s)rw^NA)OdK(*e*f%i@Oh~G ze9yB9I({LLpTYUPX4KBs*fW|wcP7uzct0&U$))1&84Fj7U-r1n{^h`6V!Uw^zSJbI ziTQ&F`6)uN?^&%fl%5z0u|dA_fFzjx5;EeW%F-wDSER4x6x+V6SaU1z_KLbZ*}OLU zd&*4?K3-fc4;kCAd2KVIls^jvJOkm&XZKIfa7rY^w@n<`9P^gz7ptpEx(o9y+38as ztRS4&&aSVAG$d;Dg?@fZVghmc9Yub4ZgysF-&f*UZx9m@D#W%e%g_S1C>w)<%d8zRb z`kAd;wnJajCSQ=uBQ7*Cy^>QZ7gJOo8Uu3-dk6%x9HoVX_NnyEgq+FC_S@HwEpxe4 zYlrw4zMZUV_gpR*C%+mBdGnH0;b;FE(50pixgW`PV$6On+op|*?NuX@Lc%X612C?o)M!jdFPV+>dvn=b_`dT8j%|*rX<5rGehmw9lM_1U3&t( z6zJ9MX!e>GKAD(C#I+j^^pE_LWP~5nD})(;98X)Zh0{V*Jk_|7+Ml$3HO+WjOj9Q~ zjZKxi?{#R9G2iEo%K`EIlXuc?s%kjPQ_8uiTqhl&-EbY4JkLFX)tq_s+`dW)GX=5& zkCWM0m_FZYy7v&!+H{f-ymK+lF$gN%jaQTM!>n~ABLo=|Gm52ioo`L~2_|tEb)roq z2o-h2k~WM+SFw^T9o5RpPjvg`%Z}Ob3~@%9xh8&JoPSfFTruQ{*)18PPDZl8Ko_86A2skgH)9R=W#t z2n<%SHj~Ws9!HQwu3HGqsgL-otK<`U&wEe-4lJ|5Y{1M5dg!jO&%SSYjDg0>@8Lo< z@8OfVCkG<)fCZ=3?)zD+p?HrSz;j4D9o9SUZi&FwD1CZGkl5 z=F`&p;T%EpOf+!86i<9HbY)&jE=~(Zto~)W=5jX;zAXe6e7OF1&B)E@A+f0tYV6+Q zlL=hppqJCH!Jd*3D`a;N#QG6ad#}V zVr?PVb_+z|527TD)L%aOa8C`8?>X$Iz!z{_rRlYw;i&l=-=L-v5dxAhp3O6g8v)6I zxW=O5dX~FPTwbT?-q*13WxtkB=peRmklwq2X+x~!G?dBz+{}6M@Ol(F(W~C;frJ}( zRX&nQyFkX)FIRv({!=Y6TU!N2HDt2m%CM-fwDd#!fzGLOvbBcc73q;rOBc$+qu2?$ zkoemcL|j9og3uJ{4X5{~v19x1ubG(sH*`o0XF(SHdir*%2Fa$F@rU-tm`EoGVA7De z$X`(J`)QC#F>Ac%R8@R1OY8%0EHF?ZFug4QU~VHJVdXa#TvemMS@qgxRf;vu{jFnT zDv;Am`YZR{h`MJyje7KO8;`$3-bA*-ii1`MOj=#$jm{!nzwWr+zU`Jp$T((F3}Obw z-DwMP7w>t)4^Pr@W5hhB`hLh3QY!YF8~a&oxr@HvO4 za@~&*58t1YXCPzc$Bqtd4(%Hmdm|kr)`=~x^4xtJ$DVapC@)YQR`B>N0r8U~fu#rw ztGHYhUS1|EIwga6fo(W*QpVJhuMW#uyN?YLjHQS`)%39k8>hCy@592L_9=KVTAUxV&+-oiKN{)9g(Wm2m1#12wwu*HGb{oa*EOsU`PQP8v^?~8hd&W z?-Iv7cmEaKuGt7;q|`jVyKvJdoJr94&S~AA=mrgD7G!)Jyj`?`1@>E?{B$KqkVHhB zK5Wmxv&>zw-as-9iD!+hkl^!p+tz57b@rjn=A4wtjH;Y}P%7|@bPnGkd31W%S8;|k z1;Q>A5tW<6HQa+mV58(AocWXidGWjZMM`kgb9ay~n@wiCa%S_bnXmvWLJqhd2VJ zF=5uEV}$x(J<=C0B6;EdVDsQjgcNQM{%zRs^n-S1==#Tsh1_kI=LBe-&z;nj@%^Xv z@Ivm@O=D93Eyq9&6pICr%*>`^Q=t@p0$V1nkcZgSAp~@h7iOy_rnFy`Xh~Ao_#)xB zMUs{CMO_uO+uj~|zXPvMJ?ZLm2gyRDU6sAD1?mhi1JQTK?sV*;H$%i1bsay+;e-8yTwjTLmx}_^O8GHzn;M+vbiO*T2Fr)^DUUP5oY8$1 z?201K#H4K2YKS$7OH&~FtGk1HBX4JEFTgX17Ps%i@cgGKG+=dxxT`F!5Q?k*=zMlV z;?muRY_QF_YB<=mw=l;@ySVE+3%OEV4(rq82pFF<)pVybqqgCK8K*&GgQWS{iXEeo ztm^K|ggaU2(psP!FR=-CIQ5Smt3B&RwO}T{M z5K-=UGh$U5M<)okGL*QA5LU*6@(my`b?3p*I~PqT_}jMdX?IsbRp&wXudm~e`VpCP zZpVJSSy|2g%vqISZu`GX-{nqxGg+c_%7tSw5a%yJb*tK^bqt{a_NX--T##EMr5QWc zNUac$wg-YXtZpy*`3!p=0dy|h-;EsA`^E(Ixt@UDB&=nj?H#}OPs9oszrst0b6E8E zmcMINkq3!Upj}9CtS0uUxcpL?(uHSpjw!($qPF@0hT65$UzGWJJCQt(`$gwCKsP>K z^b;mV81ck0<3&PYIJCHD^`v+*VrJYJR!(8K1YgiPeoV}j_i^)O`sm5+Gl{W_l`*{A z9rms4?61S+_#s63JgTo!#tlJq@YX7Mr0{*a7O=>lA)!0z#}H8Bi8|u(hKc^1^KLBK zxuAaO(CG*a@XHp?aL$BOlQ{{(pv7=w%_&6}mQ+%CZUijT+z!eu9QzD0T zaYN-x3G6ipiZGG*J^T&GS6@8(w0oe;;ER#!E~o3e)dDf}lYemfkZ|~?)aw({i~=#u z_ur}}0?Oa%Vk43pW{%Ldb0`?JbEFptP%$G$A}Px0w|qwCRM7W`Q)kB7TZ=9Wa_S{d ze_)Z}p_{&2x8wc2?4F9R4VZE3E&*SjIpRwuf$DBSUPt&3|Jm4F-KINOdO(c!4D892N2a9#xAE}}}D_$7n zCg@{M{Avr~;wvuuGTXO$a%qJ*o}h$ZzH)7r5&7Oo&bARlvmWfHUguFY`B7iK7 zYYGz9=v|>pL`n(~NPJ)2{^o&fJYiybz*IE?Ylkd5uOh=;xU6SRxe)l`k7o`A4WmhM ze_J-27I5j4=UnbTC?ZM@LXCi1yhl?9U#XeB(i_cIQ5A`6xwV4DXk(V)W(M0zz@)FN z!zU1HV_qf0QVt8&K{_THXg2w$5q(X`1-yCijKqV5Up;1Pos~)1mTS#r3lj+TH7vnk zi*mcKr6hd1) zu0TZ_drGzah`IGA?}6%k6GBA7t?K^agm~3rlKHaT@F_@t562`(ZHFlqCRP1luOE*( zcB6kKw)j}u#u&$8P|Fo&(R@C~_p%&npKGlT*TnZLA(B?Wue%j}~#|b0XkEQUiKp_}&T_cKn+Y)eJ$I z=Z@k4o=OtzSHY`LD|;*i_c6T@?Sc;W(^ng?)QOWCVZfF+GnRX@NFL*RcrL`nuoJ?S z7*+}#{_Y5w5VuALa@}j}C7pPR;xQyw014AlfZ|D2R^6k@a%)MWiw^U6z z`ggx(m{vzX-iH6Q|GtWcF+UdX8V#kx`|KaVo$I4muyrY}ny1y!!wghYv?P&p-pkU5gBkSPg`hTKeLEFw{K5u%WJ zNcw%w^S$+f z5v=pCjJs}xGugcTUy{r}Vd@Z)R2VcbC3iIr6SQttPU>UYK}HcS199u;v<5eZ>qe2! zq&D*>kpH((lNri!G@#!%O+92+KVAuClk&+}5l^;PR(K$b_=c%H7&Xed8QLEZ&GyZg zStI7W1!6ISECL z=adOj`8{4t@)z>rD)r1*R(oKeN6oLmLBHqu+(;>!XH~WR_|3u#W54lP+dqeSuN{Ow z|7Y7c^TqGRkaa;IHX+q9vu0hA!r6RR zbN7c!1=4+*aH*W=QA&5XeEk_uig|`(DM@#Jmk%Bgt~uvNh$E+m6C7i_rSK-IRY#|j zUlu+BHp=Tm9*JdiAoAAZ&>`xQC7+I$4NO)md%YZo6h2=2?<_)1|6->l;LGGH0xKcdB=fNA1R8G>Te>DJ=or^;PIu+Ps@LtNWOP z94OK~;N$);`eC+SnpB*c>D5h>)(rZ!=6M23BYn+J0xdqZ_Yhq}*{(T$koFQLWayJv z?dXv~idau3P=7#*isxwO+b$xI*mFLtFR7`L7WYheiI}(_Ko=L#?2F;i+lqtu+V|P* zu@^%b4{&fQZ4v`%@(xz$k>Vwo24!xyLro{mu3}0wZ0CdLuRWTIHptJwif2NPP3S2K znjpM#ktcOYIUprXAR{31&!@=#S@Za;HcMw7TYKSXy2X|12k$j!*wkAV#r})QmilkV zlr@b}a{~}$}5BJ*7QLnd;KnDi= zoq=6Om=gJ+gYNQvtBe7cCaMgDj80=8fQbm+BbOL zT^xnz3ES6OlUd_ZiL$5FGk9Bc`c`aGha^j9AN^w2c&jTyocm%_0i)rOZZTTe)g;`p zJ+tQ1+!`U8n#`VNT`gSaj5F=8KNoyoc_KiHbJe|<&6QD8LIuR$F~x-v6`1J@4WSzf;(MT2Abj5j4&Vvdns>b`L{9`&k>IL7cIrSCcc#`jN$fm&@VFY4f5 zuQOPdxG53Kq;NzmJ8cLhd17K`tj*fr=j|q^*qkXVg<1b#Dmt7bZ-%@jUVRSBKe@i? z2Z1m)(R?thGdptqk$Aa0ES>j7r3tWo)6?mFiANxz9UcuZ63TEljC3L^zR0Obq;2|l z_?nxa4P#CYn$k6`WU>}#Q88(^$KjARH`=WUxoiCPf5HR)2K>Gy=2y23P4iZT@LR5E zApmeXtF-#^8x{L>`D5y|TP?)INu^YuK3z2g*|(HMAH%9&`$mtTWKu?~QnsP&b}J32 zCJzZTnxJA!%DpuAD+crP-1Nz(EUBsP!IJLw+F}as9{_OF-a2PlPL^X&E zuX`4Yj0gC6T#tauK)G9)MqAMHrs4X_L5s!}oeWtNu!w$)RZ@0!)g}SD>LX#hLPBIa zR%ySr8(C;lS-)EuzXP9k59$WoUHy7;QOgjS23b(jh>}sQzxrJ_##o&JxZ6#*+zJZm}nI%Z$PT84*(e|+Al^D5Vb|bE<*0U&-wd1x7cQZ4KF}JRc zMAM2~R?17I3nr1ei1Wpmk0xC+N@A=NE@N-PxF%8}@(}5QO#N}-HC6O%_?o&iqujGD z9W{KrcmloNYca@u#Hi?b-QQjB>=E|b`42>%w|`Lk-f(+1Y&wapHMVZVQs7G3&G#Q$iSsi!S#GVX8+ zR4J(qvjS;WMU*xH8V_fKvdN;$ju!yOh!YB4gOjvN;=^UZb_$DOvJyQE9T(HMK8iU8>in6e}fM#Xs3rGX9#lFC1Hz_WaPt~$cI8hdMF#0hte2oc&I2H1e5@0z_t zjnP%V2e#vcm;*fOXkN-1$f?MtOJR8aip*8T-c~L94Yj*`Pz>;E&dB)I>XdLhlSkoA zJo^trHb9)}^p% z-2Gd8_RmEN7b-3lEu*G0UyYzR5YbVF7Up*cYAS;*^cYR5lfCi4w#4P+;-Q`JxqSCl zkmpT{m3rO5mJ7{pfDc`3-G^J;@)d5Ip|0_|_ZLW#T<+(H$6p96cgW?Y7KYR->M=z_ zgA#-LK7L`dD_6*k(>-d%mjI{qiNn^i+(@5C`{zmJ0G=}aRlTzUyE6M}qurPPOzqhWq=z*&?64a;OF?v8BCx#5_yeQnhd{9RwPuqx*C?i5Pm=O8ltDvVx*% zV+^FQC!Qs2dn%7rVOUd#VuN|KA5@u|R%?itQa@w<<|GprsU$h{8pTn9-yEqR*!4tS z+@i@IH&^uiow;h9-Di&x)1P<0nz|$Ymf4?oe_ecC_kbnGM4k6yzEuQ@$1#mm+q+4s>kiS+!VJzX#Xmck@J81|QWp zGs>09r=2k%hXpJxYUPd9c21Sh{s z=dcca&sdFKmxi`6!E!6V?)JS9@cdE@VU}zcggv%Jn7-oK34q+YayfXRwOf{egU3hb zkbK#u{YY!0J9$Com*AW)t6`Q@1&A-Kde~#v1A8#K1Dz&xC_JU=if4RBmULboWRlPu>~eb_T8YB_|fyjZLE)cX|ID|-l~HL5l5Kp^+tw6c-zU|HR5}I7jWd+6&hInx}f4I1s@!+HVtfiuof0W7Np33&WX^bk}w)J39t$ zyauW8WS5ws7`VJ_0|0aTF|d$5-vt8++ycW07NLv9A`1AvY)~JD#x-^NeOHDZ@w0b4 zV49>+f+%_7`!(VRUw(_F-3!Kpz#pC4PV?k!T2izDc?fd4|~il*;c6!S4Y>O-j^A#yM;f-k@8zyWk5>1LioRU&9`!<0!A= zA@$H5KY_9qmSo}Ew$ZtV@WW@uM*x?KVsJ@*?a5`x-cJXlbU;!w#p8pZE3PaSPxVTC zs!vk#-=vNoOH+l9-Vu`XDIN&3Rq7%IDis(Dt4v2omLTnO#E5vuVaRMbOnt#xGbd7o zV?V0P#S7Dnjb+sL+sqkR-Tl$;?cpOH>3=Z3p6WJqZBWPSL!;-;N@!NAU9-LFU(VW> zP^)6#rzia(B2I=L{hc8tTQ+U$ql4^MEyUX^gV|(i$5=Y>`@}$IZ#FM@>=pmolDs!z z-F0wF`DhV#FO`2<^F_SxB!)i)t~6y5)od00@O`(>atH8aJoE8tjU7!a-jX!_ zSvGO8FTQ<3wQq{+8r(IV5(vm!kn{G@xU7g0rd5G;-Y{UMvcUwO2!2+K8_JLGaMaj# zdZ>bdzWFTJUiljRr?{^S71*t;xn&tXpa|zl0b}?uh&ZwtXN4AZ(P!RwGH~hp)(5mJ zyR$>M9H7O6LufX_ZnT2`y%Vh^qSDMp6@P`OKJFM%B}u zPht7$$;#O(BVf{0m2RN_)kt3xWdzPkx4!x1^5*qYBogjQRULl?7hh55PYwKY3k$_` zqEQULB&A(1;ya$vGS~xSQ(`ctK+=n}aBw8)`T%tAL9ddYBjJ&{t%IorAW`4IpKQlc zd#8x0a{!$!hhH;ImR$Fq=IGc><=@i)g!Ie~M9`6J??7tO8NFd4oc0R+-gJ-n)cOtp zm5uxm)0nkArvfT6{bi};<5PCd?v8MCI`jH`cUm|l7XUZ8(gVp={^fAUJDv@q@XfdYiYy2s6meEVcs68 zlYAP|##x>E;cl@#`5#fIBc4}5)0>q_6mg09U{;`!-CZ?{j&DKs_+(AEd$nUi)FsF< zY8uKZ+iNu=H$~CM>by2@^Ap9qn5O>u_F9|1^ldSv1P_>3A?1F#rB|xP7tLIF5|H@N zlGU#_B^96eR0(sw@#oQcSLVTzD9yS(L#7ld>W}GTn*PLrcR38upj06yF%>^s7J`2P zn8UpP06{AzdFhXphE*J#0l+F&ZT7;y2Welm$s&Ek0X#ZM-f(B0W z6J=RU`)qQs`t#Lv-pQ`uf6U!oGA0(!hVJ6GJRN)+CIO0GfdijgZHCLc!wTC7-+TEU zKeD&KnuLGOkD*ndm$Qf9F$aS=pegQqBQEjYcDQaDflZX{-kzgy?zND^ai>6bH1Vq@ zQZD+Q9f$LB%vSO;8I1;v%{>O!Z$$U655_XWJ9%Y;mLBfJ1ui31%u%t-Cm0XoyYqQA zrk&dNobv_0C#anp^UJbp*DfD!oT$hM508isaa5fT_4NPyuj6o^vLF%v{b3vE+G~*n z>$nYajo2yoBI|42j)P+5d-Y*)DP$cv^bV3rv|~P-YiSm2<@xB^A5pb=QMK0F$%uY- zyY!}7%-=GY#_;Ad8GcF;a`wdEyKw?S$eSXh35w*-H$&2~eVVd=(nzKk?=}`ZPw^^r zB4&?fUUdM*os^y6gSU>~xCF3BXZP+f0pPmnDU5ub6$VyZoFz;S-wFu+(#dI1ol{Q1 z4-PWI^^~^Ml+ab+lQvs=mfQ?_rm_(Lxqf?w!O~}}qp^bM?dX`)3k^N5@&}(V7*VA>Rv>G>G zq{?HwLieck)$$GP&FfVec)7~u5OzK+ADEBItjO!WuIO>Im|r+Uo0d4J(Im?l^s~99 zBUTYRA0vovhw{=)QKv@nL#ViFTcD8++B~%~V;{^R86|=wY8*BLI4;JpbQYw?iS-R)+3+?aj@4Xav~=#gvQ7;VhbyUo{QT6dpYadaIpY`^nU z!5i~PEdiXvY#v1B-WE_wQ%C(O&odB9=yl;p9P7`48FKf-Y;b<__g=ii_f+qK$_hd5jtVT^2Q#~oI{Bmu zHyk&qiAX8$ZVv30#^Z`&N`FYt303`|LF7+PFaW-L`L@(}fUh%{%+GTN89?)WzTP#@D~@96qwW;i6G%I_jWG?@;Tx$0n;k_#a=M zu#pz<=zxVeY8k9GvX#i=7tv0c6`@bZ+}CY zugWL73X0TiP!p46OX6ZYrC9MEEUIb6$GI50#rx@aC0>eCX?^HA;FD5Zp=sMZ^TJcs0-(%x{4I3U9X|}%doZ+P_J36eP|6E`g6cpI|NH4Cwr_JC z3WE!+ms-z5X>h(ZyY*D-@r!*s1C{jY^QSBmk$4Dw33il1i?!mtu;67@<$$*Mo*To! zAE-fK=+yESM8};fn{og@o=m`FTtn(KfqyzY3dixx++Ls&gk1?sf?48glIT7sdFCi#pwG zDEhc~-HLx)3!pNB;jdyKP+oVZ0}U9Xrubk%HPev7#PS>}{)v$A;cM)zN>yGk7q?N| zSI0LLRd?c}FG)|o39HUXa};qbZ7j0aqGllnz(N*e^YIgKEcfY(RaRLt6&geCOyxE_Wp}+2N8@w2t1Jz`qmra48?kjPuXn^z>7S@& z3v~y&M`wB;00*!Yp5&gxXM}Rkuu3E_!HCtJQ((MS*dneA#X(2m;RGjcjFLpp`gTbO z&kuKK-MDqw+!}9-7?}qbYVc`tEm!7EOB1nBlmj521jjfrr_aG!Ekncr^s03C>JC2AW=6I0-1> z7n`M_KgUu>qf>QGyU*fUd|(+QpZ(>BN^$>aLe1-O*90(-%T84rcRN>{g&Mu~`2PIc zG@MG2WZr$lDg{hE#WeO}ieW>Y&347B`Fpeq*uU&{|ds#Q&sJIaf`XUWq zVY21BuMnX9s!71_y>L7$y^=bL$Hd9%fh)4f=g?!toJOF9mi9q^Oxt(&Adas3+QXg@ zciInc%iJgXC?Opm!)LjZn*{=y(V3MJKew9#C?9~i%?}P!=Q#Q*>x$>g|1{dNYP=o> z2~Pt)k!O_jck-IL6&nh&RuxO{zsz!eFxV~paqi~79lXy?WJ-3jO#G3U>m3fO+9(OM*kut#D zoQ>01x_+vi8vmeDordkkn(*`e**1sKap&0^(9fMVckGFmTuf<~4p?k`G}r*3c>8u8 z%WGus6P#X?!Yc&H?(|I_AUS&eLPIxDseZ>-z+0)?6!cO)M&L}F*9(;FHn}^6=63a% zgqCyKXkGl8e7zIekYDl$dta|F0xWo^b4*uP!Cpg?)GMQW+d+4*J5>GV3%^hnBfjTL(pAJbWd+{Z-dtf}+CnruB zsl=jj_X1XJKKivVEA9Fj;~0+|%2-$`$B`i&JnsP484!!|w=RakXQ~lHlg=$bLFS~a z*(rFge5*8_x8gG=GlNNdcWSY(lrCt3qFm7uT_xLRlhtZ|2hMmj^M1nA0UruHYDR4B zx9GvWODRM0?-noa3q>pcU(fPENFIcp(v}h}*FB^%QGSi}2y&qT4qPRQ@R~ki4D|??k_bBI zWqsi`PF$bCnHa{bX(!M-jj%3lc@kfJ-vxcuY-fbqLnpf7JUg!^j!6Ul_~I!aRHcJp zl;?pg~vy11^lF2jjqQ`cXY%(mCO19moxVTI^5x2(7x1XE?u4ks!%Qq>WuSAf`~SF)}4_X&`KrffUNrE;Hc=215HEcWjk-f zMFzU4b~q?R|4><_KCYC18YWTlbg+#m!Ia)JkZ2PeahC(#z?lU0k(#It%et?+A-(^x zTgPsp?YjJ$b1>uY5!1Sc+Y#->m;d3G*m?Yu=J4$A&Wy^v{~Sxo){08b67iXR5soU7 zLAg`OevR!rZaSuQ=$?T{Zumrg>7N}(e)O$3`@+uYk{0Fs$^A}}u*J3Dgl*_3@h8R{ z1f!bQg0TgDzMek@`j3E_!UEyq(F36Ixltw%?6)s-pbV{B5;1jvn$I|ewPhkk$b}9< ze7Ed_<6)tA#k=5iC}He8ou31ImX0enZG+;ybV-2%ZQc83Mam2_cZ~P$+8ck%TNdTy zs39@REJ>%3z0Uz4>ecte1asrp017#DpXDHC9AM(L5$)jV#{n`J%^3Ru$HYEX1Ye@e&NzRt%M*_#-NV~S-m7uy zyY471`wGO#kM3*>;Z$*6g1hApgv2*HhR;AgB<1$rJg|Eb8et}L-zZX&zW^8I0e^E? z+wOI-(nB7%W5vLr6%F7SrU-&d)cU53L?*<&@~jv+y>;k1VLH#b zwkWLS_&-9YaaH@?O%=FC!@A9GZE>Ec&8th#L|jLL8+!YBA=swsgux3t{0lNQ!Y(8p z1#%fK(;!ZQjmI}0tM(B;LoC0xOS__tpzgy(xO74UgvFlo7mFxia|dEsb-NX-VOo2b zOGjB=eqsX}Z*tZ(MO%30BaAr|K!?PV$GmRrgc7o#v#~?{O2L9v5JS_^;Xu7dojWkiy?WH!q7wYkn?o_T3 zvQBjmA=HkkrtOsD63ypGPVKg3!Xn_|!pMc4luRDg#1>JlQ=4eS$s*daegR#s+$EH2s*?9zJ6t910 z*yiXHELxM!w{+gU_8_oKfk!;d{qBz%i&L&`!Hi08P?l@*sSsrc>zH2)Rk*gtNry6oikI^DS1&qM2SWCZ;@_a5DK8@>!5c3o{A@7*|=*AjnjfX%kj9VN17f@VeuDek`|Bp#1?ghiiL)GT>A0c=q?e zsork5C{bOhc#oN8JT>Ovnl%hG37L6n$1x2buUzNg0SAoh-yd_fl0(;<2ey^O1p!jInn_*_QbmVSu;$T`ngTn{|REJy2g5pv1F8k+o;x@>PPwxt`#B;lg3{~C%+vW zY(IKCprbth({GfBmNi>aytyw!!{cJX;EFm%Oa*M;{r+XlvrS9>x@T!~mEOhsvxA|D zxZpKJkC{kaysK}Di2v;V9BK)KrSJLCN3A(*>=vfzGaw?f) zz5n*l(bdC}WISi*^dBR~35<~+lJ;jy!#GkL9%YdIDQ_Ann9%cKRqNp^eD|}(Dqnk! zhS0{l8!%D7Hcq>;jV0-d=-_G-DQWK9TD%u6)JKh7hlwlyWw|&O#jiW2h7ZHldq>(I zCQV8_*WIBL3bQ=3JPuvey~J3|J;k72B$aM{S&D^xu&Eswa?okk$jC@B@uIP&ln$Nf zf^PY{nb37pkE@tByVON2lKr_zWg8z{`M3!zLd($EdL9?FKR#>GoDE7T$^tB&tH(34 zc(*(3KgtG_&jj@FCny^*>*=ZF2h+B7e`{jv&elz>$v8`azF~Lk-d4Xi!|Gkv)z2I- z)j0R;#@uZ<3+%fQEYo2V_$@}&mR*+c!o3zz3O+9it4i_~h2{8rhi$?Ppii&X_eX8xfheD`3^;RLT>6rs(BFI1kciUm&I0VX zn%wq`Kf^H-|1Rkn0NHAx z#T61p%B|S6OAtZN?Hsho@$eiZ$B@5ew%>0*EwF8@nuVtKyh%YXiqpX($!yEMIja$P zBB9^2mnXyHcV^xSPMag!0Z$Kp3-)NM!Ak&I5z8R0lb0Zb5FkRMtY{M_Vbf(yj49Oqe;5S;XTRM zl{q%dtnxn3IGG|5_M59UOGeiAP~V!|;l<|OhT$^)ZbibkpWSrFt)8hndDFkbNXzYp zFe=wq1*huMU*_3{y<*a9Di^t?n9$u`N{_hcOOm9Y^V6T-*S}pXtPuk~ROBH6{&!>% za&&7$y32GyFm=_Q*m3im$s(*-L(T{`g#;HVKyvKtVr-u}58w48PCPSS_os8-eg$~E z``&V6W8$7ALY&Z+Kk2^Gj7{n}#uvy^R?h$WIVB{!KR(43UNXHrDIEng)TTHtUhs2u zzukg-XREW|QOi^?ANqB9Z!n|5^OLjObMeW~`@CExDK4-7#gemQZBL~~ru8!<`m6=tNLfCrj?7c&vv{$Z?cJM{~kwl{W; zbqX-;I&~&5QIfqO^3JJork=Evbw;^Hx0=oe;aa)JRJZsAS+Vz4q?u%oo|U{j#F`|W zLs)#HjpIg%GpUi~Bk|4O@p&a0S?BbhzvA$^b+9&))jM zzJ(%w@nbg$ zDC!BUvvUn7Fn=O|wx(e@)B=t|T>HVw}YR>hP%EnLE4 zf);KtRzO!7_AP0GG1)o~!b)s8@v!mjS^FHvD|qPvyqF}qd8zL9KJ`9|*KdiYx4$=g1~YBXf#JDtu+rpZTF zo03Ajw~rB7JMOpblcedSSgK3bS8^VhDY>EL_vfjLNDx#=an_t_c5pk+n?ef^ z7TBOPcOsMQsk1t}kS3qs|E)!NutgA0mT#;}Uv@ZM`6Ip0IQPZ-3?B^cw%l*q_upxq zf_4^M*8A6)1$*Te5xuEtGqh8Y&C0*8_h?eRXtztKLcMwuywAci!{YBbwmI7i-!DBs zvx64xhHvmlHNwp8R|cB2E**+>H0t{|@G0q2PeF;SJt#(x*EQa7>3QLMXKGKMcbIqM zjY+yCVlR~aiHW|jrx+d(THJIo;7zYvDk|lS|L`g&1s>Ng3@y)$5m3Z6fHs?A^ z5zw{a8E3_EohsYlt)kBFNJ>_xhmpUL!om`H**)F%P9IRrO8hqsl?N7LXBLlz$H2s1 z`NW{1vdv{JhgrIQGSN3%CaIr0>N_du?{)E?b8BoLt=yWCwY_GxQt_c%!pqptzjt>E z`sBLye;mXRgEX{HQ%~gUk*A7DBU;sEK`#nz=;EY&rS8|{LV4Y1=DS_|(Ww3WpPY5s zRWOmre?cva9UN+YxJ`eExgeWMI2u(JHJ9lG?o{rEp7V4ga7G*|}h| zO9e3JRwe^)R~$BF>sWrQu%#*gtaGj=AP*85$(AmCZjv?gsw>BYX17esXr16d!gA@g zGAkB?MyatIOQMGZ=Unyg9Y;_qL-w_3+;#5*#xhyqMC_X^WHm>l2I;|d=d&PM@-0PK zCtr}nDeBZx5j?au`gLK<$USSjNUYBfe3-MgTugJVG}OJg^^GW#@^Sy8Rg~#y68&fihyTp@#zddQ#EafOk2=mo!8iC_NF7I7&Bc%=%p&Cio2+ zVP{o+$F=XjGWPI}eKFgMb!v>N#34_aXw~@o!Ds^~t>!&?Rd4q74twD-=H^xK zdWfDMj41S?F{%2G716K9LXpb>$#%$87w(6pL+jwp(*XU=rj*U__~zx_Hp0e#Wa}p6 zu%pi*n@?ijqb#wQnj!3QUi%3(2~{1Bzd4DAoOVrQ2Jk~h4BE5Fkfh0hSd|it^m@vs z^wt%Op-E-eN}|s`G?dA-uZAu!s~+_uP}mkLem-6`x(D#3WR_(Py4sJCxCZinZatYr z9F_N*uA#^ErSTt6r$d-5h}k>Rh%Z-5-iaAbUfUcz4B_wl?$KL<5I3^J(>vtUz0Jcu zkQN^FUcj=*ToN>HXkSY*T}t3n##u&AbhY5N2ZpV?`h*jGe`hm7?&M>rt|@xIo@eC6 z_JrMpiIRLY2`icun!!o4;}hl+=cNoH>QsL9!J^5w?;pGMp?EfCnRP*UyUXs+E>0PC z{JHnynAt{kg+HEI4oYo5Y(?1SM8t#rSIPd&?L8vGj3OPBe>S50iHZEt6bda5kE=G2 zsFvBtmFGVG>UX5$v4qw0ZN?;HT-*#RmYkQ7V%n;=W1&Nx>4K)h+!yr*3&WU?2Q-aHFBUoD6SPKA-IQY6_BJ;PWGMm4|_n@we#$}yw7iNsnu zh_@{kYtNYxb<7xsG?F;axv-&2=9ymMyy$Q3p}Y5<70wZe#KOM)qc@z8kEh3+zZU1? z`fIWq=3azSM!7PxYPu%5#_mbHcppUV+rIfwDl1dm7anLM*vSTvQ;+vswgKKgA^Q+S zj#BT%2L^W3CO+tC)jn*i11}?+gEF?h{@GvOQ*taKNzeD}o1gG{ysyMn;(OHW)-$(h zl~=ISB+U`gPq0%5o$Rb`IWi~}g2RZV+TL|-eOObds# zGkIvQI8LQ_FefULQ+5F1KXKyBEg~wW?&?CU?7FA^!jsF)6br1cqi53bl?&86^Q5++ z(n)jJfnR=uB(}Vv)%FL7I#GBGAy>Z{t3SdZB2QjY<4Hp=@3^>D-TR2f_5}iiWcOcQ6d<;qgVuOaJME4!@qk{YQ~fb4;! z@;J{7^sGBMr+U=4m*eaEJ?wBj_UYRAx^srle`~Q@W7=(YaQ+{|)seIg4F9Rogd*3$ zN26Wjt=|Yc78ykIku3F?nj1{heUL@9vAIaUwx?RWW={=4R#SLv)TY;cV|3~2-+G4Z zvl2C+JB|bE;(ZVD8k#D*^re&6&_pfq+7KehrqiPLSgi@qO;rIztrc$$-i#nmLunCecs^h z3znwtQ}6}b&p8?vnYi$&p}}57H0JtSsk36UNO#`J-kQ&$aZhP~Jy5ii>-lQZ86RXq z+7S+tpnkb$52$enSK8P@Dso?*&a11N`tHv$(N)s3&0ccE*RStn$PHP4oZ(2T@G3WU zE4QbQAHtdgN^WQo7xw}cLs`RNv+?05^+Uce74z5~v&BY`e)VVrnJw++5N*dq({-aV|O884&e9CZsy3{ ziO9^X2BBHxrFvo#+RPkbYaF4JTSZ!*d#7Z-|CLwvc7B*qj;X*LyYD(ds>4sy7=8uh zn7`wnGo)4dbS!lwNn#4rpSE?Yb7wE=41bh@$IoF;=2X36ewy-d^zEAo-^BK%ZSsGZ zNJ{BY)=NK&Bt|-6|4PHA0HUnuvfpTa=M zGlL(8(_U_^UJjlF$Nt@SPWf3Hs9iI3s<||`&uv{g{3~@!fEFZ*+|Oflq$DlcB0_Z3 zV2j}=@wmJ?!zexnhs=-*|frqR->Ci+y zR;)S5kSuouXb7ZgX4$lCvF$z@=sE1qpLj{b^e&&J%>x=(XcX_-?Q&+5A&@cIL{tv8*M@7iFDJVWjYa&pDwKy}5b( z0HzE-ZQa@W0Yh6LNBiEi?^7K8{kRsHkV^Bx`@GUR$hoCc#@3`Oumn!)8%m*)+^YY& zX1u_}z@MNk5medZbj%XA z)*CjYCn8uavb93YkcYUhPEx4n8Fk{8 zHD21)TsVqRUT{Rpm0fxi(qe~%>CdbOV35c2wy%eND6nPmU}IwCyPoA3LG1bVTua9C z>60s3+DnrLAKP0Y+MbT^)nRFwN49D}9&A3K#`DHLk4ikC6On9PfSnJeE`sHO**3cK zV9}cKGCpa;u_Bq$bry!o-%g#nV$^~CdC+3d8KnQn`$-|GW*c4c_GUIN!C-&l-o zD7;+|s;SALD40+OscD#$NjtFZx4mR=g0Rz@s>`wJ&p?#$@aIe^bjRlQqcx>8(8YRv zOZ(J|f4L%CNy(mExA&Umr+Y=DweIBt`Wgva*8iJnLuZg8vSpS!b}i}JnY_MsJqRq*!yt|JoS#Y=w-7`tbumH%!5UDAPn8vS54{Rq;?(jv2p6%cek zKItzCpoL$ri!7%em6~Z$ZnRmGTWtgw5$o$*@_L3UQQp~bLH={<*wyd=?n6c5~&hV6qq@` zf5nJrHW@DD+NSB#S1aOHZNtK4P_E`0xlgs;?-Kk+-!o=3Y8#NX6fi~?=L(>GTGqPb}-^rL7E6m!LQeC_o{Iq$+=~uT<03+$S zyucp)H67b60NX{k=D)_!FWhTFDPS_r3IDETS;|r8jPBIQ+z_!OG$uE=Dk@`9Snw zfD=%UgHa+F!I1dyk*OBNzJ$a>ITmCaJAs}-ru%jD{G(Qd;rjETQ=Ef_vP&P^*L>To zy*0*Q=K6oPO)>B)9}7eKGb2PwB#Xbot3&@B?Y1v|oTBnCj#c}W_py3ls;Qu zQdLcMxCzE_VQJu0ymSAU2O88Ynm;dql)Mb%REcXkFWSGTTYIcCH%{0L&<_jRfP$h) z;}#Ztn9}i68$A9q_t9>RSTvS2=Xi&Rxi(<=BkFQ@N35Aox*l|*vURJHC^ZW>6aWCO|h4Mqmj~_Wcl}c4WG|jT?lZ0&WmiFHG()Y z<^%53>pLk;r$)39GyKr+10yK(=vwcGN|^0`>w9UEWRjTCqN~hW8jI=QGzCs#%3*>G z^w89)oMA~C;1tSZVo!B~NqQ@45MeJN+1l@qNBh6m4_*EB(5qvuW%{5kFAEL?YZbm< z>AB+R36r{w>8^KO5|#_%MD+hzV!M4CMdj>;v&&ezJy^BO%IRrrPRD|88rzouY;>`Ymd%H4p4+~-W^1?KiHuU{|3MqG=Cwhz(m`Z!5yatUpy=)nUjy5gX z{17nt8P#Ir2aaeF5#3|F6z1&r$;ELRaT-2bqMJ1_Lk^jpUknP~aiZ}kfLs{U0FBGY zgpa=(nO@>Uhx89(SfyrNtf=u|iBOT9G|LpjG`2X(8BX2pIn7_~fn6Jv>4C3qJn`r8 z*^ldh50{;Vq#)_SEk$AYmDCT!OqwYM@HOSSXdGup5}%km9^2yHXB~4nHYqXiM|cJE zKJwnxqq%AlQIg^qXn|X|bf(A^E@V^G%B>aK;>2FWd(2BEHM$cH0x>;>HmQ=8L%PHI ztF*&UMK(tw^*S9AQ(h0=1*lOde+DxARwQo7#C?9Jzn0}A<3Ha|9x97ibK!&QbvD?$ zG&s%ieo212wO&sTa~i$>mBXQ?+rvaXfbMOq4t>z#%zXoV`jX>Tm6h^ZTokMcAH!14 zR*V>(C;(vZMr^Y8{~boETx5EJbhvQO1YGz2G!Z!1;no9RH+s~i0K?L}G7Y%{=}?0= zpl_PBTM+*ieUx3$7(m)1HZwdD%-mOO)!HrTDeu%-7y@HR6?Vf(P?(palYX{gYTLHT%ZABSNa(&)>RzV!k zkK5y52F|t-5hPM2v-R`@M$w5Mp1F861p9J(1bq#)ZwOW+#nDEGD^IZ@U9u@lmyGCC zMu^!33e?{w%+Q2=qm7)AwqidMNP(xeCga07TfwMBgD@7=5H_hJU zY-h*QnRcO_z`qvfjKJgEMbXe_B5p%E?sM7Px{l4a1PUmRKEI%-!kk6S(D84-I;^YN z1$F`Z>k%I$&c|Pmc^!Z4#if!l*VmeiMwb8EEIvasJaBM$XyON3TY#$)JEac??MZoh z@wG9r!s_D@OQ9a^7pO_KX@~Q6TYR!trRGv349ToSel$3>LZQV^nIswk8|x`+2CK~b z$GBoUCIsK|p8@l5>@DD)GIj8^Z1Cp?P+*-FhZcfU_%lg@bJqfY5QPPbj+9@)BK9|L z$Dh9j_Zw#9NYiw}U1#;3?|J*wqKQ9#n#4sFcbKd_&s96&zY>~YR6wb+4nJV(y3rz-^XC7h1-fZUu_DOJ-N_@W$J_>AR z4+FQQ2F{`rxho8kD0Wnukx1t3Q!O}0GtGp27sZwMmiS&zXEb+UG&V|u?dD+K$@>s* zC@E>jI#888^KK-Fr(B0jQKc@HY-cMN20EZpDq>_$&=PHJQweY6qGbU!zl~pit8rOA zWAO#~fmw+OzvgcrGNU!m^O?_>InoP%(arx93*WLxRDT3+tt*YHj0#dUy$uSYMXshQ z7oKr6YSa)fL}lpcwB9>Af4o-V45A_a`u(mb7NADAV?-3H8jnlkr_|zjzuG%*?hH!q-XFkQcCR`vngSTXLE##IamzeIY;8JL zX3hkAQ0Cn^4s%4OhN*U>!qRnn2JV&Tgc$MaNE{$Y1;2hLv!v&ICS^H#D6QCt+p5k} zv`LNgnF6hXnWpb%LpXO0jz+>35gj`yjg1vsRYIpmL(qO|K;b@hQW?{M1;i7q*Tf6K za&!-~BJP^H$W*y6m>&#zh0jfP*ujX&uY311xSvfHPJWZbe_|Xbp6anZS?ylfkT0f& zl3Qo8vQ~{vaO?Ely;6|%|Klb1*8!C|Ys$nfyhqS93SG&}0WbM{(y2iv#3r@C^>%`QwJxp!{|T;8`)*pe2k817V?HAf4l(}UWdUtQ}3 z+{N7eak|t0)&0~LUO=1B8%Xeff(JXYZ5s3iDXmG+92|zuAPMK2TDK^frZd-K(4+kh zC6$nFH($GRPR&`Yh%{MPp-xhEQUnBPN?ctsBo@ow(zg5(Qs~2iK$4URtkj$qjRk(q zoj<;DCPUpRg0%NII|BwAYQ|!!!uZD8NRKH_C)qP zTA1C6*8l4|cW*W+Ix73$UBEzlF#9+EDS%PVi9Oq2IcJ*6Ux`c}3#&J&=`Q?LRGFXk zp9%KH{M=Y7_gAN%&nClp=A{muVjg5IIVytk`zD-%D876UGBr}tiQ+@(-4Z=eAI++) zQ6~d>vZgkP8Nivox$!!a!wz_2%k(;LM#}$m8~;hH6dm0nD3i!kA71FX<-g{n9-$p$ z!M^BdSL|hzXyK&gcbaBg6gZw{#f`^){jve#`pge)iRC{} zoDIDh)xU~q7(onqRR!g?B{Tk8Agj(xSN!=N4)Kp4GTexlsl+blh|`b+?f_$z+Mh&g zdcWR_T@_N!Q=9nX(g`4w^5#aCci^oO+mLqY?N^YXDHQS#aT0TmoMavl3sk~fi@S#Y zhem?(Igmf)=Czfo_)eV80B3=;>=nG>(Oys*i2p?WxVvjn(hZ&3sM5^Y!^D*~imZOOU!f2F5cY<%f zopbH#fnLg}14&|XjFx-wSKN^O{ zvQjQNv#&d_cfa6Pd!8lFvitxH=h=hg-uORn{1A@m#rIm+7Qp6mB{%42&PeBy5z{J!h*Rlp&nuPl%@sK1TAFrGZbiZnN~+2k&nO^M4m=?wiz@wk zF>X}e*?ZuI7A(hU|&^G~SBT2F=w67Md7$TB!5yn`Y|omRXT;?}e0&VkI)BMMC#@~AlFz}uTAJc7vCq$L3ikBqh66=(S zT)KP?D`vG1)F=Et*)b~=t+7KVuZ*+D|zdc#*g*vU)Pi7DE@}6vYe5FRu z(#c&GUzwEs@6wcZ^H>*(ygNulTaHnKCTdBk=frxXyD*_n$~o-xoZ@S7I$F*9-=uWx z*4ArlF+id0IB1y$`A>oONgsyqQ3Obx*fJjd{It2`pRl}$8a<>uusHq^cgi2Hd$|#m zt9io}!1B22g{dL!)z~wS#eVK~o0WLYA}$BFq?`MSNIv~Ec!6(0f9nqge%HT!0cRXC zQ6u<#zsrCk<>c)#IuZ%B3P9Czq7_0GdH0AqodsS(TiyEO^0i&xEjfmKYi=M-x;}jl zLc>dEZPdaFB@LSo?&(x1#KssOTHs-hMn0D^a1%b?^B$&5{o!~xQ)M`qn?ZF}w-Ak~ z!Y?~(@|O=_Dp)Ol1no8d0MyT4@JA2E-K7h=M5Y|vgn6fLo?wh{2N&`e`;eWOmXM$i zSqpEDw*gu(+j(BC`^ul_KlFlfLyCV!Ppen2ZHj+K#D-i`{?P68y`%6g=t=~eT|x7^ z4ixE4ultbd8Hq#9IUUzz+RcQp5_#>9_k>hLEy>*s9k=80pWo7>NtOcT^!KUQlOuga zK8Abs`|A-PEJ??O0G{&XA`6g>G`D07dOp4DX7l9;t%xa25cYCQf4btU+BL?H4fP?P)_Y^P38E3UA`uoUmO`Ov4@V+6! zGQ*p8xzq_fi?UN+-N)ml1-Hd2hD_TV90FIr5xI=DX?{ikh;4C1Y_Ii z9FQ{yqDH{}2N8JCe1ySLvM^Pjvd9#E{{h~(Q|`aYLy}?pSCYKJ1Y9cirVVKImkpAza9d=}OSUl(+np%G zqEpf%kQ}L{2x9xVPJnFx*LOh zULRtpjnds+Zc^}#(X#O7Te>pAp*^LW;&rq%bnB|W&R+3p9MUVoEcPEeWY4p;9K1eF>#RO{F5Tr0hj2m7X-BsKAIQhi+m7&DK9!+XKghH+|!@LhbV{( zxJH&aw>mYF>%N2<2D~UYzKq%+pSCA921E8bftLDQoyEanb z*(aX#l>m1bOs|K>>xhbj>z*}5!x_gwZ3;9mxormx7W%Mk8+sjE0-v8lL2Qi|u1JHT z76y^PBwY6kLK&K$1zd)L*6i}!Gzmw zm^^iu;nd{Tzz^Ovzcd80Xrahl9K%Cjd|inVNM3BoTp=}-^0`L>rutLYvQHs0-C{r! zo@Zp!1)P)_=LC6PWQ;G5WFd9FU8)_Aboe9-H{2?aJ6o5{L0ABNJIG(nW(FTMnjaJO zxonAH$k`4;@a-fTyPqVBB(Ly9p0;j-N(AQ^oFP}3Wg5CdYNfi@G;eTjGsx2)4Fv;e zZ$`n)1&i8H=FOvlLLDp4@iGujA8vvQs1GaaVN7k0lk?Aj4u^NG5=Jb1tWOK>%DWl6 zOcy3}W6x+AU{?!iF>Q!7E1Wlhlt&Z)FM#i%Ev4cfVu(n92VzGQm1AsJhAqy+&_l;2 zL?Yr>S*xNHQOi7#6!Z{1qLI?*aSgG-$ks&M9=wYrOOdSu4vrE9W`ez17HeFD$UGXd z1ENT!qFS^`(3y`K^o+Nv8Jg#QtUhYfy=G&`vUke|I1Y~z;>(-gy4_SZ?LIECEA!J7 z5b^)5v5YXl_(#zfY8|8Qxibwart1)fL4J$autzItKu|CC;H`>^u|JAmLOjYs1aeG$ zR~H5KDuKIC7Y&^N9iI5%R;GzVIu!_85-A*`9GzbeHxO+VOw0T-6`*`}kI)JvN8ksS z089MZEwKv-{BD+EnFj$<=DqCFAt>(q4wN;@le>-t1WKY}K1suDg;)7o5~&O8RTx^p zCD_Zcu#5GQ0?DLt?M3WU23SyteYs{}W%p#G;7}^2CG`kd?-k({m1hSy+gQcd5d#1< zYce1J%Jnn1gxnIhAC@qJu&m2zVT8wul`k1$QP9P`GKj?HmR|#V4gKBJ0l<7J2P5!S z!?X^`rW76~A5{d0T<8IH@t}R6qU;z7A7$Tl;@bh>`CukR(cyhJB6D}qM5+a!@#AY%X$R|7H0`t%BBv=m$3T?fPoEn=f2pVX7@Lf~KAKpJ+Be)<4uC-EzZ=>f0w+T*g4JnD z0~wZprs{qP?-Z|$BcZ(ZiU@%xLH2$zrj1x0wa~~P2?c6F*LDI=6OF~tN|YtfgNF<1 z`V8lJ5w#%I_^TfQTG>60;1)yhBf%sIopU>^!4X9lO;m;-!(boaO>Q{%VIJ_k5jh_v?I9+HZ zSS@IV*$JJu{&ZXcpgz;%4Hg12xupo7>_SgfXnM90ylE(=)fhZIlFQ(PqS1I`akrBI z;6&4OcUTTU01X`*tJfZSr8(an6_O5s~lxiy(k3WXC<$J7JObiwH>kZ%3$KnWCuesg$K#URze@L zGRl_1^Wfz`JRJH1U}Y4OBt&2~O9lRAyd2i+v)bV#5xS6q+-zadiol;Pj1$YZF%ds~ z1@9qM9^EIQN(?`Vflh}ixa|%-7CvZbvCV&%h3LSe!=J!mA>eL2@#@^Ukl~O5K8obo z)3c`_$Kg0+J0#7*l+#ds=3Mt?>4awnWcIm73ER%@-`;)a*v9TniEpBEJO8aqJLqbo zq0ea;dI^yNE-9Q&oZCU--m!{lp1AvjBt81)NZe{~L)+s%HXPst;@$jPfV-uL-3C$) zIKu82JjpT80@%v$UXmKR0q*V*!tz` zpuue&8Nm+Lg0jo-K-fI6FzM1D-c{?qS@cQYoKn} z;5EcIz^u^EX9UgR?QS8W8Vfj0P%^B8G{N3Es1Tgg=IHi8mdu}{uogoR!-t-(0;CwC zgr~4XIm`#A5fyzt@V4yEhd?=a*BUa=r0d>@v+z|q>jN)klO+S3!rr&Q6WSZ}VcG^U z4IYtjT)gxLAQDAvqCn}o6{;rCfybJd-t$SJ6Gp_vw5HeZ+#|pYd+Fo%Ot3?kMJM1F z3buk}H;TfQKHhLSPyNu7i_krF#ltJWMM14j2+WOHxf*yJcCF!0fIhpVBnhu~s; zLUNsCNf?Qs5!VA?K=ervKGo(4^6f^rRD#WZm3shAj~BsE(QU>cGOh^2qF{LBMI<1b zZ)zC&Ce6b*ua%)!9n7L^x&)jzW8-Ixd00V$@mprfH7M7Y~!E$LK}!ighY znmO;McIP?F3`V}58~f`-(!Z?EUl!zvT5!jx5C>|>{WP&iypM9jDu#?LmmXl^_b}{t zEiBRhdRzS=+?0P$6hZ`RYoYxA4@BH~Br^}*-~uG5qm;{oQ%dl4zan`45&+Df2#_uK zuLE9NVd25Acwd1|wl>%o5Mu^Uj#9n?O|w0}K-`VO&o5sLfPOn7xdP#l_w=DTRk6Ke z_jC9A&^;yBvwwZ@8Q>sk&$#UNBZ*6dsd7*n;?s1f-$&56n}V*Zr^6-Tt7stqgG+&Y z3(5o2L0>^OT2=ON9X4#G$j4%2r(X$BUcsazoDfDmh`S4M zL5)WFBh3sXd_RWI_yvkGrJ<6sy?d6Sx#oevrcq6$xMnYtA4az9t@TU!f8lxmqm~WS zQ(7$`0ZNmo0l%I=Kh~+pwuVAqAg_?LVrJZQa@?NQWrdlNs@fnJVEI7XT=aI|JkE9mG&~L2 z1{XAD8pF^IN@N1oU)@C&q_mT8`zd%i4?>pjMMXm_s0NFeBFK7CkO}>rC@wJe2dXcB zoZ9VpYJ1`um7}4_jS0JJ5`N_4HLYnqOR`a3$avD@1h4;!k*+wDMsmP~PP`V;y))K6Zcv%5xcQvnv&uoQZfJKVj zBn7svFxO26>A*T$2ia+ewW@n1JTc4t3lI7&xaJHm=IKxap8@+vK)+A9=(8ApEVD9D z{qwT~j%=QF9~da%zzmPrA(@g@Dxe&}pSRR^co0;1gn70YAoT39vHyG+kPrSOSbAkt z5G-d0&2rK^SU3PFat!(qq6ePPgoI-p54>LuZ7d^e3-ELwKjBKy2#fBWQiwy8+=M}u zTKnDr5u{uwT)Y+vzeY(wXA4hY6MB~-4G9NX`2#ESxDZ}O2_TJ#wX2Z(C7_QgNPb6i zicpFU$%g9ylL?NQRJ~f+P{oNk3v1RYSFgGMMV$NM-`10%3~mIHWi?AM7`gAA3-7f%HEnMz&=pHYF<@L5scS_oe+cRgH- zQZ80#!SV6XuCma=>u`jUI|g6Av4I03V@binbdVd6anvPl0te?Djc^eh5$a(ACjj@b z_h!H|yYb#1kT#F5R^<7T_3+&7egJH!zuAV9hbVHo&;vyhz>)ZPwP60xvey>XZf8&V z6Ij9BS0vmRCpLldNwmuVcEA>eCszU+q#lw2n~*8|ELKj`2*i}owqv4f8wy>RhUVm< zwCAp{E+`!H;j#F@A!d&cUAOsk+Pr(~UK)*RTrb};_;MlwFwds~t~ zxgv-Ald_g$LE}_CAF}SE3Wl={mA{UdCgQY7lTo{dgSIYI1J@2H=D+3{Al4HKg0%)# zqAGNF@9-#jeqY9Fyp@uXTnB^W?5svCiv^q@o@b@osa~Omw6qH zfR(tYN)lApU>*>>tVYImm}Fl+y88yR*jx>0AXbh$pR9TT{h;T%u@4)0K$}STfNKO) zl7}M`_qNmdOF;URcvzRm$bi= zFp)rP=WyXoWZbT5*8gTF!uTIwz$mZ{(_$Mw7G#vNTwYjM#R7^QKTlbW5iv@IIIx{w zfdzQJxP{P>y9D-PV#mw{pZ*+C8nP!ek(*%f5?S@kIR(HFq#dW@LgW!2;ahU`Hbx`n zKXZAKMzd~W$g+)2i@oM6lLC6p5~lW$cIm#Ge`1ltzan9fbxj0gYp1YGLAY762NRRf zbATd)>Os8}UB*c4d8&HP�a4A1>U;I}TtV!VZm+pzH-=`(d!wQvlTucOU}{)vj*Q z0Q3}CyTZgx_|JpB8u1@N?Ba=Q_ILrmxA-oH2J?r#Mu<35l^4_8Hkq)^v?x@z*2h`*I9jM-y;QdpP+uW$2LBI>S%0F%5C zy^((Ke-iqQTdP4(7H9$zsd%aGcOaBGx9fvpJh}wbWzyF633xMe1w;F$f(_uxo~C5F zBKzK!oAVNA5)w(fL}0Mn*@)PJh#^tX2z?xv!V~kh!@^=(nGRqAQZ}G+yEIHeU^-IF zfUuOb_#;b@2-+o!nA&FacLi=B(O}z@Jsh~P~Ox`1YW)|0jv45(z8+^bv`QoJZ~VF%L&uWpo~Kq z?I2eWizwSXCmNIB$7{MnC%j}(Kmd+Ae)&oZ#LoU77eUBye{W3(LXf)rQ?v+9mK~V8 z3Ha6iW+?TG|)r&u3YU zOCRlWZTVOBvaK@0t)N6ehb2BuMPHUhGeF5Qgh*F3<q0}6`ydRgSRh-1YYmV1>`;$@iGAKeiUTqF{X!=^vfG_%04#rXXAAV2THPg(yG>$g z+R)S+84Dz29tZ8HP}BZVtoEVR;Ep2lr_ZcZ5J`nL!s@E6a=4VB!*d&NgG#s<-`83S zKPy^t0PfX3@3*mpp%6>UdkCMarTko#40k@2FK-POvo^1M3&Jtw#2L8P`0Q7uM%alc z;=p=vXgzd!pbqO{UCWplL_~L^OP}xc9?4MdrlCdm84sLA4Tve|D#AKe04f^MA`a$2KlQ;|qxc zXeZ^15n_z~wmQ?cHd|Tp?AC=RHf|8`!d|0~WWwh#6*>BY7q}NpxW;dC93b=>Gr@F8 z`MO=;DT$~r9EPt|UVA{hvanVIDB|_$)AMD&c?bY1`+4HcvfNOhX{6-I+2!+gAsn$C z-|;>-fWOvlx)*+q*Ur=fULPx7*bBc>w(f+{CqABsV;eC)PZ)10tRy-C(uJ$3vkLCz z_*@-!km5`3gyGv~7Oz4~eAgW_G1dbjIKuG13pdzNUv=82SbYEM&r*pB*~;}6}~1s zdGV(Mq#9qHz&FmfkImfH_B}*=F0S{G43m6r!(XrXM`7~Dx+`HyS+`eKSxawl&&@}( z%5zreO>DdiTm!Ue1Rs3wC@^fm&mAPrT{<+glq+;&f@r3WdQ1G;)3$zDZnDPZMbNEc z;&eF!-7Eq3fHOucD7gfuZU=xAqVHb}pg5KIDTzd+cKCD3jzBbo9r3;ZfOOl@Pavc( z1$r`J@rJMe3wgO`WLee{pytNj-7X;|^=0DFeoU$L=eyUAaD-Q6<%iZz3aawsa&NP< zc9&k_DIlYZi4d#VlG(K^O$p(im7%48FV;C}x-8M63hA@pU0=!n1F z&QekXeV)9&QLG(avhWgYhNCUNS_C&Wy;d?;2+CS*>|-!Ckcyj1NZf)lr`&i9f6%an z$$0o>LLVlMb2b7NcNArB=U%Wn8B7%(Xzm@_Zkj%y4!Ovv&dh|0>6y?>O~y(&&FZTO zgj#OOJ;l@fSv1C|NdFMoy;+pI{EK7%sUS$A@bH=0^8RbhqHkA6-g`D^m`jD%D&KoF zyHgFeu|2QV^sj>6_C|Mdq`91*Ee0S6jD zi{f3>+J6%~8ZD;4kHpB;(HMx;a!Up1(9_EPbdXd;pmE8EOsNNN+&fG6cs3TUGGLr} zw$oPqle;_`%LpcM)#p7-uxjy^0Kdso z)>)yjwVVH8iQ%cwRKf|N?6n4x=(QP%IFLlh>y06CM!PCDWJO-yhWZUlUzR*B@>*yC zP#-#-4JO0X_jH1n&~{+N1tWfW1Nd@}2Ebc>l`Pk!0~^eSnrne3`bzULfNfKh);bj- z)L1ZofiTaek*QMtTNqjcFCYrRQw4Dk`BZm)v@!*iocZ_Ku1BT35k!1ZQrX}XOIH3Y zZAtk9N5n>d|9%3L5fOv#*pL`_%sae=kQpo1OA7J!j{GQJ`Vo9gCB~~daSh_wM-5T- zMT5I3C^Y{7uAv?B<4t^wC_h{Y5wv@15V$2y9m?Fm-4a&Ai>3#uYRFb}3h%&8t2DGP zaL^fyVWZ72p$9p=_)^@g{JBy+Zn&J%syHs;Y?3qjv}_KFw&{GG6m6h^WLXPjpEkcA(VIljz75T#xHCRs<=LB5+HT4Pio|Q?G&H|(Gn|dhysKqq^*?Ep zuXPFyeO-r$?JJ+GM&V71QN@cHS`WorqGivMhx*!1eXF`-K%gVvTVynvM@AzMcJhn? zc`<3A544)FSs=v|Z0!LwCM`eBqM4}TuqjOo;fB4fFta*ufos(cpK~$#U1 z+5A#GmF7e1y^UG$qFbNKo521imDe#O&ow)F8CLi+_U%f)0_MWk&af4@)=0xu*k zb~?JTc6(oax~HyktMlorOg2$PP`Dz$MCMihoWI{LmM4UZI+=WfKrT<^x^k|~!FC1* z@t4{7_al?rk`(QJpns|G)OO{yUpBp(Q4GmYZUJHBe<(lb&dins-}G0H$C;?*4&-2g zo{Yk?OQ6nzC8O>|kO(@#81ThOHS@XAd$Qe+ouJR%B3+cq!T6DRK;j=FGggA82^0Y@y)@CiHNJy z3KqxAA27TRk~xc=2`x3z0TPqHl{B8aB~#W8E1K?Z3r!0#amtl@dP7m(TqQ3!`-1lx zU1LdLfaL1>IbZaPtr~QfN69`+Fj?{Q?fa=j+MGvoX)zxJ)8IdV|J5)dfx4bR-Ps@EL{@z07PE+D8WSwNy-O`@UuswY4b3TIm#mxf+x zal@(1;L-gQY?EaHNb;K(y2%(0&v;F(n0-|T>cm1iq z1-fJVbMHS>>H>Oy=bpYABC2WD*S`pZI9ekF^SQNsn=UIC<4xVB!a4+p%XDqx(!#Rm zi0Pr!?0PU?11H}|*@9HL1XI=r@ur^0WuYIVx-Mdb{ARp}r#Lv!LRLj$an4sUb=j3) zf<-l~KOZWf@woalF7LeluebG2IiSYtWtA-4U6ilFc0Kj0`SsP&&aefs$TcAuQ|F)u z^hTDLD~oTuA{l#;u!m*+j>a*S=-C4+k@>zOzeyaQNOU=m>0e=_IbW)fQUTf>IHL^B zp9M{&*M@G(8`-!hePz6+67`XH`p!YCZ&~NvD|~(lK=6+a`*L~%0~u0+0!cf`e@dw% zFZIwe*}BzuVF)R0Bl+gY|Oaec_BpX z5z$#W6MQuc4zBu_LjRHT+pkGbkyy%?14dwBj1RBa4RPQrBB8KP0x@`7MS+5dn|EYX zp~S|Oc5-~eh-<{({972EFn1B=;984M*j-0r+x(wBU?JrPYpf;aaPY9v{et3<{Q{+$V&m#Z$b?esy%_bb%5++Bs zp`KsPM~+Eed_v>usR%-1h~kPK($8l&6!m>f{N)wG|F~NF?Aik5U03coRnFCkV=1YL z6`C(9CGy=p0TpTfsj_EktrwQ<`zWd^tL%1lmp6{!RNP5pgDUdsL` zgl_0W$IcJ3Ztp-jhn$vnwWv6^%W8q0!h=`aX78Lb0*qHkU; ztoluf>QVtAYE!vprEDYk8p8IZQq>S%HuHKPfd1ZF&D|o1Uv;PPwEz*1H%8Tb^??Hi zic8hFsb~!eD(td({?${V;I0_CncY^ON=uRTrZrJJmAJ#>bUi(a&0(x|=AuGeWkWtC(Ovgo-crUO&Wl4H-IP^rhFY zdtTG?#v7*hW=xyy&jqSPEksvI)$bLLtsrGw`W#s7sHy%}NmfJJ-0m`cftRIIjj4RX zx(-J8#jCM=EZ7SJOD(&kluygUXYj47?hPyE&y&NcU1u)aaNti%U zqS5cIK(EHMk0uxo@mnflmmr)Nattg0-LtMg(p@X13CzC9>rn0seC@<#Ew-*_-Rub! znH_3yxpR)IZJYo(%x?c+!(RgRh}YMe?!6R4Y`yZ|E~yMes&?LCDkQ4?gmuu6Lo7Ud zu{=}33$e>w>JJ)U5%xjv=Zhs{wFQ>~RejXtBj47$ZJ^QQ8^=zDWidTSdkkE_2j2KH z(8=@MRjWq>RSYx{0r8PgTW&jvZ)eH?zur`+jx{M|*Y8av@=O207A>xC!SdH$kPP~L zb{JyIZ>2;~R_&ckux81Js5BPA*UeP9x@QZvQ@CAC4Eu~yoQWHnN`;p%X>FFuwbEh~ z$5^B!ikH9(b`UsMZvd)_sFSXA|LRNxs(Dj zzV)^ZgBF}}r;>))rj7ldVnHGizi$CN)vU4?;UcW@@a&gSG6cd^>i7Qv$J7zjxLlJ= zZM115n0ZnJ^~ZGBZS^z%SZUtJ#1Ees2J-@aydE|BgXRE7NG;;S_Lr1nAjW>xNsu`n z%J0X(&tZn~CkHP@)8f4%nm{uPS-srda0Q`)z7;(dW1^6k#S9dKhD2B90118E&W5tO zvfgv%-w^L?u)K7n2aWUBKrI@j(*}3GK~CAN9pA^a(S@)*;OM<*8JrYDBASZ1vciG$ zk_=O>!+u+~Gy(oryXB6#*&n>lgbBcL zq2z$NCX9hzA3xf?Ukn0%3p}b|XR@^x-NTFD0J=xAr!u!{!?I&NF%cdKN{0Vf&R{XM z^ClNVtS$C?mp7zc%Ia8Jl11^0rAF-(G_!=>g>RY{X-Q&kv+K4+HY*(aD}49&h04K)&u6{*7ugt0n3AG|M_;+wh_|<#F(?{v)w(gU)11E`BOt%g%KQT3 zLB9fOytCf&J!jJ>7KbG$P>$mqvont8=EULp8*)ZxUN3Ii_mF^86xogAHymwJjj#pi z*zr(+AU@6E2t~Ub6POsu`~KbxYPP=oTR6)#*&+1LyUceB=nMYEJchs9q3|+IZ!OtD zTw2c$>(|$X!N@$1-xI^@ zp}kS$@IE2=d`mpB?IJ1FtDrUF-8o+tl;-K4nui)`6VjgqK40#t$ouV3dM{om20Vk0 z_gaxt$U#r-RSCrG{Hf0+?%sXVLiiA*gpMx~YiO6nt={+9e(lRFwOolg{z^|i0)y64KT@VLw7 zVj66J#v@XD5KGn>J$}fcTZG_?50DVaJ;;JDW{H*f=dt8mz|DsxxOkK60tWq z%DX*0MNrlnnui4ZE;xTEOd=w#$FD=fT>H=zFCaqeDE&?Nocjv^xI_PFxFZy1ng4PlfAJ*%uEeXoJdGoic zY7(&=y(>29G}S<#?qpL&MH6)m6>8=Mn2>)4wU_>|$ytkQ5B#t>JCVHYa@IpVD!zZy z&#Xz6HAm&GU0=p*f|~npg*%W&)9pG~jHHqEbDG0cJcq>$??EH9mUCoL1zw~Jys4CJ+5fF zY3(wJ{V+()@k?Bu^}qQ&{LynV`sKuM3MAD~CWxqj2z#TtqAs7jJbh3_(cg-%kh6LU z;ns*dY6>}suMf1nvR@1C?}#UTvJqf9U3&|9uc~gbj3W?!Db!vXLdh1P&%cnRFGF}5 zE!to2vYsj6v0pK_4i7FC9I@WCinW7?sc`PTD2kI_Jj&08vpE(|Xs~uUYw+M+62kO< z!*oOF)riSkpdr(PX3+iVpLiO|ioabx2NMnVetz%r$VIiWcoKT{b&VRN7u?n+<99Xs zx7=o{j8J%gIvRA4w@$>0FcN+WtuPc_N20qtp%5>#&pHjs-T5~5!|h5>eM@9j(ty-T zm)U!h5h6oNxsT40Hkn%AVGzcFuQ!v;_rJ1>hLSiL9d8|tJdkUZOCHy5s|v;KL*Q}7 zW<+6|CC{6$Mdmv8J(P&G%StGrje{ybv+kK&n#YUY4Y=YPHduSkCnZ!?HE)isxNz6? zUnz*wJ4lee_OgqEWckK2?7_*^@-K68G@e@t!U{zB%u`|HCI|9^MCdk%AsoGVf1?!U z?Uamb!ia(^%5DdcDod?Bf2^@RB+qLr67N62vtm{XMpx|V$G12XweAv1O^WiUDZ)Zad+;e1YL5$N& z3eN`}GyKESu$_Eyag}-sK6q)Rf^dPc$az|L$}<-+D{<=m=l&_?^$FFif+7rF7 z==@~`@#io>u41g)ltUldZ^6|_$+$~$Hc=|^2>-f{dM^-au_flY`E!qoZoba^9G+wU z`*Yrs)JuCbXtac=E67>_Y9G8@Cl!*;c~RO{aCP8_uA8i~dDkNC8;=(F6us6H`Wo_Qx(tDoiZ_@%8RX{7-53*Yk>h@d%({h6=L>KAkfCi|h z{4ch)kkM_^c>AmCtxup@T^fSDD0}d`u}i!J@jm49v1Y~88V8^TM477j%bq7thl$6@ zFD|qSTr7X=E-jXiNtY4tv0910LGR`6bir9!A*=HY@V|scU{JQi{v=7>2TJ_Ee2X@a z>JJQ&cEdjNGlOSQLMAWdu{M>82=^bfUYu;xwnQG1@}ToepfxpU%xpC!?`jX~DsR=X+rkPL@!t8}eFX`8vB$4)*aV~d($_YJREO>J z7xETi6?R8~ixzV_wfG81QF~>+NQ{a_t0R`_?-R3Hgo8x+!tz{{In-n<3!w-*xjwU% zWR}?`HN@K(y*u^b;zLEWuOBEk6sy-q%7416jl7EZ5R<&`5l7NtFjGE|O}-=3NIKV9~Z z6pwmi5j*|$2To$~IhG?aF%*?YediE6Kfbrhs#QCNq+$WL z`nE$AvnWDe;^btR_>NGfg%5ouK+{x)*-_V2cCUnCjgoVuknXRf-&V-qw}Tbs6Tr5X1)#xzgJO4&ee-a{r>akA?jj@PPL)fU&9 z@*(X8swWugBy-ao3c4>`Dm<}tKtnJS7oPt?NqnjLU~cl+|H1y25ZH6mBc_Co?5$!V#a;rrN%xCTbqt(!RXQxuq_V9Hh`7afY{J&_-e> zhvyqfd5yt}O}}J~9+SgQPp9`Jyw;H2`qg!?T2y??M%1K5;9RN4MY^bO1zRKiuz;=M zS;6JEFWo|S!C}TdFM8&~gj3cMMMaNvaoc%2Z-nRz`;ht0&j58V}7B7t~B8--BPNe^3yu>lugQA_Rz6ioE_n>2!Y>SVSY?)0JYix#Lg zF;UlFV`>zw)TQeTD0rhy@*F92a<(r>5hetWo^Aa`zgo1lhK3MYe5v+t)3a>?oRsvv z)&ZAZ%vZA3TpOfkSn7TYW{Lrs9XGc4AhnTT;20^{V;PtYL3$qS&wU8CQrP5iq@NQc z>n>g`13_gP7DBRnbp=A^Bjs~BW)hq3kDEPxfrYDW^=8*D6gl{lb#oPC+XKeBR1c=a zc0x{ZaL;Esk#^1xs2;!!S@1?K^;@>I5P**1YfRuwr?si zsN9Ga>BMgjo!FPAR_`>JYs1=U-thm8fuKNSz%Qjyf{KxdBBx@kKP&izuiXAiIAVBnIK0-;=%8B5eHXn;MAGh0^Q31M}DlNS2L2VEhXPb*94W8D20 z9OYqs>koI@$TFm~bwv!?3E@3YMAE|Q(vQ<*y~q(q(K!}!8i}P?`0y$bhc7i&z$9PL zZa4F7%*A^2MpUY@g3G;RkCjh1&ry8MQ%s_doNJPw47#K7?2q8-p9J{0v7CYu-ou)zrHK3IIiSNRaJEhy@u(?r3Sn}>7+X@u+c`N3 z8a}1Oel$fqwCL*X$+DATt8lmx=DNFLA z_g3N^@O_2!2g^MMZwuhAH z=aPbV5KLWhCF@#d%DJEvr0oB-(%~;nI9J9)gEqcjqMdBPW|Gt{t_!(3z5cI^NP$L_ zsP(AChcY9Iuzc755aqN>HE1*12O9dAYTt0`)&=R&*LFSHkL+`D9eo z3v!&!j>wEwLi1yeYDEe%0nuw+)wSUewiBC;Im(2o!-mzngeqeabeeadpb9qUR-CK% zQLCjcF~UEY+IWoDL1AJ;!CB|c-8hNE z64%hy(Ec-@MyotZmTW~$LzjCj&~a4Px)LWVU{{&yqNPY%c2x4p)^Ehvt}9FD7#F2J z9SGTuu}g%Wbo)21xpH<+OJ4?`&N}26c({*&Y?$ccI_=$mMa$9`ERp`IZ630_R{X`Y zH4HO{Wf%XMtc}3Nxv^UXIf)LPLt<0>8)|o9SGD_&t4_;{8Uu`mzP*4dk~&Bu@QrE2 zSkpqvyWX+eTh$iEjbee*+UZo(7_;J`MvWA~l;ibh2Vf@HT8{>0Kc9q*cORElxj$Wf zYrH7;apt10!td0Z4Bu7FTN=3Q13(XHiSs*7$;3o$qf2HKJ9!(uR=jJxR2%mJelySr2^ z3yHyL~4%( zQRjg87cO0uz(f4c-zVw6-v7>agr2UT+LI&kjcO7{*! zU0OdW>U^(~cTq0i|30-3yp+BvPOjdl>w$W2{CJgwUbQXO_UV?AjZxp6(j!HMY|paX zObW`qwfOwC?m?OUityj{pO^2ci9J@itK^2A^F(qVvuVfXs+?Xq|MKvFo2`*mLz_O; z1RU_xtjRsb-~4Fs&LNed_ZQw$&Gl{N*}DuxM|12SXP$c<_ATmmw$fO!L?v%8Qe2I4QncFEp0sO+-fxT^T$0&xvyBu* F`+qM$N96zj literal 0 HcmV?d00001 diff --git a/ros/audio_common/sound_play/sounds/say-beep.wav b/ros/audio_common/sound_play/sounds/say-beep.wav new file mode 100644 index 0000000000000000000000000000000000000000..161c43c312bc0442869a0289f958d977a5aec578 GIT binary patch literal 4058 zcmZuzzl-BW6yAlvrps|$b2iWuA&k@yNh1Ol}hNuyREP@zVJ z!}{}ViX zzPY)%#lK(gZ}{)_=4V*GJAVZ~;r%a=e>;Ig?sXbP2Z>ofESj4%j% z-w%RNM2VDXmSt%wlLTr)2>9ce=)jXY3>eDH2s$y}zFKFE4p41De0-o?&n>>$^o9BmNhy)Xf$y1qV7$-%(67kr!hog~YTYi*OyS6>Q zwoS7wGD-7tL5#3eX(lCwMgGV(E#xW{L&qA9NA|=PLA-ux+Vi<>+jUVSn4`PGG>M2J zAzxmq2&En2S`i6eEsdooNY}+k}C?%EvYrxRw_Zi*tts?ef@u>=n*Ljkchq9iMYQ&}VTaWov+!-ZI-b-h2oACKo< zRb?TqCiaI&C^2jeUQQ9mvI=Ll^$^_;C*Nj+X|j;JP4kL^9`lOLZtv*jyx1-Tu zI`(3>YPN^B)9Ki}lxd31;vo=@g3Pe-QOhie@s1XS{nfw!^89V`Jd1p}+wR^^?Rj5s zGAVrQ7VHk39Zae~9MmH%16`>Unrdyz!NilQtK zER|(!rK?-_>25L}TT?9iOS3({?+?d%U8E5*bG*P4(8h5rb2?gC<?dw}}IBcpMiTNmZ=o1Ci#wjLZI!nS&^Xc8l8cyb37?(}Gdu?A1O;r`K5IzoX zKzkJh01Ho9sys+GC>izy@yW2uF4wJ|Wa6n=lt^|7)djYqG^n!S4 zFITqZtQ<@f$n|zpt=C9{&mkW~Y(O+D6`e`GA^7s5>j;gEd@J!ag@bh6)Jev-0NSg`r@k85(z#nYvuf zp7SGxEeg{`Vyd!Vv`7xks40(r8f(`v-pn@womZ)$4;!w}qFxS(;@@*7`P+@29#{LXT@SS*LhQ%Hsgz3l+}8( z*=)CX(22vx8{6J@oyB}=TefY_=8NSDr#2yIwXP~WE1XP->ZZQge72AWJ`=9H!j~AA zgP?pQ{wpDIIq-hRg!GScMFw=y;tJrrX?xN)n|^}75;RMSg1Hcy%RyfcI+(!HXH!YQ zi4DXjijOT9gja*MJvXk6s){;dC>O`08uKw%OQ1#cav=d?^U_?OSC|!o0qmxSUKSM* zqFRLodJiSaiBRSgg%nu>Pjc&Pgpa(=fN{!A5JH=D-Jk<& zhz1$766<1LG?ax!lpMO`RkB3aO0afp3V-n^h*mH}m7+#<5=vUR(C42XDWnRn6t5I? zHn8Ym3TK2UIR9OkT2D<|#mAZvNHy}an%1PDRyTki)>w@OQ;b=6`kF9yTb2Scl{-d6 z(Ts|6YUIUtG3#DO(C=R6Y$F^^2m_ioH{;OJFV>5@Uj!wfgLJi2;EyvPyC$d}CqP{j zSoL&XG$0NEdO2#_4=lE_77^5lxSc-BF~_JHkV)YR%?oou=$;a|meJgPVHj<&CZF!c zQedNEfTnXD_bMltHRKx5+2HE@ycP;d$WB+W^fuQLoln03R(N)^k$RjtjnLOfGh;?- yE4jI=j0Q_X#nTQNUjq%Yuii8FsAfPZQ8UC|NBPLf_rD&hm;OBbKa + + + + + diff --git a/ros/audio_common/sound_play/test/CMakeLists.txt b/ros/audio_common/sound_play/test/CMakeLists.txt new file mode 100644 index 0000000..8a6f8e8 --- /dev/null +++ b/ros/audio_common/sound_play/test/CMakeLists.txt @@ -0,0 +1,4 @@ +add_executable(test_sound_play test.cpp) +target_link_libraries(test_sound_play ${catkin_LIBRARIES}) +add_dependencies(test_sound_play sound_play_gencpp) +set_target_properties(test_sound_play PROPERTIES OUTPUT_NAME test) diff --git a/ros/audio_common/sound_play/test/test.cpp b/ros/audio_common/sound_play/test/test.cpp new file mode 100644 index 0000000..4c7f6dc --- /dev/null +++ b/ros/audio_common/sound_play/test/test.cpp @@ -0,0 +1,102 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include +#include + +void sleepok(int t, ros::NodeHandle &nh) +{ + if (nh.ok()) + sleep(t); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "sound_play_test"); + + ros::NodeHandle nh; + sound_play::SoundClient sc; + + sleepok(1, nh); + + while(nh.ok()) + { + sc.say("Hello world!"); + sleepok(2, nh); + + const char *str1 = "I am annoying."; + sc.repeat(str1); + sleepok(4, nh); + sc.stopSaying(str1); + + sc.playWave("/usr/share/xemacs21/xemacs-packages/etc/sounds/boing.wav"); + sleepok(2, nh); + + const char *str2 = "/usr/share/xemacs21/xemacs-packages/etc/sounds/piano-beep.wav"; + sc.startWave(str2); + sleepok(4, nh); + sc.stopWave(str2); + + sc.play(sound_play::SoundRequest::NEEDS_UNPLUGGING); + sleepok(2, nh); + + sc.start(sound_play::SoundRequest::BACKINGUP); + sleepok(4, nh); + sc.stop(sound_play::SoundRequest::BACKINGUP); + + sleepok(2, nh); + sound_play::Sound s1 = sc.waveSound("/usr/share/xemacs21/xemacs-packages/etc/sounds/boing.wav"); + s1.repeat(); + sleepok(1, nh); + s1.stop(); + + sleepok(2, nh); + sound_play::Sound s2 = sc.voiceSound("This is a really long sentence that will get cut off."); + s2.play(); + sleepok(1, nh); + s2.stop(); + + sleepok(2, nh); + sound_play::Sound s3 = sc.builtinSound(sound_play::SoundRequest::NEEDS_UNPLUGGING_BADLY); + s3.play(); + sleepok(1, nh); + s3.stop(); + + sleepok(2, nh); + sound_play::Sound s4 = sc.waveSoundFromPkg("sound_play", "sounds/BACKINGUP.ogg"); + s4.play(); + sleepok(1, nh); + s4.stop(); + } +} diff --git a/ros/image_common/camera_calibration_parsers/CHANGELOG.rst b/ros/image_common/camera_calibration_parsers/CHANGELOG.rst new file mode 100644 index 0000000..057d2e4 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/CHANGELOG.rst @@ -0,0 +1,76 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package camera_calibration_parsers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.12 (2017-01-29) +-------------------- +* Properly detect Boost Python 2 or 3 + This fixes `#59 `_ +* 1.11.11 +* update changelogs +* Contributors: Vincent Rabaud + +1.11.11 (2016-09-24) +-------------------- + +1.11.10 (2016-01-19) +-------------------- +* Add install target for python wrapper library +* Only link against needed Boost libraries + 9829b02 introduced a python dependency into find_package(Boost..) which + results in ${Boost_LIBRARIES} containing boost_python and such a + dependency to libpython at link time. With this patch we only link + against the needed libraries. +* Contributors: Jochen Sprickerhof, Vincent Rabaud + +1.11.9 (2016-01-17) +------------------- +* Add python wrapper for readCalibration. + Reads .ini or .yaml calibration file and returns camera name and sensor_msgs/cameraInfo. +* Use $catkin_EXPORTED_TARGETS +* Contributors: Jochen Sprickerhof, Markus Roth + +1.11.8 (2015-11-29) +------------------- +* Remove no-longer-neccessary flags to allow OS X to use 0.3 and 0.5 of yaml-cpp. +* remove buggy CMake message +* Contributors: Helen Oleynikova, Vincent Rabaud + +1.11.7 (2015-07-28) +------------------- +* fix `#39 `_ +* make sure test does not fail +* Contributors: Vincent Rabaud + +1.11.6 (2015-07-16) +------------------- +* [camera_calibration_parsers] Better error message when calib file can't be written +* add rosbash as a test dependency +* add a test dependency now that we have tests +* parse distortion of arbitraty length in INI + This fixes `#33 `_ +* add a test to parse INI calibration files with 5 or 8 D param +* Add yaml-cpp case for building on Android +* Contributors: Gary Servin, Isaac IY Saito, Vincent Rabaud + +1.11.5 (2015-05-14) +------------------- +* Fix catkin_make failure (due to yaml-cpp deps) for mac os +* Contributors: Yifei Zhang + +1.11.4 (2014-09-21) +------------------- +* fix bad yaml-cpp usage in certain conditions + fixes `#24 `_ +* Contributors: Vincent Rabaud + +1.11.3 (2014-05-19) +------------------- + +1.11.2 (2014-02-13 08:32:06 +0100) +----------------------------------- +* add a dependency on pkg-config to have it work on Indigo + +1.11.1 (2014-01-26 02:32:06 +0100) +----------------------------------- +* fix YAML CPP 0.5.x compatibility diff --git a/ros/image_common/camera_calibration_parsers/CMakeLists.txt b/ros/image_common/camera_calibration_parsers/CMakeLists.txt new file mode 100644 index 0000000..b70b853 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 2.8) +project(camera_calibration_parsers) + +find_package(catkin REQUIRED sensor_msgs rosconsole roscpp roscpp_serialization) + +find_package(PythonLibs REQUIRED) +if(PYTHONLIBS_VERSION_STRING VERSION_LESS 3) + find_package(Boost REQUIRED COMPONENTS filesystem python) +else() + find_package(Boost REQUIRED COMPONENTS filesystem python3) +endif() +include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS}) + +catkin_python_setup() + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS sensor_msgs +) + +find_package(PkgConfig) + + +if (ANDROID) + find_package(yaml-cpp) + add_definitions(-DHAVE_NEW_YAMLCPP) +else() + pkg_check_modules(YAML_CPP yaml-cpp) + if(${YAML_CPP_VERSION} VERSION_GREATER 0.5) + add_definitions(-DHAVE_NEW_YAMLCPP) + endif() + link_directories(${YAML_CPP_LIBRARY_DIRS}) +endif() +include_directories(${YAML_CPP_INCLUDE_DIRS}) + +# define the library +add_library(${PROJECT_NAME} + src/parse.cpp + src/parse_ini.cpp + src/parse_yml.cpp +) + +add_library(${PROJECT_NAME}_wrapper + src/parse_wrapper.cpp) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY}) +target_link_libraries(${PROJECT_NAME}_wrapper ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES}) + +# Don't prepend wrapper library name with lib and add to Python libs. +set_target_properties(${PROJECT_NAME}_wrapper PROPERTIES + PREFIX "" + LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} + ) + +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + +install( + TARGETS ${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + COMPONENT main +) +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# define the exe to convert +add_executable(convert src/convert.cpp) +target_link_libraries(convert ${PROJECT_NAME} ${rosconsole_LIBRARIES}) + +install( + TARGETS convert + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME}_wrapper + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + add_subdirectory(test) +endif() diff --git a/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse.h b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse.h new file mode 100644 index 0000000..d0adb86 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse.h @@ -0,0 +1,81 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef CAMERA_CALIBRATION_PARSERS_PARSE_H +#define CAMERA_CALIBRATION_PARSERS_PARSE_H + +#include +#include + +/// @todo: use stream-based API, so no read/parse distinction +namespace camera_calibration_parsers { + +/** + * \brief Write calibration parameters to a file. + * + * The file name extension (.yml, .yaml, or .ini) determines the output format. + * + * \param file_name File to write + * \param camera_name Name of the camera + * \param cam_info Camera parameters + */ +bool writeCalibration(const std::string& file_name, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Read calibration parameters from a file. + * + * The file may be YAML or INI format. + * + * \param file_name File to read + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool readCalibration(const std::string& file_name, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Parse calibration parameters from a string in memory. + * + * \param buffer Calibration string + * \param format Format of calibration string, "yml" or "ini" + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool parseCalibration(const std::string& buffer, const std::string& format, + std::string& camera_name, sensor_msgs::CameraInfo& cam_info); + +} //namespace camera_calibration_parsers + +#endif diff --git a/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.h b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.h new file mode 100644 index 0000000..f36399a --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.h @@ -0,0 +1,96 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef CAMERA_CALIBRATION_PARSERS_PARSE_INI_H +#define CAMERA_CALIBRATION_PARSERS_PARSE_INI_H + +#include +#include + +namespace camera_calibration_parsers { + +/** + * \brief Write calibration parameters to a file in INI format. + * + * \param out Output stream to write to + * \param camera_name Name of the camera + * \param cam_info Camera parameters + */ +bool writeCalibrationIni(std::ostream& out, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Read calibration parameters from an INI file. + * + * \param in Input stream to read from + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool readCalibrationIni(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Write calibration parameters to a file in INI format. + * + * \param file_name File to write + * \param camera_name Name of the camera + * \param cam_info Camera parameters + */ +bool writeCalibrationIni(const std::string& file_name, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Read calibration parameters from an INI file. + * + * \param file_name File to read + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool readCalibrationIni(const std::string& file_name, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Parse calibration parameters from a string in memory of INI format. + * + * \param buffer Calibration string + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool parseCalibrationIni(const std::string& buffer, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info); + +//bool readCalibrationIni + +} //namespace camera_calibration_parsers + +#endif diff --git a/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_wrapper.h b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_wrapper.h new file mode 100644 index 0000000..1a2fc88 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_wrapper.h @@ -0,0 +1,44 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* 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. +*********************************************************************/ + +#ifndef CAMERA_CALIBRATION_PARSERS_PARSE_WRAPPER_H +#define CAMERA_CALIBRATION_PARSERS_PARSE_WRAPPER_H + +#include +#include + +namespace camera_calibration_parsers { + +boost::python::tuple readCalibrationWrapper(const std::string& file_name); + +} //namespace camera_calibration_parsers + +#endif diff --git a/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.h b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.h new file mode 100644 index 0000000..289ad2f --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.h @@ -0,0 +1,86 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef CAMERA_CALIBRATION_PARSERS_PARSE_YML_H +#define CAMERA_CALIBRATION_PARSERS_PARSE_YML_H + +#include +#include +#include +#include + +namespace camera_calibration_parsers { + +/** + * \brief Write calibration parameters to a file in YAML format. + * + * \param out Output stream to write to + * \param camera_name Name of the camera + * \param cam_info Camera parameters + */ +bool writeCalibrationYml(std::ostream& out, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Read calibration parameters from a YAML file. + * + * \param in Input stream to read from + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Write calibration parameters to a file in YAML format. + * + * \param file_name File to write + * \param camera_name Name of the camera + * \param cam_info Camera parameters + */ +bool writeCalibrationYml(const std::string& file_name, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info); + +/** + * \brief Read calibration parameters from a YAML file. + * + * \param file_name File to read + * \param[out] camera_name Name of the camera + * \param[out] cam_info Camera parameters + */ +bool readCalibrationYml(const std::string& file_name, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info); + +} //namespace camera_calibration_parsers + +#endif diff --git a/ros/image_common/camera_calibration_parsers/mainpage.dox b/ros/image_common/camera_calibration_parsers/mainpage.dox new file mode 100644 index 0000000..1e3645c --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/mainpage.dox @@ -0,0 +1,15 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b camera_calibration_parsers contains routines for reading and writing +camera calibration parameters. Two human-readable file formats are +supported: YAML and the Videre INI format. + +\section codeapi Code API + +- camera_calibration_parsers::writeCalibration - Write parameters to a file +- camera_calibration_parsers::readCalibration - Read parameters from a file +- camera_calibration_parsers::parseCalibration - Read parameters from an in-memory buffer + +*/ diff --git a/ros/image_common/camera_calibration_parsers/package.xml b/ros/image_common/camera_calibration_parsers/package.xml new file mode 100644 index 0000000..29971de --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/package.xml @@ -0,0 +1,34 @@ + + camera_calibration_parsers + 1.11.12 + + camera_calibration_parsers contains routines for reading and writing camera calibration parameters. + + Patrick Mihelich + Jack O'Quin + Vincent Rabaud + BSD + + http://ros.org/wiki/camera_calibration_parsers + https://github.com/ros-perception/image_common/issues + https://github.com/ros-perception/image_common + + catkin + + boost + pkg-config + rosconsole + sensor_msgs + yaml-cpp + roscpp + roscpp_serialization + + boost + sensor_msgs + yaml-cpp + roscpp + roscpp_serialization + + rosbash + rosunit + diff --git a/ros/image_common/camera_calibration_parsers/setup.py b/ros/image_common/camera_calibration_parsers/setup.py new file mode 100644 index 0000000..07d1142 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/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=['camera_calibration_parsers'], + package_dir={'': 'src'}) + +setup(**setup_args) diff --git a/ros/image_common/camera_calibration_parsers/src/camera_calibration_parsers/__init__.py b/ros/image_common/camera_calibration_parsers/src/camera_calibration_parsers/__init__.py new file mode 100644 index 0000000..27fbc55 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/src/camera_calibration_parsers/__init__.py @@ -0,0 +1,17 @@ +from camera_calibration_parsers.camera_calibration_parsers_wrapper import __readCalibrationWrapper +from sensor_msgs.msg import CameraInfo + +def readCalibration(file_name): + """Read .ini or .yaml calibration file and return (camera name and cameraInfo message). + + @param file_name: camera calibration file name + @type file_name: str + @return: (camera name, cameraInfo message) or None on Error + @rtype: tuple(str, sensor_msgs.msg.CameraInfo | None + """ + ret, cn, ci = __readCalibrationWrapper(file_name) + if not ret: + return None + c = CameraInfo() + c.deserialize(ci) + return cn, c diff --git a/ros/image_common/camera_calibration_parsers/src/convert.cpp b/ros/image_common/camera_calibration_parsers/src/convert.cpp new file mode 100644 index 0000000..208eec8 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/src/convert.cpp @@ -0,0 +1,62 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "camera_calibration_parsers/parse.h" +#include +#include + +using namespace camera_calibration_parsers; + +int main(int argc, char** argv) +{ + if (argc < 3) { + printf("Usage: %s input.yml output.ini\n" + " %s input.ini output.yml\n", argv[0], argv[0]); + return 0; + } + + std::string name; + sensor_msgs::CameraInfo cam_info; + if (!readCalibration(argv[1], name, cam_info)) { + ROS_ERROR("Failed to load camera model from file %s", argv[1]); + return -1; + } + if (!writeCalibration(argv[2], name, cam_info)) { + ROS_ERROR("Failed to save camera model to file %s", argv[2]); + return -1; + } + + ROS_INFO("Saved %s", argv[2]); + return 0; +} diff --git a/ros/image_common/camera_calibration_parsers/src/parse.cpp b/ros/image_common/camera_calibration_parsers/src/parse.cpp new file mode 100644 index 0000000..7738239 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/src/parse.cpp @@ -0,0 +1,74 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "camera_calibration_parsers/parse.h" +#include "camera_calibration_parsers/parse_ini.h" +#include "camera_calibration_parsers/parse_yml.h" + +#include + +namespace camera_calibration_parsers { + +bool writeCalibration(const std::string& file_name, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info) +{ + if (boost::iends_with(file_name, ".ini")) + return writeCalibrationIni(file_name, camera_name, cam_info); + if (boost::iends_with(file_name, ".yml") || boost::iends_with(file_name, ".yaml")) + return writeCalibrationYml(file_name, camera_name, cam_info); + + return false; +} + +bool readCalibration(const std::string& file_name, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info) +{ + if (boost::iends_with(file_name, ".ini")) + return readCalibrationIni(file_name, camera_name, cam_info); + if (boost::iends_with(file_name, ".yml") || boost::iends_with(file_name, ".yaml")) + return readCalibrationYml(file_name, camera_name, cam_info); + + return false; +} + +bool parseCalibration(const std::string& buffer, const std::string& format, + std::string& camera_name, sensor_msgs::CameraInfo& cam_info) +{ + if (format != "ini") + return false; + + return parseCalibrationIni(buffer, camera_name, cam_info); +} + +} //namespace camera_calibration_parsers diff --git a/ros/image_common/camera_calibration_parsers/src/parse_ini.cpp b/ros/image_common/camera_calibration_parsers/src/parse_ini.cpp new file mode 100644 index 0000000..c955934 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/src/parse_ini.cpp @@ -0,0 +1,248 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "camera_calibration_parsers/parse_ini.h" +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace camera_calibration_parsers { + +/// @todo Move to new spirit +using namespace BOOST_SPIRIT_CLASSIC_NS; + +/// \cond + +struct SimpleMatrix +{ + int rows; + int cols; + const double* data; + + SimpleMatrix(int rows, int cols, const double* data) + : rows(rows), cols(cols), data(data) + {} +}; + +std::ostream& operator << (std::ostream& out, const SimpleMatrix& m) +{ + for (int i = 0; i < m.rows; ++i) { + for (int j = 0; j < m.cols; ++j) { + out << m.data[m.cols*i+j] << " "; + } + out << std::endl; + } + return out; +} + +/// \endcond + +bool writeCalibrationIni(std::ostream& out, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info) +{ + // Videre INI format is legacy, only supports plumb bob distortion model. + if (cam_info.distortion_model != sensor_msgs::distortion_models::PLUMB_BOB || + cam_info.D.size() != 5) { + ROS_ERROR("Videre INI format can only save calibrations using the plumb bob distortion model. " + "Use the YAML format instead.\n" + "\tdistortion_model = '%s', expected '%s'\n" + "\tD.size() = %d, expected 5", cam_info.distortion_model.c_str(), + sensor_msgs::distortion_models::PLUMB_BOB.c_str(), (int)cam_info.D.size()); + return false; + } + + out.precision(5); + out << std::fixed; + + out << "# Camera intrinsics\n\n"; + /// @todo time? + out << "[image]\n\n"; + out << "width\n" << cam_info.width << "\n\n"; + out << "height\n" << cam_info.height << "\n\n"; + out << "[" << camera_name << "]\n\n"; + + out << "camera matrix\n" << SimpleMatrix(3, 3, &cam_info.K[0]); + out << "\ndistortion\n" << SimpleMatrix(1, 5, &cam_info.D[0]); + out << "\n\nrectification\n" << SimpleMatrix(3, 3, &cam_info.R[0]); + out << "\nprojection\n" << SimpleMatrix(3, 4, &cam_info.P[0]); + + return true; +} + +bool writeCalibrationIni(const std::string& file_name, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info) +{ + boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path()); + if (!dir.empty() && !boost::filesystem::exists(dir) && + !boost::filesystem::create_directories(dir)){ + ROS_ERROR("Unable to create directory for camera calibration file [%s]", dir.c_str()); + } + std::ofstream out(file_name.c_str()); + if (!out.is_open()) + { + ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str()); + return false; + } + return writeCalibrationIni(out, camera_name, cam_info); +} + +/// \cond +// Semantic action to store a sequence of values in an array +template +struct ArrayAssignActor +{ + ArrayAssignActor(T* start) + : ptr_(start) + {} + + void operator()(T val) const + { + *ptr_++ = val; + } + + mutable T* ptr_; +}; + +// Semantic action generator +template +ArrayAssignActor array_assign_a(T* start) +{ + return ArrayAssignActor(start); +} + +template +bool parseCalibrationIniRange(Iterator first, Iterator last, + std::string& camera_name, sensor_msgs::CameraInfo& cam_info) +{ + cam_info.D.clear(); + + // We don't actually use the [externals] info, but it's part of the format + bool have_externals = false; + double trans[3], rot[3]; + + /// @todo separate grammar out into separate function + + // Image section (width, height) + BOOST_AUTO(image, + str_p("[image]") + >> "width" + >> uint_p[assign_a(cam_info.width)] + >> "height" + >> uint_p[assign_a(cam_info.height)] + ); + + // Optional externals section + BOOST_AUTO(externals, + str_p("[externals]") + >> "translation" + >> repeat_p(3)[real_p[array_assign_a(trans)]] + >> "rotation" + >> repeat_p(3)[real_p[array_assign_a(rot)]] + ); + + // Parser to save name of camera section + BOOST_AUTO(name, confix_p('[', (*anychar_p)[assign_a(camera_name)], ']')); + + // Camera section (intrinsics) + BOOST_AUTO(camera, + name + >> "camera matrix" + >> repeat_p(9)[real_p[array_assign_a(&cam_info.K[0])]] + >> "distortion" + >> *(real_p[push_back_a(cam_info.D)]) + >> "rectification" + >> repeat_p(9)[real_p[array_assign_a(&cam_info.R[0])]] + >> "projection" + >> repeat_p(12)[real_p[array_assign_a(&cam_info.P[0])]] + ); + + // Full grammar + BOOST_AUTO(ini_grammar, + image + >> !externals[assign_a(have_externals, true)] + >> camera); + + // Skip whitespace and line comments + BOOST_AUTO(skip, space_p | comment_p('#')); + + parse_info info = parse(first, last, ini_grammar, skip); + + // Figure out the distortion model + if (cam_info.D.size() == 5) + cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + else if (cam_info.D.size() == 8) + cam_info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL; + + return info.hit; +} +/// \endcond + +bool readCalibrationIni(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info) +{ + std::istream_iterator first(in), last; + return parseCalibrationIniRange(first, last, camera_name, cam_info); +} + +bool readCalibrationIni(const std::string& file_name, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info) +{ + typedef file_iterator Iterator; + + Iterator first(file_name); + if (!first) { + ROS_INFO("Unable to open camera calibration file [%s]", file_name.c_str()); + return false; + } + Iterator last = first.make_end(); + + return parseCalibrationIniRange(first, last, camera_name, cam_info); +} + +bool parseCalibrationIni(const std::string& buffer, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info) +{ + return parseCalibrationIniRange(buffer.begin(), buffer.end(), camera_name, cam_info); +} + +} //namespace camera_calibration_parsers diff --git a/ros/image_common/camera_calibration_parsers/src/parse_wrapper.cpp b/ros/image_common/camera_calibration_parsers/src/parse_wrapper.cpp new file mode 100644 index 0000000..6a99faa --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/src/parse_wrapper.cpp @@ -0,0 +1,74 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* 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. +*********************************************************************/ + +#include "camera_calibration_parsers/parse.h" +#include "camera_calibration_parsers/parse_wrapper.h" + +#include +#include + +namespace camera_calibration_parsers { + +/* Write a ROS message into a serialized string. + * @from https://github.com/galou/python_bindings_tutorial/blob/master/src/add_two_ints_wrapper.cpp#L27 +*/ +template +std::string to_python(const M& msg) +{ + size_t serial_size = ros::serialization::serializationLength(msg); + boost::shared_array buffer(new uint8_t[serial_size]); + ros::serialization::OStream stream(buffer.get(), serial_size); + ros::serialization::serialize(stream, msg); + std::string str_msg; + str_msg.reserve(serial_size); + for (size_t i = 0; i < serial_size; ++i) + { + str_msg.push_back(buffer[i]); + } + return str_msg; +} + +// Wrapper for readCalibration() +boost::python::tuple readCalibrationWrapper(const std::string& file_name) +{ + std::string camera_name; + sensor_msgs::CameraInfo camera_info; + bool result = readCalibration(file_name, camera_name, camera_info); + std::string cam_info = to_python(camera_info); + return boost::python::make_tuple(result, camera_name, cam_info); +} + +BOOST_PYTHON_MODULE(camera_calibration_parsers_wrapper) +{ + boost::python::def("__readCalibrationWrapper", readCalibrationWrapper, boost::python::args("file_name"), ""); +} + +} //namespace camera_calibration_parsers diff --git a/ros/image_common/camera_calibration_parsers/src/parse_yml.cpp b/ros/image_common/camera_calibration_parsers/src/parse_yml.cpp new file mode 100644 index 0000000..c07d146 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/src/parse_yml.cpp @@ -0,0 +1,240 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "camera_calibration_parsers/parse_yml.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace camera_calibration_parsers { + +/// \cond + +static const char CAM_YML_NAME[] = "camera_name"; +static const char WIDTH_YML_NAME[] = "image_width"; +static const char HEIGHT_YML_NAME[] = "image_height"; +static const char K_YML_NAME[] = "camera_matrix"; +static const char D_YML_NAME[] = "distortion_coefficients"; +static const char R_YML_NAME[] = "rectification_matrix"; +static const char P_YML_NAME[] = "projection_matrix"; +static const char DMODEL_YML_NAME[] = "distortion_model"; + +struct SimpleMatrix +{ + int rows; + int cols; + double* data; + + SimpleMatrix(int rows, int cols, double* data) + : rows(rows), cols(cols), data(data) + {} +}; + +YAML::Emitter& operator << (YAML::Emitter& out, const SimpleMatrix& m) +{ + out << YAML::BeginMap; + out << YAML::Key << "rows" << YAML::Value << m.rows; + out << YAML::Key << "cols" << YAML::Value << m.cols; + //out << YAML::Key << "dt" << YAML::Value << "d"; // OpenCV data type specifier + out << YAML::Key << "data" << YAML::Value; + out << YAML::Flow; + out << YAML::BeginSeq; + for (int i = 0; i < m.rows*m.cols; ++i) + out << m.data[i]; + out << YAML::EndSeq; + out << YAML::EndMap; + return out; +} + +#ifdef HAVE_NEW_YAMLCPP +template +void operator >> (const YAML::Node& node, T& i) +{ + i = node.as(); +} +#endif + +void operator >> (const YAML::Node& node, SimpleMatrix& m) +{ + int rows, cols; + node["rows"] >> rows; + assert(rows == m.rows); + node["cols"] >> cols; + assert(cols == m.cols); + const YAML::Node& data = node["data"]; + for (int i = 0; i < rows*cols; ++i) + data[i] >> m.data[i]; +} + +/// \endcond + +bool writeCalibrationYml(std::ostream& out, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info) +{ + YAML::Emitter emitter; + emitter << YAML::BeginMap; + +#if 0 + // Calibration time + /// @todo Emitting the time breaks yaml-cpp on reading for some reason + time_t raw_time; + time( &raw_time ); + emitter << YAML::Key << "calibration_time"; + emitter << YAML::Value << asctime(localtime(&raw_time)); +#endif + + // Image dimensions + emitter << YAML::Key << WIDTH_YML_NAME << YAML::Value << (int)cam_info.width; + emitter << YAML::Key << HEIGHT_YML_NAME << YAML::Value << (int)cam_info.height; + + // Camera name and intrinsics + emitter << YAML::Key << CAM_YML_NAME << YAML::Value << camera_name; + emitter << YAML::Key << K_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast(&cam_info.K[0])); + emitter << YAML::Key << DMODEL_YML_NAME << YAML::Value << cam_info.distortion_model; + emitter << YAML::Key << D_YML_NAME << YAML::Value << SimpleMatrix(1, cam_info.D.size(), + const_cast(&cam_info.D[0])); + emitter << YAML::Key << R_YML_NAME << YAML::Value << SimpleMatrix(3, 3, const_cast(&cam_info.R[0])); + emitter << YAML::Key << P_YML_NAME << YAML::Value << SimpleMatrix(3, 4, const_cast(&cam_info.P[0])); + + emitter << YAML::EndMap; + + out << emitter.c_str(); + return true; +} + +bool writeCalibrationYml(const std::string& file_name, const std::string& camera_name, + const sensor_msgs::CameraInfo& cam_info) +{ + boost::filesystem::path dir(boost::filesystem::path(file_name).parent_path()); + if (!dir.empty() && !boost::filesystem::exists(dir) && + !boost::filesystem::create_directories(dir)){ + ROS_ERROR("Unable to create directory for camera calibration file [%s]", dir.c_str()); + } + std::ofstream out(file_name.c_str()); + if (!out.is_open()) + { + ROS_ERROR("Unable to open camera calibration file [%s] for writing", file_name.c_str()); + return false; + } + return writeCalibrationYml(out, camera_name, cam_info); +} + +bool readCalibrationYml(std::istream& in, std::string& camera_name, sensor_msgs::CameraInfo& cam_info) +{ + try { +#ifdef HAVE_NEW_YAMLCPP + YAML::Node doc = YAML::Load(in); + + if (doc[CAM_YML_NAME]) + doc[CAM_YML_NAME] >> camera_name; + else + camera_name = "unknown"; +#else + YAML::Parser parser(in); + if (!parser) { + ROS_ERROR("Unable to create YAML parser for camera calibration"); + return false; + } + YAML::Node doc; + parser.GetNextDocument(doc); + + if (const YAML::Node* name_node = doc.FindValue(CAM_YML_NAME)) + *name_node >> camera_name; + else + camera_name = "unknown"; +#endif + + doc[WIDTH_YML_NAME] >> cam_info.width; + doc[HEIGHT_YML_NAME] >> cam_info.height; + + // Read in fixed-size matrices + SimpleMatrix K_(3, 3, &cam_info.K[0]); + doc[K_YML_NAME] >> K_; + SimpleMatrix R_(3, 3, &cam_info.R[0]); + doc[R_YML_NAME] >> R_; + SimpleMatrix P_(3, 4, &cam_info.P[0]); + doc[P_YML_NAME] >> P_; + + // Different distortion models may have different numbers of parameters +#ifdef HAVE_NEW_YAMLCPP + if (doc[DMODEL_YML_NAME]) { + doc[DMODEL_YML_NAME] >> cam_info.distortion_model; + } +#else + if (const YAML::Node* model_node = doc.FindValue(DMODEL_YML_NAME)) { + *model_node >> cam_info.distortion_model; + } +#endif + else { + // Assume plumb bob for backwards compatibility + cam_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + ROS_WARN("Camera calibration file did not specify distortion model, assuming plumb bob"); + } + const YAML::Node& D_node = doc[D_YML_NAME]; + int D_rows, D_cols; + D_node["rows"] >> D_rows; + D_node["cols"] >> D_cols; + const YAML::Node& D_data = D_node["data"]; + cam_info.D.resize(D_rows*D_cols); + for (int i = 0; i < D_rows*D_cols; ++i) + D_data[i] >> cam_info.D[i]; + + return true; + } + catch (YAML::Exception& e) { + ROS_ERROR("Exception parsing YAML camera calibration:\n%s", e.what()); + return false; + } +} + +bool readCalibrationYml(const std::string& file_name, std::string& camera_name, + sensor_msgs::CameraInfo& cam_info) +{ + std::ifstream fin(file_name.c_str()); + if (!fin.good()) { + ROS_INFO("Unable to open camera calibration file [%s]", file_name.c_str()); + return false; + } + bool success = readCalibrationYml(fin, camera_name, cam_info); + if (!success) + ROS_ERROR("Failed to parse camera calibration from file [%s]", file_name.c_str()); + return success; +} + +} //namespace camera_calibration_parsers diff --git a/ros/image_common/camera_calibration_parsers/test/CMakeLists.txt b/ros/image_common/camera_calibration_parsers/test/CMakeLists.txt new file mode 100644 index 0000000..3dcc6a9 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/test/CMakeLists.txt @@ -0,0 +1,3 @@ +file(COPY calib5.ini calib8.ini DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test) + +catkin_add_nosetests(parser.py) diff --git a/ros/image_common/camera_calibration_parsers/test/calib5.ini b/ros/image_common/camera_calibration_parsers/test/calib5.ini new file mode 100644 index 0000000..b6eecba --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/test/calib5.ini @@ -0,0 +1,27 @@ +[image] + +width +640 + +height +480 + +[mono_left] + +camera matrix +369.344588 0.000000 320.739078 +0.000000 367.154330 203.592450 +0.000000 0.000000 1.000000 + +distortion +0.189544 -0.018229 -0.000630 0.000054 -0.000212 + +rectification +1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 0.000000 1.000000 + +projection +262.927429 0.000000 320.984481 0.000000 +0.000000 302.056213 188.592437 0.000000 +0.000000 0.000000 1.000000 0.000000 diff --git a/ros/image_common/camera_calibration_parsers/test/calib8.ini b/ros/image_common/camera_calibration_parsers/test/calib8.ini new file mode 100644 index 0000000..71e44a8 --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/test/calib8.ini @@ -0,0 +1,27 @@ +[image] + +width +640 + +height +480 + +[mono_left] + +camera matrix +369.344588 0.000000 320.739078 +0.000000 367.154330 203.592450 +0.000000 0.000000 1.000000 + +distortion +0.189544 -0.018229 -0.000630 0.000054 -0.000212 0.543582 -0.027892 0.000000 + +rectification +1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 0.000000 1.000000 + +projection +262.927429 0.000000 320.984481 0.000000 +0.000000 302.056213 188.592437 0.000000 +0.000000 0.000000 1.000000 0.000000 diff --git a/ros/image_common/camera_calibration_parsers/test/parser.py b/ros/image_common/camera_calibration_parsers/test/parser.py new file mode 100644 index 0000000..2d0af4c --- /dev/null +++ b/ros/image_common/camera_calibration_parsers/test/parser.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +# +# 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 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 rosunit +import subprocess +import unittest +import os +from camera_calibration_parsers import readCalibration + +class TestParser(unittest.TestCase): + def test_ini(self): + for files in [('calib5.ini', 'calib5.yaml'), ('calib8.ini', 'calib8.yaml')]: + for dir in [ '', './']: + p = subprocess.Popen('rosrun camera_calibration_parsers convert $(rospack find camera_calibration_parsers)/test/%s %s%s' % (files[0], dir, files[1]), shell=True, stderr=subprocess.PIPE) + out, err = p.communicate() + self.assertEqual(err, '') + + def test_readCalibration(self): + script_dir = os.path.dirname(os.path.realpath(__file__)) + camera_name, camera_info = readCalibration(os.path.join(script_dir, 'calib5.ini')) + self.assertEqual(camera_name, 'mono_left') + self.assertEqual(camera_info.height, 480) + self.assertEqual(camera_info.width, 640) + self.assertEqual(camera_info.P[0], 262.927429) + + camera_name, camera_info = readCalibration(os.path.join(script_dir, 'calib8.ini')) + self.assertEqual(camera_info.distortion_model, 'rational_polynomial') + +if __name__ == '__main__': + rosunit.unitrun('camera_calibration_parsers', 'parser', TestParser) diff --git a/ros/image_common/camera_info_manager/CHANGELOG.rst b/ros/image_common/camera_info_manager/CHANGELOG.rst new file mode 100644 index 0000000..04d8201 --- /dev/null +++ b/ros/image_common/camera_info_manager/CHANGELOG.rst @@ -0,0 +1,210 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package camera_info_manager +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.12 (2017-01-29) +-------------------- +* 1.11.11 +* update changelogs +* Return empty CameraInfo when !ros::ok() +* Contributors: Enrique Fernandez, Vincent Rabaud + +1.11.11 (2016-09-24) +-------------------- +* Return empty CameraInfo when !ros::ok() +* Contributors: Enrique Fernandez + +1.11.10 (2016-01-19) +-------------------- + +1.11.9 (2016-01-17) +------------------- + +1.11.8 (2015-11-29) +------------------- +* fix compilation on Fedora, fixes `#42 `_ +* Contributors: Vincent Rabaud + +1.11.7 (2015-07-28) +------------------- + +1.11.6 (2015-07-16) +------------------- +* simplify target_link_libraries + That should fix `#35 `_ +* Contributors: Vincent Rabaud + +1.11.5 (2015-05-14) +------------------- + +1.11.4 (2014-09-21) +------------------- + +1.11.3 (2014-05-19) +------------------- +* Add public member function to manually set camera info (`#19 + `_) +* make rostest in CMakeLists optional (`ros/rosdistro#3010 + `_) +* Contributors: Jack O'Quin, Jonathan Bohren, Lukas Bulwahn + +1.11.2 (2014-02-13) +------------------- + +1.11.1 (2014-01-26 02:33) +------------------------- +* check for CATKIN_ENABLE_TESTING +* Contributors: Lukas Bulwahn + +1.11.0 (2013-07-20 12:23) +------------------------- + +1.10.5 (2014-01-26 02:34) +------------------------- + +1.10.4 (2013-07-20 11:42) +------------------------- +* add Jack as maintainer +* Contributors: Vincent Rabaud + +1.10.3 (2013-02-21 05:33) +------------------------- +* add gtest libraries linkage +* Contributors: Vincent Rabaud + +1.10.2 (2013-02-21 04:48) +------------------------- +* fix the rostest dependency +* Contributors: Vincent Rabaud + +1.10.1 (2013-02-21 04:16) +------------------------- +* fix catkin gtest and rostest problem +* fix unit test dependencies +* Removed duplicated test dependancy + Test dependencies should never duplicate build or run dependencies. +* Contributors: Aaron Blasdel, Jack O'Quin + +1.10.0 (2013-01-13) +------------------- +* fix the urls +* Contributors: Vincent Rabaud + +1.9.22 (2012-12-16) +------------------- + +1.9.21 (2012-12-14) +------------------- +* Updated package.xml file(s) to handle new catkin buildtool_depend + requirement +* Contributors: mirzashah + +1.9.20 (2012-12-04) +------------------- + +1.9.19 (2012-11-08) +------------------- + +1.9.18 (2012-11-06) +------------------- +* remove the brief attribute +* Contributors: Vincent Rabaud + +1.9.17 (2012-10-30 19:32) +------------------------- + +1.9.16 (2012-10-30 09:10) +------------------------- + +1.9.15 (2012-10-13 08:43) +------------------------- +* fix bad folder/libraries +* Contributors: Vincent Rabaud + +1.9.14 (2012-10-13 01:07) +------------------------- + +1.9.13 (2012-10-06) +------------------- + +1.9.12 (2012-10-04) +------------------- + +1.9.11 (2012-10-02 02:56) +------------------------- +* add missing rostest dependency +* Contributors: Vincent Rabaud + +1.9.10 (2012-10-02 02:42) +------------------------- +* fix bad dependency +* Contributors: Vincent Rabaud + +1.9.9 (2012-10-01) +------------------ +* fix dependencies +* Contributors: Vincent Rabaud + +1.9.8 (2012-09-30) +------------------ +* add catkin as a dependency +* comply to the catkin API +* Contributors: Vincent Rabaud + +1.9.7 (2012-09-18 11:39) +------------------------ +* add missing linkage +* Contributors: Vincent Rabaud + +1.9.6 (2012-09-18 11:07) +------------------------ + +1.9.5 (2012-09-13) +------------------ +* install the include directories +* Contributors: Vincent Rabaud + +1.9.4 (2012-09-12 23:37) +------------------------ + +1.9.3 (2012-09-12 20:44) +------------------------ + +1.9.2 (2012-09-10) +------------------ +* fix build issues +* Contributors: Vincent Rabaud + +1.9.1 (2012-09-07 15:33) +------------------------ +* make the libraries public +* Contributors: Vincent Rabaud + +1.9.0 (2012-09-07 13:03) +------------------------ +* API documentation review update +* suppress misleading camera_info_manager error messages [`#5273 + `_] +* remove deprecated global CameraInfoManager symbol for Fuerte (`#4971 + `_) +* Revert to using boost::mutex, not boost::recursive_mutex. +* Hack saveCalibrationFile() to stat() the containing directory and + attempt to create it if necessary. Test for this case. +* Reload camera info when camera name changes. +* Implement most new Electric API changes, with test cases. +* Add ${ROS_HOME} expansion, with unit test cases. + Do not use "$$" for a single '$', look for "${" instead. +* Use case-insensitive comparisons for parsing URL tags (`#4761 + `_). + Add unit test cases to cover this. Add unit test case for camera + name containing video mode. +* add test for resolving an empty URL +* Deprecate use of global CameraInfoManager symbol in E-turtle (`#4786 + `_). + Modify unit tests accordingly. +* provide camera_info_manager namespace, fixes `#4760 + `_ +* Add support for "package://" URLs. +* Fixed tests to work with new CameraInfo. +* Moved image_common from camera_drivers. +* Contributors: Vincent Rabaud, blaise, Jack O'Quin, mihelich diff --git a/ros/image_common/camera_info_manager/CMakeLists.txt b/ros/image_common/camera_info_manager/CMakeLists.txt new file mode 100644 index 0000000..bcc873f --- /dev/null +++ b/ros/image_common/camera_info_manager/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8) +project(camera_info_manager) + +find_package(catkin REQUIRED COMPONENTS camera_calibration_parsers image_transport roscpp roslib sensor_msgs) + +find_package(Boost) +catkin_package(INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS camera_calibration_parsers + DEPENDS Boost roscpp sensor_msgs +) + +include_directories(${catkin_INCLUDE_DIRS}) +include_directories(include) + +# add a library +add_library(${PROJECT_NAME} src/camera_info_manager.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + COMPONENT main +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest) + # Unit test uses gtest, but needs rostest to create a ROS environment. + # Hence, it must be created as a normal executable, not using + # catkin_add_gtest() which runs an additional test without rostest. + add_executable(unit_test tests/unit_test.cpp) + target_link_libraries(unit_test ${PROJECT_NAME} ${GTEST_LIBRARIES} ${catkin_LIBRARIES}) + + add_rostest(tests/unit_test.test DEPENDENCIES unit_test) +endif() diff --git a/ros/image_common/camera_info_manager/include/camera_info_manager/camera_info_manager.h b/ros/image_common/camera_info_manager/include/camera_info_manager/camera_info_manager.h new file mode 100644 index 0000000..56bb5d9 --- /dev/null +++ b/ros/image_common/camera_info_manager/include/camera_info_manager/camera_info_manager.h @@ -0,0 +1,243 @@ +/* -*- mode: C++ -*- */ +/* $Id$ */ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010-2012 Jack O'Quin +* 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 author nor other 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. +*********************************************************************/ + +#ifndef _CAMERA_INFO_MANAGER_H_ +#define _CAMERA_INFO_MANAGER_H_ + +#include +#include +#include +#include + +/** @file + + @brief CameraInfo Manager interface + + @author Jack O'Quin + */ + +namespace camera_info_manager +{ + +/** @brief CameraInfo Manager class + + Provides CameraInfo, handles the sensor_msgs/SetCameraInfo service + requests, saves and restores sensor_msgs/CameraInfo data. + + @par ROS Service + + - @b set_camera_info (sensor_msgs/SetCameraInfo) stores + calibration information + + Typically, these service requests are made by a calibration + package, such as: + + - http://www.ros.org/wiki/camera_calibration + + The calling node @em must invoke ros::spin() or ros::spinOnce() in + some thread, so CameraInfoManager can handle arriving service + requests. + + @par Camera Name + + The device driver sets a camera name via the + CameraInfoManager::CameraInfoManager constructor or the + setCameraName() method. This name is written when CameraInfo is + saved, and checked when data are loaded, with a warning logged if + the name read does not match. + + Syntax: a camera name contains any combination of alphabetic, + numeric and '_' characters. Case is significant. + + Camera drivers may use any syntactically valid name they please. + Where possible, it is best for the name to be unique to the + device, such as a GUID, or the make, model and serial number. Any + parameters that affect calibration, such as resolution, focus, + zoom, etc., may also be included in the name, uniquely identifying + each CameraInfo file. + + Beginning with Electric Emys, the camera name can be resolved as + part of the URL, allowing direct access to device-specific + calibration information. + + @par Uniform Resource Locator + + The location for getting and saving calibration data is expressed + by Uniform Resource Locator. The driver defines a URL via the + CameraInfoManager::CameraInfoManager constructor or the + loadCameraInfo() method. Many drivers provide a @c + ~camera_info_url parameter so users may customize this URL, but + that is handled outside this class. + + Typically, cameras store calibration information in a file, which + can be in any format supported by @c camera_calibration_parsers. + Currently, that includes YAML and Videre INI files, identified by + their .yaml or .ini extensions as shown in the examples. These + file formats are described here: + + - http://www.ros.org/wiki/camera_calibration_parsers#File_formats + + Example URL syntax: + + - file:///full/path/to/local/file.yaml + - file:///full/path/to/videre/file.ini + - package://camera_info_manager/tests/test_calibration.yaml + - package://ros_package_name/calibrations/camera3.yaml + + The @c file: URL specifies a full path name in the local system. + The @c package: URL is handled the same as @c file:, except the + path name is resolved relative to the location of the named ROS + package, which @em must be reachable via @c $ROS_PACKAGE_PATH. + + Beginning with Electric Emys, the URL may contain substitution + variables delimited by ${...}, including: + + - @c ${NAME} resolved to the current camera name defined by the + device driver. + - @c ${ROS_HOME} resolved to the @c $ROS_HOME environment variable + if defined, ~/.ros if not. + + Resolution is done in a single pass through the URL string. + Variable values containing substitutable strings are not resolved + recursively. Unrecognized variable names are treated literally + with no substitution, but an error is logged. + + Examples with variable substitution: + + - package://my_cameras/calibrations/${NAME}.yaml + - file://${ROS_HOME}/camera_info/left_front_camera.yaml + + In C-turtle and Diamondback, if the URL was empty, no calibration + data were loaded, and any data provided via `set_camera_info` + would be stored in: + + - file:///tmp/calibration_${NAME}.yaml + + Beginning in Electric, the default URL changed to: + + - file://${ROS_HOME}/camera_info/${NAME}.yaml. + + If that file exists, its contents are used. Any new calibration + will be stored there, missing parent directories being created if + necessary and possible. + + @par Loading Calibration Data + + Prior to Fuerte, calibration information was loaded in the + constructor, and again each time the URL or camera name was + updated. This frequently caused logging of confusing and + misleading error messages. + + Beginning in Fuerte, camera_info_manager loads nothing until the + @c loadCameraInfo(), @c isCalibrated() or @c getCameraInfo() + method is called. That suppresses bogus error messages, but allows + (valid) load errors to occur during the first @c getCameraInfo(), + or @c isCalibrated(). To avoid that, do an explicit @c + loadCameraInfo() first. + +*/ + +class CameraInfoManager +{ + public: + + CameraInfoManager(ros::NodeHandle nh, + const std::string &cname="camera", + const std::string &url=""); + + sensor_msgs::CameraInfo getCameraInfo(void); + bool isCalibrated(void); + bool loadCameraInfo(const std::string &url); + std::string resolveURL(const std::string &url, + const std::string &cname); + bool setCameraName(const std::string &cname); + bool setCameraInfo(const sensor_msgs::CameraInfo &camera_info); + bool validateURL(const std::string &url); + + private: + + // recognized URL types + typedef enum + { + // supported URLs + URL_empty = 0, // empty string + URL_file, // file: + URL_package, // package: + // URLs not supported + URL_invalid, // anything >= is invalid + URL_flash, // flash: + } url_type_t; + + // private methods + std::string getPackageFileName(const std::string &url); + bool loadCalibration(const std::string &url, + const std::string &cname); + bool loadCalibrationFile(const std::string &filename, + const std::string &cname); + url_type_t parseURL(const std::string &url); + bool saveCalibration(const sensor_msgs::CameraInfo &new_info, + const std::string &url, + const std::string &cname); + bool saveCalibrationFile(const sensor_msgs::CameraInfo &new_info, + const std::string &filename, + const std::string &cname); + bool setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req, + sensor_msgs::SetCameraInfo::Response &rsp); + + /** @brief mutual exclusion lock for private data + * + * This non-recursive mutex is only held for a short time while + * accessing or changing private class variables. To avoid + * deadlocks and contention, it is never held during I/O or while + * invoking a callback. Most private methods operate on copies of + * class variables, keeping the mutex hold time short. + */ + boost::mutex mutex_; + + // private data + ros::NodeHandle nh_; ///< node handle for service + ros::ServiceServer info_service_; ///< set_camera_info service + std::string camera_name_; ///< camera name + std::string url_; ///< URL for calibration data + sensor_msgs::CameraInfo cam_info_; ///< current CameraInfo + bool loaded_cam_info_; ///< cam_info_ load attempted + +}; // class CameraInfoManager + +}; // namespace camera_info_manager + +#endif // _CAMERA_INFO_MANAGER_H_ diff --git a/ros/image_common/camera_info_manager/mainpage.dox b/ros/image_common/camera_info_manager/mainpage.dox new file mode 100644 index 0000000..4189548 --- /dev/null +++ b/ros/image_common/camera_info_manager/mainpage.dox @@ -0,0 +1,9 @@ +/** +\mainpage +\htmlinclude manifest.html + +\section codeapi C++ API + +See: . + +*/ diff --git a/ros/image_common/camera_info_manager/package.xml b/ros/image_common/camera_info_manager/package.xml new file mode 100644 index 0000000..799a516 --- /dev/null +++ b/ros/image_common/camera_info_manager/package.xml @@ -0,0 +1,40 @@ + + camera_info_manager + 1.11.12 + + + This package provides a C++ interface for camera calibration + information. It provides CameraInfo, and handles SetCameraInfo + service requests, saving and restoring the camera calibration + data. + + + Jack O'Quin + Jack O'Quin + Vincent Rabaud + BSD + + http://ros.org/wiki/camera_info_manager + https://github.com/ros-perception/image_common/issues + https://github.com/ros-perception/image_common + + catkin + + boost + camera_calibration_parsers + image_transport + roscpp + roslib + rostest + sensor_msgs + + boost + camera_calibration_parsers + image_transport + roscpp + roslib + sensor_msgs + + gtest + + diff --git a/ros/image_common/camera_info_manager/src/camera_info_manager.cpp b/ros/image_common/camera_info_manager/src/camera_info_manager.cpp new file mode 100644 index 0000000..71d6ef1 --- /dev/null +++ b/ros/image_common/camera_info_manager/src/camera_info_manager.cpp @@ -0,0 +1,659 @@ +/* $Id$ */ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010-2012 Jack O'Quin +* 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 author nor other 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. +*********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "camera_info_manager/camera_info_manager.h" + +/** @file + + @brief CameraInfo Manager implementation + + Provides CameraInfo, handles the SetCameraInfo service requests, + saves and restores sensor_msgs/CameraInfo data. + + @author Jack O'Quin + */ + +namespace camera_info_manager +{ + +using namespace camera_calibration_parsers; + +/** URL to use when no other is defined. */ +const std::string + default_camera_info_url = "file://${ROS_HOME}/camera_info/${NAME}.yaml"; + +/** Constructor + * + * @param nh node handle, normally for the driver's streaming name + * space ("camera"). The service name is relative to this + * handle. Nodes supporting multiple cameras may use + * subordinate names, like "left/camera" and "right/camera". + * @param cname default camera name + * @param url default Uniform Resource Locator for loading and saving data. + */ +CameraInfoManager::CameraInfoManager(ros::NodeHandle nh, + const std::string &cname, + const std::string &url): + nh_(nh), + camera_name_(cname), + url_(url), + loaded_cam_info_(false) +{ + // register callback for camera calibration service request + info_service_ = nh_.advertiseService("set_camera_info", + &CameraInfoManager::setCameraInfoService, this); +} + +/** Get the current CameraInfo data. + * + * If CameraInfo has not yet been loaded, an attempt must be made + * here. To avoid that, ensure that loadCameraInfo() ran previously. + * If the load is attempted but fails, an empty CameraInfo will be + * supplied. + * + * The matrices are all zeros if no calibration is available. The + * image pipeline handles that as uncalibrated data. + * + * @warning The caller @em must fill in the message Header of the + * CameraInfo returned. The time stamp and frame_id should + * normally be the same as the corresponding Image message + * Header fields. + */ +sensor_msgs::CameraInfo CameraInfoManager::getCameraInfo(void) +{ + while (ros::ok()) + { + std::string cname; + std::string url; + { + boost::mutex::scoped_lock lock_(mutex_); + if (loaded_cam_info_) + { + return cam_info_; // all done + } + + // load being attempted now + loaded_cam_info_ = true; + + // copy the name and URL strings + url = url_; + cname = camera_name_; + + } // release the lock + + // attempt load without the lock, it is not recursive + loadCalibration(url, cname); + } + + return sensor_msgs::CameraInfo(); +} + +/** Get file name corresponding to a @c package: URL. + * + * @param url a copy of the Uniform Resource Locator + * @return file name if package found, "" otherwise + */ +std::string CameraInfoManager::getPackageFileName(const std::string &url) +{ + ROS_DEBUG_STREAM("camera calibration URL: " << url); + + // Scan URL from after "package://" until next '/' and extract + // package name. The parseURL() already checked that it's present. + size_t prefix_len = std::string("package://").length(); + size_t rest = url.find('/', prefix_len); + std::string package(url.substr(prefix_len, rest - prefix_len)); + + // Look up the ROS package path name. + std::string pkgPath(ros::package::getPath(package)); + if (pkgPath.empty()) // package not found? + { + ROS_WARN_STREAM("unknown package: " << package << " (ignored)"); + return pkgPath; + } + else + { + // Construct file name from package location and remainder of URL. + return pkgPath + url.substr(rest); + } +} + +/** Is the current CameraInfo calibrated? + * + * If CameraInfo has not yet been loaded, an attempt must be made + * here. To avoid that, ensure that loadCameraInfo() ran previously. + * If the load failed, CameraInfo will be empty and this predicate + * will return false. + * + * @return true if the current CameraInfo is calibrated. + */ +bool CameraInfoManager::isCalibrated(void) +{ + while (true) + { + std::string cname; + std::string url; + { + boost::mutex::scoped_lock lock_(mutex_); + if (loaded_cam_info_) + { + return (cam_info_.K[0] != 0.0); + } + + // load being attempted now + loaded_cam_info_ = true; + + // copy the name and URL strings + url = url_; + cname = camera_name_; + + } // release the lock + + // attempt load without the lock, it is not recursive + loadCalibration(url, cname); + } +} + +/** Load CameraInfo calibration data (if any). + * + * @pre mutex_ unlocked + * + * @param url a copy of the Uniform Resource Locator + * @param cname is a copy of the camera_name_ + * @return true if URL contains calibration data. + * + * sets cam_info_, if successful + */ +bool CameraInfoManager::loadCalibration(const std::string &url, + const std::string &cname) +{ + bool success = false; // return value + + const std::string resURL(resolveURL(url, cname)); + url_type_t url_type = parseURL(resURL); + + if (url_type != URL_empty) + { + ROS_INFO_STREAM("camera calibration URL: " << resURL); + } + + switch (url_type) + { + case URL_empty: + { + ROS_INFO("using default calibration URL"); + success = loadCalibration(default_camera_info_url, cname); + break; + } + case URL_file: + { + success = loadCalibrationFile(resURL.substr(7), cname); + break; + } + case URL_flash: + { + ROS_WARN("[CameraInfoManager] reading from flash not implemented yet"); + break; + } + case URL_package: + { + std::string filename(getPackageFileName(resURL)); + if (!filename.empty()) + success = loadCalibrationFile(filename, cname); + break; + } + default: + { + ROS_ERROR_STREAM("Invalid camera calibration URL: " << resURL); + break; + } + } + + return success; +} + +/** Load CameraInfo calibration data from a file. + * + * @pre mutex_ unlocked + * + * @param filename containing CameraInfo to read + * @param cname is a copy of the camera_name_ + * @return true if URL contains calibration data. + * + * Sets cam_info_, if successful + */ +bool CameraInfoManager::loadCalibrationFile(const std::string &filename, + const std::string &cname) +{ + bool success = false; + + ROS_DEBUG_STREAM("reading camera calibration from " << filename); + std::string cam_name; + sensor_msgs::CameraInfo cam_info; + + if (readCalibration(filename, cam_name, cam_info)) + { + if (cname != cam_name) + { + ROS_WARN_STREAM("[" << cname << "] does not match name " + << cam_name << " in file " << filename); + } + success = true; + { + // lock only while updating cam_info_ + boost::mutex::scoped_lock lock(mutex_); + cam_info_ = cam_info; + } + } + else + { + ROS_WARN_STREAM("Camera calibration file " << filename << " not found."); + } + + return success; +} + +/** Set a new URL and load its calibration data (if any). + * + * If multiple threads call this method simultaneously with different + * URLs, there is no guarantee which will prevail. + * + * @param url new Uniform Resource Locator for CameraInfo. + * @return true if new URL contains calibration data. + * + * @post @c loaded_cam_info_ true (meaning a load was attempted, even + * if it failed); @c cam_info_ updated, if successful. + */ +bool CameraInfoManager::loadCameraInfo(const std::string &url) +{ + std::string cname; + { + boost::mutex::scoped_lock lock(mutex_); + url_ = url; + cname = camera_name_; + loaded_cam_info_ = true; + } + + // load using copies of the parameters, no need to hold the lock + return loadCalibration(url, cname); +} + + +/** Resolve Uniform Resource Locator string. + * + * @param url a copy of the Uniform Resource Locator, which may + * include ${...} substitution variables. + * @param cname is a copy of the camera_name_ + * + * @return a copy of the URL with any variable information resolved. + */ +std::string CameraInfoManager::resolveURL(const std::string &url, + const std::string &cname) +{ + std::string resolved; + size_t rest = 0; + + while (true) + { + // find the next '$' in the URL string + size_t dollar = url.find('$', rest); + + if (dollar >= url.length()) + { + // no more variables left in the URL + resolved += url.substr(rest); + break; + } + + // copy characters up to the next '$' + resolved += url.substr(rest, dollar-rest); + + if (url.substr(dollar+1, 1) != "{") + { + // no '{' follows, so keep the '$' + resolved += "$"; + } + else if (url.substr(dollar+1, 6) == "{NAME}") + { + // substitute camera name + resolved += cname; + dollar += 6; + } + else if (url.substr(dollar+1, 10) == "{ROS_HOME}") + { + // substitute $ROS_HOME + std::string ros_home; + char *ros_home_env; + if ((ros_home_env = getenv("ROS_HOME"))) + { + // use environment variable + ros_home = ros_home_env; + } + else if ((ros_home_env = getenv("HOME"))) + { + // use "$HOME/.ros" + ros_home = ros_home_env; + ros_home += "/.ros"; + } + resolved += ros_home; + dollar += 10; + } + else + { + // not a valid substitution variable + ROS_ERROR_STREAM("[CameraInfoManager]" + " invalid URL substitution (not resolved): " + << url); + resolved += "$"; // keep the bogus '$' + } + + // look for next '$' + rest = dollar + 1; + } + + return resolved; +} + +/** Parse calibration Uniform Resource Locator. + * + * @param url string to parse + * @return URL type + * + * @note Recognized but unsupported URL types have enum values >= URL_invalid. + */ +CameraInfoManager::url_type_t CameraInfoManager::parseURL(const std::string &url) +{ + if (url == "") + { + return URL_empty; + } + if (boost::iequals(url.substr(0, 8), "file:///")) + { + return URL_file; + } + if (boost::iequals(url.substr(0, 9), "flash:///")) + { + return URL_flash; + } + if (boost::iequals(url.substr(0, 10), "package://")) + { + // look for a '/' following the package name, make sure it is + // there, the name is not empty, and something follows it + size_t rest = url.find('/', 10); + if (rest < url.length()-1 && rest > 10) + return URL_package; + } + return URL_invalid; +} + +/** Save CameraInfo calibration data. + * + * @pre mutex_ unlocked + * + * @param new_info contains CameraInfo to save + * @param url is a copy of the URL storage location (if empty, use + * @c file://${ROS_HOME}/camera_info/${NAME}.yaml) + * @param cname is a copy of the camera_name_ + * @return true, if successful + */ +bool +CameraInfoManager::saveCalibration(const sensor_msgs::CameraInfo &new_info, + const std::string &url, + const std::string &cname) +{ + bool success = false; + + const std::string resURL(resolveURL(url, cname)); + + switch (parseURL(resURL)) + { + case URL_empty: + { + // store using default file name + success = saveCalibration(new_info, default_camera_info_url, cname); + break; + } + case URL_file: + { + success = saveCalibrationFile(new_info, resURL.substr(7), cname); + break; + } + case URL_package: + { + std::string filename(getPackageFileName(resURL)); + if (!filename.empty()) + success = saveCalibrationFile(new_info, filename, cname); + break; + } + default: + { + // invalid URL, save to default location + ROS_ERROR_STREAM("invalid url: " << resURL << " (ignored)"); + success = saveCalibration(new_info, default_camera_info_url, cname); + break; + } + } + + return success; +} + +/** Save CameraInfo calibration data to a file. + * + * @pre mutex_ unlocked + * + * @param new_info contains CameraInfo to save + * @param filename is local file to store data + * @param cname is a copy of the camera_name_ + * @return true, if successful + */ +bool +CameraInfoManager::saveCalibrationFile(const sensor_msgs::CameraInfo &new_info, + const std::string &filename, + const std::string &cname) +{ + ROS_INFO_STREAM("writing calibration data to " << filename); + + // isolate the name of the containing directory + size_t last_slash = filename.rfind('/'); + if (last_slash >= filename.length()) + { + // No slash in the name. This should never happen, the URL + // parser ensures there is at least one '/' at the beginning. + ROS_ERROR_STREAM("filename [" << filename << "] has no '/'"); + return false; // not a valid URL + } + + // make sure the directory exists and is writable + std::string dirname(filename.substr(0, last_slash+1)); + struct stat stat_data; + int rc = stat(dirname.c_str(), &stat_data); + if (rc != 0) + { + if (errno == ENOENT) + { + // directory does not exist, try to create it and its parents + std::string command("mkdir -p " + dirname); + rc = system(command.c_str()); + if (rc != 0) + { + // mkdir failed + ROS_ERROR_STREAM("unable to create path to directory [" + << dirname << "]"); + return false; + } + } + else + { + // not accessible, or something screwy + ROS_ERROR_STREAM("directory [" << dirname << "] not accessible"); + return false; + } + } + else if (!S_ISDIR(stat_data.st_mode)) + { + // dirname exists but is not a directory + ROS_ERROR_STREAM("[" << dirname << "] is not a directory"); + return false; + } + + // Directory exists and is accessible. Permissions might still be bad. + + // Currently, writeCalibration() always returns true no matter what + // (ros-pkg ticket #5010). + return writeCalibration(filename, cname, new_info); +} + +/** Callback for SetCameraInfo request. + * + * Always updates cam_info_ class variable, even if save fails. + * + * @param req SetCameraInfo request message + * @param rsp SetCameraInfo response message + * @return true if message handled + */ +bool +CameraInfoManager::setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req, + sensor_msgs::SetCameraInfo::Response &rsp) +{ + // copies of class variables needed for saving calibration + std::string url_copy; + std::string cname; + { + boost::mutex::scoped_lock lock(mutex_); + cam_info_ = req.camera_info; + url_copy = url_; + cname = camera_name_; + loaded_cam_info_ = true; + } + + if (!nh_.ok()) + { + ROS_ERROR("set_camera_info service called, but driver not running."); + rsp.status_message = "Camera driver not running."; + rsp.success = false; + return false; + } + + rsp.success = saveCalibration(req.camera_info, url_copy, cname); + if (!rsp.success) + rsp.status_message = "Error storing camera calibration."; + + return true; +} + +/** Set a new camera name. + * + * @param cname new camera name to use for saving calibration data + * + * @return true if new name has valid syntax; valid names contain only + * alphabetic, numeric, or '_' characters. + * + * @post @c cam_name_ updated, if valid; since it may affect the URL, + * @c cam_info_ will be reloaded before being used again. + */ +bool CameraInfoManager::setCameraName(const std::string &cname) +{ + // the camera name may not be empty + if (cname.empty()) + return false; + + // validate the camera name characters + for (unsigned i = 0; i < cname.size(); ++i) + { + if (!isalnum(cname[i]) && cname[i] != '_') + return false; + } + + // The name is valid, so update our private copy. Since the new + // name might cause the existing URL to resolve somewhere else, + // force @c cam_info_ to be reloaded before being used again. + { + boost::mutex::scoped_lock lock(mutex_); + camera_name_ = cname; + loaded_cam_info_ = false; + } + + return true; +} + +/** Set the camera info manually + * + * @param camera_info new camera calibration data + * + * @return true if new camera info is set + * + * @post @c cam_info_ updated, if valid; + */ +bool CameraInfoManager::setCameraInfo(const sensor_msgs::CameraInfo &camera_info) +{ + boost::mutex::scoped_lock lock(mutex_); + + cam_info_ = camera_info; + loaded_cam_info_ = true; + + return true; +} + +/** Validate URL syntax. + * + * @param url Uniform Resource Locator to check + * + * @return true if URL syntax is supported by CameraInfoManager + * (although the resource need not actually exist) + */ +bool CameraInfoManager::validateURL(const std::string &url) +{ + std::string cname; // copy of camera name + { + boost::mutex::scoped_lock lock(mutex_); + cname = camera_name_; + } // release the lock + + url_type_t url_type = parseURL(resolveURL(url, cname)); + return (url_type < URL_invalid); +} + +} // namespace camera_info_manager diff --git a/ros/image_common/camera_info_manager/tests/test_calibration.yaml b/ros/image_common/camera_info_manager/tests/test_calibration.yaml new file mode 100644 index 0000000..8df9d70 --- /dev/null +++ b/ros/image_common/camera_info_manager/tests/test_calibration.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: 08144361026320a0 +camera_matrix: + rows: 3 + cols: 3 + data: [1168.68, 0, 295.015, 0, 1169.01, 252.247, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-1.04482, 1.59252, -0.0196308, 0.0287906, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [1168.68, 0, 295.015, 0, 0, 1169.01, 252.247, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/ros/image_common/camera_info_manager/tests/unit_test.cpp b/ros/image_common/camera_info_manager/tests/unit_test.cpp new file mode 100644 index 0000000..0d72225 --- /dev/null +++ b/ros/image_common/camera_info_manager/tests/unit_test.cpp @@ -0,0 +1,719 @@ +/* $Id$ */ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010 Jack O'Quin +* 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 author nor other 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. +*********************************************************************/ + +#include +#include +#include +#include +#include "camera_info_manager/camera_info_manager.h" +#include +#include +#include + +/////////////////////////////////////////////////////////////// +// global test data +/////////////////////////////////////////////////////////////// + +namespace +{ + const std::string g_package_name("camera_info_manager"); + const std::string g_test_name("test_calibration"); + const std::string g_package_filename("/tests/" + g_test_name +".yaml"); + const std::string g_package_url("package://" + g_package_name + + g_package_filename); + const std::string g_package_name_url("package://" + g_package_name + + "/tests/${NAME}.yaml"); + const std::string g_default_url("file://${ROS_HOME}/camera_info/${NAME}.yaml"); + const std::string g_camera_name("08144361026320a0"); +} + +/////////////////////////////////////////////////////////////// +// utility functions +/////////////////////////////////////////////////////////////// + +// compare CameraInfo fields that are saved and loaded for calibration +void compare_calibration(const sensor_msgs::CameraInfo &exp, + const sensor_msgs::CameraInfo &ci) +{ + // check image size + EXPECT_EQ(exp.width, ci.width); + EXPECT_EQ(exp.height, ci.height); + + // check distortion coefficients + EXPECT_EQ(exp.distortion_model, ci.distortion_model); + EXPECT_EQ(exp.D.size(), ci.D.size()); + for (unsigned i = 0; i < ci.D.size(); ++i) + { + EXPECT_EQ(exp.D[i], ci.D[i]); + } + + // check camera matrix + for (unsigned i = 0; i < ci.K.size(); ++i) + { + EXPECT_EQ(exp.K[i], ci.K[i]); + } + + // check rectification matrix + for (unsigned i = 0; i < ci.R.size(); ++i) + { + EXPECT_EQ(exp.R[i], ci.R[i]); + } + + // check projection matrix + for (unsigned i = 0; i < ci.P.size(); ++i) + { + EXPECT_EQ(exp.P[i], ci.P[i]); + } +} + +// make sure this file does not exist +void delete_file(std::string filename) +{ + int rc = unlink(filename.c_str()); + if (rc != 0) + { + if (errno != ENOENT) + ROS_INFO_STREAM("unexpected unlink() error: " << errno); + } +} + +void delete_default_file(void) +{ + std::string ros_home("/tmp"); + setenv("ROS_HOME", ros_home.c_str(), true); + std::string tmpFile(ros_home + "/camera_info/camera.yaml"); + delete_file(tmpFile); +} + +void do_system(const std::string &command) +{ + int rc = system(command.c_str()); + if (rc) + std::cout << command << " returns " << rc; +} + +void delete_tmp_camera_info_directory(void) +{ + do_system(std::string("rm -rf /tmp/camera_info")); +} + +void make_tmp_camera_info_directory(void) +{ + do_system(std::string("mkdir -p /tmp/camera_info")); +} + +// These data must match the contents of test_calibration.yaml. +sensor_msgs::CameraInfo expected_calibration(void) +{ + sensor_msgs::CameraInfo ci; + + ci.width = 640u; + ci.height = 480u; + + // set distortion coefficients + ci.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + ci.D.resize(5); + ci.D[0] = -1.04482; + ci.D[1] = 1.59252; + ci.D[2] = -0.0196308; + ci.D[3] = 0.0287906; + ci.D[4] = 0.0; + + // set camera matrix + ci.K[0] = 1168.68; + ci.K[1] = 0.0; + ci.K[2] = 295.015; + ci.K[3] = 0.0; + ci.K[4] = 1169.01; + ci.K[5] = 252.247; + ci.K[6] = 0.0; + ci.K[7] = 0.0; + ci.K[8] = 1.0; + + // set rectification matrix + ci.R[0] = 1.0; + ci.R[1] = 0.0; + ci.R[2] = 0.0; + ci.R[3] = 0.0; + ci.R[4] = 1.0; + ci.R[5] = 0.0; + ci.R[6] = 0.0; + ci.R[7] = 0.0; + ci.R[8] = 1.0; + + // set projection matrix + ci.P[0] = ci.K[0]; + ci.P[1] = ci.K[1]; + ci.P[2] = ci.K[2]; + ci.P[3] = 0.0; + ci.P[4] = ci.K[3]; + ci.P[5] = ci.K[4]; + ci.P[6] = ci.K[5]; + ci.P[7] = 0.0; + ci.P[8] = ci.K[6]; + ci.P[9] = ci.K[7]; + ci.P[10] = ci.K[8]; + ci.P[11] = 0.0; + + return ci; +} + +// issue SetCameraInfo service request +bool set_calibration(ros::NodeHandle node, + const sensor_msgs::CameraInfo &calib) +{ + ros::ServiceClient client = + node.serviceClient("set_camera_info"); + sensor_msgs::SetCameraInfo set_camera_info; + set_camera_info.request.camera_info = calib; + bool success; + EXPECT_TRUE((success = client.call(set_camera_info))); + return success; +} + +// resolve URL string, result should be as expected +void check_url_substitution(ros::NodeHandle node, + const std::string &url, + const std::string &exp_url, + const std::string &camera_name) +{ + camera_info_manager::CameraInfoManager cinfo(node, camera_name, url); + std::string sub_url = cinfo.resolveURL(url, camera_name); + EXPECT_EQ(sub_url, exp_url); +} + +/////////////////////////////////////////////////////////////// +// Test cases +/////////////////////////////////////////////////////////////// + +// Test that valid camera names are accepted +TEST(CameraName, validNames) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_TRUE(cinfo.setCameraName(std::string("a"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("1"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("_"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("abcdefghijklmnopqrstuvwxyz"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("ABCDEFGHIJKLMNOPQRSTUVWXYZ"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("0123456789"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("0123456789abcdef"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("A1"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("9z"))); + EXPECT_TRUE(cinfo.setCameraName(std::string("08144361026320a0_640x480_mono8"))); + +} + +// Test that invalid camera names are rejected +TEST(CameraName, invalidNames) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_FALSE(cinfo.setCameraName(std::string(""))); + EXPECT_FALSE(cinfo.setCameraName(std::string("-21"))); + EXPECT_FALSE(cinfo.setCameraName(std::string("C++"))); + EXPECT_FALSE(cinfo.setCameraName(std::string("file:///tmp/url.yaml"))); + EXPECT_FALSE(cinfo.setCameraName(std::string("file://${INVALID}/xxx.yaml"))); +} + +// Test that valid URLs are accepted +TEST(UrlValidation, validURLs) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_TRUE(cinfo.validateURL(std::string(""))); + EXPECT_TRUE(cinfo.validateURL(std::string("file:///"))); + EXPECT_TRUE(cinfo.validateURL(std::string("file:///tmp/url.yaml"))); + EXPECT_TRUE(cinfo.validateURL(std::string("File:///tmp/url.ini"))); + EXPECT_TRUE(cinfo.validateURL(std::string("FILE:///tmp/url.yaml"))); + EXPECT_TRUE(cinfo.validateURL(g_default_url)); + EXPECT_TRUE(cinfo.validateURL(g_package_url)); + EXPECT_TRUE(cinfo.validateURL(std::string("package://no_such_package/calibration.yaml"))); + EXPECT_TRUE(cinfo.validateURL(std::string("packAge://camera_info_manager/x"))); +} + +// Test that invalid URLs are rejected +TEST(UrlValidation, invalidURLs) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_FALSE(cinfo.validateURL(std::string("file://"))); + EXPECT_FALSE(cinfo.validateURL(std::string("flash:///"))); + EXPECT_FALSE(cinfo.validateURL(std::string("html://ros.org/wiki/camera_info_manager"))); + EXPECT_FALSE(cinfo.validateURL(std::string("package://"))); + EXPECT_FALSE(cinfo.validateURL(std::string("package:///"))); + EXPECT_FALSE(cinfo.validateURL(std::string("package://calibration.yaml"))); + EXPECT_FALSE(cinfo.validateURL(std::string("package://camera_info_manager/"))); +} + +// Test ability to provide uncalibrated CameraInfo +TEST(GetInfo, uncalibrated) +{ + ros::NodeHandle node; + + delete_default_file(); + + camera_info_manager::CameraInfoManager cinfo(node); + EXPECT_FALSE(cinfo.isCalibrated()); + + sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::CameraInfo exp; + compare_calibration(exp, ci); +} + +// Test ability to load calibrated CameraInfo +TEST(GetInfo, calibrated) +{ + ros::NodeHandle node; + + delete_default_file(); + + camera_info_manager::CameraInfoManager cinfo(node); + EXPECT_FALSE(cinfo.isCalibrated()); + + std::string pkgPath(ros::package::getPath(g_package_name)); + std::string url("file://" + pkgPath + "/tests/test_calibration.yaml"); + EXPECT_TRUE(cinfo.loadCameraInfo(url)); + EXPECT_TRUE(cinfo.isCalibrated()); + + sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::CameraInfo exp(expected_calibration()); + compare_calibration(exp, ci); +} + +// Test ability to load calibrated CameraInfo from package +TEST(GetInfo, fromPackage) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_TRUE(cinfo.loadCameraInfo(g_package_url)); + EXPECT_TRUE(cinfo.isCalibrated()); + + sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::CameraInfo exp(expected_calibration()); + compare_calibration(exp, ci); +} + +// Test ability to access named calibrated CameraInfo from package +TEST(GetInfo, fromPackageWithName) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node, g_test_name, + g_package_name_url); + EXPECT_TRUE(cinfo.isCalibrated()); + + sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::CameraInfo exp(expected_calibration()); + compare_calibration(exp, ci); +} + +// Test load of unresolved "package:" URL files +TEST(GetInfo, unresolvedLoads) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://"))); + EXPECT_FALSE(cinfo.isCalibrated()); + + EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package:///calibration.yaml"))); + EXPECT_FALSE(cinfo.isCalibrated()); + + EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://no_such_package/calibration.yaml"))); + EXPECT_FALSE(cinfo.isCalibrated()); + + EXPECT_FALSE(cinfo.loadCameraInfo(std::string("package://camera_info_manager/no_such_file.yaml"))); + EXPECT_FALSE(cinfo.isCalibrated()); +} + +// Test load of "package:" URL after changing name +TEST(GetInfo, nameChange) +{ + ros::NodeHandle node; + const std::string missing_file("no_such_file"); + + // first declare using non-existent camera name + camera_info_manager::CameraInfoManager cinfo(node, missing_file, + g_package_name_url); + EXPECT_FALSE(cinfo.isCalibrated()); + + // set name so it resolves to a test file that does exist + EXPECT_TRUE(cinfo.setCameraName(g_test_name)); + EXPECT_TRUE(cinfo.isCalibrated()); + sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::CameraInfo exp(expected_calibration()); + compare_calibration(exp, ci); +} + +// Test load of invalid CameraInfo URLs +TEST(GetInfo, invalidLoads) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + EXPECT_FALSE(cinfo.loadCameraInfo(std::string("flash:///"))); + EXPECT_FALSE(cinfo.isCalibrated()); + + EXPECT_FALSE(cinfo.loadCameraInfo(std::string("html://ros.org/wiki/camera_info_manager"))); + EXPECT_FALSE(cinfo.isCalibrated()); + + sensor_msgs::CameraInfo ci(cinfo.getCameraInfo()); + sensor_msgs::CameraInfo exp; + compare_calibration(exp, ci); +} + +// Test ability to set CameraInfo directly +TEST(SetInfo, setCameraInfo) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + // issue calibration service request + sensor_msgs::CameraInfo exp(expected_calibration()); + bool success = cinfo.setCameraInfo(exp); + EXPECT_TRUE(success); + + // only check results if the service succeeded, avoiding confusing + // and redundant failure messages + if (success) + { + // check that it worked + EXPECT_TRUE(cinfo.isCalibrated()); + sensor_msgs::CameraInfo ci = cinfo.getCameraInfo(); + compare_calibration(exp, ci); + } +} + +// Test ability to set calibrated CameraInfo +TEST(SetInfo, setCalibration) +{ + ros::NodeHandle node; + camera_info_manager::CameraInfoManager cinfo(node); + + // issue calibration service request + sensor_msgs::CameraInfo exp(expected_calibration()); + bool success = set_calibration(node, exp); + + // only check results if the service succeeded, avoiding confusing + // and redundant failure messages + if (success) + { + // check that it worked + EXPECT_TRUE(cinfo.isCalibrated()); + sensor_msgs::CameraInfo ci = cinfo.getCameraInfo(); + compare_calibration(exp, ci); + } +} + +// Test ability to save calibrated CameraInfo in default URL +TEST(SetInfo, saveCalibrationDefault) +{ + ros::NodeHandle node; + sensor_msgs::CameraInfo exp(expected_calibration()); + bool success; + + // Set ${ROS_HOME} to /tmp, then delete the /tmp/camera_info + // directory and everything in it. + setenv("ROS_HOME", "/tmp", true); + delete_tmp_camera_info_directory(); + + { + // create instance to save calibrated data + camera_info_manager::CameraInfoManager cinfo(node); + EXPECT_FALSE(cinfo.isCalibrated()); + + // issue calibration service request + success = set_calibration(node, exp); + EXPECT_TRUE(cinfo.isCalibrated()); + } + + // only check results if the service succeeded, avoiding confusing + // and redundant failure messages + if (success) + { + // create a new instance to load saved calibration + camera_info_manager::CameraInfoManager cinfo2(node); + EXPECT_TRUE(cinfo2.isCalibrated()); + if (cinfo2.isCalibrated()) + { + sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + compare_calibration(exp, ci); + } + } +} + +// Test ability to save calibrated CameraInfo to default location with +// explicit camera name +TEST(SetInfo, saveCalibrationCameraName) +{ + ros::NodeHandle node; + sensor_msgs::CameraInfo exp(expected_calibration()); + bool success; + + // set ${ROS_HOME} to /tmp, delete the calibration file + std::string ros_home("/tmp"); + setenv("ROS_HOME", ros_home.c_str(), true); + std::string tmpFile(ros_home + "/camera_info/" + g_camera_name + ".yaml"); + delete_file(tmpFile); + + { + // create instance to save calibrated data + camera_info_manager::CameraInfoManager cinfo(node, g_camera_name); + success = set_calibration(node, exp); + EXPECT_TRUE(cinfo.isCalibrated()); + } + + // only check results if the service succeeded, avoiding confusing + // and redundant failure messages + if (success) + { + // create a new instance to load saved calibration + camera_info_manager::CameraInfoManager cinfo2(node); + std::string url = "file://" + tmpFile; + cinfo2.loadCameraInfo(std::string(url)); + EXPECT_TRUE(cinfo2.isCalibrated()); + if (cinfo2.isCalibrated()) + { + sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + compare_calibration(exp, ci); + } + } +} + +// Test ability to save calibrated CameraInfo in a file +TEST(SetInfo, saveCalibrationFile) +{ + return; + + ros::NodeHandle node; + sensor_msgs::CameraInfo exp(expected_calibration()); + std::string cname("camera"); + std::string tmpFile("/tmp/camera_info_manager_calibration_test.yaml"); + std::string url = "file://" + tmpFile; + bool success; + + // first, delete the file + delete_file(tmpFile); + + { + // create instance to save calibrated data + camera_info_manager::CameraInfoManager cinfo(node, cname, url); + success = set_calibration(node, exp); + EXPECT_TRUE(cinfo.isCalibrated()); + } + + // only check results if the service succeeded, avoiding confusing + // and redundant failure messages + if (success) + { + // create a new instance to load saved calibration + camera_info_manager::CameraInfoManager cinfo2(node, cname, url); + EXPECT_TRUE(cinfo2.isCalibrated()); + if (cinfo2.isCalibrated()) + { + sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + compare_calibration(exp, ci); + } + } +} + +// Test ability to save calibrated CameraInfo in a package +// (needs write access). +TEST(SetInfo, saveCalibrationPackage) +{ + ros::NodeHandle node; + sensor_msgs::CameraInfo exp(expected_calibration()); + std::string pkgPath(ros::package::getPath(g_package_name)); + std::string filename(pkgPath + g_package_filename); + bool success; + + // first, delete the file + delete_file(filename); + + { + // create instance to save calibrated data + camera_info_manager::CameraInfoManager cinfo(node, g_camera_name, + g_package_url); + success = set_calibration(node, exp); + EXPECT_TRUE(cinfo.isCalibrated()); + } + + // only check results if the service succeeded, avoiding confusing + // and redundant failure messages + if (success) + { + // create a new instance to load saved calibration + camera_info_manager::CameraInfoManager cinfo2(node, g_camera_name, + g_package_url); + EXPECT_TRUE(cinfo2.isCalibrated()); + if (cinfo2.isCalibrated()) + { + sensor_msgs::CameraInfo ci(cinfo2.getCameraInfo()); + compare_calibration(exp, ci); + } + } +} + +TEST(UrlSubstitution, cameraName) +{ + ros::NodeHandle node; + std::string name_url; + std::string exp_url; + + // resolve a GUID camera name + name_url = "package://" + g_package_name + "/tests/${NAME}.yaml"; + exp_url = "package://" + g_package_name + "/tests/" + g_camera_name + ".yaml"; + check_url_substitution(node, name_url, exp_url, g_camera_name); + + // substitute camera name "test" + name_url = "package://" + g_package_name + "/tests/${NAME}_calibration.yaml"; + std::string test_name("test"); + exp_url = "package://" + g_package_name + "/tests/" + test_name + + "_calibration.yaml"; + check_url_substitution(node, name_url, exp_url, test_name); + + // with an '_' in the name + test_name = "camera_1024x768"; + exp_url = "package://" + g_package_name + "/tests/" + test_name + + "_calibration.yaml"; + check_url_substitution(node, name_url, exp_url, test_name); + + // substitute empty camera name + name_url = "package://" + g_package_name + "/tests/${NAME}_calibration.yaml"; + std::string empty_name(""); + exp_url = "package://" + g_package_name + "/tests/" + empty_name + + "_calibration.yaml"; + check_url_substitution(node, name_url, exp_url, empty_name); + + // substitute test camera calibration from this package + check_url_substitution(node, g_package_name_url, g_package_url, g_test_name); +} + +TEST(UrlSubstitution, rosHome) +{ + ros::NodeHandle node; + std::string name_url; + std::string exp_url; + char *home_env = getenv("HOME"); + std::string home(home_env); + + // resolve ${ROS_HOME} with environment variable undefined + unsetenv("ROS_HOME"); + name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml"; + exp_url = "file://" + home + "/.ros/camera_info/test_camera.yaml"; + check_url_substitution(node, name_url, exp_url, g_camera_name); + + // resolve ${ROS_HOME} with environment variable defined + setenv("ROS_HOME", "/my/ros/home", true); + name_url = "file://${ROS_HOME}/camera_info/test_camera.yaml"; + exp_url = "file:///my/ros/home/camera_info/test_camera.yaml"; + check_url_substitution(node, name_url, exp_url, g_camera_name); +} + +TEST(UrlSubstitution, unmatchedDollarSigns) +{ + ros::NodeHandle node; + + // test for "$$" in the URL (NAME should be resolved) + std::string name_url("file:///tmp/$${NAME}.yaml"); + std::string exp_url("file:///tmp/$" + g_camera_name + ".yaml"); + check_url_substitution(node, name_url, exp_url, g_camera_name); + + // test for "$" in middle of string + name_url = "file:///$whatever.yaml"; + check_url_substitution(node, name_url, name_url, g_camera_name); + + // test for "$$" in middle of string + name_url = "file:///something$$whatever.yaml"; + check_url_substitution(node, name_url, name_url, g_camera_name); + + // test for "$$" at end of string + name_url = "file:///$$"; + check_url_substitution(node, name_url, name_url, g_camera_name); +} + +TEST(UrlSubstitution, emptyURL) +{ + // test that empty URL is handled correctly + ros::NodeHandle node; + std::string empty_url(""); + check_url_substitution(node, empty_url, empty_url, g_camera_name); +} + +TEST(UrlSubstitution, invalidVariables) +{ + ros::NodeHandle node; + std::string name_url; + + // missing "{...}" + name_url = "file:///tmp/$NAME.yaml"; + check_url_substitution(node, name_url, name_url, g_camera_name); + + // invalid substitution variable name + name_url = "file:///tmp/${INVALID}/calibration.yaml"; + check_url_substitution(node, name_url, name_url, g_camera_name); + + // truncated substitution variable + name_url = "file:///tmp/${NAME"; + check_url_substitution(node, name_url, name_url, g_camera_name); + + // missing substitution variable + name_url = "file:///tmp/${}"; + check_url_substitution(node, name_url, name_url, g_camera_name); + + // no exception thrown for single "$" at end of string + name_url = "file:///$"; + check_url_substitution(node, name_url, name_url, g_camera_name); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + ros::init(argc, argv, "camera_info_manager_unit_test"); + testing::InitGoogleTest(&argc, argv); + + // create asynchronous thread for handling service requests + ros::AsyncSpinner spinner(1); + spinner.start(); + + // run the tests in this thread + return RUN_ALL_TESTS(); +} diff --git a/ros/image_common/camera_info_manager/tests/unit_test.test b/ros/image_common/camera_info_manager/tests/unit_test.test new file mode 100644 index 0000000..e70b573 --- /dev/null +++ b/ros/image_common/camera_info_manager/tests/unit_test.test @@ -0,0 +1,17 @@ + + + + + + + + + diff --git a/ros/image_common/image_common/CHANGELOG.rst b/ros/image_common/image_common/CHANGELOG.rst new file mode 100644 index 0000000..67db48c --- /dev/null +++ b/ros/image_common/image_common/CHANGELOG.rst @@ -0,0 +1,139 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package image_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.12 (2017-01-29) +-------------------- +* 1.11.11 +* update changelogs +* Contributors: Vincent Rabaud + +1.11.11 (2016-09-24) +-------------------- + +1.11.10 (2016-01-19) +-------------------- + +1.11.9 (2016-01-17) +------------------- + +1.11.8 (2015-11-29) +------------------- + +1.11.7 (2015-07-28) +------------------- + +1.11.6 (2015-07-16) +------------------- + +1.11.5 (2015-05-14) +------------------- + +1.11.4 (2014-09-21) +------------------- + +1.11.3 (2014-05-19) +------------------- + +1.11.2 (2014-02-13) +------------------- + +1.11.1 (2014-01-26 02:33) +------------------------- + +1.11.0 (2013-07-20 12:23) +------------------------- + +1.10.5 (2014-01-26 02:34) +------------------------- + +1.10.4 (2013-07-20 11:42) +------------------------- +* add Jack as maintainer +* comply to REP 0127 +* Contributors: Vincent Rabaud + +1.10.3 (2013-02-21 05:33) +------------------------- + +1.10.2 (2013-02-21 04:48) +------------------------- + +1.10.1 (2013-02-21 04:16) +------------------------- + +1.10.0 (2013-01-13) +------------------- + +1.9.22 (2012-12-16) +------------------- + +1.9.21 (2012-12-14) +------------------- + +1.9.20 (2012-12-04) +------------------- + +1.9.19 (2012-11-08) +------------------- + +1.9.18 (2012-11-06) +------------------- + +1.9.17 (2012-10-30 19:32) +------------------------- + +1.9.16 (2012-10-30 09:10) +------------------------- + +1.9.15 (2012-10-13 08:43) +------------------------- + +1.9.14 (2012-10-13 01:07) +------------------------- + +1.9.13 (2012-10-06) +------------------- +* add missing description +* Contributors: Vincent Rabaud + +1.9.12 (2012-10-04) +------------------- +* define metapackage +* Contributors: Vincent Rabaud + +1.9.11 (2012-10-02 02:56) +------------------------- + +1.9.10 (2012-10-02 02:42) +------------------------- + +1.9.9 (2012-10-01) +------------------ + +1.9.8 (2012-09-30) +------------------ + +1.9.7 (2012-09-18 11:39) +------------------------ + +1.9.6 (2012-09-18 11:07) +------------------------ + +1.9.5 (2012-09-13) +------------------ + +1.9.4 (2012-09-12 23:37) +------------------------ + +1.9.3 (2012-09-12 20:44) +------------------------ + +1.9.2 (2012-09-10) +------------------ + +1.9.1 (2012-09-07 15:33) +------------------------ + +1.9.0 (2012-09-07 13:03) +------------------------ diff --git a/ros/image_common/image_common/CMakeLists.txt b/ros/image_common/image_common/CMakeLists.txt new file mode 100644 index 0000000..9b8a748 --- /dev/null +++ b/ros/image_common/image_common/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(image_common) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/ros/image_common/image_common/package.xml b/ros/image_common/image_common/package.xml new file mode 100644 index 0000000..e618faa --- /dev/null +++ b/ros/image_common/image_common/package.xml @@ -0,0 +1,27 @@ + + image_common + 1.11.12 + Common code for working with images in ROS. + Patrick Mihelich + James Bowman + Jack O'Quin + Vincent Rabaud + BSD + + http://www.ros.org/wiki/image_common + https://github.com/ros-perception/image_common/issues + https://github.com/ros-perception/image_common + + catkin + + + camera_calibration_parsers + camera_info_manager + image_transport + polled_camera + + + + + + diff --git a/ros/image_common/image_transport/CHANGELOG.rst b/ros/image_common/image_transport/CHANGELOG.rst new file mode 100644 index 0000000..a83a806 --- /dev/null +++ b/ros/image_common/image_transport/CHANGELOG.rst @@ -0,0 +1,193 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package image_transport +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.12 (2017-01-29) +-------------------- +* Fix CMake of image_transport/tutorial and polled_camera + Fix loads of problems with the CMakeLists. +* image_transport/tutorial: Add dependency on generated msg + Without this, build fails on Kinetic because ResizedImage.h has not been + generated yet. +* image_transport/tutorial: Add missing catkin_INCLUDE_DIRS + Without this, compilation files on Kinetic because ros.h cannot be found. +* 1.11.11 +* update changelogs +* Contributors: Martin Guenther, Vincent Rabaud + +1.11.11 (2016-09-24) +-------------------- + +1.11.10 (2016-01-19) +-------------------- + +1.11.9 (2016-01-17) +------------------- +* fix linkage in tutorials +* Use $catkin_EXPORTED_TARGETS +* Contributors: Jochen Sprickerhof, Vincent Rabaud + +1.11.8 (2015-11-29) +------------------- + +1.11.7 (2015-07-28) +------------------- + +1.11.6 (2015-07-16) +------------------- + +1.11.5 (2015-05-14) +------------------- +* image_transport: fix CameraSubscriber shutdown (circular shared_ptr ref) + CameraSubscriber uses a private boost::shared_ptr to share an impl object + between copied instances. In CameraSubscriber::CameraSubscriber(), it + handed this shared_ptr to boost::bind() and saved the created wall timer + in the impl object, thus creating a circular reference. The impl object + was therefore never freed. + Fix that by passing a plain pointer to boost::bind(). +* avoid a memory copy for the raw publisher +* add a way to publish an image with only the data pointer +* Make function inline to avoid duplicated names when linking statically +* add plugin examples for the tutorial +* update instructions for catkin +* remove uselessly linked library + fixes `#28 `_ +* add a tutorial for image_transport +* Contributors: Gary Servin, Max Schwarz, Vincent Rabaud + +1.11.4 (2014-09-21) +------------------- + +1.11.3 (2014-05-19) +------------------- + +1.11.2 (2014-02-13) +------------------- + +1.11.1 (2014-01-26 02:33) +------------------------- + +1.11.0 (2013-07-20 12:23) +------------------------- + +1.10.5 (2014-01-26 02:34) +------------------------- + +1.10.4 (2013-07-20 11:42) +------------------------- +* add Jack as maintainer +* update my email address +* Contributors: Vincent Rabaud + +1.10.3 (2013-02-21 05:33) +------------------------- + +1.10.2 (2013-02-21 04:48) +------------------------- + +1.10.1 (2013-02-21 04:16) +------------------------- + +1.10.0 (2013-01-13) +------------------- +* fix the urls +* use the pluginlib script to remove some warnings +* added license headers to various cpp and h files +* Contributors: Aaron Blasdel, Vincent Rabaud + +1.9.22 (2012-12-16) +------------------- +* get rid of the deprecated class_loader interface +* Contributors: Vincent Rabaud + +1.9.21 (2012-12-14) +------------------- +* CMakeLists.txt clean up +* Updated package.xml file(s) to handle new catkin buildtool_depend + requirement +* Contributors: William Woodall, mirzashah + +1.9.20 (2012-12-04) +------------------- + +1.9.19 (2012-11-08) +------------------- +* add the right link libraries +* Contributors: Vincent Rabaud + +1.9.18 (2012-11-06) +------------------- +* Isolated plugins into their own library to follow new + class_loader/pluginlib guidelines. +* remove the brief attribute +* Contributors: Mirza Shah, Vincent Rabaud + +1.9.17 (2012-10-30 19:32) +------------------------- + +1.9.16 (2012-10-30 09:10) +------------------------- +* add xml file +* Contributors: Vincent Rabaud + +1.9.15 (2012-10-13 08:43) +------------------------- +* fix bad folder/libraries +* Contributors: Vincent Rabaud + +1.9.14 (2012-10-13 01:07) +------------------------- + +1.9.13 (2012-10-06) +------------------- + +1.9.12 (2012-10-04) +------------------- + +1.9.11 (2012-10-02 02:56) +------------------------- + +1.9.10 (2012-10-02 02:42) +------------------------- + +1.9.9 (2012-10-01) +------------------ +* fix dependencies +* Contributors: Vincent Rabaud + +1.9.8 (2012-09-30) +------------------ +* add catkin as a dependency +* comply to the catkin API +* Contributors: Vincent Rabaud + +1.9.7 (2012-09-18 11:39) +------------------------ + +1.9.6 (2012-09-18 11:07) +------------------------ + +1.9.5 (2012-09-13) +------------------ +* install the include directories +* Contributors: Vincent Rabaud + +1.9.4 (2012-09-12 23:37) +------------------------ + +1.9.3 (2012-09-12 20:44) +------------------------ + +1.9.2 (2012-09-10) +------------------ + +1.9.1 (2012-09-07 15:33) +------------------------ +* make the libraries public +* Contributors: Vincent Rabaud + +1.9.0 (2012-09-07 13:03) +------------------------ +* catkinize for Groovy +* Initial image_common stack check-in, containing image_transport. +* Contributors: Vincent Rabaud, gerkey, kwc, mihelich, pmihelich, straszheim, vrabaud diff --git a/ros/image_common/image_transport/CMakeLists.txt b/ros/image_common/image_transport/CMakeLists.txt new file mode 100644 index 0000000..6e43f2f --- /dev/null +++ b/ros/image_common/image_transport/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 2.8.3) +project(image_transport) + +find_package(catkin REQUIRED + COMPONENTS + message_filters + pluginlib + rosconsole + roscpp + roslib + sensor_msgs +) + +find_package(Boost REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + DEPENDS message_filters pluginlib rosconsole roscpp roslib sensor_msgs +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +# Build libimage_transport +add_library(${PROJECT_NAME} + src/camera_common.cpp + src/camera_publisher.cpp + src/camera_subscriber.cpp + src/image_transport.cpp + src/publisher.cpp + src/single_subscriber_publisher.cpp + src/subscriber.cpp +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +# Build libimage_transport_plugins +add_library(${PROJECT_NAME}_plugins src/manifest.cpp src/raw_publisher.cpp) +target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME}) + +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + COMPONENT main +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# add two execs +add_executable(republish src/republish.cpp) +target_link_libraries(republish ${PROJECT_NAME}) + +add_executable(list_transports src/list_transports.cpp) +target_link_libraries(list_transports + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +install(TARGETS list_transports republish + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# add xml file +install(FILES default_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/ros/image_common/image_transport/default_plugins.xml b/ros/image_common/image_transport/default_plugins.xml new file mode 100644 index 0000000..f3500ec --- /dev/null +++ b/ros/image_common/image_transport/default_plugins.xml @@ -0,0 +1,13 @@ + + + + This is the default publisher. It publishes the Image as-is on the base topic. + + + + + + This is the default pass-through subscriber for topics of type sensor_msgs/Image. + + + diff --git a/ros/image_common/image_transport/include/image_transport/camera_common.h b/ros/image_common/image_transport/include/image_transport/camera_common.h new file mode 100644 index 0000000..f4c7745 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/camera_common.h @@ -0,0 +1,47 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_CAMERA_COMMON_H +#define IMAGE_TRANSPORT_CAMERA_COMMON_H + +#include + +namespace image_transport { + +/// \brief Form the camera info topic name, sibling to the base topic +std::string getCameraInfoTopic(const std::string& base_topic); + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/camera_publisher.h b/ros/image_common/image_transport/include/image_transport/camera_publisher.h new file mode 100644 index 0000000..222e68a --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/camera_publisher.h @@ -0,0 +1,135 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_CAMERA_PUBLISHER_H +#define IMAGE_TRANSPORT_CAMERA_PUBLISHER_H + +#include +#include +#include +#include "image_transport/single_subscriber_publisher.h" + +namespace image_transport { + +class ImageTransport; + +/** + * \brief Manages advertisements for publishing camera images. + * + * CameraPublisher is a convenience class for publishing synchronized image and + * camera info topics using the standard topic naming convention, where the info + * topic name is "camera_info" in the same namespace as the base image topic. + * + * On the client side, CameraSubscriber simplifies subscribing to camera images. + * + * A CameraPublisher should always be created through a call to + * ImageTransport::advertiseCamera(), or copied from one that was. + * Once all copies of a specific CameraPublisher go out of scope, any subscriber callbacks + * associated with that handle will stop being called. Once all CameraPublisher for a + * given base topic go out of scope the topic (and all subtopics) will be unadvertised. + */ +class CameraPublisher +{ +public: + CameraPublisher() {} + + /*! + * \brief Returns the number of subscribers that are currently connected to + * this CameraPublisher. + * + * Returns max(image topic subscribers, info topic subscribers). + */ + uint32_t getNumSubscribers() const; + + /*! + * \brief Returns the base (image) topic of this CameraPublisher. + */ + std::string getTopic() const; + + /** + * \brief Returns the camera info topic of this CameraPublisher. + */ + std::string getInfoTopic() const; + + /*! + * \brief Publish an (image, info) pair on the topics associated with this CameraPublisher. + */ + void publish(const sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info) const; + + /*! + * \brief Publish an (image, info) pair on the topics associated with this CameraPublisher. + */ + void publish(const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::CameraInfoConstPtr& info) const; + + /*! + * \brief Publish an (image, info) pair with given timestamp on the topics associated with + * this CameraPublisher. + * + * Convenience version, which sets the timestamps of both image and info to stamp before + * publishing. + */ + void publish(sensor_msgs::Image& image, sensor_msgs::CameraInfo& info, ros::Time stamp) const; + + /*! + * \brief Shutdown the advertisements associated with this Publisher. + */ + void shutdown(); + + operator void*() const; + bool operator< (const CameraPublisher& rhs) const { return impl_ < rhs.impl_; } + bool operator!=(const CameraPublisher& rhs) const { return impl_ != rhs.impl_; } + bool operator==(const CameraPublisher& rhs) const { return impl_ == rhs.impl_; } + +private: + CameraPublisher(ImageTransport& image_it, ros::NodeHandle& info_nh, + const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& image_connect_cb, + const SubscriberStatusCallback& image_disconnect_cb, + const ros::SubscriberStatusCallback& info_connect_cb, + const ros::SubscriberStatusCallback& info_disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch); + + struct Impl; + typedef boost::shared_ptr ImplPtr; + typedef boost::weak_ptr ImplWPtr; + + ImplPtr impl_; + + friend class ImageTransport; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/camera_subscriber.h b/ros/image_common/image_transport/include/image_transport/camera_subscriber.h new file mode 100644 index 0000000..aef447d --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/camera_subscriber.h @@ -0,0 +1,118 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_CAMERA_SUBSCRIBER_H +#define IMAGE_TRANSPORT_CAMERA_SUBSCRIBER_H + +#include +#include +#include +#include "image_transport/transport_hints.h" + +namespace image_transport { + +class ImageTransport; + +/** + * \brief Manages a subscription callback on synchronized Image and CameraInfo topics. + * + * CameraSubscriber is the client-side counterpart to CameraPublisher, and assumes the + * same topic naming convention. The callback is of type: +\verbatim +void callback(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&); +\endverbatim + * + * A CameraSubscriber should always be created through a call to + * ImageTransport::subscribeCamera(), or copied from one that was. + * Once all copies of a specific CameraSubscriber go out of scope, the subscription callback + * associated with that handle will stop being called. Once all CameraSubscriber for a given + * topic go out of scope the topic will be unsubscribed. + */ +class CameraSubscriber +{ +public: + typedef boost::function Callback; + + CameraSubscriber() {} + + /** + * \brief Get the base topic (on which the raw image is published). + */ + std::string getTopic() const; + + /** + * \brief Get the camera info topic. + */ + std::string getInfoTopic() const; + + /** + * \brief Returns the number of publishers this subscriber is connected to. + */ + uint32_t getNumPublishers() const; + + /** + * \brief Returns the name of the transport being used. + */ + std::string getTransport() const; + + /** + * \brief Unsubscribe the callback associated with this CameraSubscriber. + */ + void shutdown(); + + operator void*() const; + bool operator< (const CameraSubscriber& rhs) const { return impl_ < rhs.impl_; } + bool operator!=(const CameraSubscriber& rhs) const { return impl_ != rhs.impl_; } + bool operator==(const CameraSubscriber& rhs) const { return impl_ == rhs.impl_; } + +private: + CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh, + const std::string& base_topic, uint32_t queue_size, + const Callback& callback, + const ros::VoidPtr& tracked_object = ros::VoidPtr(), + const TransportHints& transport_hints = TransportHints()); + + struct Impl; + typedef boost::shared_ptr ImplPtr; + typedef boost::weak_ptr ImplWPtr; + + ImplPtr impl_; + + friend class ImageTransport; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/exception.h b/ros/image_common/image_transport/include/image_transport/exception.h new file mode 100644 index 0000000..1b3e2f9 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/exception.h @@ -0,0 +1,71 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_EXCEPTION_H +#define IMAGE_TRANSPORT_EXCEPTION_H + +#include + +namespace image_transport { + +/** + * \brief A base class for all image_transport exceptions inheriting from std::runtime_error. + */ +class Exception : public std::runtime_error +{ +public: + Exception(const std::string& message) : std::runtime_error(message) {} +}; + +/** + * \brief An exception class thrown when image_transport is unable to load a requested transport. + */ +class TransportLoadException : public Exception +{ +public: + TransportLoadException(const std::string& transport, const std::string& message) + : Exception("Unable to load plugin for transport '" + transport + "', error string:\n" + message), + transport_(transport.c_str()) + { + } + + std::string getTransport() const { return transport_; } + +protected: + const char* transport_; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/image_transport.h b/ros/image_common/image_transport/include/image_transport/image_transport.h new file mode 100644 index 0000000..1957999 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/image_transport.h @@ -0,0 +1,204 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_IMAGE_TRANSPORT_H +#define IMAGE_TRANSPORT_IMAGE_TRANSPORT_H + +#include "image_transport/publisher.h" +#include "image_transport/subscriber.h" +#include "image_transport/camera_publisher.h" +#include "image_transport/camera_subscriber.h" + +namespace image_transport { + +/** + * \brief Advertise and subscribe to image topics. + * + * ImageTransport is analogous to ros::NodeHandle in that it contains advertise() and + * subscribe() functions for creating advertisements and subscriptions of image topics. + */ +class ImageTransport +{ +public: + explicit ImageTransport(const ros::NodeHandle& nh); + + ~ImageTransport(); + + /*! + * \brief Advertise an image topic, simple version. + */ + Publisher advertise(const std::string& base_topic, uint32_t queue_size, bool latch = false); + + /*! + * \brief Advertise an image topic with subcriber status callbacks. + */ + Publisher advertise(const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& connect_cb, + const SubscriberStatusCallback& disconnect_cb = SubscriberStatusCallback(), + const ros::VoidPtr& tracked_object = ros::VoidPtr(), bool latch = false); + + /** + * \brief Subscribe to an image topic, version for arbitrary boost::function object. + */ + Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, + const boost::function& callback, + const ros::VoidPtr& tracked_object = ros::VoidPtr(), + const TransportHints& transport_hints = TransportHints()); + + /** + * \brief Subscribe to an image topic, version for bare function. + */ + Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, + void(*fp)(const sensor_msgs::ImageConstPtr&), + const TransportHints& transport_hints = TransportHints()) + { + return subscribe(base_topic, queue_size, + boost::function(fp), + ros::VoidPtr(), transport_hints); + } + + /** + * \brief Subscribe to an image topic, version for class member function with bare pointer. + */ + template + Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, + void(T::*fp)(const sensor_msgs::ImageConstPtr&), T* obj, + const TransportHints& transport_hints = TransportHints()) + { + return subscribe(base_topic, queue_size, boost::bind(fp, obj, _1), ros::VoidPtr(), transport_hints); + } + + /** + * \brief Subscribe to an image topic, version for class member function with shared_ptr. + */ + template + Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, + void(T::*fp)(const sensor_msgs::ImageConstPtr&), + const boost::shared_ptr& obj, + const TransportHints& transport_hints = TransportHints()) + { + return subscribe(base_topic, queue_size, boost::bind(fp, obj.get(), _1), obj, transport_hints); + } + + /*! + * \brief Advertise a synchronized camera raw image + info topic pair, simple version. + */ + CameraPublisher advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch = false); + + /*! + * \brief Advertise a synchronized camera raw image + info topic pair with subscriber status + * callbacks. + */ + CameraPublisher advertiseCamera(const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& image_connect_cb, + const SubscriberStatusCallback& image_disconnect_cb = SubscriberStatusCallback(), + const ros::SubscriberStatusCallback& info_connect_cb = ros::SubscriberStatusCallback(), + const ros::SubscriberStatusCallback& info_disconnect_cb = ros::SubscriberStatusCallback(), + const ros::VoidPtr& tracked_object = ros::VoidPtr(), bool latch = false); + + /** + * \brief Subscribe to a synchronized image & camera info topic pair, version for arbitrary + * boost::function object. + * + * This version assumes the standard topic naming scheme, where the info topic is + * named "camera_info" in the same namespace as the base image topic. + */ + CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size, + const CameraSubscriber::Callback& callback, + const ros::VoidPtr& tracked_object = ros::VoidPtr(), + const TransportHints& transport_hints = TransportHints()); + + /** + * \brief Subscribe to a synchronized image & camera info topic pair, version for bare function. + */ + CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size, + void(*fp)(const sensor_msgs::ImageConstPtr&, + const sensor_msgs::CameraInfoConstPtr&), + const TransportHints& transport_hints = TransportHints()) + { + return subscribeCamera(base_topic, queue_size, CameraSubscriber::Callback(fp), ros::VoidPtr(), + transport_hints); + } + + /** + * \brief Subscribe to a synchronized image & camera info topic pair, version for class member + * function with bare pointer. + */ + template + CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size, + void(T::*fp)(const sensor_msgs::ImageConstPtr&, + const sensor_msgs::CameraInfoConstPtr&), T* obj, + const TransportHints& transport_hints = TransportHints()) + { + return subscribeCamera(base_topic, queue_size, boost::bind(fp, obj, _1, _2), ros::VoidPtr(), + transport_hints); + } + + /** + * \brief Subscribe to a synchronized image & camera info topic pair, version for class member + * function with shared_ptr. + */ + template + CameraSubscriber subscribeCamera(const std::string& base_topic, uint32_t queue_size, + void(T::*fp)(const sensor_msgs::ImageConstPtr&, + const sensor_msgs::CameraInfoConstPtr&), + const boost::shared_ptr& obj, + const TransportHints& transport_hints = TransportHints()) + { + return subscribeCamera(base_topic, queue_size, boost::bind(fp, obj.get(), _1, _2), obj, + transport_hints); + } + + /** + * \brief Returns the names of all transports declared in the system. Declared + * transports are not necessarily built or loadable. + */ + std::vector getDeclaredTransports() const; + + /** + * \brief Returns the names of all transports that are loadable in the system. + */ + std::vector getLoadableTransports() const; + +private: + struct Impl; + typedef boost::shared_ptr ImplPtr; + typedef boost::weak_ptr ImplWPtr; + + ImplPtr impl_; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/loader_fwds.h b/ros/image_common/image_transport/include/image_transport/loader_fwds.h new file mode 100644 index 0000000..2d2dd31 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/loader_fwds.h @@ -0,0 +1,56 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_LOADER_FWDS_H +#define IMAGE_TRANSPORT_LOADER_FWDS_H + +// Forward-declare some classes most users shouldn't care about so that +// image_transport.h doesn't bring them in. + +namespace pluginlib { + template class ClassLoader; +} + +namespace image_transport { + class PublisherPlugin; + class SubscriberPlugin; + + typedef pluginlib::ClassLoader PubLoader; + typedef boost::shared_ptr PubLoaderPtr; + + typedef pluginlib::ClassLoader SubLoader; + typedef boost::shared_ptr SubLoaderPtr; +} + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/publisher.h b/ros/image_common/image_transport/include/image_transport/publisher.h new file mode 100644 index 0000000..593e56a --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/publisher.h @@ -0,0 +1,125 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_PUBLISHER_H +#define IMAGE_TRANSPORT_PUBLISHER_H + +#include +#include +#include "image_transport/single_subscriber_publisher.h" +#include "image_transport/exception.h" +#include "image_transport/loader_fwds.h" + +namespace image_transport { + +/** + * \brief Manages advertisements of multiple transport options on an Image topic. + * + * Publisher is a drop-in replacement for ros::Publisher when publishing + * Image topics. In a minimally built environment, they behave the same; however, + * Publisher is extensible via plugins to publish alternative representations of + * the image on related subtopics. This is especially useful for limiting bandwidth and + * latency over a network connection, when you might (for example) use the theora plugin + * to transport the images as streamed video. All topics are published only on demand + * (i.e. if there are subscribers). + * + * A Publisher should always be created through a call to ImageTransport::advertise(), + * or copied from one that was. + * Once all copies of a specific Publisher go out of scope, any subscriber callbacks + * associated with that handle will stop being called. Once all Publisher for a + * given base topic go out of scope the topic (and all subtopics) will be unadvertised. + */ +class Publisher +{ +public: + Publisher() {} + + /*! + * \brief Returns the number of subscribers that are currently connected to + * this Publisher. + * + * Returns the total number of subscribers to all advertised topics. + */ + uint32_t getNumSubscribers() const; + + /*! + * \brief Returns the base topic of this Publisher. + */ + std::string getTopic() const; + + /*! + * \brief Publish an image on the topics associated with this Publisher. + */ + void publish(const sensor_msgs::Image& message) const; + + /*! + * \brief Publish an image on the topics associated with this Publisher. + */ + void publish(const sensor_msgs::ImageConstPtr& message) const; + + /*! + * \brief Shutdown the advertisements associated with this Publisher. + */ + void shutdown(); + + operator void*() const; + bool operator< (const Publisher& rhs) const { return impl_ < rhs.impl_; } + bool operator!=(const Publisher& rhs) const { return impl_ != rhs.impl_; } + bool operator==(const Publisher& rhs) const { return impl_ == rhs.impl_; } + +private: + Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& connect_cb, + const SubscriberStatusCallback& disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch, + const PubLoaderPtr& loader); + + struct Impl; + typedef boost::shared_ptr ImplPtr; + typedef boost::weak_ptr ImplWPtr; + + ImplPtr impl_; + + static void weakSubscriberCb(const ImplWPtr& impl_wptr, + const SingleSubscriberPublisher& plugin_pub, + const SubscriberStatusCallback& user_cb); + + SubscriberStatusCallback rebindCB(const SubscriberStatusCallback& user_cb); + + friend class ImageTransport; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/publisher_plugin.h b/ros/image_common/image_transport/include/image_transport/publisher_plugin.h new file mode 100644 index 0000000..cc9c086 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/publisher_plugin.h @@ -0,0 +1,150 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_PUBLISHER_PLUGIN_H +#define IMAGE_TRANSPORT_PUBLISHER_PLUGIN_H + +#include +#include +#include "image_transport/single_subscriber_publisher.h" + +namespace image_transport { + +/** + * \brief Base class for plugins to Publisher. + */ +class PublisherPlugin : boost::noncopyable +{ +public: + virtual ~PublisherPlugin() {} + + /** + * \brief Get a string identifier for the transport provided by + * this plugin. + */ + virtual std::string getTransportName() const = 0; + + /** + * \brief Advertise a topic, simple version. + */ + void advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + bool latch = true) + { + advertiseImpl(nh, base_topic, queue_size, SubscriberStatusCallback(), + SubscriberStatusCallback(), ros::VoidPtr(), latch); + } + + /** + * \brief Advertise a topic with subscriber status callbacks. + */ + void advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& connect_cb, + const SubscriberStatusCallback& disconnect_cb = SubscriberStatusCallback(), + const ros::VoidPtr& tracked_object = ros::VoidPtr(), bool latch = true) + { + advertiseImpl(nh, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch); + } + + /** + * \brief Returns the number of subscribers that are currently connected to + * this PublisherPlugin. + */ + virtual uint32_t getNumSubscribers() const = 0; + + /** + * \brief Returns the communication topic that this PublisherPlugin will publish on. + */ + virtual std::string getTopic() const = 0; + + /** + * \brief Publish an image using the transport associated with this PublisherPlugin. + */ + virtual void publish(const sensor_msgs::Image& message) const = 0; + + /** + * \brief Publish an image using the transport associated with this PublisherPlugin. + */ + virtual void publish(const sensor_msgs::ImageConstPtr& message) const + { + publish(*message); + } + + /** + * \brief Publish an image using the transport associated with this PublisherPlugin. + * This version of the function can be used to optimize cases where you don't want to + * fill a ROS message first to avoid useless copies. + * @param message an image message to use information from (but not data) + * @param data a pointer to the image data to use to fill the Image message + */ + virtual void publish(const sensor_msgs::Image& message, const uint8_t* data) const + { + sensor_msgs::Image msg; + msg.header = message.header; + msg.height = message.height; + msg.width = message.width; + msg.encoding = message.encoding; + msg.is_bigendian = message.is_bigendian; + msg.step = message.step; + msg.data = std::vector(data, data + msg.step*msg.height); + + publish(msg); + } + + /** + * \brief Shutdown any advertisements associated with this PublisherPlugin. + */ + virtual void shutdown() = 0; + + /** + * \brief Return the lookup name of the PublisherPlugin associated with a specific + * transport identifier. + */ + static std::string getLookupName(const std::string& transport_name) + { + return "image_transport/" + transport_name + "_pub"; + } + +protected: + /** + * \brief Advertise a topic. Must be implemented by the subclass. + */ + virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& connect_cb, + const SubscriberStatusCallback& disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch) = 0; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/raw_publisher.h b/ros/image_common/image_transport/include/image_transport/raw_publisher.h new file mode 100644 index 0000000..4ba36e3 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/raw_publisher.h @@ -0,0 +1,82 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_RAW_PUBLISHER_H +#define IMAGE_TRANSPORT_RAW_PUBLISHER_H + +#include "image_transport/simple_publisher_plugin.h" + +namespace image_transport { + +/** + * \brief The default PublisherPlugin. + * + * RawPublisher is a simple wrapper for ros::Publisher, publishing unaltered Image + * messages on the base topic. + */ +class RawPublisher : public SimplePublisherPlugin +{ +public: + virtual ~RawPublisher() {} + + virtual std::string getTransportName() const + { + return "raw"; + } + + // Override the default implementation because publishing the message pointer allows + // the no-copy intraprocess optimization. + virtual void publish(const sensor_msgs::ImageConstPtr& message) const + { + getPublisher().publish(message); + } + + // Override the default implementation to not copy data to a sensor_msgs::Image first + virtual void publish(const sensor_msgs::Image& message, const uint8_t* data) const; + +protected: + virtual void publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const + { + publish_fn(message); + } + + virtual std::string getTopicToAdvertise(const std::string& base_topic) const + { + return base_topic; + } +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/raw_subscriber.h b/ros/image_common/image_transport/include/image_transport/raw_subscriber.h new file mode 100644 index 0000000..8241191 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/raw_subscriber.h @@ -0,0 +1,72 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_RAW_SUBSCRIBER_H +#define IMAGE_TRANSPORT_RAW_SUBSCRIBER_H + +#include "image_transport/simple_subscriber_plugin.h" + +namespace image_transport { + +/** + * \brief The default SubscriberPlugin. + * + * RawSubscriber is a simple wrapper for ros::Subscriber which listens for Image messages + * and passes them through to the callback. + */ +class RawSubscriber : public SimpleSubscriberPlugin +{ +public: + virtual ~RawSubscriber() {} + + virtual std::string getTransportName() const + { + return "raw"; + } + +protected: + virtual void internalCallback(const sensor_msgs::ImageConstPtr& message, const Callback& user_cb) + { + user_cb(message); + } + + virtual std::string getTopicToSubscribe(const std::string& base_topic) const + { + return base_topic; + } +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/simple_publisher_plugin.h b/ros/image_common/image_transport/include/image_transport/simple_publisher_plugin.h new file mode 100644 index 0000000..691d2ed --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/simple_publisher_plugin.h @@ -0,0 +1,240 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H +#define IMAGE_TRANSPORT_SIMPLE_PUBLISHER_PLUGIN_H + +#include "image_transport/publisher_plugin.h" +#include + +namespace image_transport { + +/** + * \brief Base class to simplify implementing most plugins to Publisher. + * + * This base class vastly simplifies implementing a PublisherPlugin in the common + * case that all communication with the matching SubscriberPlugin happens over a + * single ROS topic using a transport-specific message type. SimplePublisherPlugin + * is templated on the transport-specific message type. + * + * A subclass need implement only two methods: + * - getTransportName() from PublisherPlugin + * - publish() with an extra PublishFn argument + * + * For access to the parameter server and name remappings, use nh(). + * + * getTopicToAdvertise() controls the name of the internal communication topic. + * It defaults to \/\. + */ +template +class SimplePublisherPlugin : public PublisherPlugin +{ +public: + virtual ~SimplePublisherPlugin() {} + + virtual uint32_t getNumSubscribers() const + { + if (simple_impl_) return simple_impl_->pub_.getNumSubscribers(); + return 0; + } + + virtual std::string getTopic() const + { + if (simple_impl_) return simple_impl_->pub_.getTopic(); + return std::string(); + } + + virtual void publish(const sensor_msgs::Image& message) const + { + if (!simple_impl_ || !simple_impl_->pub_) { + ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::SimplePublisherPlugin"); + return; + } + + publish(message, bindInternalPublisher(simple_impl_->pub_)); + } + + virtual void shutdown() + { + if (simple_impl_) simple_impl_->pub_.shutdown(); + } + +protected: + virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& user_connect_cb, + const SubscriberStatusCallback& user_disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch) + { + std::string transport_topic = getTopicToAdvertise(base_topic); + ros::NodeHandle param_nh(transport_topic); + simple_impl_.reset(new SimplePublisherPluginImpl(param_nh)); + simple_impl_->pub_ = nh.advertise(transport_topic, queue_size, + bindCB(user_connect_cb, &SimplePublisherPlugin::connectCallback), + bindCB(user_disconnect_cb, &SimplePublisherPlugin::disconnectCallback), + tracked_object, latch); + } + + //! Generic function for publishing the internal message type. + typedef boost::function PublishFn; + + /** + * \brief Publish an image using the specified publish function. Must be implemented by + * the subclass. + * + * The PublishFn publishes the transport-specific message type. This indirection allows + * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and + * single subscriber publishing (in subscription callbacks). + */ + virtual void publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const = 0; + + /** + * \brief Return the communication topic name for a given base topic. + * + * Defaults to \/\. + */ + virtual std::string getTopicToAdvertise(const std::string& base_topic) const + { + return base_topic + "/" + getTransportName(); + } + + /** + * \brief Function called when a subscriber connects to the internal publisher. + * + * Defaults to noop. + */ + virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) {} + + /** + * \brief Function called when a subscriber disconnects from the internal publisher. + * + * Defaults to noop. + */ + virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) {} + + /** + * \brief Returns the ros::NodeHandle to be used for parameter lookup. + */ + const ros::NodeHandle& nh() const + { + return simple_impl_->param_nh_; + } + + /** + * \brief Returns the internal ros::Publisher. + * + * This really only exists so RawPublisher can implement no-copy intraprocess message + * passing easily. + */ + const ros::Publisher& getPublisher() const + { + ROS_ASSERT(simple_impl_); + return simple_impl_->pub_; + } + +private: + struct SimplePublisherPluginImpl + { + SimplePublisherPluginImpl(const ros::NodeHandle& nh) + : param_nh_(nh) + { + } + + const ros::NodeHandle param_nh_; + ros::Publisher pub_; + }; + + boost::scoped_ptr simple_impl_; + + typedef void (SimplePublisherPlugin::*SubscriberStatusMemFn)(const ros::SingleSubscriberPublisher& pub); + + /** + * Binds the user callback to subscriberCB(), which acts as an intermediary to expose + * a publish(Image) interface to the user while publishing to an internal topic. + */ + ros::SubscriberStatusCallback bindCB(const SubscriberStatusCallback& user_cb, + SubscriberStatusMemFn internal_cb_fn) + { + ros::SubscriberStatusCallback internal_cb = boost::bind(internal_cb_fn, this, _1); + if (user_cb) + return boost::bind(&SimplePublisherPlugin::subscriberCB, this, _1, user_cb, internal_cb); + else + return internal_cb; + } + + /** + * Forms the ros::SingleSubscriberPublisher for the internal communication topic into + * an image_transport::SingleSubscriberPublisher for Image messages and passes it + * to the user subscriber status callback. + */ + void subscriberCB(const ros::SingleSubscriberPublisher& ros_ssp, + const SubscriberStatusCallback& user_cb, + const ros::SubscriberStatusCallback& internal_cb) + { + // First call the internal callback (for sending setup headers, etc.) + internal_cb(ros_ssp); + + // Construct a function object for publishing sensor_msgs::Image through the + // subclass-implemented publish() using the ros::SingleSubscriberPublisher to send + // messages of the transport-specific type. + typedef void (SimplePublisherPlugin::*PublishMemFn)(const sensor_msgs::Image&, const PublishFn&) const; + PublishMemFn pub_mem_fn = &SimplePublisherPlugin::publish; + ImagePublishFn image_publish_fn = boost::bind(pub_mem_fn, this, _1, bindInternalPublisher(ros_ssp)); + + SingleSubscriberPublisher ssp(ros_ssp.getSubscriberName(), getTopic(), + boost::bind(&SimplePublisherPlugin::getNumSubscribers, this), + image_publish_fn); + user_cb(ssp); + } + + typedef boost::function ImagePublishFn; + + /** + * Returns a function object for publishing the transport-specific message type + * through some ROS publisher type. + * + * @param pub An object with method void publish(const M&) + */ + template + PublishFn bindInternalPublisher(const PubT& pub) const + { + // Bind PubT::publish(const Message&) as PublishFn + typedef void (PubT::*InternalPublishMemFn)(const M&) const; + InternalPublishMemFn internal_pub_mem_fn = &PubT::publish; + return boost::bind(internal_pub_mem_fn, &pub, _1); + } +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h b/ros/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h new file mode 100644 index 0000000..dd6ea3c --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/simple_subscriber_plugin.h @@ -0,0 +1,141 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_SIMPLE_SUBSCRIBER_PLUGIN_H +#define IMAGE_TRANSPORT_SIMPLE_SUBSCRIBER_PLUGIN_H + +#include "image_transport/subscriber_plugin.h" +#include + +namespace image_transport { + +/** + * \brief Base class to simplify implementing most plugins to Subscriber. + * + * The base class simplifies implementing a SubscriberPlugin in the common case that + * all communication with the matching PublisherPlugin happens over a single ROS + * topic using a transport-specific message type. SimpleSubscriberPlugin is templated + * on the transport-specific message type. + * + * A subclass need implement only two methods: + * - getTransportName() from SubscriberPlugin + * - internalCallback() - processes a message and invoked the user Image callback if + * appropriate. + * + * For access to the parameter server and name remappings, use nh(). + * + * getTopicToSubscribe() controls the name of the internal communication topic. It + * defaults to \/\. + */ +template +class SimpleSubscriberPlugin : public SubscriberPlugin +{ +public: + virtual ~SimpleSubscriberPlugin() {} + + virtual std::string getTopic() const + { + if (simple_impl_) return simple_impl_->sub_.getTopic(); + return std::string(); + } + + virtual uint32_t getNumPublishers() const + { + if (simple_impl_) return simple_impl_->sub_.getNumPublishers(); + return 0; + } + + virtual void shutdown() + { + if (simple_impl_) simple_impl_->sub_.shutdown(); + } + +protected: + /** + * \brief Process a message. Must be implemented by the subclass. + * + * @param message A message from the PublisherPlugin. + * @param user_cb The user Image callback to invoke, if appropriate. + */ + virtual void internalCallback(const typename M::ConstPtr& message, const Callback& user_cb) = 0; + + /** + * \brief Return the communication topic name for a given base topic. + * + * Defaults to \/\. + */ + virtual std::string getTopicToSubscribe(const std::string& base_topic) const + { + return base_topic + "/" + getTransportName(); + } + + virtual void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const Callback& callback, const ros::VoidPtr& tracked_object, + const TransportHints& transport_hints) + { + // Push each group of transport-specific parameters into a separate sub-namespace + ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); + simple_impl_.reset(new SimpleSubscriberPluginImpl(param_nh)); + + simple_impl_->sub_ = nh.subscribe(getTopicToSubscribe(base_topic), queue_size, + boost::bind(&SimpleSubscriberPlugin::internalCallback, this, _1, callback), + tracked_object, transport_hints.getRosHints()); + } + + /** + * \brief Returns the ros::NodeHandle to be used for parameter lookup. + */ + const ros::NodeHandle& nh() const + { + return simple_impl_->param_nh_; + } + +private: + struct SimpleSubscriberPluginImpl + { + SimpleSubscriberPluginImpl(const ros::NodeHandle& nh) + : param_nh_(nh) + { + } + + const ros::NodeHandle param_nh_; + ros::Subscriber sub_; + }; + + boost::scoped_ptr simple_impl_; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/single_subscriber_publisher.h b/ros/image_common/image_transport/include/image_transport/single_subscriber_publisher.h new file mode 100644 index 0000000..025a99e --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/single_subscriber_publisher.h @@ -0,0 +1,80 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_SINGLE_SUBSCRIBER_PUBLISHER +#define IMAGE_TRANSPORT_SINGLE_SUBSCRIBER_PUBLISHER + +#include +#include +#include + +namespace image_transport { + +/** + * \brief Allows publication of an image to a single subscriber. Only available inside + * subscriber connection callbacks. + */ +class SingleSubscriberPublisher : boost::noncopyable +{ +public: + typedef boost::function GetNumSubscribersFn; + typedef boost::function PublishFn; + + SingleSubscriberPublisher(const std::string& caller_id, const std::string& topic, + const GetNumSubscribersFn& num_subscribers_fn, + const PublishFn& publish_fn); + + std::string getSubscriberName() const; + + std::string getTopic() const; + + uint32_t getNumSubscribers() const; + + void publish(const sensor_msgs::Image& message) const; + void publish(const sensor_msgs::ImageConstPtr& message) const; + +private: + std::string caller_id_; + std::string topic_; + GetNumSubscribersFn num_subscribers_fn_; + PublishFn publish_fn_; + + friend class Publisher; // to get publish_fn_ directly +}; + +typedef boost::function SubscriberStatusCallback; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/subscriber.h b/ros/image_common/image_transport/include/image_transport/subscriber.h new file mode 100644 index 0000000..f887d59 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/subscriber.h @@ -0,0 +1,111 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_SUBSCRIBER_H +#define IMAGE_TRANSPORT_SUBSCRIBER_H + +#include +#include +#include "image_transport/transport_hints.h" +#include "image_transport/exception.h" +#include "image_transport/loader_fwds.h" + +namespace image_transport { + +/** + * \brief Manages a subscription callback on a specific topic that can be interpreted + * as an Image topic. + * + * Subscriber is the client-side counterpart to Publisher. By loading the + * appropriate plugin, it can subscribe to a base image topic using any available + * transport. The complexity of what transport is actually used is hidden from the user, + * who sees only a normal Image callback. + * + * A Subscriber should always be created through a call to ImageTransport::subscribe(), + * or copied from one that was. + * Once all copies of a specific Subscriber go out of scope, the subscription callback + * associated with that handle will stop being called. Once all Subscriber for a given + * topic go out of scope the topic will be unsubscribed. + */ +class Subscriber +{ +public: + Subscriber() {} + + /** + * \brief Returns the base image topic. + * + * The Subscriber may actually be subscribed to some transport-specific topic that + * differs from the base topic. + */ + std::string getTopic() const; + + /** + * \brief Returns the number of publishers this subscriber is connected to. + */ + uint32_t getNumPublishers() const; + + /** + * \brief Returns the name of the transport being used. + */ + std::string getTransport() const; + + /** + * \brief Unsubscribe the callback associated with this Subscriber. + */ + void shutdown(); + + operator void*() const; + bool operator< (const Subscriber& rhs) const { return impl_ < rhs.impl_; } + bool operator!=(const Subscriber& rhs) const { return impl_ != rhs.impl_; } + bool operator==(const Subscriber& rhs) const { return impl_ == rhs.impl_; } + +private: + Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const boost::function& callback, + const ros::VoidPtr& tracked_object, const TransportHints& transport_hints, + const SubLoaderPtr& loader); + + struct Impl; + typedef boost::shared_ptr ImplPtr; + typedef boost::weak_ptr ImplWPtr; + + ImplPtr impl_; + + friend class ImageTransport; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/subscriber_filter.h b/ros/image_common/image_transport/include/image_transport/subscriber_filter.h new file mode 100644 index 0000000..3d5be7b --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/subscriber_filter.h @@ -0,0 +1,163 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_SUBSCRIBER_FILTER_H +#define IMAGE_TRANSPORT_SUBSCRIBER_FILTER_H + +#include +#include + +#include "image_transport/image_transport.h" + +namespace image_transport { + +/** + * \brief Image subscription filter. + * + * This class wraps Subscriber as a "filter" compatible with the message_filters + * package. It acts as a highest-level filter, simply passing messages from an image + * transport subscription through to the filters which have connected to it. + * + * When this object is destroyed it will unsubscribe from the ROS subscription. + * + * \section connections CONNECTIONS + * + * SubscriberFilter has no input connection. + * + * The output connection for the SubscriberFilter object is the same signature as for roscpp + * subscription callbacks, ie. +\verbatim +void callback(const boost::shared_ptr&); +\endverbatim + */ +class SubscriberFilter : public message_filters::SimpleFilter +{ +public: + /** + * \brief Constructor + * + * See the ros::NodeHandle::subscribe() variants for more information on the parameters + * + * \param nh The ros::NodeHandle to use to subscribe. + * \param base_topic The topic to subscribe to. + * \param queue_size The subscription queue size + * \param transport_hints The transport hints to pass along + */ + SubscriberFilter(ImageTransport& it, const std::string& base_topic, uint32_t queue_size, + const TransportHints& transport_hints = TransportHints()) + { + subscribe(it, base_topic, queue_size, transport_hints); + } + + /** + * \brief Empty constructor, use subscribe() to subscribe to a topic + */ + SubscriberFilter() + { + } + + ~SubscriberFilter() + { + unsubscribe(); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param nh The ros::NodeHandle to use to subscribe. + * \param base_topic The topic to subscribe to. + * \param queue_size The subscription queue size + * \param transport_hints The transport hints to pass along + */ + void subscribe(ImageTransport& it, const std::string& base_topic, uint32_t queue_size, + const TransportHints& transport_hints = TransportHints()) + { + unsubscribe(); + + sub_ = it.subscribe(base_topic, queue_size, boost::bind(&SubscriberFilter::cb, this, _1), + ros::VoidPtr(), transport_hints); + } + + /** + * \brief Force immediate unsubscription of this subscriber from its topic + */ + void unsubscribe() + { + sub_.shutdown(); + } + + std::string getTopic() const + { + return sub_.getTopic(); + } + + /** + * \brief Returns the number of publishers this subscriber is connected to. + */ + uint32_t getNumPublishers() const + { + return sub_.getNumPublishers(); + } + + /** + * \brief Returns the name of the transport being used. + */ + std::string getTransport() const + { + return sub_.getTransport(); + } + + /** + * \brief Returns the internal image_transport::Subscriber object. + */ + const Subscriber& getSubscriber() const + { + return sub_; + } + +private: + + void cb(const sensor_msgs::ImageConstPtr& m) + { + signalMessage(m); + } + + Subscriber sub_; +}; + +} + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/subscriber_plugin.h b/ros/image_common/image_transport/include/image_transport/subscriber_plugin.h new file mode 100644 index 0000000..4601dc9 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/subscriber_plugin.h @@ -0,0 +1,141 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_SUBSCRIBER_PLUGIN_H +#define IMAGE_TRANSPORT_SUBSCRIBER_PLUGIN_H + +#include +#include +#include +#include "image_transport/transport_hints.h" + +namespace image_transport { + +/** + * \brief Base class for plugins to Subscriber. + */ +class SubscriberPlugin : boost::noncopyable +{ +public: + typedef boost::function Callback; + + virtual ~SubscriberPlugin() {} + + /** + * \brief Get a string identifier for the transport provided by + * this plugin. + */ + virtual std::string getTransportName() const = 0; + + /** + * \brief Subscribe to an image topic, version for arbitrary boost::function object. + */ + void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const Callback& callback, const ros::VoidPtr& tracked_object = ros::VoidPtr(), + const TransportHints& transport_hints = TransportHints()) + { + return subscribeImpl(nh, base_topic, queue_size, callback, tracked_object, transport_hints); + } + + /** + * \brief Subscribe to an image topic, version for bare function. + */ + void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + void(*fp)(const sensor_msgs::ImageConstPtr&), + const TransportHints& transport_hints = TransportHints()) + { + return subscribe(nh, base_topic, queue_size, + boost::function(fp), + ros::VoidPtr(), transport_hints); + } + + /** + * \brief Subscribe to an image topic, version for class member function with bare pointer. + */ + template + void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + void(T::*fp)(const sensor_msgs::ImageConstPtr&), T* obj, + const TransportHints& transport_hints = TransportHints()) + { + return subscribe(nh, base_topic, queue_size, boost::bind(fp, obj, _1), ros::VoidPtr(), transport_hints); + } + + /** + * \brief Subscribe to an image topic, version for class member function with shared_ptr. + */ + template + void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + void(T::*fp)(const sensor_msgs::ImageConstPtr&), + const boost::shared_ptr& obj, + const TransportHints& transport_hints = TransportHints()) + { + return subscribe(nh, base_topic, queue_size, boost::bind(fp, obj.get(), _1), obj, transport_hints); + } + + /** + * \brief Get the transport-specific communication topic. + */ + virtual std::string getTopic() const = 0; + + /** + * \brief Returns the number of publishers this subscriber is connected to. + */ + virtual uint32_t getNumPublishers() const = 0; + + /** + * \brief Unsubscribe the callback associated with this SubscriberPlugin. + */ + virtual void shutdown() = 0; + + /** + * \brief Return the lookup name of the SubscriberPlugin associated with a specific + * transport identifier. + */ + static std::string getLookupName(const std::string& transport_type) + { + return "image_transport/" + transport_type + "_sub"; + } + +protected: + /** + * \brief Subscribe to an image transport topic. Must be implemented by the subclass. + */ + virtual void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const Callback& callback, const ros::VoidPtr& tracked_object, + const TransportHints& transport_hints) = 0; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/include/image_transport/transport_hints.h b/ros/image_common/image_transport/include/image_transport/transport_hints.h new file mode 100644 index 0000000..1524d18 --- /dev/null +++ b/ros/image_common/image_transport/include/image_transport/transport_hints.h @@ -0,0 +1,94 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef IMAGE_TRANSPORT_TRANSPORT_HINTS_H +#define IMAGE_TRANSPORT_TRANSPORT_HINTS_H + +#include + +namespace image_transport { + +/** + * \brief Stores transport settings for an image topic subscription. + */ +class TransportHints +{ +public: + /** + * \brief Constructor. + * + * The default transport can be overridden by setting a certain parameter to the + * name of the desired transport. By default this parameter is named "image_transport" + * in the node's local namespace. For consistency across ROS applications, the + * name of this parameter should not be changed without good reason. + * + * @param default_transport Preferred transport to use + * @param ros_hints Hints to pass through to ROS subscriptions + * @param parameter_nh Node handle to use when looking up the transport parameter. + * Defaults to the local namespace. + * @param parameter_name The name of the transport parameter + */ + TransportHints(const std::string& default_transport = "raw", + const ros::TransportHints& ros_hints = ros::TransportHints(), + const ros::NodeHandle& parameter_nh = ros::NodeHandle("~"), + const std::string& parameter_name = "image_transport") + : ros_hints_(ros_hints), parameter_nh_(parameter_nh) + { + parameter_nh_.param(parameter_name, transport_, default_transport); + } + + const std::string& getTransport() const + { + return transport_; + } + + const ros::TransportHints& getRosHints() const + { + return ros_hints_; + } + + const ros::NodeHandle& getParameterNH() const + { + return parameter_nh_; + } + +private: + std::string transport_; + ros::TransportHints ros_hints_; + ros::NodeHandle parameter_nh_; +}; + +} //namespace image_transport + +#endif diff --git a/ros/image_common/image_transport/mainpage.dox b/ros/image_common/image_transport/mainpage.dox new file mode 100644 index 0000000..2df6f5b --- /dev/null +++ b/ros/image_common/image_transport/mainpage.dox @@ -0,0 +1,45 @@ +/** +\mainpage +\htmlinclude manifest.html + +\section codeapi Code API +When transporting images, you should use image_transport's classes as drop-in +replacements for ros::Publisher and ros::Subscriber. +- image_transport::ImageTransport - use this to create a Publisher or Subscriber +- image_transport::Publisher - manage advertisements for an image topic, using all available transport options +- image_transport::Subscriber - manage an Image subscription callback using a particular transport + +Camera drivers publish a "camera_info" sibling topic containing important metadata on how to +interpret an image for vision applications. image_transport included helper classes to +publish (image, info) message pairs and re-synchronize them on the client side: +- image_transport::CameraPublisher - manage advertisements for camera images +- image_transport::CameraSubscriber - manage a single subscription callback to synchronized image (using any transport) and CameraInfo topics + +For other synchronization or filtering needs, see the low-level filter class: +- image_transport::SubscriberFilter - a wrapper for image_transport::Subscriber compatible with message_filters + +\subsection writing_plugin Writing a plugin +If you are an advanced user implementing your own image transport option, you will need to +implement these base-level interfaces: +- image_transport::PublisherPlugin +- image_transport::SubscriberPlugin + +In the common case that all communication between PublisherPlugin and SubscriberPlugin happens +over a single ROS topic using a transport-specific message type, writing the plugins is vastly +simplified by using these base classes instead: +- image_transport::SimplePublisherPlugin - see image_transport::RawPublisher for a trivial example +- image_transport::SimpleSubscriberPlugin - see image_transport::RawSubscriber for a trivial example + +\section rosapi ROS API + +\subsection pub_sub_rosapi Publishers and Subscribers + +Because they encapsulate complicated communication behavior, image_transport publisher +and subscriber classes have a public ROS API as well as a code API. See the wiki +documentation for details. + +Although image_transport::Publisher may publish many topics, in all code interfaces you should +use only the name of the "base topic." The image transport classes will figure out the name of +the dedicated ROS topic to use for the desired transport. + +*/ diff --git a/ros/image_common/image_transport/package.xml b/ros/image_common/image_transport/package.xml new file mode 100644 index 0000000..388e6cd --- /dev/null +++ b/ros/image_common/image_transport/package.xml @@ -0,0 +1,39 @@ + + image_transport + 1.11.12 + + + image_transport should always be used to subscribe to and publish images. It provides transparent + support for transporting images in low-bandwidth compressed formats. Examples (provided by separate + plugin packages) include JPEG/PNG compression and Theora streaming video. + + + Patrick Mihelich + Jack O'Quin + Vincent Rabaud + BSD + + http://ros.org/wiki/image_transport + https://github.com/ros-perception/image_common/issues + https://github.com/ros-perception/image_common + + + + + + catkin + + message_filters + pluginlib + rosconsole + roscpp + roslib + sensor_msgs + + message_filters + pluginlib + rosconsole + roscpp + roslib + sensor_msgs + diff --git a/ros/image_common/image_transport/src/camera_common.cpp b/ros/image_common/image_transport/src/camera_common.cpp new file mode 100644 index 0000000..a09f764 --- /dev/null +++ b/ros/image_common/image_transport/src/camera_common.cpp @@ -0,0 +1,58 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/camera_common.h" +#include +#include +#include +#include + +namespace image_transport { + +std::string getCameraInfoTopic(const std::string& base_topic) +{ + // Split into separate names + std::vector names; + boost::algorithm::split(names, base_topic, boost::algorithm::is_any_of("/"), + boost::algorithm::token_compress_on); + // Get rid of empty tokens from trailing slashes + while (names.back().empty()) + names.pop_back(); + // Replace image name with "camera_info" + names.back() = "camera_info"; + // Join back together into topic name + return boost::algorithm::join(names, "/"); +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/camera_publisher.cpp b/ros/image_common/image_transport/src/camera_publisher.cpp new file mode 100644 index 0000000..232905a --- /dev/null +++ b/ros/image_common/image_transport/src/camera_publisher.cpp @@ -0,0 +1,160 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/image_transport.h" +#include "image_transport/camera_common.h" + +namespace image_transport { + +struct CameraPublisher::Impl +{ + Impl() + : unadvertised_(false) + { + } + + ~Impl() + { + shutdown(); + } + + bool isValid() const + { + return !unadvertised_; + } + + void shutdown() + { + if (!unadvertised_) { + unadvertised_ = true; + image_pub_.shutdown(); + info_pub_.shutdown(); + } + } + + Publisher image_pub_; + ros::Publisher info_pub_; + bool unadvertised_; + //double constructed_; +}; + +CameraPublisher::CameraPublisher(ImageTransport& image_it, ros::NodeHandle& info_nh, + const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& image_connect_cb, + const SubscriberStatusCallback& image_disconnect_cb, + const ros::SubscriberStatusCallback& info_connect_cb, + const ros::SubscriberStatusCallback& info_disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch) + : impl_(new Impl) +{ + // Explicitly resolve name here so we compute the correct CameraInfo topic when the + // image topic is remapped (#4539). + std::string image_topic = info_nh.resolveName(base_topic); + std::string info_topic = getCameraInfoTopic(image_topic); + + impl_->image_pub_ = image_it.advertise(image_topic, queue_size, image_connect_cb, + image_disconnect_cb, tracked_object, latch); + impl_->info_pub_ = info_nh.advertise(info_topic, queue_size, info_connect_cb, + info_disconnect_cb, tracked_object, latch); +} + +uint32_t CameraPublisher::getNumSubscribers() const +{ + if (impl_ && impl_->isValid()) + return std::max(impl_->image_pub_.getNumSubscribers(), impl_->info_pub_.getNumSubscribers()); + return 0; +} + +std::string CameraPublisher::getTopic() const +{ + if (impl_) return impl_->image_pub_.getTopic(); + return std::string(); +} + +std::string CameraPublisher::getInfoTopic() const +{ + if (impl_) return impl_->info_pub_.getTopic(); + return std::string(); +} + +void CameraPublisher::publish(const sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info) const +{ + if (!impl_ || !impl_->isValid()) { + ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher"); + return; + } + + impl_->image_pub_.publish(image); + impl_->info_pub_.publish(info); +} + +void CameraPublisher::publish(const sensor_msgs::ImageConstPtr& image, + const sensor_msgs::CameraInfoConstPtr& info) const +{ + if (!impl_ || !impl_->isValid()) { + ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher"); + return; + } + + impl_->image_pub_.publish(image); + impl_->info_pub_.publish(info); +} + +void CameraPublisher::publish(sensor_msgs::Image& image, sensor_msgs::CameraInfo& info, + ros::Time stamp) const +{ + if (!impl_ || !impl_->isValid()) { + ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher"); + return; + } + + image.header.stamp = stamp; + info.header.stamp = stamp; + publish(image, info); +} + +void CameraPublisher::shutdown() +{ + if (impl_) { + impl_->shutdown(); + impl_.reset(); + } +} + +CameraPublisher::operator void*() const +{ + return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/camera_subscriber.cpp b/ros/image_common/image_transport/src/camera_subscriber.cpp new file mode 100644 index 0000000..a91a14e --- /dev/null +++ b/ros/image_common/image_transport/src/camera_subscriber.cpp @@ -0,0 +1,160 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/camera_subscriber.h" +#include "image_transport/subscriber_filter.h" +#include "image_transport/camera_common.h" +#include +#include + +inline void increment(int* value) +{ + ++(*value); +} + +namespace image_transport { + +struct CameraSubscriber::Impl +{ + Impl(uint32_t queue_size) + : sync_(queue_size), + unsubscribed_(false), + image_received_(0), info_received_(0), both_received_(0) + {} + + ~Impl() + { + shutdown(); + } + + bool isValid() const + { + return !unsubscribed_; + } + + void shutdown() + { + if (!unsubscribed_) { + unsubscribed_ = true; + image_sub_.unsubscribe(); + info_sub_.unsubscribe(); + } + } + + void checkImagesSynchronized() + { + int threshold = 3 * both_received_; + if (image_received_ > threshold || info_received_ > threshold) { + ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else + "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. " + "In the last 10s:\n" + "\tImage messages received: %d\n" + "\tCameraInfo messages received: %d\n" + "\tSynchronized pairs: %d", + image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), + image_received_, info_received_, both_received_); + } + image_received_ = info_received_ = both_received_ = 0; + } + + SubscriberFilter image_sub_; + message_filters::Subscriber info_sub_; + message_filters::TimeSynchronizer sync_; + bool unsubscribed_; + // For detecting when the topics aren't synchronized + ros::WallTimer check_synced_timer_; + int image_received_, info_received_, both_received_; +}; + +CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh, + const std::string& base_topic, uint32_t queue_size, + const Callback& callback, const ros::VoidPtr& tracked_object, + const TransportHints& transport_hints) + : impl_(new Impl(queue_size)) +{ + // Must explicitly remap the image topic since we then do some string manipulation on it + // to figure out the sibling camera_info topic. + std::string image_topic = info_nh.resolveName(base_topic); + std::string info_topic = getCameraInfoTopic(image_topic); + impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints); + impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints()); + impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_); + // need for Boost.Bind here is kind of broken + impl_->sync_.registerCallback(boost::bind(callback, _1, _2)); + + // Complain every 10s if it appears that the image and info topics are not synchronized + impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_)); + impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_)); + impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_)); + impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0), + boost::bind(&Impl::checkImagesSynchronized, impl_.get())); +} + +std::string CameraSubscriber::getTopic() const +{ + if (impl_) return impl_->image_sub_.getTopic(); + return std::string(); +} + +std::string CameraSubscriber::getInfoTopic() const +{ + if (impl_) return impl_->info_sub_.getTopic(); + return std::string(); +} + +uint32_t CameraSubscriber::getNumPublishers() const +{ + /// @todo Fix this when message_filters::Subscriber has getNumPublishers() + //if (impl_) return std::max(impl_->image_sub_.getNumPublishers(), impl_->info_sub_.getNumPublishers()); + if (impl_) return impl_->image_sub_.getNumPublishers(); + return 0; +} + +std::string CameraSubscriber::getTransport() const +{ + if (impl_) return impl_->image_sub_.getTransport(); + return std::string(); +} + +void CameraSubscriber::shutdown() +{ + if (impl_) impl_->shutdown(); +} + +CameraSubscriber::operator void*() const +{ + return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/image_transport.cpp b/ros/image_common/image_transport/src/image_transport.cpp new file mode 100644 index 0000000..88ba125 --- /dev/null +++ b/ros/image_common/image_transport/src/image_transport.cpp @@ -0,0 +1,148 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/image_transport.h" +#include "image_transport/publisher_plugin.h" +#include "image_transport/subscriber_plugin.h" +#include +#include +#include +#include + +namespace image_transport { + +struct ImageTransport::Impl +{ + ros::NodeHandle nh_; + PubLoaderPtr pub_loader_; + SubLoaderPtr sub_loader_; + + Impl(const ros::NodeHandle& nh) + : nh_(nh), + pub_loader_( boost::make_shared("image_transport", "image_transport::PublisherPlugin") ), + sub_loader_( boost::make_shared("image_transport", "image_transport::SubscriberPlugin") ) + { + } +}; + +ImageTransport::ImageTransport(const ros::NodeHandle& nh) + : impl_(new Impl(nh)) +{ +} + +ImageTransport::~ImageTransport() +{ +} + +Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size, bool latch) +{ + return advertise(base_topic, queue_size, SubscriberStatusCallback(), + SubscriberStatusCallback(), ros::VoidPtr(), latch); +} + +Publisher ImageTransport::advertise(const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& connect_cb, + const SubscriberStatusCallback& disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch) +{ + return Publisher(impl_->nh_, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch, impl_->pub_loader_); +} + +Subscriber ImageTransport::subscribe(const std::string& base_topic, uint32_t queue_size, + const boost::function& callback, + const ros::VoidPtr& tracked_object, const TransportHints& transport_hints) +{ + return Subscriber(impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints, impl_->sub_loader_); +} + +CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size, bool latch) +{ + return advertiseCamera(base_topic, queue_size, + SubscriberStatusCallback(), SubscriberStatusCallback(), + ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), + ros::VoidPtr(), latch); +} + +CameraPublisher ImageTransport::advertiseCamera(const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& image_connect_cb, + const SubscriberStatusCallback& image_disconnect_cb, + const ros::SubscriberStatusCallback& info_connect_cb, + const ros::SubscriberStatusCallback& info_disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch) +{ + return CameraPublisher(*this, impl_->nh_, base_topic, queue_size, image_connect_cb, image_disconnect_cb, + info_connect_cb, info_disconnect_cb, tracked_object, latch); +} + +CameraSubscriber ImageTransport::subscribeCamera(const std::string& base_topic, uint32_t queue_size, + const CameraSubscriber::Callback& callback, + const ros::VoidPtr& tracked_object, + const TransportHints& transport_hints) +{ + return CameraSubscriber(*this, impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints); +} + +std::vector ImageTransport::getDeclaredTransports() const +{ + std::vector transports = impl_->sub_loader_->getDeclaredClasses(); + // Remove the "_sub" at the end of each class name. + BOOST_FOREACH(std::string& transport, transports) { + transport = boost::erase_last_copy(transport, "_sub"); + } + return transports; +} + +std::vector ImageTransport::getLoadableTransports() const +{ + std::vector loadableTransports; + + BOOST_FOREACH( const std::string& transportPlugin, impl_->sub_loader_->getDeclaredClasses() ) + { + // If the plugin loads without throwing an exception, add its + // transport name to the list of valid plugins, otherwise ignore + // it. + try + { + boost::shared_ptr sub = impl_->sub_loader_->createInstance(transportPlugin); + loadableTransports.push_back(boost::erase_last_copy(transportPlugin, "_sub")); // Remove the "_sub" at the end of each class name. + } + catch (const pluginlib::LibraryLoadException& e) {} + catch (const pluginlib::CreateClassException& e) {} + } + + return loadableTransports; + +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/list_transports.cpp b/ros/image_common/image_transport/src/list_transports.cpp new file mode 100644 index 0000000..dba8812 --- /dev/null +++ b/ros/image_common/image_transport/src/list_transports.cpp @@ -0,0 +1,141 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/publisher_plugin.h" +#include "image_transport/subscriber_plugin.h" +#include +#include +#include +#include + +using namespace image_transport; +using namespace pluginlib; + +enum PluginStatus {SUCCESS, CREATE_FAILURE, LIB_LOAD_FAILURE, DOES_NOT_EXIST}; + +/// \cond +struct TransportDesc +{ + TransportDesc() + : pub_status(DOES_NOT_EXIST), sub_status(DOES_NOT_EXIST) + {} + + std::string package_name; + std::string pub_name; + PluginStatus pub_status; + std::string sub_name; + PluginStatus sub_status; +}; +/// \endcond + +int main(int argc, char** argv) +{ + ClassLoader pub_loader("image_transport", "image_transport::PublisherPlugin"); + ClassLoader sub_loader("image_transport", "image_transport::SubscriberPlugin"); + typedef std::map StatusMap; + StatusMap transports; + + BOOST_FOREACH(const std::string& lookup_name, pub_loader.getDeclaredClasses()) { + std::string transport_name = boost::erase_last_copy(lookup_name, "_pub"); + transports[transport_name].pub_name = lookup_name; + transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); + try { + boost::shared_ptr pub = pub_loader.createInstance(lookup_name); + transports[transport_name].pub_status = SUCCESS; + } + catch (const LibraryLoadException& e) { + transports[transport_name].pub_status = LIB_LOAD_FAILURE; + } + catch (const CreateClassException& e) { + transports[transport_name].pub_status = CREATE_FAILURE; + } + } + + BOOST_FOREACH(const std::string& lookup_name, sub_loader.getDeclaredClasses()) { + std::string transport_name = boost::erase_last_copy(lookup_name, "_sub"); + transports[transport_name].sub_name = lookup_name; + transports[transport_name].package_name = sub_loader.getClassPackage(lookup_name); + try { + boost::shared_ptr sub = sub_loader.createInstance(lookup_name); + transports[transport_name].sub_status = SUCCESS; + } + catch (const LibraryLoadException& e) { + transports[transport_name].sub_status = LIB_LOAD_FAILURE; + } + catch (const CreateClassException& e) { + transports[transport_name].sub_status = CREATE_FAILURE; + } + } + + bool problem_package = false; + printf("Declared transports:\n"); + BOOST_FOREACH(const StatusMap::value_type& value, transports) { + const TransportDesc& td = value.second; + printf("%s", value.first.c_str()); + if ((td.pub_status == CREATE_FAILURE || td.pub_status == LIB_LOAD_FAILURE) || + (td.sub_status == CREATE_FAILURE || td.sub_status == LIB_LOAD_FAILURE)) { + printf(" (*): Not available. Try 'catkin_make --pkg %s'.", td.package_name.c_str()); + problem_package = true; + } + printf("\n"); + } +#if 0 + if (problem_package) + printf("(*) \n"); +#endif + + printf("\nDetails:\n"); + BOOST_FOREACH(const StatusMap::value_type& value, transports) { + const TransportDesc& td = value.second; + printf("----------\n"); + printf("\"%s\"\n", value.first.c_str()); + if (td.pub_status == CREATE_FAILURE || td.sub_status == CREATE_FAILURE) { + printf("*** Plugins are built, but could not be loaded. The package may need to be rebuilt or may not be compatible with this release of image_common. ***\n"); + } + else if (td.pub_status == LIB_LOAD_FAILURE || td.sub_status == LIB_LOAD_FAILURE) { + printf("*** Plugins are not built. ***\n"); + } + printf(" - Provided by package: %s\n", td.package_name.c_str()); + if (td.pub_status == DOES_NOT_EXIST) + printf(" - No publisher provided\n"); + else + printf(" - Publisher: %s\n", pub_loader.getClassDescription(td.pub_name).c_str()); + if (td.sub_status == DOES_NOT_EXIST) + printf(" - No subscriber provided\n"); + else + printf(" - Subscriber: %s\n", sub_loader.getClassDescription(td.sub_name).c_str()); + } + + return 0; +} diff --git a/ros/image_common/image_transport/src/manifest.cpp b/ros/image_common/image_transport/src/manifest.cpp new file mode 100644 index 0000000..40a8acd --- /dev/null +++ b/ros/image_common/image_transport/src/manifest.cpp @@ -0,0 +1,41 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include +#include "image_transport/raw_publisher.h" +#include "image_transport/raw_subscriber.h" + +PLUGINLIB_EXPORT_CLASS( image_transport::RawPublisher, image_transport::PublisherPlugin) + +PLUGINLIB_EXPORT_CLASS( image_transport::RawSubscriber, image_transport::SubscriberPlugin) diff --git a/ros/image_common/image_transport/src/publisher.cpp b/ros/image_common/image_transport/src/publisher.cpp new file mode 100644 index 0000000..8822afe --- /dev/null +++ b/ros/image_common/image_transport/src/publisher.cpp @@ -0,0 +1,203 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/publisher.h" +#include "image_transport/publisher_plugin.h" +#include +#include +#include + +namespace image_transport { + +struct Publisher::Impl +{ + Impl() + : unadvertised_(false) + { + } + + ~Impl() + { + shutdown(); + } + + uint32_t getNumSubscribers() const + { + uint32_t count = 0; + BOOST_FOREACH(const boost::shared_ptr& pub, publishers_) + count += pub->getNumSubscribers(); + return count; + } + + std::string getTopic() const + { + return base_topic_; + } + + bool isValid() const + { + return !unadvertised_; + } + + void shutdown() + { + if (!unadvertised_) { + unadvertised_ = true; + BOOST_FOREACH(boost::shared_ptr& pub, publishers_) + pub->shutdown(); + publishers_.clear(); + } + } + + void subscriberCB(const SingleSubscriberPublisher& plugin_pub, + const SubscriberStatusCallback& user_cb) + { + SingleSubscriberPublisher ssp(plugin_pub.getSubscriberName(), getTopic(), + boost::bind(&Publisher::Impl::getNumSubscribers, this), + plugin_pub.publish_fn_); + user_cb(ssp); + } + + std::string base_topic_; + PubLoaderPtr loader_; + std::vector > publishers_; + bool unadvertised_; + //double constructed_; +}; + + +Publisher::Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const SubscriberStatusCallback& connect_cb, + const SubscriberStatusCallback& disconnect_cb, + const ros::VoidPtr& tracked_object, bool latch, + const PubLoaderPtr& loader) + : impl_(new Impl) +{ + // Resolve the name explicitly because otherwise the compressed topics don't remap + // properly (#3652). + impl_->base_topic_ = nh.resolveName(base_topic); + impl_->loader_ = loader; + + BOOST_FOREACH(const std::string& lookup_name, loader->getDeclaredClasses()) { + try { + boost::shared_ptr pub = loader->createInstance(lookup_name); + impl_->publishers_.push_back(pub); + pub->advertise(nh, impl_->base_topic_, queue_size, rebindCB(connect_cb), + rebindCB(disconnect_cb), tracked_object, latch); + } + catch (const std::runtime_error& e) { + ROS_DEBUG("Failed to load plugin %s, error string: %s", + lookup_name.c_str(), e.what()); + } + } + + if (impl_->publishers_.empty()) + throw Exception("No plugins found! Does `rospack plugins --attrib=plugin " + "image_transport` find any packages?"); +} + +uint32_t Publisher::getNumSubscribers() const +{ + if (impl_ && impl_->isValid()) return impl_->getNumSubscribers(); + return 0; +} + +std::string Publisher::getTopic() const +{ + if (impl_) return impl_->getTopic(); + return std::string(); +} + +void Publisher::publish(const sensor_msgs::Image& message) const +{ + if (!impl_ || !impl_->isValid()) { + ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher"); + return; + } + + BOOST_FOREACH(const boost::shared_ptr& pub, impl_->publishers_) { + if (pub->getNumSubscribers() > 0) + pub->publish(message); + } +} + +void Publisher::publish(const sensor_msgs::ImageConstPtr& message) const +{ + if (!impl_ || !impl_->isValid()) { + ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::Publisher"); + return; + } + + BOOST_FOREACH(const boost::shared_ptr& pub, impl_->publishers_) { + if (pub->getNumSubscribers() > 0) + pub->publish(message); + } +} + +void Publisher::shutdown() +{ + if (impl_) { + impl_->shutdown(); + impl_.reset(); + } +} + +Publisher::operator void*() const +{ + return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; +} + +void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr, + const SingleSubscriberPublisher& plugin_pub, + const SubscriberStatusCallback& user_cb) +{ + if (ImplPtr impl = impl_wptr.lock()) + impl->subscriberCB(plugin_pub, user_cb); +} + +SubscriberStatusCallback Publisher::rebindCB(const SubscriberStatusCallback& user_cb) +{ + // Note: the subscriber callback must be bound to the internal Impl object, not + // 'this'. Due to copying behavior the Impl object may outlive the original Publisher + // instance. But it should not outlive the last Publisher, so we use a weak_ptr. + if (user_cb) + { + ImplWPtr impl_wptr(impl_); + return boost::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb); + } + else + return SubscriberStatusCallback(); +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/raw_publisher.cpp b/ros/image_common/image_transport/src/raw_publisher.cpp new file mode 100644 index 0000000..c1eda75 --- /dev/null +++ b/ros/image_common/image_transport/src/raw_publisher.cpp @@ -0,0 +1,140 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include +#include +#include + +/** The code in the following namespace copies a lof of code from cv_bridge + * It does not depend on cv_bridge to not depend on OpenCV + * It re-defines a CvImage so that we can publish that object and not a + * sensor_msgs::Image, which requires a memory copy + */ + +class ImageTransportImage +{ +public: + sensor_msgs::Image image_; //!< ROS header + const uint8_t* data_; //!< Image data for use with OpenCV + + /** + * \brief Empty constructor. + */ + ImageTransportImage() {} + + /** + * \brief Constructor. + */ + ImageTransportImage(const sensor_msgs::Image& image, const uint8_t* data) + : image_(image), data_(data) + { + } +}; + +/// @cond DOXYGEN_IGNORE +namespace ros { + +namespace message_traits { + +template<> struct MD5Sum +{ + static const char* value() { return MD5Sum::value(); } + static const char* value(const ImageTransportImage&) { return value(); } + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; + + // If the definition of sensor_msgs/Image changes, we'll get a compile error here. + ROS_STATIC_ASSERT(MD5Sum::static_value1 == 0x060021388200f6f0ULL); + ROS_STATIC_ASSERT(MD5Sum::static_value2 == 0xf447d0fcd9c64743ULL); +}; + +template<> struct DataType +{ + static const char* value() { return DataType::value(); } + static const char* value(const ImageTransportImage&) { return value(); } +}; + +template<> struct Definition +{ + static const char* value() { return Definition::value(); } + static const char* value(const ImageTransportImage&) { return value(); } +}; + +template<> struct HasHeader : TrueType {}; + +} // namespace ros::message_traits + +namespace serialization { + +template<> struct Serializer +{ + /// @todo Still ignoring endianness... + + template + inline static void write(Stream& stream, const ImageTransportImage& m) + { + stream.next(m.image_.header); + stream.next((uint32_t)m.image_.height); // height + stream.next((uint32_t)m.image_.width); // width + stream.next(m.image_.encoding); + uint8_t is_bigendian = 0; + stream.next(is_bigendian); + stream.next((uint32_t)m.image_.step); + size_t data_size = m.image_.step*m.image_.height; + stream.next((uint32_t)data_size); + if (data_size > 0) + memcpy(stream.advance(data_size), m.data_, data_size); + } + + inline static uint32_t serializedLength(const ImageTransportImage& m) + { + size_t data_size = m.image_.step*m.image_.height; + return serializationLength(m.image_.header) + serializationLength(m.image_.encoding) + 17 + data_size; + } +}; + +} // namespace ros::serialization + +} // namespace ros + + +namespace image_transport { + +void RawPublisher::publish(const sensor_msgs::Image& message, const uint8_t* data) const +{ + getPublisher().publish(ImageTransportImage(message, data)); +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/republish.cpp b/ros/image_common/image_transport/src/republish.cpp new file mode 100644 index 0000000..14fb276 --- /dev/null +++ b/ros/image_common/image_transport/src/republish.cpp @@ -0,0 +1,86 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/image_transport.h" +#include "image_transport/publisher_plugin.h" +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "image_republisher", ros::init_options::AnonymousName); + if (argc < 2) { + printf("Usage: %s in_transport in:= [out_transport] out:=\n", argv[0]); + return 0; + } + ros::NodeHandle nh; + std::string in_topic = nh.resolveName("in"); + std::string in_transport = argv[1]; + std::string out_topic = nh.resolveName("out"); + + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub; + + if (argc < 3) { + // Use all available transports for output + image_transport::Publisher pub = it.advertise(out_topic, 1); + + // Use Publisher::publish as the subscriber callback + typedef void (image_transport::Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; + PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; + sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport); + + ros::spin(); + } + else { + // Use one specific transport for output + std::string out_transport = argv[2]; + + // Load transport plugin + typedef image_transport::PublisherPlugin Plugin; + pluginlib::ClassLoader loader("image_transport", "image_transport::PublisherPlugin"); + std::string lookup_name = Plugin::getLookupName(out_transport); + boost::shared_ptr pub( loader.createInstance(lookup_name) ); + pub->advertise(nh, out_topic, 1, image_transport::SubscriberStatusCallback(), + image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false); + + // Use PublisherPlugin::publish as the subscriber callback + typedef void (Plugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; + PublishMemFn pub_mem_fn = &Plugin::publish; + sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport); + + ros::spin(); + } + + return 0; +} diff --git a/ros/image_common/image_transport/src/single_subscriber_publisher.cpp b/ros/image_common/image_transport/src/single_subscriber_publisher.cpp new file mode 100644 index 0000000..10fc294 --- /dev/null +++ b/ros/image_common/image_transport/src/single_subscriber_publisher.cpp @@ -0,0 +1,74 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/single_subscriber_publisher.h" +#include "image_transport/publisher.h" + +namespace image_transport { + +SingleSubscriberPublisher::SingleSubscriberPublisher(const std::string& caller_id, const std::string& topic, + const GetNumSubscribersFn& num_subscribers_fn, + const PublishFn& publish_fn) + : caller_id_(caller_id), topic_(topic), + num_subscribers_fn_(num_subscribers_fn), + publish_fn_(publish_fn) +{ +} + +std::string SingleSubscriberPublisher::getSubscriberName() const +{ + return caller_id_; +} + +std::string SingleSubscriberPublisher::getTopic() const +{ + return topic_; +} + +uint32_t SingleSubscriberPublisher::getNumSubscribers() const +{ + return num_subscribers_fn_(); +} + +void SingleSubscriberPublisher::publish(const sensor_msgs::Image& message) const +{ + publish_fn_(message); +} + +void SingleSubscriberPublisher::publish(const sensor_msgs::ImageConstPtr& message) const +{ + publish_fn_(*message); +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/src/subscriber.cpp b/ros/image_common/image_transport/src/subscriber.cpp new file mode 100644 index 0000000..b7ef654 --- /dev/null +++ b/ros/image_common/image_transport/src/subscriber.cpp @@ -0,0 +1,142 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "image_transport/subscriber.h" +#include "image_transport/subscriber_plugin.h" +#include +#include +#include + +namespace image_transport { + +struct Subscriber::Impl +{ + Impl() + : unsubscribed_(false) + { + } + + ~Impl() + { + shutdown(); + } + + bool isValid() const + { + return !unsubscribed_; + } + + void shutdown() + { + if (!unsubscribed_) { + unsubscribed_ = true; + if (subscriber_) + subscriber_->shutdown(); + } + } + + SubLoaderPtr loader_; + boost::shared_ptr subscriber_; + bool unsubscribed_; + //double constructed_; +}; + +Subscriber::Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, + const boost::function& callback, + const ros::VoidPtr& tracked_object, const TransportHints& transport_hints, + const SubLoaderPtr& loader) + : impl_(new Impl) +{ + // Load the plugin for the chosen transport. + std::string lookup_name = SubscriberPlugin::getLookupName(transport_hints.getTransport()); + try { + impl_->subscriber_ = loader->createInstance(lookup_name); + } + catch (pluginlib::PluginlibException& e) { + throw TransportLoadException(transport_hints.getTransport(), e.what()); + } + // Hang on to the class loader so our shared library doesn't disappear from under us. + impl_->loader_ = loader; + + // Try to catch if user passed in a transport-specific topic as base_topic. + std::string clean_topic = ros::names::clean(base_topic); + size_t found = clean_topic.rfind('/'); + if (found != std::string::npos) { + std::string transport = clean_topic.substr(found+1); + std::string plugin_name = SubscriberPlugin::getLookupName(transport); + std::vector plugins = loader->getDeclaredClasses(); + if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) { + std::string real_base_topic = clean_topic.substr(0, found); + ROS_WARN("[image_transport] It looks like you are trying to subscribe directly to a " + "transport-specific image topic '%s', in which case you will likely get a connection " + "error. Try subscribing to the base topic '%s' instead with parameter ~image_transport " + "set to '%s' (on the command line, _image_transport:=%s). " + "See http://ros.org/wiki/image_transport for details.", + clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); + } + } + + // Tell plugin to subscribe. + impl_->subscriber_->subscribe(nh, base_topic, queue_size, callback, tracked_object, transport_hints); +} + +std::string Subscriber::getTopic() const +{ + if (impl_) return impl_->subscriber_->getTopic(); + return std::string(); +} + +uint32_t Subscriber::getNumPublishers() const +{ + if (impl_) return impl_->subscriber_->getNumPublishers(); + return 0; +} + +std::string Subscriber::getTransport() const +{ + if (impl_) return impl_->subscriber_->getTransportName(); + return std::string(); +} + +void Subscriber::shutdown() +{ + if (impl_) impl_->shutdown(); +} + +Subscriber::operator void*() const +{ + return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; +} + +} //namespace image_transport diff --git a/ros/image_common/image_transport/tutorial/CMakeLists.txt b/ros/image_common/image_transport/tutorial/CMakeLists.txt new file mode 100644 index 0000000..ed5f28f --- /dev/null +++ b/ros/image_common/image_transport/tutorial/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 2.8) +project(image_transport_tutorial) + +find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport message_generation sensor_msgs) + +# add the resized image message +add_message_files(DIRECTORY msg + FILES ResizedImage.msg +) +generate_messages(DEPENDENCIES sensor_msgs) + +catkin_package(CATKIN_DEPENDS cv_bridge image_transport message_runtime sensor_msgs) + +find_package(OpenCV) + +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) + +# add the publisher example +add_executable(my_publisher src/my_publisher.cpp) +add_dependencies(my_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(my_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +# add the subscriber example +add_executable(my_subscriber src/my_subscriber.cpp) +add_dependencies(my_subscriber ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(my_subscriber ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +# add the plugin example +add_library(resized_publisher src/manifest.cpp src/resized_publisher.cpp src/resized_subscriber.cpp) +add_dependencies(resized_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(resized_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + + +# Mark executables and/or libraries for installation +install(TARGETS my_publisher my_subscriber resized_publisher + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES resized_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h b/ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h new file mode 100644 index 0000000..63ab3eb --- /dev/null +++ b/ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_publisher.h @@ -0,0 +1,15 @@ +#include +#include + +class ResizedPublisher : public image_transport::SimplePublisherPlugin +{ +public: + virtual std::string getTransportName() const + { + return "resized"; + } + +protected: + virtual void publish(const sensor_msgs::Image& message, + const PublishFn& publish_fn) const; +}; diff --git a/ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h b/ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h new file mode 100644 index 0000000..94b55f5 --- /dev/null +++ b/ros/image_common/image_transport/tutorial/include/image_transport_tutorial/resized_subscriber.h @@ -0,0 +1,17 @@ +#include +#include + +class ResizedSubscriber : public image_transport::SimpleSubscriberPlugin +{ +public: + virtual ~ResizedSubscriber() {} + + virtual std::string getTransportName() const + { + return "resized"; + } + +protected: + virtual void internalCallback(const typename image_transport_tutorial::ResizedImage::ConstPtr& message, + const Callback& user_cb); +}; diff --git a/ros/image_common/image_transport/tutorial/msg/ResizedImage.msg b/ros/image_common/image_transport/tutorial/msg/ResizedImage.msg new file mode 100644 index 0000000..d8c8fad --- /dev/null +++ b/ros/image_common/image_transport/tutorial/msg/ResizedImage.msg @@ -0,0 +1,3 @@ +uint32 original_height +uint32 original_width +sensor_msgs/Image image diff --git a/ros/image_common/image_transport/tutorial/package.xml b/ros/image_common/image_transport/tutorial/package.xml new file mode 100644 index 0000000..668cd0a --- /dev/null +++ b/ros/image_common/image_transport/tutorial/package.xml @@ -0,0 +1,26 @@ + + image_transport_tutorial + 0.0.0 + Tutorial for image_transport. This is useful for the tutorials at http://wiki.ros.org/image_transport/Tutorials/ + Vincent Rabaud + Vincent Rabaud + Apache 2.0 + + cv_bridge + image_transport + message_generation + opencv2 + sensor_msgs + + cv_bridge + image_transport + message_runtime + opencv2 + sensor_msgs + + catkin + + + + + diff --git a/ros/image_common/image_transport/tutorial/resized_plugins.xml b/ros/image_common/image_transport/tutorial/resized_plugins.xml new file mode 100644 index 0000000..bd2dff1 --- /dev/null +++ b/ros/image_common/image_transport/tutorial/resized_plugins.xml @@ -0,0 +1,13 @@ + + + + This plugin publishes a decimated version of the image. + + + + + + This plugin rescales a decimated image to its original size. + + + diff --git a/ros/image_common/image_transport/tutorial/src/manifest.cpp b/ros/image_common/image_transport/tutorial/src/manifest.cpp new file mode 100644 index 0000000..2c13b09 --- /dev/null +++ b/ros/image_common/image_transport/tutorial/src/manifest.cpp @@ -0,0 +1,8 @@ +#include +#include +#include + +PLUGINLIB_REGISTER_CLASS(resized_pub, ResizedPublisher, image_transport::PublisherPlugin) + +PLUGINLIB_REGISTER_CLASS(resized_sub, ResizedSubscriber, image_transport::SubscriberPlugin) + diff --git a/ros/image_common/image_transport/tutorial/src/my_publisher.cpp b/ros/image_common/image_transport/tutorial/src/my_publisher.cpp new file mode 100644 index 0000000..701dbdd --- /dev/null +++ b/ros/image_common/image_transport/tutorial/src/my_publisher.cpp @@ -0,0 +1,23 @@ +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "image_publisher"); + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + image_transport::Publisher pub = it.advertise("camera/image", 1); + + cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR); + sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg(); + + ros::Rate loop_rate(5); + while (nh.ok()) { + pub.publish(msg); + ros::spinOnce(); + loop_rate.sleep(); + } +} + diff --git a/ros/image_common/image_transport/tutorial/src/my_subscriber.cpp b/ros/image_common/image_transport/tutorial/src/my_subscriber.cpp new file mode 100644 index 0000000..50573b9 --- /dev/null +++ b/ros/image_common/image_transport/tutorial/src/my_subscriber.cpp @@ -0,0 +1,28 @@ +#include +#include +#include +#include + +void imageCallback(const sensor_msgs::ImageConstPtr& msg) +{ + try + { + cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "image_listener"); + ros::NodeHandle nh; + cv::namedWindow("view"); + cv::startWindowThread(); + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback); + ros::spin(); + cv::destroyWindow("view"); +} diff --git a/ros/image_common/image_transport/tutorial/src/resized_publisher.cpp b/ros/image_common/image_transport/tutorial/src/resized_publisher.cpp new file mode 100644 index 0000000..178f482 --- /dev/null +++ b/ros/image_common/image_transport/tutorial/src/resized_publisher.cpp @@ -0,0 +1,37 @@ +#include +#include +#include + +void ResizedPublisher::publish(const sensor_msgs::Image& message, + const PublishFn& publish_fn) const +{ + cv::Mat cv_image; + boost::shared_ptr tracked_object; + try + { + cv_image = cv_bridge::toCvShare(message, tracked_object, message.encoding)->image; + } + catch (cv::Exception &e) + { + ROS_ERROR("Could not convert from '%s' to '%s'.", message.encoding.c_str(), message.encoding.c_str()); + return; + } + + // Retrieve subsampling factor from the parameter server + double subsampling_factor; + std::string param_name; + nh().param("resized_image_transport_subsampling_factor", subsampling_factor, 2.0); + + // Rescale image + int new_width = cv_image.cols / subsampling_factor + 0.5; + int new_height = cv_image.rows / subsampling_factor + 0.5; + cv::Mat buffer; + cv::resize(cv_image, buffer, cv::Size(new_width, new_height)); + + // Set up ResizedImage and publish + image_transport_tutorial::ResizedImage resized_image; + resized_image.original_height = cv_image.rows; + resized_image.original_width = cv_image.cols; + resized_image.image = *(cv_bridge::CvImage(message.header, "bgr8", cv_image).toImageMsg()); + publish_fn(resized_image); +} diff --git a/ros/image_common/image_transport/tutorial/src/resized_subscriber.cpp b/ros/image_common/image_transport/tutorial/src/resized_subscriber.cpp new file mode 100644 index 0000000..299dc83 --- /dev/null +++ b/ros/image_common/image_transport/tutorial/src/resized_subscriber.cpp @@ -0,0 +1,18 @@ +#include +#include +#include + +void ResizedSubscriber::internalCallback(const image_transport_tutorial::ResizedImage::ConstPtr& msg, + const Callback& user_cb) +{ + // This is only for optimization, not to copy the image + boost::shared_ptr tracked_object_tmp; + cv::Mat img_rsz = cv_bridge::toCvShare(msg->image, tracked_object_tmp)->image; + // Resize the image to its original size + cv::Mat img_restored; + cv::resize(img_rsz, img_restored, cv::Size(msg->original_width, msg->original_height)); + + // Call the user callback with the restored image + cv_bridge::CvImage cv_img(msg->image.header, msg->image.encoding, img_restored); + user_cb(cv_img.toImageMsg()); +}; diff --git a/ros/image_common/polled_camera/CHANGELOG.rst b/ros/image_common/polled_camera/CHANGELOG.rst new file mode 100644 index 0000000..38c7065 --- /dev/null +++ b/ros/image_common/polled_camera/CHANGELOG.rst @@ -0,0 +1,186 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package polled_camera +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.12 (2017-01-29) +-------------------- +* Fix CMake of image_transport/tutorial and polled_camera + Fix loads of problems with the CMakeLists. +* 1.11.11 +* update changelogs +* address gcc6 build error in polled_camera + With gcc6, compiling fails with `stdlib.h: No such file or directory`, + as including '-isystem /usr/include' breaks with gcc6, cf., + https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129. + This commit addresses this issue for this package in the same way + it was addressed in various other ROS packages. A list of related + commits and pull requests is at: + https://github.com/ros/rosdistro/issues/12783 + Signed-off-by: Lukas Bulwahn +* Contributors: Lukas Bulwahn, Martin Guenther, Vincent Rabaud + +1.11.11 (2016-09-24) +-------------------- +* address gcc6 build error in polled_camera + With gcc6, compiling fails with `stdlib.h: No such file or directory`, + as including '-isystem /usr/include' breaks with gcc6, cf., + https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129. + This commit addresses this issue for this package in the same way + it was addressed in various other ROS packages. A list of related + commits and pull requests is at: + https://github.com/ros/rosdistro/issues/12783 + Signed-off-by: Lukas Bulwahn +* Contributors: Lukas Bulwahn + +1.11.10 (2016-01-19) +-------------------- + +1.11.9 (2016-01-17) +------------------- + +1.11.8 (2015-11-29) +------------------- + +1.11.7 (2015-07-28) +------------------- + +1.11.6 (2015-07-16) +------------------- + +1.11.5 (2015-05-14) +------------------- + +1.11.4 (2014-09-21) +------------------- + +1.11.3 (2014-05-19) +------------------- + +1.11.2 (2014-02-13) +------------------- + +1.11.1 (2014-01-26 02:33) +------------------------- + +1.11.0 (2013-07-20 12:23) +------------------------- + +1.10.5 (2014-01-26 02:34) +------------------------- + +1.10.4 (2013-07-20 11:42) +------------------------- +* add Jack as maintainer +* Contributors: Vincent Rabaud + +1.10.3 (2013-02-21 05:33) +------------------------- + +1.10.2 (2013-02-21 04:48) +------------------------- + +1.10.1 (2013-02-21 04:16) +------------------------- + +1.10.0 (2013-01-13) +------------------- +* use CATKIN_DEVEL_PREFIX instead of obsolete CATKIN_BUILD_PREFIX +* fix the urls +* Missing link flags exposed on OS X +* added license headers to various cpp and h files +* Contributors: Aaron Blasdel, Dirk Thomas, Vincent Rabaud, William Woodall + +1.9.22 (2012-12-16) +------------------- +* replace genmsg by message_generation +* Contributors: Vincent Rabaud + +1.9.21 (2012-12-14) +------------------- +* Updated package.xml file(s) to handle new catkin buildtool_depend requirement +* Contributors: mirzashah + +1.9.20 (2012-12-04) +------------------- + +1.9.19 (2012-11-08) +------------------- + +1.9.18 (2012-11-06) +------------------- +* remove the brief attribute +* Contributors: Vincent Rabaud + +1.9.17 (2012-10-30 19:32) +------------------------- +* comlpy to the new catkin API +* Contributors: Vincent Rabaud + +1.9.16 (2012-10-30 09:10) +------------------------- + +1.9.15 (2012-10-13 08:43) +------------------------- +* fix bad folder/libraries +* Contributors: Vincent Rabaud + +1.9.14 (2012-10-13 01:07) +------------------------- +* fix typo that resulted in bad installation of include folder +* Contributors: Vincent Rabaud + +1.9.13 (2012-10-06) +------------------- + +1.9.12 (2012-10-04) +------------------- + +1.9.11 (2012-10-02 02:56) +------------------------- + +1.9.10 (2012-10-02 02:42) +------------------------- + +1.9.9 (2012-10-01) +------------------ +* fix dependencies +* Contributors: Vincent Rabaud + +1.9.8 (2012-09-30) +------------------ +* add catkin as a dependency +* comply to the catkin API +* Contributors: Vincent Rabaud + +1.9.7 (2012-09-18 11:39) +------------------------ + +1.9.6 (2012-09-18 11:07) +------------------------ + +1.9.5 (2012-09-13) +------------------ +* install the include directories +* Contributors: Vincent Rabaud + +1.9.4 (2012-09-12 23:37) +------------------------ +* make sure we depend on the server +* Contributors: Vincent Rabaud + +1.9.3 (2012-09-12 20:44) +------------------------ + +1.9.2 (2012-09-10) +------------------ + +1.9.1 (2012-09-07 15:33) +------------------------ + +1.9.0 (2012-09-07 13:03) +------------------------ +* catkinize for Groovy +* polled_camera (rep0104): Changed callback to allow filling + status_message field. +* polled_camera (rep0104): Applied changes to GetPolledImage service. +* Contributors: Vincent Rabaud, eitan, gerkey, kwc, mihelich diff --git a/ros/image_common/polled_camera/CMakeLists.txt b/ros/image_common/polled_camera/CMakeLists.txt new file mode 100644 index 0000000..ca29988 --- /dev/null +++ b/ros/image_common/polled_camera/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 2.8) +project(polled_camera) + +# generate the server +find_package(catkin REQUIRED COMPONENTS image_transport message_generation roscpp sensor_msgs std_msgs) + +add_service_files(DIRECTORY srv FILES GetPolledImage.srv) + +generate_messages(DEPENDENCIES sensor_msgs std_msgs) + +# define the project +catkin_package(CATKIN_DEPENDS image_transport message_runtime roscpp sensor_msgs std_msgs + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +) + + +# create some library and exe +include_directories(include + ${catkin_INCLUDE_DIRS} + ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION} +) + +add_library(${PROJECT_NAME} src/publication_server.cpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +add_executable(poller src/poller.cpp) +add_dependencies(poller ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(poller ${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +install(TARGETS poller + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/ros/image_common/polled_camera/include/polled_camera/publication_server.h b/ros/image_common/polled_camera/include/polled_camera/publication_server.h new file mode 100644 index 0000000..d270e2a --- /dev/null +++ b/ros/image_common/polled_camera/include/polled_camera/publication_server.h @@ -0,0 +1,136 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#ifndef POLLED_CAMERA_PUBLICATION_SERVER_H +#define POLLED_CAMERA_PUBLICATION_SERVER_H + +#include +#include +#include +#include "polled_camera/GetPolledImage.h" + +namespace polled_camera { + +/** + * \brief Manage image requests from one or more clients. + * + * Instances of polled_camera::PublicationServer should be created using one of + * the overloads of polled_camera::advertise(). You must specify a driver + * callback that populates the requested data: +\code +void callback(polled_camera::GetPolledImage::Request& req, + polled_camera::GetPolledImage::Response& rsp, + sensor_msgs::Image& image, sensor_msgs::CameraInfo& info) +{ + // Capture an image and fill in the Image and CameraInfo messages here. + + // On success, set rsp.success = true. rsp.timestamp will be filled in + // automatically. + + // On failure, set rsp.success = false and fill rsp.status_message with an + // informative error message. +} +\endcode + */ +class PublicationServer +{ +public: + typedef boost::function DriverCallback; + + PublicationServer() {} + + /** + * \brief Unadvertise the request service and shut down all published topics. + */ + void shutdown(); + + std::string getService() const; + + operator void*() const; + bool operator< (const PublicationServer& rhs) const { return impl_ < rhs.impl_; } + bool operator==(const PublicationServer& rhs) const { return impl_ == rhs.impl_; } + bool operator!=(const PublicationServer& rhs) const { return impl_ != rhs.impl_; } + +private: + PublicationServer(const std::string& service, ros::NodeHandle& nh, + const DriverCallback& cb, const ros::VoidPtr& tracked_object); + + class Impl; + + boost::shared_ptr impl_; + + friend + PublicationServer advertise(ros::NodeHandle&, const std::string&, const DriverCallback&, + const ros::VoidPtr&); +}; + +/** + * \brief Advertise a polled image service, version for arbitrary boost::function object. + */ +PublicationServer advertise(ros::NodeHandle& nh, const std::string& service, + const PublicationServer::DriverCallback& cb, + const ros::VoidPtr& tracked_object = ros::VoidPtr()); + +/** + * \brief Advertise a polled image service, version for class member function with bare pointer. + */ +template +PublicationServer advertise(ros::NodeHandle& nh, const std::string& service, + void(T::*fp)(polled_camera::GetPolledImage::Request&, + polled_camera::GetPolledImage::Response&, + sensor_msgs::Image&, sensor_msgs::CameraInfo&), + T* obj) +{ + return advertise(nh, service, boost::bind(fp, obj, _1, _2, _3, _4)); +} + +/** + * \brief Advertise a polled image service, version for class member function with bare pointer. + */ +template +PublicationServer advertise(ros::NodeHandle& nh, const std::string& service, + void(T::*fp)(polled_camera::GetPolledImage::Request&, + polled_camera::GetPolledImage::Response&, + sensor_msgs::Image&, sensor_msgs::CameraInfo&), + const boost::shared_ptr& obj) +{ + return advertise(nh, service, boost::bind(fp, obj.get(), _1, _2, _3, _4), obj); +} + +} //namespace polled_camera + +#endif diff --git a/ros/image_common/polled_camera/mainpage.dox b/ros/image_common/polled_camera/mainpage.dox new file mode 100644 index 0000000..fc6fa6b --- /dev/null +++ b/ros/image_common/polled_camera/mainpage.dox @@ -0,0 +1,48 @@ +/** +\mainpage +\htmlinclude manifest.html + +NOTE: This package's API is not yet released. It may change from its current form. + +\b polled_camera contains a service definition for requesting polled images, +as well as a C++ server class to simplify publishing polled images to clients. + +The protocol for polling images from a camera driver node that supports it is as +follows: + - The camera driver advertises a service call \c \/request_image. + - The client calls the service, specifying an output namespace in the request. + - On receiving a request, the driver captures an image and returns its time stamp +in the service response. + - The \c Image and \c CameraInfo are published to \c \/image_raw +and \c \/camera_info, latched. + - Clients subscribe to the response topics just like any other camera image stream. + +\section codeapi Code API +Use polled_camera::PublicationServer in camera driver nodes (or similar) to +track client connections and respond to image requests. + +There is not currently a matching client class, but receiving polled images +is identical to subscribing to any other image topic. The only additional +step is using a ros::ServiceClient to make explicit requests: + +\code +#include +#include +#include + +void callback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& info); + +ros::NodeHandle nh; +image_transport::ImageTransport it(nh); + +image_transport::CameraSubscriber sub = it.subscribeCamera("output_ns/image_raw", 1, callback); +ros::ServiceClient client = nh.serviceClient("my_camera/request_image"); +polled_camera::GetPolledImage srv; +srv.request.response_namespace = "output_ns"; +if (client.call(srv)) +{ + ROS_INFO_STREAM("Image captured with timestamp " << srv.response.stamp); +} +\endcode + +*/ diff --git a/ros/image_common/polled_camera/package.xml b/ros/image_common/polled_camera/package.xml new file mode 100644 index 0000000..440b93d --- /dev/null +++ b/ros/image_common/polled_camera/package.xml @@ -0,0 +1,33 @@ + + polled_camera + 1.11.12 + + + polled_camera contains a service and C++ helper classes for implementing a polled + camera driver node and requesting images from it. The package is currently for + internal use as the API is still under development. + + + Patrick Mihelich + Jack O'Quin + Vincent Rabaud + BSD + + http://ros.org/wiki/polled_camera + https://github.com/ros-perception/image_common/issues + https://github.com/ros-perception/image_common + + catkin + + image_transport + message_generation + roscpp + sensor_msgs + std_msgs + + image_transport + message_runtime + roscpp + sensor_msgs + std_msgs + diff --git a/ros/image_common/polled_camera/src/poller.cpp b/ros/image_common/polled_camera/src/poller.cpp new file mode 100644 index 0000000..7426324 --- /dev/null +++ b/ros/image_common/polled_camera/src/poller.cpp @@ -0,0 +1,68 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include +#include +#include +#include "polled_camera/GetPolledImage.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "poller", ros::init_options::AnonymousName); + if (argc < 2) { + printf("Usage: %s camera:= output:=\n", argv[0]); + return 0; + } + double hz = boost::lexical_cast(argv[1]); + + ros::NodeHandle nh; + std::string service_name = nh.resolveName("camera") + "/request_image"; + ros::ServiceClient client = nh.serviceClient(service_name); + + polled_camera::GetPolledImage::Request req; + polled_camera::GetPolledImage::Response rsp; + req.response_namespace = nh.resolveName("output"); + + ros::Rate loop_rate(hz); + while (nh.ok()) { + if (client.call(req, rsp)) { + std::cout << "Timestamp: " << rsp.stamp << std::endl; + loop_rate.sleep(); + } + else { + ROS_ERROR("Service call failed"); + client.waitForExistence(); + } + } +} diff --git a/ros/image_common/polled_camera/src/publication_server.cpp b/ros/image_common/polled_camera/src/publication_server.cpp new file mode 100644 index 0000000..99097ea --- /dev/null +++ b/ros/image_common/polled_camera/src/publication_server.cpp @@ -0,0 +1,161 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include "polled_camera/publication_server.h" +#include + +namespace polled_camera { + +/// \cond + +class PublicationServer::Impl +{ +public: + ros::ServiceServer srv_server_; + DriverCallback driver_cb_; + ros::VoidPtr tracked_object_; + image_transport::ImageTransport it_; + std::map client_map_; + bool unadvertised_; + double constructed_; + + Impl(const ros::NodeHandle& nh) + : it_(nh), + unadvertised_(false), + constructed_(ros::WallTime::now().toSec()) + { + } + + ~Impl() + { + if (ros::WallTime::now().toSec() - constructed_ < 0.001) + ROS_WARN("PublicationServer destroyed immediately after creation. Did you forget to store the handle?"); + unadvertise(); + } + + bool isValid() const + { + return !unadvertised_; + } + + void unadvertise() + { + if (!unadvertised_) { + unadvertised_ = true; + srv_server_.shutdown(); + client_map_.clear(); + } + } + + bool requestCallback(polled_camera::GetPolledImage::Request& req, + polled_camera::GetPolledImage::Response& rsp) + { + std::string image_topic = req.response_namespace + "/image_raw"; + image_transport::CameraPublisher& pub = client_map_[image_topic]; + if (!pub) { + // Create a latching camera publisher. + pub = it_.advertiseCamera(image_topic, 1, image_transport::SubscriberStatusCallback(), + boost::bind(&Impl::disconnectCallback, this, _1), + ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), + ros::VoidPtr(), true /*latch*/); + ROS_INFO("Advertising %s", pub.getTopic().c_str()); + } + + // Correct zero binning values to one for convenience + req.binning_x = std::max(req.binning_x, (uint32_t)1); + req.binning_y = std::max(req.binning_y, (uint32_t)1); + + /// @todo Use pointers in prep for nodelet drivers? + sensor_msgs::Image image; + sensor_msgs::CameraInfo info; + driver_cb_(req, rsp, image, info); + + if (rsp.success) { + assert(image.header.stamp == info.header.stamp); + rsp.stamp = image.header.stamp; + pub.publish(image, info); + } + else { + ROS_ERROR("Failed to capture requested image, status message: '%s'", + rsp.status_message.c_str()); + } + + return true; // Success/failure indicated by rsp.success + } + + void disconnectCallback(const image_transport::SingleSubscriberPublisher& ssp) + { + // Shut down the publication when the subscription count drops to zero. + if (ssp.getNumSubscribers() == 0) { + ROS_INFO("Shutting down %s", ssp.getTopic().c_str()); + client_map_.erase(ssp.getTopic()); + } + } +}; + +/// \endcond + +PublicationServer::PublicationServer(const std::string& service, ros::NodeHandle& nh, + const DriverCallback& cb, const ros::VoidPtr& tracked_object) + : impl_(new Impl(nh)) +{ + impl_->driver_cb_ = cb; + impl_->tracked_object_ = tracked_object; + impl_->srv_server_ = nh.advertiseService<>(service, &Impl::requestCallback, impl_); +} + +void PublicationServer::shutdown() +{ + if (impl_) impl_->unadvertise(); +} + +std::string PublicationServer::getService() const +{ + if (impl_) return impl_->srv_server_.getService(); + return std::string(); +} + +PublicationServer::operator void*() const +{ + return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; +} + +PublicationServer advertise(ros::NodeHandle& nh, const std::string& service, + const PublicationServer::DriverCallback& cb, + const ros::VoidPtr& tracked_object) +{ + return PublicationServer(service, nh, cb, tracked_object); +} + +} //namespace polled_camera diff --git a/ros/image_common/polled_camera/srv/GetPolledImage.srv b/ros/image_common/polled_camera/srv/GetPolledImage.srv new file mode 100644 index 0000000..f73d825 --- /dev/null +++ b/ros/image_common/polled_camera/srv/GetPolledImage.srv @@ -0,0 +1,22 @@ +# Namespace to publish response topics in. A polled camera driver node +# should publish: +# /image_raw +# /camera_info +string response_namespace + +# Timeout for attempting to capture data from the device. This does not +# include latency from ROS communication, post-processing of raw camera +# data, etc. A zero duration indicates no time limit. +duration timeout + +# Binning settings, if supported by the camera. +uint32 binning_x +uint32 binning_y + +# Region of interest, if supported by the camera. +sensor_msgs/RegionOfInterest roi +--- +bool success # Could the image be captured? +string status_message # Error message in case of failure +time stamp # Timestamp of the captured image. Can be matched + # against incoming sensor_msgs/Image header. diff --git a/ros/vision_opencv/README.rst b/ros/vision_opencv/README.rst new file mode 100644 index 0000000..d9387bf --- /dev/null +++ b/ros/vision_opencv/README.rst @@ -0,0 +1,7 @@ +vision_opencv +============= + +.. image:: https://travis-ci.org/ros-perception/vision_opencv.svg?branch=indigo + :target: https://travis-ci.org/ros-perception/vision_opencv + +Packages for interfacing ROS with OpenCV, a library of programming functions for real time computer vision. diff --git a/ros/vision_opencv/cv_bridge/CHANGELOG.rst b/ros/vision_opencv/cv_bridge/CHANGELOG.rst new file mode 100644 index 0000000..01c397f --- /dev/null +++ b/ros/vision_opencv/cv_bridge/CHANGELOG.rst @@ -0,0 +1,369 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package cv_bridge +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.15 (2017-01-29) +-------------------- +* properly find Boost Python 2 or 3 + This fixes `#158 `_ +* Fill black color to depth nan region +* address gcc6 build error in cv_bridge and tune + With gcc6, compiling fails with `stdlib.h: No such file or directory`, + as including '-isystem /usr/include' breaks with gcc6, cf., + https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129 + This commit addresses this issue for cv_bridge in the same way + it was done in the commit ead421b8 [1] for image_geometry. + This issue was also addressed in various other ROS packages. + A list of related commits and pull requests is at: + https://github.com/ros/rosdistro/issues/12783 + [1] https://github.com/ros-perception/vision_opencv/commit/ead421b85eeb750cbf7988657015296ed6789bcf + Signed-off-by: Lukas Bulwahn +* cv_bridge: Add missing test_depend on numpy +* Contributors: Kentaro Wada, Lukas Bulwahn, Maarten de Vries, Vincent Rabaud + +1.11.14 (2016-09-24) +-------------------- +* Specify background label when colorizing label image +* Adjust to arbitrary image channels like 32FC40 + Proper fix for `#141 `_ +* Remove unexpectedly included print statement +* Contributors: Kentaro Wada, Vincent Rabaud + +1.11.13 (2016-07-11) +-------------------- +* split the conversion tests out of enumerants +* support is_bigendian in Python + Fixes `#114 `_ + Also fixes mono16 test +* Support compressed Images messages in python for indigo + - Add cv2_to_comprssed_imgmsg: Convert from cv2 image to compressed image ros msg. + - Add comprssed_imgmsg_to_cv2: Convert the compress message to a new image. + - Add compressed image tests. + - Add time to msgs (compressed and regular). + add enumerants test for compressed image. + merge the compressed tests with the regular ones. + better comment explanation. I will squash this commit. + Fix indentation + fix typo mistage: from .imgmsg_to_compressed_cv2 to .compressed_imgmsg_to_cv2. + remove cv2.CV_8UC1 + remove rospy and time depndency. + change from IMREAD_COLOR to IMREAD_ANYCOLOR. + - make indentaion of 4. + - remove space trailer. + - remove space from empty lines. + - another set of for loops, it will make things easier to track. In that new set, just have the number of channels in ([],1,3,4) (ignore two for jpg). from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721943 + - keep the OpenCV error message. from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721013 + add debug print for test. + add case for 4 channels in test. + remove 4 channels case from compressed test. + add debug print for test. + change typo of format. + fix typo in format. change from dip to dib. + change to IMREAD_ANYCOLOR as python code. (as it should). + rename TIFF to tiff + Sperate the tests one for regular images and one for compressed. + update comment +* Add CvtColorForDisplayOptions with new colormap param +* fix doc jobs +* Add python binding for cv_bridge::cvtColorForDisplay +* Fix compilation of cv_bridge with opencv3 and python3. +* Don't colorize float image as label image + This is a bug and image whose encoding is other than 32SC1 should not be + colorized. (currently, depth images with 32FC1 is also colorized.) +* Contributors: Kentaro Wada, Maarten de Vries, Vincent Rabaud, talregev + +1.11.12 (2016-03-10) +-------------------- +* Fix my typo +* Remove another eval + Because `cvtype2_to_dtype_with_channels('8UCimport os; os.system("rm -rf /")')` should never have a chance of happening. +* Remove eval, and other fixes + Also, extend from object, so as not to get a python 2.2-style class, and use the new-style raise statement +* Contributors: Eric Wieser + +1.11.11 (2016-01-31) +-------------------- +* clean up the doc files +* fix a few warnings in doc jobs +* Contributors: Vincent Rabaud + +1.11.10 (2016-01-16) +-------------------- +* fix OpenCV3 build +* Describe about converting label to bgr image in cvtColorForDisplay +* Convert label to BGR image to display +* Add test for rgb_colors.cpp +* Add rgb_colors util +* Update doc for converting to BGR in cvtColorForDisplay +* Convert to BGR from any encoding +* Refactor: sensor_msgs::image_encodings -> enc +* Contributors: Kentaro Wada, Vincent Rabaud + +1.11.9 (2015-11-29) +------------------- +* deal with endianness +* add cvtColorForDisplay +* Improved efficiency by using toCvShare instead of toCvCopy. +* Add format enum for easy use and choose format. +* fix compilation warnings +* start to extend the cv_bridge with cvCompressedImage class, that will convert from cv::Mat opencv images to CompressedImage ros messages and vice versa +* Contributors: Carlos Costa, Vincent Rabaud, talregev + +1.11.8 (2015-07-15) +------------------- +* Simplify some OpenCV3 distinction +* fix tests +* fix test under OpenCV3 +* Remove Python for Android +* Contributors: Gary Servin, Vincent Rabaud + +1.11.7 (2014-12-14) +------------------- +* check that the type is indeed a Numpy one + This is in response to `#51 `_ +* Contributors: Vincent Rabaud + +1.11.6 (2014-11-16) +------------------- +* chnage the behavior when there is only one channel +* cleanup tests +* Contributors: Vincent Rabaud + +1.11.5 (2014-09-21) +------------------- +* get code to work with OpenCV3 + actually fixes `#46 `_ properly +* Contributors: Vincent Rabaud + +1.11.4 (2014-07-27) +------------------- +* Fix `#42 `_ +* Contributors: Libor Wagner + +1.11.3 (2014-06-08) +------------------- +* Correct dependency from non-existent package to cv_bridge +* Contributors: Isaac Isao Saito + +1.11.2 (2014-04-28) +------------------- +* Add depend on python for cv_bridge +* Contributors: Scott K Logan + +1.11.1 (2014-04-16) +------------------- +* fixes `#34 `_ +* Contributors: Vincent Rabaud + +1.11.0 (2014-02-15) +------------------- +* remove deprecated API and fixes `#33 `_ +* fix OpenCV dependencies +* Contributors: Vincent Rabaud + +1.10.15 (2014-02-07) +-------------------- +* fix python 3 error at configure time +* Contributors: Dirk Thomas + +1.10.14 (2013-11-23 16:17) +-------------------------- +* update changelog +* Find NumPy include directory +* Contributors: Brian Jensen, Vincent Rabaud + +1.10.13 (2013-11-23 09:19) +-------------------------- +* fix compilation on older NumPy +* Contributors: Vincent Rabaud + +1.10.12 (2013-11-22) +-------------------- +* bump changelog +* Fixed issue with image message step size +* fix crash for non char data +* fix `#26 `_ +* Contributors: Brian Jensen, Vincent Rabaud + +1.10.11 (2013-10-23) +-------------------- +* fix bad image check and improve it too +* Contributors: Vincent Rabaud + +1.10.10 (2013-10-19) +-------------------- +* fixes `#25 `_ +* Contributors: Vincent Rabaud + +1.10.9 (2013-10-07) +------------------- +* fixes `#20 `_ +* Contributors: Vincent Rabaud + +1.10.8 (2013-09-09) +------------------- +* fixes `#22 `_ +* fixes `#17 `_ +* check for CATKIN_ENABLE_TESTING +* fixes `#16 `_ +* update email address +* Contributors: Lukas Bulwahn, Vincent Rabaud + +1.10.7 (2013-07-17) +------------------- + +1.10.6 (2013-03-01) +------------------- +* make sure conversion are applied for depth differences +* Contributors: Vincent Rabaud + +1.10.5 (2013-02-11) +------------------- + +1.10.4 (2013-02-02) +------------------- +* fix installation of the boost package +* Contributors: Vincent Rabaud + +1.10.3 (2013-01-17) +------------------- +* Link against PTYHON_LIBRARIES +* Contributors: William Woodall + +1.10.2 (2013-01-13) +------------------- +* use CATKIN_DEVEL_PREFIX instead of obsolete CATKIN_BUILD_PREFIX +* Contributors: Dirk Thomas + +1.10.1 (2013-01-10) +------------------- +* add licenses +* fixes `#5 `_ by removing the logic from Python and using wrapped C++ and adding a test for it +* fix a bug discovered when running the opencv_tests +* use some C++ logic +* add a Boost Python module to have the C++ logix used directly in Python +* Contributors: Vincent Rabaud + +1.10.0 (2013-01-03) +------------------- +* add conversion from Bayer to gray +* Contributors: Vincent Rabaud + +1.9.15 (2013-01-02) +------------------- +* use the reverted isColor behavior +* Contributors: Vincent Rabaud + +1.9.14 (2012-12-30) +------------------- + +1.9.13 (2012-12-15) +------------------- +* use the catkin macros for the setup.py +* fix `#3 `_ +* Contributors: Vincent Rabaud + +1.9.12 (2012-12-14) +------------------- +* buildtool_depend catkin fix +* CMakeLists.txt clean up. +* Contributors: William Woodall + +1.9.11 (2012-12-10) +------------------- +* fix issue `#1 `_ +* Cleanup of package.xml +* Contributors: Vincent Rabaud, William Woodall + +1.9.10 (2012-10-04) +------------------- +* fix the bad include folder +* Contributors: Vincent Rabaud + +1.9.9 (2012-10-01) +------------------ +* fix dependencies +* Contributors: Vincent Rabaud + +1.9.8 (2012-09-30) +------------------ +* fix some dependencies +* add rosconsole as a dependency +* fix missing Python at install and fix some dependencies +* Contributors: Vincent Rabaud + +1.9.7 (2012-09-28 21:07) +------------------------ +* add missing stuff +* make sure we find catkin +* Contributors: Vincent Rabaud + +1.9.6 (2012-09-28 15:17) +------------------------ +* move the test to where it belongs +* fix the tests and the API to not handle conversion from CV_TYPE to Color type (does not make sense) +* comply to the new Catkin API +* backport the YUV422 bug fix from Fuerte +* apply patch from https://code.ros.org/trac/ros-pkg/ticket/5556 +* Contributors: Vincent Rabaud + +1.9.5 (2012-09-15) +------------------ +* remove dependencies to the opencv2 ROS package +* Contributors: Vincent Rabaud + +1.9.4 (2012-09-13) +------------------ +* make sure the include folders are copied to the right place +* Contributors: Vincent Rabaud + +1.9.3 (2012-09-12) +------------------ + +1.9.2 (2012-09-07) +------------------ +* be more compliant to the latest catkin +* added catkin_project() to cv_bridge, image_geometry, and opencv_tests +* Contributors: Jonathan Binney, Vincent Rabaud + +1.9.1 (2012-08-28 22:06) +------------------------ +* remove things that were marked as ROS_DEPRECATED +* Contributors: Vincent Rabaud + +1.9.0 (2012-08-28 14:29) +------------------------ +* catkinized opencv_tests by Jon Binney +* catkinized cv_bridge package... others disable for now by Jon Binney +* remove the version check, let's trust OpenCV :) +* revert the removal of opencv2 +* vision_opencv: Export OpenCV flags in manifests for image_geometry, cv_bridge. +* finally get rid of opencv2 as it is a system dependency now +* bump REQUIRED version of OpenCV to 2.3.2, which is what's in ros-fuerte-opencv +* switch rosdep name to opencv2, to refer to ros-fuerte-opencv2 +* added missing header +* Added constructor to CvImage to make converting a cv::Mat to sensor_msgs::Image less verbose. +* cv_bridge: Added unit test for `#5206 `_ +* cv_bridge: Applied patch from mdesnoyer to fix handling of non-continuous OpenCV images. `#5206 `_ +* Adding opencv2 to all manifests, so that client packages may + not break when using them. +* baking in opencv debs and attempting a pre-release +* cv_bridge: Support for new 16-bit encodings. +* cv_bridge: Deprecate old C++ cv_bridge API. +* cv_bridge: Correctly scale for MONO8 <-> MONO16 conversions. +* cv_bridge: Fixed issue where pointer version to toCvCopy would ignore the requested encoding (http://answers.ros.org/question/258/converting-kinect-rgb-image-to-opencv-gives-wrong). +* fixed doc build by taking a static snapshot +* cv_bridge: Marking doc reviewed. +* cv_bridge: Tweaks to make docs look better. +* cv_bridge: Added cvtColor(). License notices. Documented that CvBridge class is obsolete. +* cv_bridge: Added redesigned C++ cv_bridge. +* Doc cleanup +* Trigger doc rebuild +* mono16 -> bgr conversion tested and fixed in C +* Added Ubuntu platform tags to manifest +* Handle mono16 properly +* Raise exception when imgMsgToCv() gets an image encoding it does not recognise, `#3489 `_ +* Remove use of deprecated rosbuild macros +* Fixed example +* cv_bridge split from opencv2 +* Contributors: Vincent Rabaud, ethanrublee, gerkey, jamesb, mihelich, vrabaud, wheeler diff --git a/ros/vision_opencv/cv_bridge/CMakeLists.txt b/ros/vision_opencv/cv_bridge/CMakeLists.txt new file mode 100644 index 0000000..455a603 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 2.8) +project(cv_bridge) + +find_package(catkin REQUIRED COMPONENTS rosconsole sensor_msgs) + +if(NOT ANDROID) + find_package(PythonLibs) + if(PYTHONLIBS_VERSION_STRING VERSION_LESS 3) + find_package(Boost REQUIRED python) + else() + find_package(Boost REQUIRED python3) + endif() +else() +find_package(Boost REQUIRED) +endif() +find_package(OpenCV REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS rosconsole sensor_msgs + DEPENDS OpenCV + CFG_EXTRAS cv_bridge-extras.cmake +) + +catkin_python_setup() + +include_directories(include ${Boost_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +if(NOT ANDROID) +add_subdirectory(python) +endif() +add_subdirectory(src) +if(CATKIN_ENABLE_TESTING) + add_subdirectory(test) +endif() + +# install the include folder +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/ros/vision_opencv/cv_bridge/cmake/cv_bridge-extras.cmake.in b/ros/vision_opencv/cv_bridge/cmake/cv_bridge-extras.cmake.in new file mode 100644 index 0000000..a4beda5 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/cmake/cv_bridge-extras.cmake.in @@ -0,0 +1,12 @@ +set(OpenCV_VERSION @OpenCV_VERSION@) +set(OpenCV_VERSION_MAJOR @OpenCV_VERSION_MAJOR@) +set(OpenCV_VERSION_MINOR @OpenCV_VERSION_MINOR@) +set(OpenCV_VERSION_PATCH @OpenCV_VERSION_PATCH@) +set(OpenCV_SHARED @OpenCV_SHARED@) +set(OpenCV_CONFIG_PATH @OpenCV_CONFIG_PATH@) +set(OpenCV_INSTALL_PATH @OpenCV_INSTALL_PATH@) +set(OpenCV_LIB_COMPONENTS @OpenCV_LIB_COMPONENTS@) +set(OpenCV_USE_MANGLED_PATHS @OpenCV_USE_MANGLED_PATHS@) +set(OpenCV_MODULES_SUFFIX @OpenCV_MODULES_SUFFIX@) + + diff --git a/ros/vision_opencv/cv_bridge/doc/conf.py b/ros/vision_opencv/cv_bridge/doc/conf.py new file mode 100644 index 0000000..c2af74f --- /dev/null +++ b/ros/vision_opencv/cv_bridge/doc/conf.py @@ -0,0 +1,201 @@ +# -*- coding: utf-8 -*- +# +# cv_bridge documentation build configuration file, created by +# sphinx-quickstart on Mon Jun 1 14:21:53 2009. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.append(os.path.abspath('.')) + +# -- General configuration ----------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'cv_bridge' +copyright = u'2009, Willow Garage, Inc.' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.1' +# The full version, including alpha/beta/rc tags. +release = '0.1.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of documents that shouldn't be included in the build. +#unused_docs = [] + +# List of directories, relative to source directory, that shouldn't be searched +# for source files. +exclude_trees = ['_build'] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. Major themes that come with +# Sphinx are currently 'default' and 'sphinxdoc'. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_use_modindex = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = '' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'cv_bridgedoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +# The paper size ('letter' or 'a4'). +#latex_paper_size = 'letter' + +# The font size ('10pt', '11pt' or '12pt'). +#latex_font_size = '10pt' + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'cv_bridge.tex', u'stereo\\_utils Documentation', + u'James Bowman', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# Additional stuff for the LaTeX preamble. +#latex_preamble = '' + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_use_modindex = True + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = { + 'http://docs.python.org/': None, + 'http://docs.scipy.org/doc/numpy' : None, + } diff --git a/ros/vision_opencv/cv_bridge/doc/index.rst b/ros/vision_opencv/cv_bridge/doc/index.rst new file mode 100644 index 0000000..c455220 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/doc/index.rst @@ -0,0 +1,18 @@ +cv_bridge +========= + +``cv_bridge`` contains a single class :class:`CvBridge` that converts ROS Image messages to +OpenCV images. + +.. module:: cv_bridge + +.. autoclass:: cv_bridge.CvBridge + :members: + +.. autoclass:: cv_bridge.CvBridgeError + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/ros/vision_opencv/cv_bridge/doc/mainpage.dox b/ros/vision_opencv/cv_bridge/doc/mainpage.dox new file mode 100644 index 0000000..e4ed1cb --- /dev/null +++ b/ros/vision_opencv/cv_bridge/doc/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b cv_bridge contains classes for easily converting between ROS +sensor_msgs/Image messages and OpenCV images. + +\section codeapi Code API + + - cv_bridge::CvImage + - toCvCopy() + - toCvShare() + +*/ diff --git a/ros/vision_opencv/cv_bridge/include/cv_bridge/cv_bridge.h b/ros/vision_opencv/cv_bridge/include/cv_bridge/cv_bridge.h new file mode 100644 index 0000000..2b44303 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/include/cv_bridge/cv_bridge.h @@ -0,0 +1,429 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011, Willow Garage, Inc, +* Copyright (c) 2015, Tal Regev. +* 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. +*********************************************************************/ + +#ifndef CV_BRIDGE_CV_BRIDGE_H +#define CV_BRIDGE_CV_BRIDGE_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cv_bridge { + +class Exception : public std::runtime_error +{ +public: + Exception(const std::string& description) : std::runtime_error(description) {} +}; + +class CvImage; + +typedef boost::shared_ptr CvImagePtr; +typedef boost::shared_ptr CvImageConstPtr; + +//from: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) +typedef enum { + BMP, DIB, + JPG, JPEG, JPE, + JP2, + PNG, + PBM, PGM, PPM, + SR, RAS, + TIFF, TIF, +} Format; + +/** + * \brief Image message class that is interoperable with sensor_msgs/Image but uses a + * more convenient cv::Mat representation for the image data. + */ +class CvImage +{ +public: + std_msgs::Header header; //!< ROS header + std::string encoding; //!< Image encoding ("mono8", "bgr8", etc.) + cv::Mat image; //!< Image data for use with OpenCV + + /** + * \brief Empty constructor. + */ + CvImage() {} + + /** + * \brief Constructor. + */ + CvImage(const std_msgs::Header& header, const std::string& encoding, + const cv::Mat& image = cv::Mat()) + : header(header), encoding(encoding), image(image) + { + } + + /** + * \brief Convert this message to a ROS sensor_msgs::Image message. + * + * The returned sensor_msgs::Image message contains a copy of the image data. + */ + sensor_msgs::ImagePtr toImageMsg() const; + + /** + * dst_format is compress the image to desire format. + * Default value is empty string that will convert to jpg format. + * can be: jpg, jp2, bmp, png, tif at the moment + * support this format from opencv: + * http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) + */ + sensor_msgs::CompressedImagePtr toCompressedImageMsg(const Format dst_format = JPG) const; + + /** + * \brief Copy the message data to a ROS sensor_msgs::Image message. + * + * This overload is intended mainly for aggregate messages such as stereo_msgs::DisparityImage, + * which contains a sensor_msgs::Image as a data member. + */ + void toImageMsg(sensor_msgs::Image& ros_image) const; + + /** + * dst_format is compress the image to desire format. + * Default value is empty string that will convert to jpg format. + * can be: jpg, jp2, bmp, png, tif at the moment + * support this format from opencv: + * http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) + */ + void toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format = JPG) const; + + + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + +protected: + boost::shared_ptr tracked_object_; // for sharing ownership + + /// @cond DOXYGEN_IGNORE + friend + CvImageConstPtr toCvShare(const sensor_msgs::Image& source, + const boost::shared_ptr& tracked_object, + const std::string& encoding); + /// @endcond +}; + + +/** + * \brief Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the + * image data. + * + * \param source A shared_ptr to a sensor_msgs::Image message + * \param encoding The desired encoding of the image data, one of the following strings: + * - \c "mono8" + * - \c "bgr8" + * - \c "bgra8" + * - \c "rgb8" + * - \c "rgba8" + * - \c "mono16" + * + * If \a encoding is the empty string (the default), the returned CvImage has the same encoding + * as \a source. + */ +CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source, + const std::string& encoding = std::string()); + +CvImagePtr toCvCopy(const sensor_msgs::CompressedImageConstPtr& source, + const std::string& encoding = std::string()); + +/** + * \brief Convert a sensor_msgs::Image message to an OpenCV-compatible CvImage, copying the + * image data. + * + * \param source A sensor_msgs::Image message + * \param encoding The desired encoding of the image data, one of the following strings: + * - \c "mono8" + * - \c "bgr8" + * - \c "bgra8" + * - \c "rgb8" + * - \c "rgba8" + * - \c "mono16" + * + * If \a encoding is the empty string (the default), the returned CvImage has the same encoding + * as \a source. + * If the source is 8bit and the encoding 16 or vice-versa, a scaling is applied (65535/255 and + * 255/65535 respectively). Otherwise, no scaling is applied and the rules from the convertTo OpenCV + * function are applied (capping): http://docs.opencv.org/modules/core/doc/basic_structures.html#mat-convertto + */ +CvImagePtr toCvCopy(const sensor_msgs::Image& source, + const std::string& encoding = std::string()); + +CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source, + const std::string& encoding = std::string()); + +/** + * \brief Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing + * the image data if possible. + * + * If the source encoding and desired encoding are the same, the returned CvImage will share + * the image data with \a source without copying it. The returned CvImage cannot be modified, as that + * could modify the \a source data. + * + * \param source A shared_ptr to a sensor_msgs::Image message + * \param encoding The desired encoding of the image data, one of the following strings: + * - \c "mono8" + * - \c "bgr8" + * - \c "bgra8" + * - \c "rgb8" + * - \c "rgba8" + * - \c "mono16" + * + * If \a encoding is the empty string (the default), the returned CvImage has the same encoding + * as \a source. + */ +CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source, + const std::string& encoding = std::string()); + +/** + * \brief Convert an immutable sensor_msgs::Image message to an OpenCV-compatible CvImage, sharing + * the image data if possible. + * + * If the source encoding and desired encoding are the same, the returned CvImage will share + * the image data with \a source without copying it. The returned CvImage cannot be modified, as that + * could modify the \a source data. + * + * This overload is useful when you have a shared_ptr to a message that contains a + * sensor_msgs::Image, and wish to share ownership with the containing message. + * + * \param source The sensor_msgs::Image message + * \param tracked_object A shared_ptr to an object owning the sensor_msgs::Image + * \param encoding The desired encoding of the image data, one of the following strings: + * - \c "mono8" + * - \c "bgr8" + * - \c "bgra8" + * - \c "rgb8" + * - \c "rgba8" + * - \c "mono16" + * + * If \a encoding is the empty string (the default), the returned CvImage has the same encoding + * as \a source. + */ +CvImageConstPtr toCvShare(const sensor_msgs::Image& source, + const boost::shared_ptr& tracked_object, + const std::string& encoding = std::string()); + +/** + * \brief Convert a CvImage to another encoding using the same rules as toCvCopy + */ +CvImagePtr cvtColor(const CvImageConstPtr& source, + const std::string& encoding); + +struct CvtColorForDisplayOptions { + CvtColorForDisplayOptions() : + do_dynamic_scaling(false), + min_image_value(0.0), + max_image_value(0.0), + colormap(-1), + bg_label(-1) {} + bool do_dynamic_scaling; + double min_image_value; + double max_image_value; + int colormap; + int bg_label; +}; + + +/** + * \brief Converts an immutable sensor_msgs::Image message to another CvImage for display purposes, + * using practical conversion rules if needed. + * + * Data will be shared between input and output if possible. + * + * Recall: sensor_msgs::image_encodings::isColor and isMono tell whether an image contains R,G,B,A, mono + * (or any combination/subset) with 8 or 16 bit depth. + * + * The following rules apply: + * - if the output encoding is empty, the fact that the input image is mono or multiple-channel is + * preserved in the ouput image. The bit depth will be 8. it tries to convert to BGR no matter what + * encoding image is passed. + * - if the output encoding is not empty, it must have sensor_msgs::image_encodings::isColor and + * isMono return true. It must also be 8 bit in depth + * - if the input encoding is an OpenCV format (e.g. 8UC1), and if we have 1,3 or 4 channels, it is + * respectively converted to mono, BGR or BGRA. + * - if the input encoding is 32SC1, this estimate that image as label image and will convert it as + * bgr image with different colors for each label. + * + * \param source A shared_ptr to a sensor_msgs::Image message + * \param encoding Either an encoding string that returns true in sensor_msgs::image_encodings::isColor + * isMono or the empty string as explained above. + * \param options (cv_bridge::CvtColorForDisplayOptions) Options to convert the source image with. + * - do_dynamic_scaling If true, the image is dynamically scaled between its minimum and maximum value + * before being converted to its final encoding. + * - min_image_value Independently from do_dynamic_scaling, if min_image_value and max_image_value are + * different, the image is scaled between these two values before being converted to its final encoding. + * - max_image_value Maximum image value + * - colormap Colormap which the source image converted with. + */ +CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source, + const std::string& encoding = std::string(), + const CvtColorForDisplayOptions options = CvtColorForDisplayOptions()); + +/** + * \brief Get the OpenCV type enum corresponding to the encoding. + * + * For example, "bgr8" -> CV_8UC3. + */ +int getCvType(const std::string& encoding); + +} // namespace cv_bridge + + +// CvImage as a first class message type + +// The rest of this file hooks into the roscpp serialization API to make CvImage +// a first-class message type you can publish and subscribe to directly. +// Unfortunately this doesn't yet work with image_transport, so don't rewrite all +// your callbacks to use CvImage! It might be useful for specific tasks, like +// processing bag files. + +/// @cond DOXYGEN_IGNORE +namespace ros { + +namespace message_traits { + +template<> struct MD5Sum +{ + static const char* value() { return MD5Sum::value(); } + static const char* value(const cv_bridge::CvImage&) { return value(); } + + static const uint64_t static_value1 = MD5Sum::static_value1; + static const uint64_t static_value2 = MD5Sum::static_value2; + + // If the definition of sensor_msgs/Image changes, we'll get a compile error here. + ROS_STATIC_ASSERT(MD5Sum::static_value1 == 0x060021388200f6f0ULL); + ROS_STATIC_ASSERT(MD5Sum::static_value2 == 0xf447d0fcd9c64743ULL); +}; + +template<> struct DataType +{ + static const char* value() { return DataType::value(); } + static const char* value(const cv_bridge::CvImage&) { return value(); } +}; + +template<> struct Definition +{ + static const char* value() { return Definition::value(); } + static const char* value(const cv_bridge::CvImage&) { return value(); } +}; + +template<> struct HasHeader : TrueType {}; + +} // namespace ros::message_traits + +namespace serialization { + +template<> struct Serializer +{ + /// @todo Still ignoring endianness... + + template + inline static void write(Stream& stream, const cv_bridge::CvImage& m) + { + stream.next(m.header); + stream.next((uint32_t)m.image.rows); // height + stream.next((uint32_t)m.image.cols); // width + stream.next(m.encoding); + uint8_t is_bigendian = 0; + stream.next(is_bigendian); + stream.next((uint32_t)m.image.step); + size_t data_size = m.image.step*m.image.rows; + stream.next((uint32_t)data_size); + if (data_size > 0) + memcpy(stream.advance(data_size), m.image.data, data_size); + } + + template + inline static void read(Stream& stream, cv_bridge::CvImage& m) + { + stream.next(m.header); + uint32_t height, width; + stream.next(height); + stream.next(width); + stream.next(m.encoding); + uint8_t is_bigendian; + stream.next(is_bigendian); + uint32_t step, data_size; + stream.next(step); + stream.next(data_size); + int type = cv_bridge::getCvType(m.encoding); + // Construct matrix pointing to the stream data, then copy it to m.image + cv::Mat tmp((int)height, (int)width, type, stream.advance(data_size), (size_t)step); + tmp.copyTo(m.image); + } + + inline static uint32_t serializedLength(const cv_bridge::CvImage& m) + { + size_t data_size = m.image.step*m.image.rows; + return serializationLength(m.header) + serializationLength(m.encoding) + 17 + data_size; + } +}; + +} // namespace ros::serialization + +namespace message_operations { + +template<> struct Printer +{ + template + static void stream(Stream& s, const std::string& indent, const cv_bridge::CvImage& m) + { + /// @todo Replicate printing for sensor_msgs::Image + } +}; + +} // namespace ros::message_operations + +} // namespace ros + +namespace cv_bridge { + +inline std::ostream& operator<<(std::ostream& s, const CvImage& m) +{ + ros::message_operations::Printer::stream(s, "", m); + return s; +} + +} // namespace cv_bridge + +/// @endcond + +#endif diff --git a/ros/vision_opencv/cv_bridge/include/cv_bridge/rgb_colors.h b/ros/vision_opencv/cv_bridge/include/cv_bridge/rgb_colors.h new file mode 100644 index 0000000..1eaa88b --- /dev/null +++ b/ros/vision_opencv/cv_bridge/include/cv_bridge/rgb_colors.h @@ -0,0 +1,211 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Original color definition is at scikit-image distributed with + * following license disclaimer: + * + * Copyright (C) 2011, the scikit-image team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name of skimage 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 AUTHOR ``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 AUTHOR 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. + *********************************************************************/ + +#ifndef CV_BRIDGE_RGB_COLORS_H_ +#define CV_BRIDGE_RGB_COLORS_H_ + +#include + + +namespace cv_bridge +{ + +namespace rgb_colors +{ + + /** + * @brief + * 146 rgb colors + */ + enum Colors { + ALICEBLUE, + ANTIQUEWHITE, + AQUA, + AQUAMARINE, + AZURE, + BEIGE, + BISQUE, + BLACK, + BLANCHEDALMOND, + BLUE, + BLUEVIOLET, + BROWN, + BURLYWOOD, + CADETBLUE, + CHARTREUSE, + CHOCOLATE, + CORAL, + CORNFLOWERBLUE, + CORNSILK, + CRIMSON, + CYAN, + DARKBLUE, + DARKCYAN, + DARKGOLDENROD, + DARKGRAY, + DARKGREEN, + DARKGREY, + DARKKHAKI, + DARKMAGENTA, + DARKOLIVEGREEN, + DARKORANGE, + DARKORCHID, + DARKRED, + DARKSALMON, + DARKSEAGREEN, + DARKSLATEBLUE, + DARKSLATEGRAY, + DARKSLATEGREY, + DARKTURQUOISE, + DARKVIOLET, + DEEPPINK, + DEEPSKYBLUE, + DIMGRAY, + DIMGREY, + DODGERBLUE, + FIREBRICK, + FLORALWHITE, + FORESTGREEN, + FUCHSIA, + GAINSBORO, + GHOSTWHITE, + GOLD, + GOLDENROD, + GRAY, + GREEN, + GREENYELLOW, + GREY, + HONEYDEW, + HOTPINK, + INDIANRED, + INDIGO, + IVORY, + KHAKI, + LAVENDER, + LAVENDERBLUSH, + LAWNGREEN, + LEMONCHIFFON, + LIGHTBLUE, + LIGHTCORAL, + LIGHTCYAN, + LIGHTGOLDENRODYELLOW, + LIGHTGRAY, + LIGHTGREEN, + LIGHTGREY, + LIGHTPINK, + LIGHTSALMON, + LIGHTSEAGREEN, + LIGHTSKYBLUE, + LIGHTSLATEGRAY, + LIGHTSLATEGREY, + LIGHTSTEELBLUE, + LIGHTYELLOW, + LIME, + LIMEGREEN, + LINEN, + MAGENTA, + MAROON, + MEDIUMAQUAMARINE, + MEDIUMBLUE, + MEDIUMORCHID, + MEDIUMPURPLE, + MEDIUMSEAGREEN, + MEDIUMSLATEBLUE, + MEDIUMSPRINGGREEN, + MEDIUMTURQUOISE, + MEDIUMVIOLETRED, + MIDNIGHTBLUE, + MINTCREAM, + MISTYROSE, + MOCCASIN, + NAVAJOWHITE, + NAVY, + OLDLACE, + OLIVE, + OLIVEDRAB, + ORANGE, + ORANGERED, + ORCHID, + PALEGOLDENROD, + PALEGREEN, + PALEVIOLETRED, + PAPAYAWHIP, + PEACHPUFF, + PERU, + PINK, + PLUM, + POWDERBLUE, + PURPLE, + RED, + ROSYBROWN, + ROYALBLUE, + SADDLEBROWN, + SALMON, + SANDYBROWN, + SEAGREEN, + SEASHELL, + SIENNA, + SILVER, + SKYBLUE, + SLATEBLUE, + SLATEGRAY, + SLATEGREY, + SNOW, + SPRINGGREEN, + STEELBLUE, + TAN, + TEAL, + THISTLE, + TOMATO, + TURQUOISE, + VIOLET, + WHEAT, + WHITE, + WHITESMOKE, + YELLOW, + YELLOWGREEN, + }; + + /** + * @brief + * get rgb color with enum. + */ + cv::Vec3d getRGBColor(const int color); + +} // namespace rgb_colors + +} // namespace cv_bridge + +#endif diff --git a/ros/vision_opencv/cv_bridge/package.xml b/ros/vision_opencv/cv_bridge/package.xml new file mode 100644 index 0000000..c634ca6 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/package.xml @@ -0,0 +1,40 @@ + + cv_bridge + 1.11.15 + + This contains CvBridge, which converts between ROS + Image messages and OpenCV images. + + Patrick Mihelich + James Bowman + Vincent Rabaud + BSD + http://www.ros.org/wiki/cv_bridge + https://github.com/ros-perception/vision_opencv + https://github.com/ros-perception/vision_opencv/issues + + + + + + catkin + + boost + libopencv-dev + python + python-opencv + rosconsole + sensor_msgs + + boost + libopencv-dev + python + python-opencv + rosconsole + sensor_msgs + + rostest + python-numpy + + dvipng + diff --git a/ros/vision_opencv/cv_bridge/python/CMakeLists.txt b/ros/vision_opencv/cv_bridge/python/CMakeLists.txt new file mode 100644 index 0000000..1b677d3 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/python/CMakeLists.txt @@ -0,0 +1,8 @@ +configure_file(__init__.py.plain.in + ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/boost/__init__.py + @ONLY +) + +install(FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/boost/__init__.py + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}/boost/ +) diff --git a/ros/vision_opencv/cv_bridge/python/__init__.py.plain.in b/ros/vision_opencv/cv_bridge/python/__init__.py.plain.in new file mode 100644 index 0000000..e69de29 diff --git a/ros/vision_opencv/cv_bridge/python/cv_bridge/__init__.py b/ros/vision_opencv/cv_bridge/python/cv_bridge/__init__.py new file mode 100644 index 0000000..5189c1a --- /dev/null +++ b/ros/vision_opencv/cv_bridge/python/cv_bridge/__init__.py @@ -0,0 +1,8 @@ +from .core import CvBridge, CvBridgeError + +# python bindings +try: + # This try is just to satisfy doc jobs that are built differently. + from cv_bridge.boost.cv_bridge_boost import cvtColorForDisplay, getCvType +except ImportError: + pass diff --git a/ros/vision_opencv/cv_bridge/python/cv_bridge/core.py b/ros/vision_opencv/cv_bridge/python/cv_bridge/core.py new file mode 100644 index 0000000..b392989 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/python/cv_bridge/core.py @@ -0,0 +1,266 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2011, Willow Garage, Inc. +# Copyright (c) 2016, Tal Regev. +# 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. + +import sensor_msgs.msg +import sys + + +class CvBridgeError(TypeError): + """ + This is the error raised by :class:`cv_bridge.CvBridge` methods when they fail. + """ + pass + + +class CvBridge(object): + """ + The CvBridge is an object that converts between OpenCV Images and ROS Image messages. + + .. doctest:: + :options: -ELLIPSIS, +NORMALIZE_WHITESPACE + + >>> import cv2 + >>> import numpy as np + >>> from cv_bridge import CvBridge + >>> br = CvBridge() + >>> dtype, n_channels = br.encoding_as_cvtype2('8UC3') + >>> im = np.ndarray(shape=(480, 640, n_channels), dtype=dtype) + >>> msg = br.cv2_to_imgmsg(im) # Convert the image to a message + >>> im2 = br.imgmsg_to_cv2(msg) # Convert the message to a new image + >>> cmprsmsg = br.cv2_to_compressed_imgmsg(im) # Convert the image to a compress message + >>> im22 = br.compressed_imgmsg_to_cv2(msg) # Convert the compress message to a new image + >>> cv2.imwrite("this_was_a_message_briefly.png", im2) + + """ + + def __init__(self): + import cv2 + self.cvtype_to_name = {} + self.cvdepth_to_numpy_depth = {cv2.CV_8U: 'uint8', cv2.CV_8S: 'int8', cv2.CV_16U: 'uint16', + cv2.CV_16S: 'int16', cv2.CV_32S:'int32', cv2.CV_32F:'float32', + cv2.CV_64F: 'float64'} + + for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F"]: + for c in [1, 2, 3, 4]: + nm = "%sC%d" % (t, c) + self.cvtype_to_name[getattr(cv2, "CV_%s" % nm)] = nm + + self.numpy_type_to_cvtype = {'uint8': '8U', 'int8': '8S', 'uint16': '16U', + 'int16': '16S', 'int32': '32S', 'float32': '32F', + 'float64': '64F'} + self.numpy_type_to_cvtype.update(dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items())) + + def dtype_with_channels_to_cvtype2(self, dtype, n_channels): + return '%sC%d' % (self.numpy_type_to_cvtype[dtype.name], n_channels) + + def cvtype2_to_dtype_with_channels(self, cvtype): + from cv_bridge.boost.cv_bridge_boost import CV_MAT_CNWrap, CV_MAT_DEPTHWrap + return self.cvdepth_to_numpy_depth[CV_MAT_DEPTHWrap(cvtype)], CV_MAT_CNWrap(cvtype) + + def encoding_to_cvtype2(self, encoding): + from cv_bridge.boost.cv_bridge_boost import getCvType + + try: + return getCvType(encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + def encoding_to_dtype_with_channels(self, encoding): + return self.cvtype2_to_dtype_with_channels(self.encoding_to_cvtype2(encoding)) + + def compressed_imgmsg_to_cv2(self, cmprs_img_msg, desired_encoding = "passthrough"): + """ + Convert a sensor_msgs::CompressedImage message to an OpenCV :cpp:type:`cv::Mat`. + + :param cmprs_img_msg: A :cpp:type:`sensor_msgs::CompressedImage` message + :param desired_encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: :cpp:type:`cv::Mat` + :raises CvBridgeError: when conversion is not possible. + + If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure. + + If the image only has one channel, the shape has size 2 (width and height) + """ + import cv2 + import numpy as np + + str_msg = cmprs_img_msg.data + buf = np.ndarray(shape=(1, len(str_msg)), + dtype=np.uint8, buffer=cmprs_img_msg.data) + im = cv2.imdecode(buf, cv2.IMREAD_ANYCOLOR) + + if desired_encoding == "passthrough": + return im + + from cv_bridge.boost.cv_bridge_boost import cvtColor2 + + try: + res = cvtColor2(im, "bgr8", desired_encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + return res + + def imgmsg_to_cv2(self, img_msg, desired_encoding = "passthrough"): + """ + Convert a sensor_msgs::Image message to an OpenCV :cpp:type:`cv::Mat`. + + :param img_msg: A :cpp:type:`sensor_msgs::Image` message + :param desired_encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: :cpp:type:`cv::Mat` + :raises CvBridgeError: when conversion is not possible. + + If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure. + + If the image only has one channel, the shape has size 2 (width and height) + """ + import cv2 + import numpy as np + dtype, n_channels = self.encoding_to_dtype_with_channels(img_msg.encoding) + dtype = np.dtype(dtype) + dtype = dtype.newbyteorder('>' if img_msg.is_bigendian else '<') + if n_channels == 1: + im = np.ndarray(shape=(img_msg.height, img_msg.width), + dtype=dtype, buffer=img_msg.data) + else: + im = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels), + dtype=dtype, buffer=img_msg.data) + # If the byt order is different between the message and the system. + if img_msg.is_bigendian == (sys.byteorder == 'little'): + im = im.byteswap().newbyteorder() + + if desired_encoding == "passthrough": + return im + + from cv_bridge.boost.cv_bridge_boost import cvtColor2 + + try: + res = cvtColor2(im, img_msg.encoding, desired_encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + return res + + def cv2_to_compressed_imgmsg(self, cvim, dst_format = "jpg"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::CompressedImage message. + + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param dst_format: The format of the image data, one of the following strings: + + * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html + * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) + * bmp, dib + * jpeg, jpg, jpe + * jp2 + * png + * pbm, pgm, ppm + * sr, ras + * tiff, tif + + :rtype: A sensor_msgs.msg.CompressedImage message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``format`` + + + This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure. + """ + import cv2 + import numpy as np + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError('Your input type is not a numpy array') + cmprs_img_msg = sensor_msgs.msg.CompressedImage() + cmprs_img_msg.format = dst_format + ext_format = '.' + dst_format + try: + cmprs_img_msg.data = np.array(cv2.imencode(ext_format, cvim)[1]).tostring() + except RuntimeError as e: + raise CvBridgeError(e) + + return cmprs_img_msg + + def cv2_to_imgmsg(self, cvim, encoding = "passthrough"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message. + + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: A sensor_msgs.msg.Image message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding`` + + If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure. + """ + import cv2 + import numpy as np + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError('Your input type is not a numpy array') + img_msg = sensor_msgs.msg.Image() + img_msg.height = cvim.shape[0] + img_msg.width = cvim.shape[1] + if len(cvim.shape) < 3: + cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1) + else: + cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, cvim.shape[2]) + if encoding == "passthrough": + img_msg.encoding = cv_type + else: + img_msg.encoding = encoding + # Verify that the supplied encoding is compatible with the type of the OpenCV image + if self.cvtype_to_name[self.encoding_to_cvtype2(encoding)] != cv_type: + raise CvBridgeError("encoding specified as %s, but image has incompatible type %s" % (encoding, cv_type)) + if cvim.dtype.byteorder == '>': + img_msg.is_bigendian = True + img_msg.data = cvim.tostring() + img_msg.step = len(img_msg.data) / img_msg.height + + return img_msg diff --git a/ros/vision_opencv/cv_bridge/rosdoc.yaml b/ros/vision_opencv/cv_bridge/rosdoc.yaml new file mode 100644 index 0000000..0efc7fd --- /dev/null +++ b/ros/vision_opencv/cv_bridge/rosdoc.yaml @@ -0,0 +1,8 @@ + - builder: doxygen + name: C++ API + output_dir: c++ + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' + - builder: sphinx + name: Python API + output_dir: python + sphinx_root_dir: doc diff --git a/ros/vision_opencv/cv_bridge/setup.py b/ros/vision_opencv/cv_bridge/setup.py new file mode 100644 index 0000000..e922dfe --- /dev/null +++ b/ros/vision_opencv/cv_bridge/setup.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup() + +d['packages'] = ['cv_bridge'] +d['package_dir'] = {'' : 'python/'} + +setup(**d) diff --git a/ros/vision_opencv/cv_bridge/src/CMakeLists.txt b/ros/vision_opencv/cv_bridge/src/CMakeLists.txt new file mode 100644 index 0000000..37ba30e --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/CMakeLists.txt @@ -0,0 +1,56 @@ +# add library +include_directories(./) +add_library(${PROJECT_NAME} cv_bridge.cpp rgb_colors.cpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +if(NOT ANDROID) +# add a Boost Python library +find_package(PythonInterp REQUIRED) +find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") + +#Get the numpy include directory from its python module +if(NOT PYTHON_NUMPY_INCLUDE_DIR) + execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import numpy; print(numpy.get_include())" + RESULT_VARIABLE PYTHON_NUMPY_PROCESS + OUTPUT_VARIABLE PYTHON_NUMPY_INCLUDE_DIR + OUTPUT_STRIP_TRAILING_WHITESPACE) + + if(PYTHON_NUMPY_PROCESS EQUAL 0) + file(TO_CMAKE_PATH "${PYTHON_NUMPY_INCLUDE_DIR}" PYTHON_NUMPY_INCLUDE_CMAKE_PATH) + set(PYTHON_NUMPY_INCLUDE_DIR ${PYTHON_NUMPY_INCLUDE_CMAKE_PATH} CACHE PATH "Numpy include directory") + else(PYTHON_NUMPY_PROCESS EQUAL 0) + message(SEND_ERROR "Could not determine the NumPy include directory, verify that NumPy was installed correctly.") + endif(PYTHON_NUMPY_PROCESS EQUAL 0) + endif(NOT PYTHON_NUMPY_INCLUDE_DIR) + +include_directories(${PYTHON_INCLUDE_PATH} ${Boost_INCLUDE_DIRS} ${PYTHON_NUMPY_INCLUDE_DIR}) + +if (PYTHON_VERSION_MAJOR VERSION_EQUAL 3) + add_definitions(-DPYTHON3) +endif() + +if (OpenCV_VERSION_MAJOR VERSION_EQUAL 3) +add_library(${PROJECT_NAME}_boost module.cpp module_opencv3.cpp) +else() +add_library(${PROJECT_NAME}_boost module.cpp module_opencv2.cpp) +endif() +target_link_libraries(${PROJECT_NAME}_boost ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${PYTHON_LIBRARIES} + ${PROJECT_NAME} +) + +set_target_properties(${PROJECT_NAME}_boost PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME}/boost/ + PREFIX "" +) +if(APPLE) + set_target_properties(${PROJECT_NAME}_boost PROPERTIES + SUFFIX ".so") +endif() + +install(TARGETS ${PROJECT_NAME}_boost DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}/boost/) +endif() diff --git a/ros/vision_opencv/cv_bridge/src/boost/README b/ros/vision_opencv/cv_bridge/src/boost/README new file mode 100644 index 0000000..f3a3d3c --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/README @@ -0,0 +1,2 @@ +this code is taken from Boost at https://github.com/boostorg/endian.git +We should remove this folder once Boost 1.58 or above is the default. diff --git a/ros/vision_opencv/cv_bridge/src/boost/core/scoped_enum.hpp b/ros/vision_opencv/cv_bridge/src/boost/core/scoped_enum.hpp new file mode 100644 index 0000000..78c548b --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/core/scoped_enum.hpp @@ -0,0 +1,192 @@ +// scoped_enum.hpp ---------------------------------------------------------// + +// Copyright Beman Dawes, 2009 +// Copyright (C) 2011-2012 Vicente J. Botet Escriba +// Copyright (C) 2012 Anthony Williams + +// Distributed under the Boost Software License, Version 1.0. +// See http://www.boost.org/LICENSE_1_0.txt + +#ifndef BOOST_CORE_SCOPED_ENUM_HPP +#define BOOST_CORE_SCOPED_ENUM_HPP + +#include + +#ifdef BOOST_HAS_PRAGMA_ONCE +#pragma once +#endif + +namespace boost +{ + +#ifdef BOOST_NO_CXX11_SCOPED_ENUMS + + /** + * Meta-function to get the native enum type associated to an enum class or its emulation. + */ + template + struct native_type + { + /** + * The member typedef type names the native enum type associated to the scoped enum, + * which is it self if the compiler supports scoped enums or EnumType::enum_type if it is an emulated scoped enum. + */ + typedef typename EnumType::enum_type type; + }; + + /** + * Casts a scoped enum to its underlying type. + * + * This function is useful when working with scoped enum classes, which doens't implicitly convert to the underlying type. + * @param v A scoped enum. + * @returns The underlying type. + * @throws No-throws. + */ + template + UnderlyingType underlying_cast(EnumType v) + { + return v.get_underlying_value_(); + } + + /** + * Casts a scoped enum to its native enum type. + * + * This function is useful to make programs portable when the scoped enum emulation can not be use where native enums can. + * + * EnumType the scoped enum type + * + * @param v A scoped enum. + * @returns The native enum value. + * @throws No-throws. + */ + template + inline + typename EnumType::enum_type native_value(EnumType e) + { + return e.get_native_value_(); + } + +#else // BOOST_NO_CXX11_SCOPED_ENUMS + + template + struct native_type + { + typedef EnumType type; + }; + + template + UnderlyingType underlying_cast(EnumType v) + { + return static_cast(v); + } + + template + inline + EnumType native_value(EnumType e) + { + return e; + } + +#endif // BOOST_NO_CXX11_SCOPED_ENUMS +} + + +#ifdef BOOST_NO_CXX11_SCOPED_ENUMS + +#ifndef BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS + +#define BOOST_SCOPED_ENUM_UT_DECLARE_CONVERSION_OPERATOR \ + explicit operator underlying_type() const BOOST_NOEXCEPT { return get_underlying_value_(); } + +#else + +#define BOOST_SCOPED_ENUM_UT_DECLARE_CONVERSION_OPERATOR + +#endif + +/** + * Start a declaration of a scoped enum. + * + * @param EnumType The new scoped enum. + * @param UnderlyingType The underlying type. + */ +#define BOOST_SCOPED_ENUM_UT_DECLARE_BEGIN(EnumType, UnderlyingType) \ + struct EnumType { \ + typedef void is_boost_scoped_enum_tag; \ + typedef UnderlyingType underlying_type; \ + EnumType() BOOST_NOEXCEPT {} \ + explicit EnumType(underlying_type v) BOOST_NOEXCEPT : v_(v) {} \ + underlying_type get_underlying_value_() const BOOST_NOEXCEPT { return v_; } \ + BOOST_SCOPED_ENUM_UT_DECLARE_CONVERSION_OPERATOR \ + private: \ + underlying_type v_; \ + typedef EnumType self_type; \ + public: \ + enum enum_type + +#define BOOST_SCOPED_ENUM_DECLARE_END2() \ + enum_type get_native_value_() const BOOST_NOEXCEPT { return enum_type(v_); } \ + friend bool operator ==(self_type lhs, self_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)==enum_type(rhs.v_); } \ + friend bool operator ==(self_type lhs, enum_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)==rhs; } \ + friend bool operator ==(enum_type lhs, self_type rhs) BOOST_NOEXCEPT { return lhs==enum_type(rhs.v_); } \ + friend bool operator !=(self_type lhs, self_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)!=enum_type(rhs.v_); } \ + friend bool operator !=(self_type lhs, enum_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)!=rhs; } \ + friend bool operator !=(enum_type lhs, self_type rhs) BOOST_NOEXCEPT { return lhs!=enum_type(rhs.v_); } \ + friend bool operator <(self_type lhs, self_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)(self_type lhs, self_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)>enum_type(rhs.v_); } \ + friend bool operator >(self_type lhs, enum_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)>rhs; } \ + friend bool operator >(enum_type lhs, self_type rhs) BOOST_NOEXCEPT { return lhs>enum_type(rhs.v_); } \ + friend bool operator >=(self_type lhs, self_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)>=enum_type(rhs.v_); } \ + friend bool operator >=(self_type lhs, enum_type rhs) BOOST_NOEXCEPT { return enum_type(lhs.v_)>=rhs; } \ + friend bool operator >=(enum_type lhs, self_type rhs) BOOST_NOEXCEPT { return lhs>=enum_type(rhs.v_); } \ + }; + +#define BOOST_SCOPED_ENUM_DECLARE_END(EnumType) \ + ; \ + EnumType(enum_type v) BOOST_NOEXCEPT : v_(v) {} \ + BOOST_SCOPED_ENUM_DECLARE_END2() + +/** + * Starts a declaration of a scoped enum with the default int underlying type. + * + * @param EnumType The new scoped enum. + */ +#define BOOST_SCOPED_ENUM_DECLARE_BEGIN(EnumType) \ + BOOST_SCOPED_ENUM_UT_DECLARE_BEGIN(EnumType,int) + +/** + * Name of the native enum type. + * + * @param EnumType The new scoped enum. + */ +#define BOOST_SCOPED_ENUM_NATIVE(EnumType) EnumType::enum_type +/** + * Forward declares an scoped enum. + * + * @param EnumType The scoped enum. + */ +#define BOOST_SCOPED_ENUM_FORWARD_DECLARE(EnumType) struct EnumType + +#else // BOOST_NO_CXX11_SCOPED_ENUMS + +#define BOOST_SCOPED_ENUM_UT_DECLARE_BEGIN(EnumType,UnderlyingType) enum class EnumType : UnderlyingType +#define BOOST_SCOPED_ENUM_DECLARE_BEGIN(EnumType) enum class EnumType +#define BOOST_SCOPED_ENUM_DECLARE_END2() +#define BOOST_SCOPED_ENUM_DECLARE_END(EnumType) ; + +#define BOOST_SCOPED_ENUM_NATIVE(EnumType) EnumType +#define BOOST_SCOPED_ENUM_FORWARD_DECLARE(EnumType) enum class EnumType + +#endif // BOOST_NO_CXX11_SCOPED_ENUMS + +// Deprecated macros +#define BOOST_SCOPED_ENUM_START(name) BOOST_SCOPED_ENUM_DECLARE_BEGIN(name) +#define BOOST_SCOPED_ENUM_END BOOST_SCOPED_ENUM_DECLARE_END2() +#define BOOST_SCOPED_ENUM(name) BOOST_SCOPED_ENUM_NATIVE(name) + +#endif // BOOST_CORE_SCOPED_ENUM_HPP diff --git a/ros/vision_opencv/cv_bridge/src/boost/endian/conversion.hpp b/ros/vision_opencv/cv_bridge/src/boost/endian/conversion.hpp new file mode 100644 index 0000000..7c145d9 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/endian/conversion.hpp @@ -0,0 +1,488 @@ +// boost/endian/conversion.hpp -------------------------------------------------------// + +// Copyright Beman Dawes 2010, 2011, 2014 + +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt + +#ifndef BOOST_ENDIAN_CONVERSION_HPP +#define BOOST_ENDIAN_CONVERSION_HPP + +#include +#include +#include +#include +#include +#include +#include +#include // for memcpy + +//------------------------------------- synopsis ---------------------------------------// + +namespace boost +{ +namespace endian +{ + BOOST_SCOPED_ENUM_START(order) + { + big, little, +# ifdef BOOST_BIG_ENDIAN + native = big +# else + native = little +# endif + }; BOOST_SCOPED_ENUM_END + +//--------------------------------------------------------------------------------------// +// // +// return-by-value interfaces // +// suggested by Phil Endecott // +// // +// user-defined types (UDTs) // +// // +// All return-by-value conversion function templates are required to be implemented in // +// terms of an unqualified call to "endian_reverse(x)", a function returning the // +// value of x with endianness reversed. This provides a customization point for any // +// UDT that provides a "endian_reverse" free-function meeting the requirements. // +// It must be defined in the same namespace as the UDT itself so that it will be found // +// by argument dependent lookup (ADL). // +// // +//--------------------------------------------------------------------------------------// + + // customization for exact-length arithmetic types. See doc/conversion.html/#FAQ. + // Note: The omission of an overloads for the arithmetic type (typically long, or + // long long) not assigned to one of the exact length typedefs is a deliberate + // design decision. Such overloads would be non-portable and thus error prone. + + inline int8_t endian_reverse(int8_t x) BOOST_NOEXCEPT; + inline int16_t endian_reverse(int16_t x) BOOST_NOEXCEPT; + inline int32_t endian_reverse(int32_t x) BOOST_NOEXCEPT; + inline int64_t endian_reverse(int64_t x) BOOST_NOEXCEPT; + inline uint8_t endian_reverse(uint8_t x) BOOST_NOEXCEPT; + inline uint16_t endian_reverse(uint16_t x) BOOST_NOEXCEPT; + inline uint32_t endian_reverse(uint32_t x) BOOST_NOEXCEPT; + inline uint64_t endian_reverse(uint64_t x) BOOST_NOEXCEPT; + + // reverse byte order unless native endianness is big + template + inline EndianReversible big_to_native(EndianReversible x) BOOST_NOEXCEPT; + // Returns: x if native endian order is big, otherwise endian_reverse(x) + template + inline EndianReversible native_to_big(EndianReversible x) BOOST_NOEXCEPT; + // Returns: x if native endian order is big, otherwise endian_reverse(x) + + // reverse byte order unless native endianness is little + template + inline EndianReversible little_to_native(EndianReversible x) BOOST_NOEXCEPT; + // Returns: x if native endian order is little, otherwise endian_reverse(x) + template + inline EndianReversible native_to_little(EndianReversible x) BOOST_NOEXCEPT; + // Returns: x if native endian order is little, otherwise endian_reverse(x) + + // generic conditional reverse byte order + template + inline EndianReversible conditional_reverse(EndianReversible from) BOOST_NOEXCEPT; + // Returns: If From == To have different values, from. + // Otherwise endian_reverse(from). + // Remarks: The From == To test, and as a consequence which form the return takes, is + // is determined at compile time. + + // runtime conditional reverse byte order + template + inline EndianReversible conditional_reverse(EndianReversible from, + BOOST_SCOPED_ENUM(order) from_order, BOOST_SCOPED_ENUM(order) to_order) + BOOST_NOEXCEPT; + // Returns: from_order == to_order ? from : endian_reverse(from). + + //------------------------------------------------------------------------------------// + + + // Q: What happended to bswap, htobe, and the other synonym functions based on names + // popularized by BSD, OS X, and Linux? + // A: Turned out these may be implemented as macros on some systems. Ditto POSIX names + // for such functionality. Since macros would cause endless problems with functions + // of the same names, and these functions are just synonyms anyhow, they have been + // removed. + + + //------------------------------------------------------------------------------------// + // // + // reverse in place interfaces // + // // + // user-defined types (UDTs) // + // // + // All reverse in place function templates are required to be implemented in terms // + // of an unqualified call to "endian_reverse_inplace(x)", a function reversing // + // the endianness of x, which is a non-const reference. This provides a // + // customization point for any UDT that provides a "reverse_inplace" free-function // + // meeting the requirements. The free-function must be declared in the same // + // namespace as the UDT itself so that it will be found by argument-dependent // + // lookup (ADL). // + // // + //------------------------------------------------------------------------------------// + + // reverse in place + template + inline void endian_reverse_inplace(EndianReversible& x) BOOST_NOEXCEPT; + // Effects: x = endian_reverse(x) + + // reverse in place unless native endianness is big + template + inline void big_to_native_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT; + // Effects: none if native byte-order is big, otherwise endian_reverse_inplace(x) + template + inline void native_to_big_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT; + // Effects: none if native byte-order is big, otherwise endian_reverse_inplace(x) + + // reverse in place unless native endianness is little + template + inline void little_to_native_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT; + // Effects: none if native byte-order is little, otherwise endian_reverse_inplace(x); + template + inline void native_to_little_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT; + // Effects: none if native byte-order is little, otherwise endian_reverse_inplace(x); + + // generic conditional reverse in place + template + inline void conditional_reverse_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT; + + // runtime reverse in place + template + inline void conditional_reverse_inplace(EndianReversibleInplace& x, + BOOST_SCOPED_ENUM(order) from_order, BOOST_SCOPED_ENUM(order) to_order) + BOOST_NOEXCEPT; + +//----------------------------------- end synopsis -------------------------------------// + + namespace detail + { + // generic reverse function template implementation approach using std::reverse + // suggested by Mathias Gaunard. Primary motivation for inclusion is to have an + // independent implementation to test against. + + template + inline T std_endian_reverse(T x) BOOST_NOEXCEPT + { + T tmp(x); + std::reverse( + reinterpret_cast(&tmp), + reinterpret_cast(&tmp) + sizeof(T)); + return tmp; + } + + // conditional unaligned reverse copy, patterned after std::reverse_copy + template + inline void big_reverse_copy(T from, char* to) BOOST_NOEXCEPT; + template + inline void big_reverse_copy(const char* from, T& to) BOOST_NOEXCEPT; + template + inline void little_reverse_copy(T from, char* to) BOOST_NOEXCEPT; + template + inline void little_reverse_copy(const char* from, T& to) BOOST_NOEXCEPT; + } // namespace detail + +//--------------------------------------------------------------------------------------// +// // +// return-by-value implementation // +// // +// -- portable approach suggested by tymofey, with avoidance of undefined behavior // +// as suggested by Giovanni Piero Deretta, with a further refinement suggested // +// by Pyry Jahkola. // +// -- intrinsic approach suggested by reviewers, and by David Stone, who provided // +// his Boost licensed macro implementation (detail/intrinsic.hpp) // +// // +//--------------------------------------------------------------------------------------// + + inline int8_t endian_reverse(int8_t x) BOOST_NOEXCEPT + { + return x; + } + + inline int16_t endian_reverse(int16_t x) BOOST_NOEXCEPT + { +# ifdef BOOST_ENDIAN_NO_INTRINSICS + return (static_cast(x) << 8) + | (static_cast(x) >> 8); +# else + return BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_2(static_cast(x)); +# endif + } + + inline int32_t endian_reverse(int32_t x) BOOST_NOEXCEPT + { +# ifdef BOOST_ENDIAN_NO_INTRINSICS + uint32_t step16; + step16 = static_cast(x) << 16 | static_cast(x) >> 16; + return + ((static_cast(step16) << 8) & 0xff00ff00) + | ((static_cast(step16) >> 8) & 0x00ff00ff); +# else + return BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_4(static_cast(x)); +# endif + } + + inline int64_t endian_reverse(int64_t x) BOOST_NOEXCEPT + { +# ifdef BOOST_ENDIAN_NO_INTRINSICS + uint64_t step32, step16; + step32 = static_cast(x) << 32 | static_cast(x) >> 32; + step16 = (step32 & 0x0000FFFF0000FFFFULL) << 16 + | (step32 & 0xFFFF0000FFFF0000ULL) >> 16; + return static_cast((step16 & 0x00FF00FF00FF00FFULL) << 8 + | (step16 & 0xFF00FF00FF00FF00ULL) >> 8); +# else + return BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_8(static_cast(x)); +# endif + } + + inline uint8_t endian_reverse(uint8_t x) BOOST_NOEXCEPT + { + return x; + } + + inline uint16_t endian_reverse(uint16_t x) BOOST_NOEXCEPT + { +# ifdef BOOST_ENDIAN_NO_INTRINSICS + return (x << 8) + | (x >> 8); +# else + return BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_2(x); +# endif + } + + inline uint32_t endian_reverse(uint32_t x) BOOST_NOEXCEPT + { +# ifdef BOOST_ENDIAN_NO_INTRINSICS + uint32_t step16; + step16 = x << 16 | x >> 16; + return + ((step16 << 8) & 0xff00ff00) + | ((step16 >> 8) & 0x00ff00ff); +# else + return BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_4(x); +# endif + } + + inline uint64_t endian_reverse(uint64_t x) BOOST_NOEXCEPT + { +# ifdef BOOST_ENDIAN_NO_INTRINSICS + uint64_t step32, step16; + step32 = x << 32 | x >> 32; + step16 = (step32 & 0x0000FFFF0000FFFFULL) << 16 + | (step32 & 0xFFFF0000FFFF0000ULL) >> 16; + return (step16 & 0x00FF00FF00FF00FFULL) << 8 + | (step16 & 0xFF00FF00FF00FF00ULL) >> 8; +# else + return BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_8(x); +# endif + } + + template + inline EndianReversible big_to_native(EndianReversible x) BOOST_NOEXCEPT + { +# ifdef BOOST_BIG_ENDIAN + return x; +# else + return endian_reverse(x); +# endif + } + + template + inline EndianReversible native_to_big(EndianReversible x) BOOST_NOEXCEPT + { +# ifdef BOOST_BIG_ENDIAN + return x; +# else + return endian_reverse(x); +# endif + } + + template + inline EndianReversible little_to_native(EndianReversible x) BOOST_NOEXCEPT + { +# ifdef BOOST_LITTLE_ENDIAN + return x; +# else + return endian_reverse(x); +# endif + } + + template + inline EndianReversible native_to_little(EndianReversible x) BOOST_NOEXCEPT + { +# ifdef BOOST_LITTLE_ENDIAN + return x; +# else + return endian_reverse(x); +# endif + } + + namespace detail + { + // Primary template and specializations to support endian_reverse(). + // See rationale in endian_reverse() below. + template + class value_converter ; // primary template + template class value_converter + {public: T operator()(T x) BOOST_NOEXCEPT {return x;}}; + template class value_converter + {public: T operator()(T x) BOOST_NOEXCEPT {return x;}}; + template class value_converter + {public: T operator()(T x) BOOST_NOEXCEPT {return endian_reverse(x);}}; + template class value_converter + {public: T operator()(T x) BOOST_NOEXCEPT {return endian_reverse(x);}}; + } + + // generic conditional reverse + template + inline EndianReversible conditional_reverse(EndianReversible from) BOOST_NOEXCEPT { + // work around lack of function template partial specialization by instantiating + // a function object of a class that is partially specialized on the two order + // template parameters, and then calling its operator(). + detail::value_converter tmp; + return tmp(from); + } + + // runtime conditional reverse + template + inline EndianReversible conditional_reverse(EndianReversible from, + BOOST_SCOPED_ENUM(order) from_order, BOOST_SCOPED_ENUM(order) to_order) BOOST_NOEXCEPT + { + return from_order == to_order ? from : endian_reverse(from); + } + +//--------------------------------------------------------------------------------------// +// reverse-in-place implementation // +//--------------------------------------------------------------------------------------// + + // reverse in place + template + inline void endian_reverse_inplace(EndianReversible& x) BOOST_NOEXCEPT + { + x = endian_reverse(x); + } + + template +# ifdef BOOST_BIG_ENDIAN + inline void big_to_native_inplace(EndianReversibleInplace&) BOOST_NOEXCEPT {} +# else + inline void big_to_native_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT + { endian_reverse_inplace(x); } +# endif + template +# ifdef BOOST_BIG_ENDIAN + inline void native_to_big_inplace(EndianReversibleInplace&) BOOST_NOEXCEPT {} +# else + inline void native_to_big_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT + { + endian_reverse_inplace(x); + } +# endif + + template +# ifdef BOOST_LITTLE_ENDIAN + inline void little_to_native_inplace(EndianReversibleInplace&) BOOST_NOEXCEPT {} +# else + inline void little_to_native_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT + { endian_reverse_inplace(x); } +# endif + template +# ifdef BOOST_LITTLE_ENDIAN + inline void native_to_little_inplace(EndianReversibleInplace&) BOOST_NOEXCEPT {} +# else + inline void native_to_little_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT + { + endian_reverse_inplace(x); + } +# endif + + namespace detail + { + // Primary template and specializations support generic + // endian_reverse_inplace(). + // See rationale in endian_reverse_inplace() below. + template + class converter; // primary template + template class converter + {public: void operator()(T&) BOOST_NOEXCEPT {/*no effect*/}}; + template class converter + {public: void operator()(T&) BOOST_NOEXCEPT {/*no effect*/}}; + template class converter + {public: void operator()(T& x) BOOST_NOEXCEPT { endian_reverse_inplace(x); }}; + template class converter + {public: void operator()(T& x) BOOST_NOEXCEPT { endian_reverse_inplace(x); }}; + } // namespace detail + + // generic conditional reverse in place + template + inline void conditional_reverse_inplace(EndianReversibleInplace& x) BOOST_NOEXCEPT + { + // work around lack of function template partial specialization by instantiating + // a function object of a class that is partially specialized on the two order + // template parameters, and then calling its operator(). + detail::converter tmp; + tmp(x); // call operator () + } + + // runtime reverse in place + template + inline void conditional_reverse_inplace(EndianReversibleInplace& x, + BOOST_SCOPED_ENUM(order) from_order, BOOST_SCOPED_ENUM(order) to_order) + BOOST_NOEXCEPT + { + if (from_order != to_order) + endian_reverse_inplace(x); + } + + + namespace detail + { + template + inline void big_reverse_copy(T from, char* to) BOOST_NOEXCEPT + { +# ifdef BOOST_BIG_ENDIAN + std::memcpy(to, reinterpret_cast(&from), sizeof(T)); +# else + std::reverse_copy(reinterpret_cast(&from), + reinterpret_cast(&from) + sizeof(T), to); +# endif + } + template + inline void big_reverse_copy(const char* from, T& to) BOOST_NOEXCEPT + { +# ifdef BOOST_BIG_ENDIAN + std::memcpy(reinterpret_cast(&to), from, sizeof(T)); +# else + std::reverse_copy(from, from + sizeof(T), reinterpret_cast(&to)); +# endif + } + template + inline void little_reverse_copy(T from, char* to) BOOST_NOEXCEPT + { +# ifdef BOOST_LITTLE_ENDIAN + std::memcpy(to, reinterpret_cast(&from), sizeof(T)); +# else + std::reverse_copy(reinterpret_cast(&from), + reinterpret_cast(&from) + sizeof(T), to); +# endif + } + template + inline void little_reverse_copy(const char* from, T& to) BOOST_NOEXCEPT + { +# ifdef BOOST_LITTLE_ENDIAN + std::memcpy(reinterpret_cast(&to), from, sizeof(T)); +# else + std::reverse_copy(from, from + sizeof(T), reinterpret_cast(&to)); +# endif + } + } // namespace detail +} // namespace endian +} // namespace boost + +#endif // BOOST_ENDIAN_CONVERSION_HPP diff --git a/ros/vision_opencv/cv_bridge/src/boost/endian/detail/intrinsic.hpp b/ros/vision_opencv/cv_bridge/src/boost/endian/detail/intrinsic.hpp new file mode 100644 index 0000000..6ead681 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/endian/detail/intrinsic.hpp @@ -0,0 +1,64 @@ +// endian/detail/intrinsic.hpp -------------------------------------------------------// + +// Copyright (C) 2012 David Stone +// Copyright Beman Dawes 2013 + +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt + +#ifndef BOOST_ENDIAN_INTRINSIC_HPP +#define BOOST_ENDIAN_INTRINSIC_HPP + +// Allow user to force BOOST_ENDIAN_NO_INTRINSICS in case they aren't available for a +// particular platform/compiler combination. Please report such platform/compiler +// combinations to the Boost mailing list. +#ifndef BOOST_ENDIAN_NO_INTRINSICS + +#ifndef __has_builtin // Optional of course + #define __has_builtin(x) 0 // Compatibility with non-clang compilers +#endif + +// GCC and Clang recent versions provide intrinsic byte swaps via builtins +#if (defined(__clang__) && __has_builtin(__builtin_bswap32) && __has_builtin(__builtin_bswap64)) \ + || (defined(__GNUC__ ) && \ + (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 3))) +# define BOOST_ENDIAN_INTRINSIC_MSG "__builtin_bswap16, etc." +// prior to 4.8, gcc did not provide __builtin_bswap16 on some platforms so we emulate it +// see http://gcc.gnu.org/bugzilla/show_bug.cgi?id=52624 +// Clang has a similar problem, but their feature test macros make it easier to detect +# if (defined(__clang__) && __has_builtin(__builtin_bswap16)) \ + || (defined(__GNUC__) &&(__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8))) +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_2(x) __builtin_bswap16(x) +# else +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_2(x) __builtin_bswap32((x) << 16) +# endif +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_4(x) __builtin_bswap32(x) +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_8(x) __builtin_bswap64(x) + +// Linux systems provide the byteswap.h header, with +#elif defined(__linux__) +// don't check for obsolete forms defined(linux) and defined(__linux) on the theory that +// compilers that predefine only these are so old that byteswap.h probably isn't present. +# define BOOST_ENDIAN_INTRINSIC_MSG "byteswap.h bswap_16, etc." +# include +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_2(x) bswap_16(x) +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_4(x) bswap_32(x) +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_8(x) bswap_64(x) + +#elif defined(_MSC_VER) +// Microsoft documents these as being compatible since Windows 95 and specificly +// lists runtime library support since Visual Studio 2003 (aka 7.1). +# define BOOST_ENDIAN_INTRINSIC_MSG "cstdlib _byteswap_ushort, etc." +# include +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_2(x) _byteswap_ushort(x) +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_4(x) _byteswap_ulong(x) +# define BOOST_ENDIAN_INTRINSIC_BYTE_SWAP_8(x) _byteswap_uint64(x) +#else +# define BOOST_ENDIAN_NO_INTRINSICS +# define BOOST_ENDIAN_INTRINSIC_MSG "no byte swap intrinsics" +#endif + +#elif !defined(BOOST_ENDIAN_INTRINSIC_MSG) +# define BOOST_ENDIAN_INTRINSIC_MSG "no byte swap intrinsics" +#endif // BOOST_ENDIAN_NO_INTRINSICS +#endif // BOOST_ENDIAN_INTRINSIC_HPP diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/detail/_cassert.h b/ros/vision_opencv/cv_bridge/src/boost/predef/detail/_cassert.h new file mode 100644 index 0000000..940e944 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/detail/_cassert.h @@ -0,0 +1,17 @@ +/* +Copyright Rene Rivera 2011-2012 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_DETAIL__CASSERT_H +#define BOOST_PREDEF_DETAIL__CASSERT_H + +#if defined(__cplusplus) +#include +#else +#include +#endif + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/detail/endian_compat.h b/ros/vision_opencv/cv_bridge/src/boost/predef/detail/endian_compat.h new file mode 100644 index 0000000..7725e68 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/detail/endian_compat.h @@ -0,0 +1,26 @@ +/* +Copyright Rene Rivera 2013 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_DETAIL_ENDIAN_COMPAT_H +#define BOOST_PREDEF_DETAIL_ENDIAN_COMPAT_H + +#include + +#if BOOST_ENDIAN_BIG_BYTE +# define BOOST_BIG_ENDIAN +# define BOOST_BYTE_ORDER 4321 +#endif +#if BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_LITTLE_ENDIAN +# define BOOST_BYTE_ORDER 1234 +#endif +#if BOOST_ENDIAN_LITTLE_WORD +# define BOOST_PDP_ENDIAN +# define BOOST_BYTE_ORDER 2134 +#endif + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/detail/test.h b/ros/vision_opencv/cv_bridge/src/boost/predef/detail/test.h new file mode 100644 index 0000000..546a9e4 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/detail/test.h @@ -0,0 +1,17 @@ +/* +Copyright Rene Rivera 2011-2012 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_DETAIL_TEST_H +#define BOOST_PREDEF_DETAIL_TEST_H + +#if !defined(BOOST_PREDEF_INTERNAL_GENERATE_TESTS) + +#define BOOST_PREDEF_DECLARE_TEST(x,s) + +#endif + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/library/c/_prefix.h b/ros/vision_opencv/cv_bridge/src/boost/predef/library/c/_prefix.h new file mode 100644 index 0000000..12bcb0f --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/library/c/_prefix.h @@ -0,0 +1,13 @@ +/* +Copyright Rene Rivera 2008-2013 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_LIBRARY_C__PREFIX_H +#define BOOST_PREDEF_LIBRARY_C__PREFIX_H + +#include + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/library/c/gnu.h b/ros/vision_opencv/cv_bridge/src/boost/predef/library/c/gnu.h new file mode 100644 index 0000000..9e4ca89 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/library/c/gnu.h @@ -0,0 +1,61 @@ +/* +Copyright Rene Rivera 2008-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_LIBRARY_C_GNU_H +#define BOOST_PREDEF_LIBRARY_C_GNU_H + +#include +#include + +#include + +#if defined(__STDC__) +#include +#elif defined(__cplusplus) +#include +#endif + +/*` +[heading `BOOST_LIB_C_GNU`] + +[@http://en.wikipedia.org/wiki/Glibc GNU glibc] Standard C library. +Version number available as major, and minor. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__GLIBC__`] [__predef_detection__]] + [[`__GNU_LIBRARY__`] [__predef_detection__]] + + [[`__GLIBC__`, `__GLIBC_MINOR__`] [V.R.0]] + [[`__GNU_LIBRARY__`, `__GNU_LIBRARY_MINOR__`] [V.R.0]] + ] + */ + +#define BOOST_LIB_C_GNU BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if defined(__GLIBC__) || defined(__GNU_LIBRARY__) +# undef BOOST_LIB_C_GNU +# if defined(__GLIBC__) +# define BOOST_LIB_C_GNU \ + BOOST_VERSION_NUMBER(__GLIBC__,__GLIBC_MINOR__,0) +# else +# define BOOST_LIB_C_GNU \ + BOOST_VERSION_NUMBER(__GNU_LIBRARY__,__GNU_LIBRARY_MINOR__,0) +# endif +#endif + +#if BOOST_LIB_C_GNU +# define BOOST_LIB_C_GNU_AVAILABLE +#endif + +#define BOOST_LIB_C_GNU_NAME "GNU" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_LIB_C_GNU,BOOST_LIB_C_GNU_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/make.h b/ros/vision_opencv/cv_bridge/src/boost/predef/make.h new file mode 100644 index 0000000..4f2f9ee --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/make.h @@ -0,0 +1,89 @@ +/* +Copyright Rene Rivera 2008-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ +#include + +#ifndef BOOST_PREDEF_MAKE_H +#define BOOST_PREDEF_MAKE_H + +/* +Shorthands for the common version number formats used by vendors... +*/ + +/*` +[heading `BOOST_PREDEF_MAKE_..` macros] + +These set of macros decompose common vendor version number +macros which are composed version, revision, and patch digits. +The naming convention indicates: + +* The base of the specified version number. "`BOOST_PREDEF_MAKE_0X`" for + hexadecimal digits, and "`BOOST_PREDEF_MAKE_10`" for decimal digits. +* The format of the vendor version number. Where "`V`" indicates the version digits, + "`R`" indicates the revision digits, "`P`" indicates the patch digits, and "`0`" + indicates an ignored digit. + +Macros are: +*/ +/*` `BOOST_PREDEF_MAKE_0X_VRP(V)` */ +#define BOOST_PREDEF_MAKE_0X_VRP(V) BOOST_VERSION_NUMBER((V&0xF00)>>8,(V&0xF0)>>4,(V&0xF)) +/*` `BOOST_PREDEF_MAKE_0X_VVRP(V)` */ +#define BOOST_PREDEF_MAKE_0X_VVRP(V) BOOST_VERSION_NUMBER((V&0xFF00)>>8,(V&0xF0)>>4,(V&0xF)) +/*` `BOOST_PREDEF_MAKE_0X_VRPP(V)` */ +#define BOOST_PREDEF_MAKE_0X_VRPP(V) BOOST_VERSION_NUMBER((V&0xF000)>>12,(V&0xF00)>>8,(V&0xFF)) +/*` `BOOST_PREDEF_MAKE_0X_VVRR(V)` */ +#define BOOST_PREDEF_MAKE_0X_VVRR(V) BOOST_VERSION_NUMBER((V&0xFF00)>>8,(V&0xFF),0) +/*` `BOOST_PREDEF_MAKE_0X_VRRPPPP(V)` */ +#define BOOST_PREDEF_MAKE_0X_VRRPPPP(V) BOOST_VERSION_NUMBER((V&0xF000000)>>24,(V&0xFF0000)>>16,(V&0xFFFF)) +/*` `BOOST_PREDEF_MAKE_0X_VVRRP(V)` */ +#define BOOST_PREDEF_MAKE_0X_VVRRP(V) BOOST_VERSION_NUMBER((V&0xFF000)>>12,(V&0xFF0)>>4,(V&0xF)) +/*` `BOOST_PREDEF_MAKE_0X_VRRPP000(V)` */ +#define BOOST_PREDEF_MAKE_0X_VRRPP000(V) BOOST_VERSION_NUMBER((V&0xF0000000)>>28,(V&0xFF00000)>>20,(V&0xFF000)>>12) +/*` `BOOST_PREDEF_MAKE_0X_VVRRPP(V)` */ +#define BOOST_PREDEF_MAKE_0X_VVRRPP(V) BOOST_VERSION_NUMBER((V&0xFF0000)>>16,(V&0xFF00)>>8,(V&0xFF)) +/*` `BOOST_PREDEF_MAKE_10_VPPP(V)` */ +#define BOOST_PREDEF_MAKE_10_VPPP(V) BOOST_VERSION_NUMBER(((V)/1000)%10,0,(V)%1000) +/*` `BOOST_PREDEF_MAKE_10_VRP(V)` */ +#define BOOST_PREDEF_MAKE_10_VRP(V) BOOST_VERSION_NUMBER(((V)/100)%10,((V)/10)%10,(V)%10) +/*` `BOOST_PREDEF_MAKE_10_VRP000(V)` */ +#define BOOST_PREDEF_MAKE_10_VRP000(V) BOOST_VERSION_NUMBER(((V)/100000)%10,((V)/10000)%10,((V)/1000)%10) +/*` `BOOST_PREDEF_MAKE_10_VRPP(V)` */ +#define BOOST_PREDEF_MAKE_10_VRPP(V) BOOST_VERSION_NUMBER(((V)/1000)%10,((V)/100)%10,(V)%100) +/*` `BOOST_PREDEF_MAKE_10_VRR(V)` */ +#define BOOST_PREDEF_MAKE_10_VRR(V) BOOST_VERSION_NUMBER(((V)/100)%10,(V)%100,0) +/*` `BOOST_PREDEF_MAKE_10_VRRPP(V)` */ +#define BOOST_PREDEF_MAKE_10_VRRPP(V) BOOST_VERSION_NUMBER(((V)/10000)%10,((V)/100)%100,(V)%100) +/*` `BOOST_PREDEF_MAKE_10_VRR000(V)` */ +#define BOOST_PREDEF_MAKE_10_VRR000(V) BOOST_VERSION_NUMBER(((V)/100000)%10,((V)/1000)%100,0) +/*` `BOOST_PREDEF_MAKE_10_VV00(V)` */ +#define BOOST_PREDEF_MAKE_10_VV00(V) BOOST_VERSION_NUMBER(((V)/100)%100,0,0) +/*` `BOOST_PREDEF_MAKE_10_VVRR(V)` */ +#define BOOST_PREDEF_MAKE_10_VVRR(V) BOOST_VERSION_NUMBER(((V)/100)%100,(V)%100,0) +/*` `BOOST_PREDEF_MAKE_10_VVRRPP(V)` */ +#define BOOST_PREDEF_MAKE_10_VVRRPP(V) BOOST_VERSION_NUMBER(((V)/10000)%100,((V)/100)%100,(V)%100) +/*` `BOOST_PREDEF_MAKE_10_VVRR0PP00(V)` */ +#define BOOST_PREDEF_MAKE_10_VVRR0PP00(V) BOOST_VERSION_NUMBER(((V)/10000000)%100,((V)/100000)%100,((V)/100)%100) +/*` `BOOST_PREDEF_MAKE_10_VVRR0PPPP(V)` */ +#define BOOST_PREDEF_MAKE_10_VVRR0PPPP(V) BOOST_VERSION_NUMBER(((V)/10000000)%100,((V)/100000)%100,(V)%10000) +/*` `BOOST_PREDEF_MAKE_10_VVRR00PP00(V)` */ +#define BOOST_PREDEF_MAKE_10_VVRR00PP00(V) BOOST_VERSION_NUMBER(((V)/100000000)%100,((V)/1000000)%100,((V)/100)%100) +/*` +[heading `BOOST_PREDEF_MAKE_*..` date macros] + +Date decomposition macros return a date in the relative to the 1970 +Epoch date. If the month is not available, January 1st is used as the month and day. +If the day is not available, but the month is, the 1st of the month is used as the day. +*/ +/*` `BOOST_PREDEF_MAKE_DATE(Y,M,D)` */ +#define BOOST_PREDEF_MAKE_DATE(Y,M,D) BOOST_VERSION_NUMBER((Y)%10000-1970,(M)%100,(D)%100) +/*` `BOOST_PREDEF_MAKE_YYYYMMDD(V)` */ +#define BOOST_PREDEF_MAKE_YYYYMMDD(V) BOOST_PREDEF_MAKE_DATE(((V)/10000)%10000,((V)/100)%100,(V)%100) +/*` `BOOST_PREDEF_MAKE_YYYY(V)` */ +#define BOOST_PREDEF_MAKE_YYYY(V) BOOST_PREDEF_MAKE_DATE(V,1,1) +/*` `BOOST_PREDEF_MAKE_YYYYMM(V)` */ +#define BOOST_PREDEF_MAKE_YYYYMM(V) BOOST_PREDEF_MAKE_DATE((V)/100,(V)%100,1) + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/android.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/android.h new file mode 100644 index 0000000..00836e7 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/android.h @@ -0,0 +1,45 @@ +/* +Copyright Rene Rivera 2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_ADROID_H +#define BOOST_PREDEF_OS_ADROID_H + +#include +#include + +/*` +[heading `BOOST_OS_ANDROID`] + +[@http://en.wikipedia.org/wiki/Android_%28operating_system%29 Android] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__ANDROID__`] [__predef_detection__]] + ] + */ + +#define BOOST_OS_ANDROID BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__ANDROID__) \ + ) +# undef BOOST_OS_ANDROID +# define BOOST_OS_ANDROID BOOST_VERSION_NUMBER_AVAILABLE +#endif + +#if BOOST_OS_ANDROID +# define BOOST_OS_ANDROID_AVAILABLE +# include +#endif + +#define BOOST_OS_ANDROID_NAME "Android" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_ANDROID,BOOST_OS_ANDROID_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd.h new file mode 100644 index 0000000..fad9aed --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd.h @@ -0,0 +1,103 @@ +/* +Copyright Rene Rivera 2008-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_BSD_H +#define BOOST_PREDEF_OS_BSD_H + +/* Special case: OSX will define BSD predefs if the sys/param.h + * header is included. We can guard against that, but only if we + * detect OSX first. Hence we will force include OSX detection + * before doing any BSD detection. + */ +#include + +#include +#include + +/*` +[heading `BOOST_OS_BSD`] + +[@http://en.wikipedia.org/wiki/Berkeley_Software_Distribution BSD] operating system. + +BSD has various branch operating systems possible and each detected +individually. This detects the following variations and sets a specific +version number macro to match: + +* `BOOST_OS_BSD_DRAGONFLY` [@http://en.wikipedia.org/wiki/DragonFly_BSD DragonFly BSD] +* `BOOST_OS_BSD_FREE` [@http://en.wikipedia.org/wiki/Freebsd FreeBSD] +* `BOOST_OS_BSD_BSDI` [@http://en.wikipedia.org/wiki/BSD/OS BSDi BSD/OS] +* `BOOST_OS_BSD_NET` [@http://en.wikipedia.org/wiki/Netbsd NetBSD] +* `BOOST_OS_BSD_OPEN` [@http://en.wikipedia.org/wiki/Openbsd OpenBSD] + +[note The general `BOOST_OS_BSD` is set in all cases to indicate some form +of BSD. If the above variants is detected the corresponding macro is also set.] + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`BSD`] [__predef_detection__]] + [[`_SYSTYPE_BSD`] [__predef_detection__]] + + [[`BSD4_2`] [4.2.0]] + [[`BSD4_3`] [4.3.0]] + [[`BSD4_4`] [4.4.0]] + [[`BSD`] [V.R.0]] + ] + */ + +#include +#include +#include +#include +#include + +#ifndef BOOST_OS_BSD +#define BOOST_OS_BSD BOOST_VERSION_NUMBER_NOT_AVAILABLE +#endif + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(BSD) || \ + defined(_SYSTYPE_BSD) \ + ) +# undef BOOST_OS_BSD +# include +# if !defined(BOOST_OS_BSD) && defined(BSD4_4) +# define BOOST_OS_BSD BOOST_VERSION_NUMBER(4,4,0) +# endif +# if !defined(BOOST_OS_BSD) && defined(BSD4_3) +# define BOOST_OS_BSD BOOST_VERSION_NUMBER(4,3,0) +# endif +# if !defined(BOOST_OS_BSD) && defined(BSD4_2) +# define BOOST_OS_BSD BOOST_VERSION_NUMBER(4,2,0) +# endif +# if !defined(BOOST_OS_BSD) && defined(BSD) +# define BOOST_OS_BSD BOOST_PREDEF_MAKE_10_VVRR(BSD) +# endif +# if !defined(BOOST_OS_BSD) +# define BOOST_OS_BSD BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +#if BOOST_OS_BSD +# define BOOST_OS_BSD_AVAILABLE +# include +#endif + +#define BOOST_OS_BSD_NAME "BSD" + +#else + +#include +#include +#include +#include +#include + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_BSD,BOOST_OS_BSD_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/bsdi.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/bsdi.h new file mode 100644 index 0000000..afdcd3e --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/bsdi.h @@ -0,0 +1,48 @@ +/* +Copyright Rene Rivera 2012-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_BSD_BSDI_H +#define BOOST_PREDEF_OS_BSD_BSDI_H + +#include + +/*` +[heading `BOOST_OS_BSD_BSDI`] + +[@http://en.wikipedia.org/wiki/BSD/OS BSDi BSD/OS] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__bsdi__`] [__predef_detection__]] + ] + */ + +#define BOOST_OS_BSD_BSDI BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__bsdi__) \ + ) +# ifndef BOOST_OS_BSD_AVAILABLE +# define BOOST_OS_BSD BOOST_VERSION_NUMBER_AVAILABLE +# define BOOST_OS_BSD_AVAILABLE +# endif +# undef BOOST_OS_BSD_BSDI +# define BOOST_OS_BSD_BSDI BOOST_VERSION_NUMBER_AVAILABLE +#endif + +#if BOOST_OS_BSD_BSDI +# define BOOST_OS_BSD_BSDI_AVAILABLE +# include +#endif + +#define BOOST_OS_BSD_BSDI_NAME "BSDi BSD/OS" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_BSD_BSDI,BOOST_OS_BSD_BSDI_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/dragonfly.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/dragonfly.h new file mode 100644 index 0000000..1d07579 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/dragonfly.h @@ -0,0 +1,50 @@ +/* +Copyright Rene Rivera 2012-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_BSD_DRAGONFLY_H +#define BOOST_PREDEF_OS_BSD_DRAGONFLY_H + +#include + +/*` +[heading `BOOST_OS_BSD_DRAGONFLY`] + +[@http://en.wikipedia.org/wiki/DragonFly_BSD DragonFly BSD] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__DragonFly__`] [__predef_detection__]] + ] + */ + +#define BOOST_OS_BSD_DRAGONFLY BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__DragonFly__) \ + ) +# ifndef BOOST_OS_BSD_AVAILABLE +# define BOOST_OS_BSD BOOST_VERSION_NUMBER_AVAILABLE +# define BOOST_OS_BSD_AVAILABLE +# endif +# undef BOOST_OS_BSD_DRAGONFLY +# if defined(__DragonFly__) +# define BOOST_OS_DRAGONFLY_BSD BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +#if BOOST_OS_BSD_DRAGONFLY +# define BOOST_OS_BSD_DRAGONFLY_AVAILABLE +# include +#endif + +#define BOOST_OS_BSD_DRAGONFLY_NAME "DragonFly BSD" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_BSD_DRAGONFLY,BOOST_OS_BSD_DRAGONFLY_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/free.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/free.h new file mode 100644 index 0000000..248011a --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/free.h @@ -0,0 +1,60 @@ +/* +Copyright Rene Rivera 2012-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_BSD_FREE_H +#define BOOST_PREDEF_OS_BSD_FREE_H + +#include + +/*` +[heading `BOOST_OS_BSD_FREE`] + +[@http://en.wikipedia.org/wiki/Freebsd FreeBSD] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__FreeBSD__`] [__predef_detection__]] + + [[`__FreeBSD_version`] [V.R.P]] + ] + */ + +#define BOOST_OS_BSD_FREE BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__FreeBSD__) \ + ) +# ifndef BOOST_OS_BSD_AVAILABLE +# define BOOST_OS_BSD BOOST_VERSION_NUMBER_AVAILABLE +# define BOOST_OS_BSD_AVAILABLE +# endif +# undef BOOST_OS_BSD_FREE +# if defined(__FreeBSD_version) +# if __FreeBSD_version < 500000 +# define BOOST_OS_BSD_FREE \ + BOOST_PREDEF_MAKE_10_VRP000(__FreeBSD_version) +# else +# define BOOST_OS_BSD_FREE \ + BOOST_PREDEF_MAKE_10_VRR000(__FreeBSD_version) +# endif +# else +# define BOOST_OS_BSD_FREE BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +#if BOOST_OS_BSD_FREE +# define BOOST_OS_BSD_FREE_AVAILABLE +# include +#endif + +#define BOOST_OS_BSD_FREE_NAME "Free BSD" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_BSD_FREE,BOOST_OS_BSD_FREE_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/net.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/net.h new file mode 100644 index 0000000..387cbde --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/net.h @@ -0,0 +1,84 @@ +/* +Copyright Rene Rivera 2012-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_BSD_NET_H +#define BOOST_PREDEF_OS_BSD_NET_H + +#include + +/*` +[heading `BOOST_OS_BSD_NET`] + +[@http://en.wikipedia.org/wiki/Netbsd NetBSD] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__NETBSD__`] [__predef_detection__]] + [[`__NetBSD__`] [__predef_detection__]] + + [[`__NETBSD_version`] [V.R.P]] + [[`NetBSD0_8`] [0.8.0]] + [[`NetBSD0_9`] [0.9.0]] + [[`NetBSD1_0`] [1.0.0]] + [[`__NetBSD_Version`] [V.R.P]] + ] + */ + +#define BOOST_OS_BSD_NET BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__NETBSD__) || defined(__NetBSD__) \ + ) +# ifndef BOOST_OS_BSD_AVAILABLE +# define BOOST_OS_BSD BOOST_VERSION_NUMBER_AVAILABLE +# define BOOST_OS_BSD_AVAILABLE +# endif +# undef BOOST_OS_BSD_NET +# if defined(__NETBSD__) +# if defined(__NETBSD_version) +# if __NETBSD_version < 500000 +# define BOOST_OS_BSD_NET \ + BOOST_PREDEF_MAKE_10_VRP000(__NETBSD_version) +# else +# define BOOST_OS_BSD_NET \ + BOOST_PREDEF_MAKE_10_VRR000(__NETBSD_version) +# endif +# else +# define BOOST_OS_BSD_NET BOOST_VERSION_NUMBER_AVAILABLE +# endif +# elif defined(__NetBSD__) +# if !defined(BOOST_OS_BSD_NET) && defined(NetBSD0_8) +# define BOOST_OS_BSD_NET BOOST_VERSION_NUMBER(0,8,0) +# endif +# if !defined(BOOST_OS_BSD_NET) && defined(NetBSD0_9) +# define BOOST_OS_BSD_NET BOOST_VERSION_NUMBER(0,9,0) +# endif +# if !defined(BOOST_OS_BSD_NET) && defined(NetBSD1_0) +# define BOOST_OS_BSD_NET BOOST_VERSION_NUMBER(1,0,0) +# endif +# if !defined(BOOST_OS_BSD_NET) && defined(__NetBSD_Version) +# define BOOST_OS_BSD_NET \ + BOOST_PREDEF_MAKE_10_VVRR00PP00(__NetBSD_Version) +# endif +# if !defined(BOOST_OS_BSD_NET) +# define BOOST_OS_BSD_NET BOOST_VERSION_NUMBER_AVAILABLE +# endif +# endif +#endif + +#if BOOST_OS_BSD_NET +# define BOOST_OS_BSD_NET_AVAILABLE +# include +#endif + +#define BOOST_OS_BSD_NET_NAME "DragonFly BSD" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_BSD_NET,BOOST_OS_BSD_NET_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/open.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/open.h new file mode 100644 index 0000000..423103a --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/bsd/open.h @@ -0,0 +1,171 @@ +/* +Copyright Rene Rivera 2012-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_BSD_OPEN_H +#define BOOST_PREDEF_OS_BSD_OPEN_H + +#include + +/*` +[heading `BOOST_OS_BSD_OPEN`] + +[@http://en.wikipedia.org/wiki/Openbsd OpenBSD] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__OpenBSD__`] [__predef_detection__]] + + [[`OpenBSD2_0`] [2.0.0]] + [[`OpenBSD2_1`] [2.1.0]] + [[`OpenBSD2_2`] [2.2.0]] + [[`OpenBSD2_3`] [2.3.0]] + [[`OpenBSD2_4`] [2.4.0]] + [[`OpenBSD2_5`] [2.5.0]] + [[`OpenBSD2_6`] [2.6.0]] + [[`OpenBSD2_7`] [2.7.0]] + [[`OpenBSD2_8`] [2.8.0]] + [[`OpenBSD2_9`] [2.9.0]] + [[`OpenBSD3_0`] [3.0.0]] + [[`OpenBSD3_1`] [3.1.0]] + [[`OpenBSD3_2`] [3.2.0]] + [[`OpenBSD3_3`] [3.3.0]] + [[`OpenBSD3_4`] [3.4.0]] + [[`OpenBSD3_5`] [3.5.0]] + [[`OpenBSD3_6`] [3.6.0]] + [[`OpenBSD3_7`] [3.7.0]] + [[`OpenBSD3_8`] [3.8.0]] + [[`OpenBSD3_9`] [3.9.0]] + [[`OpenBSD4_0`] [4.0.0]] + [[`OpenBSD4_1`] [4.1.0]] + [[`OpenBSD4_2`] [4.2.0]] + [[`OpenBSD4_3`] [4.3.0]] + [[`OpenBSD4_4`] [4.4.0]] + [[`OpenBSD4_5`] [4.5.0]] + [[`OpenBSD4_6`] [4.6.0]] + [[`OpenBSD4_7`] [4.7.0]] + [[`OpenBSD4_8`] [4.8.0]] + [[`OpenBSD4_9`] [4.9.0]] + ] + */ + +#define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__OpenBSD__) \ + ) +# ifndef BOOST_OS_BSD_AVAILABLE +# define BOOST_OS_BSD BOOST_VERSION_NUMBER_AVAILABLE +# define BOOST_OS_BSD_AVAILABLE +# endif +# undef BOOST_OS_BSD_OPEN +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_0) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,0,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_1) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,1,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_2) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,2,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_3) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,3,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_4) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,4,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_5) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,5,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_6) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,6,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_7) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,7,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_8) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,8,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD2_9) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(2,9,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_0) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,0,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_1) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,1,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_2) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,2,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_3) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,3,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_4) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,4,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_5) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,5,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_6) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,6,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_7) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,7,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_8) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,8,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD3_9) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(3,9,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_0) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,0,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_1) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,1,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_2) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,2,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_3) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,3,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_4) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,4,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_5) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,5,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_6) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,6,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_7) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,7,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_8) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,8,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) && defined(OpenBSD4_9) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER(4,9,0) +# endif +# if !defined(BOOST_OS_BSD_OPEN) +# define BOOST_OS_BSD_OPEN BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +#if BOOST_OS_BSD_OPEN +# define BOOST_OS_BSD_OPEN_AVAILABLE +# include +#endif + +#define BOOST_OS_BSD_OPEN_NAME "OpenBSD" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_BSD_OPEN,BOOST_OS_BSD_OPEN_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/ios.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/ios.h new file mode 100644 index 0000000..f853815 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/ios.h @@ -0,0 +1,51 @@ +/* +Copyright Franz Detro 2014 +Copyright Rene Rivera 2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_IOS_H +#define BOOST_PREDEF_OS_IOS_H + +#include +#include + +/*` +[heading `BOOST_OS_IOS`] + +[@http://en.wikipedia.org/wiki/iOS iOS] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`__APPLE__`] [__predef_detection__]] + [[`__MACH__`] [__predef_detection__]] + [[`__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__`] [__predef_detection__]] + + [[`__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__`] [__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__*1000]] + ] + */ + +#define BOOST_OS_IOS BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(__APPLE__) && defined(__MACH__) && \ + defined(__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__) \ + ) +# undef BOOST_OS_IOS +# define BOOST_OS_IOS (__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__*1000) +#endif + +#if BOOST_OS_IOS +# define BOOST_OS_IOS_AVAILABLE +# include +#endif + +#define BOOST_OS_IOS_NAME "iOS" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_IOS,BOOST_OS_IOS_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/os/macos.h b/ros/vision_opencv/cv_bridge/src/boost/predef/os/macos.h new file mode 100644 index 0000000..4afb30d --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/os/macos.h @@ -0,0 +1,65 @@ +/* +Copyright Rene Rivera 2008-2015 +Copyright Franz Detro 2014 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_OS_MACOS_H +#define BOOST_PREDEF_OS_MACOS_H + +/* Special case: iOS will define the same predefs as MacOS, and additionally + '__ENVIRONMENT_IPHONE_OS_VERSION_MIN_REQUIRED__'. We can guard against that, + but only if we detect iOS first. Hence we will force include iOS detection + * before doing any MacOS detection. + */ +#include + +#include +#include + +/*` +[heading `BOOST_OS_MACOS`] + +[@http://en.wikipedia.org/wiki/Mac_OS Mac OS] operating system. + +[table + [[__predef_symbol__] [__predef_version__]] + + [[`macintosh`] [__predef_detection__]] + [[`Macintosh`] [__predef_detection__]] + [[`__APPLE__`] [__predef_detection__]] + [[`__MACH__`] [__predef_detection__]] + + [[`__APPLE__`, `__MACH__`] [10.0.0]] + [[ /otherwise/ ] [9.0.0]] + ] + */ + +#define BOOST_OS_MACOS BOOST_VERSION_NUMBER_NOT_AVAILABLE + +#if !defined(BOOST_PREDEF_DETAIL_OS_DETECTED) && ( \ + defined(macintosh) || defined(Macintosh) || \ + (defined(__APPLE__) && defined(__MACH__)) \ + ) +# undef BOOST_OS_MACOS +# if !defined(BOOST_OS_MACOS) && defined(__APPLE__) && defined(__MACH__) +# define BOOST_OS_MACOS BOOST_VERSION_NUMBER(10,0,0) +# endif +# if !defined(BOOST_OS_MACOS) +# define BOOST_OS_MACOS BOOST_VERSION_NUMBER(9,0,0) +# endif +#endif + +#if BOOST_OS_MACOS +# define BOOST_OS_MACOS_AVAILABLE +# include +#endif + +#define BOOST_OS_MACOS_NAME "Mac OS" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_OS_MACOS,BOOST_OS_MACOS_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/other/endian.h b/ros/vision_opencv/cv_bridge/src/boost/predef/other/endian.h new file mode 100644 index 0000000..6d1f43f --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/other/endian.h @@ -0,0 +1,204 @@ +/* +Copyright Rene Rivera 2013-2015 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_ENDIAN_H +#define BOOST_PREDEF_ENDIAN_H + +#include +#include +#include +#include +#include +#include + +/*` +[heading `BOOST_ENDIAN_*`] + +Detection of endian memory ordering. There are four defined macros +in this header that define the various generally possible endian +memory orderings: + +* `BOOST_ENDIAN_BIG_BYTE`, byte-swapped big-endian. +* `BOOST_ENDIAN_BIG_WORD`, word-swapped big-endian. +* `BOOST_ENDIAN_LITTLE_BYTE`, byte-swapped little-endian. +* `BOOST_ENDIAN_LITTLE_WORD`, word-swapped little-endian. + +The detection is conservative in that it only identifies endianness +that it knows for certain. In particular bi-endianness is not +indicated as is it not practically possible to determine the +endianness from anything but an operating system provided +header. And the currently known headers do not define that +programatic bi-endianness is available. + +This implementation is a compilation of various publicly available +information and acquired knowledge: + +# The indispensable documentation of "Pre-defined Compiler Macros" + [@http://sourceforge.net/p/predef/wiki/Endianness Endianness]. +# The various endian specifications available in the + [@http://wikipedia.org/ Wikipedia] computer architecture pages. +# Generally available searches for headers that define endianness. + */ + +#define BOOST_ENDIAN_BIG_BYTE BOOST_VERSION_NUMBER_NOT_AVAILABLE +#define BOOST_ENDIAN_BIG_WORD BOOST_VERSION_NUMBER_NOT_AVAILABLE +#define BOOST_ENDIAN_LITTLE_BYTE BOOST_VERSION_NUMBER_NOT_AVAILABLE +#define BOOST_ENDIAN_LITTLE_WORD BOOST_VERSION_NUMBER_NOT_AVAILABLE + +/* GNU libc provides a header defining __BYTE_ORDER, or _BYTE_ORDER. + * And some OSs provide some for of endian header also. + */ +#if !BOOST_ENDIAN_BIG_BYTE && !BOOST_ENDIAN_BIG_WORD && \ + !BOOST_ENDIAN_LITTLE_BYTE && !BOOST_ENDIAN_LITTLE_WORD +# if BOOST_LIB_C_GNU || BOOST_OS_ANDROID +# include +# else +# if BOOST_OS_MACOS +# include +# else +# if BOOST_OS_BSD +# if BOOST_OS_BSD_OPEN +# include +# else +# include +# endif +# endif +# endif +# endif +# if defined(__BYTE_ORDER) +# if defined(__BIG_ENDIAN) && (__BYTE_ORDER == __BIG_ENDIAN) +# undef BOOST_ENDIAN_BIG_BYTE +# define BOOST_ENDIAN_BIG_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +# if defined(__LITTLE_ENDIAN) && (__BYTE_ORDER == __LITTLE_ENDIAN) +# undef BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_ENDIAN_LITTLE_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +# if defined(__PDP_ENDIAN) && (__BYTE_ORDER == __PDP_ENDIAN) +# undef BOOST_ENDIAN_LITTLE_WORD +# define BOOST_ENDIAN_LITTLE_WORD BOOST_VERSION_NUMBER_AVAILABLE +# endif +# endif +# if !defined(__BYTE_ORDER) && defined(_BYTE_ORDER) +# if defined(_BIG_ENDIAN) && (_BYTE_ORDER == _BIG_ENDIAN) +# undef BOOST_ENDIAN_BIG_BYTE +# define BOOST_ENDIAN_BIG_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +# if defined(_LITTLE_ENDIAN) && (_BYTE_ORDER == _LITTLE_ENDIAN) +# undef BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_ENDIAN_LITTLE_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +# if defined(_PDP_ENDIAN) && (_BYTE_ORDER == _PDP_ENDIAN) +# undef BOOST_ENDIAN_LITTLE_WORD +# define BOOST_ENDIAN_LITTLE_WORD BOOST_VERSION_NUMBER_AVAILABLE +# endif +# endif +#endif + +/* Built-in byte-swpped big-endian macros. + */ +#if !BOOST_ENDIAN_BIG_BYTE && !BOOST_ENDIAN_BIG_WORD && \ + !BOOST_ENDIAN_LITTLE_BYTE && !BOOST_ENDIAN_LITTLE_WORD +# if (defined(__BIG_ENDIAN__) && !defined(__LITTLE_ENDIAN__)) || \ + (defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)) || \ + defined(__ARMEB__) || \ + defined(__THUMBEB__) || \ + defined(__AARCH64EB__) || \ + defined(_MIPSEB) || \ + defined(__MIPSEB) || \ + defined(__MIPSEB__) +# undef BOOST_ENDIAN_BIG_BYTE +# define BOOST_ENDIAN_BIG_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +/* Built-in byte-swpped little-endian macros. + */ +#if !BOOST_ENDIAN_BIG_BYTE && !BOOST_ENDIAN_BIG_WORD && \ + !BOOST_ENDIAN_LITTLE_BYTE && !BOOST_ENDIAN_LITTLE_WORD +# if (defined(__LITTLE_ENDIAN__) && !defined(__BIG_ENDIAN__)) || \ + (defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)) || \ + defined(__ARMEL__) || \ + defined(__THUMBEL__) || \ + defined(__AARCH64EL__) || \ + defined(_MIPSEL) || \ + defined(__MIPSEL) || \ + defined(__MIPSEL__) +# undef BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_ENDIAN_LITTLE_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +/* Some architectures are strictly one endianess (as opposed + * the current common bi-endianess). + */ +#if !BOOST_ENDIAN_BIG_BYTE && !BOOST_ENDIAN_BIG_WORD && \ + !BOOST_ENDIAN_LITTLE_BYTE && !BOOST_ENDIAN_LITTLE_WORD +# include +# if BOOST_ARCH_M68K || \ + BOOST_ARCH_PARISC || \ + BOOST_ARCH_SPARC || \ + BOOST_ARCH_SYS370 || \ + BOOST_ARCH_SYS390 || \ + BOOST_ARCH_Z +# undef BOOST_ENDIAN_BIG_BYTE +# define BOOST_ENDIAN_BIG_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +# if BOOST_ARCH_AMD64 || \ + BOOST_ARCH_IA64 || \ + BOOST_ARCH_X86 || \ + BOOST_ARCH_BLACKFIN +# undef BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_ENDIAN_LITTLE_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +#endif + +/* Windows on ARM, if not otherwise detected/specified, is always + * byte-swaped little-endian. + */ +#if !BOOST_ENDIAN_BIG_BYTE && !BOOST_ENDIAN_BIG_WORD && \ + !BOOST_ENDIAN_LITTLE_BYTE && !BOOST_ENDIAN_LITTLE_WORD +# if BOOST_ARCH_ARM +# include +# if BOOST_OS_WINDOWS +# undef BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_ENDIAN_LITTLE_BYTE BOOST_VERSION_NUMBER_AVAILABLE +# endif +# endif +#endif + +#if BOOST_ENDIAN_BIG_BYTE +# define BOOST_ENDIAN_BIG_BYTE_AVAILABLE +#endif +#if BOOST_ENDIAN_BIG_WORD +# define BOOST_ENDIAN_BIG_WORD_BYTE_AVAILABLE +#endif +#if BOOST_ENDIAN_LITTLE_BYTE +# define BOOST_ENDIAN_LITTLE_BYTE_AVAILABLE +#endif +#if BOOST_ENDIAN_LITTLE_WORD +# define BOOST_ENDIAN_LITTLE_WORD_BYTE_AVAILABLE +#endif + +#define BOOST_ENDIAN_BIG_BYTE_NAME "Byte-Swapped Big-Endian" +#define BOOST_ENDIAN_BIG_WORD_NAME "Word-Swapped Big-Endian" +#define BOOST_ENDIAN_LITTLE_BYTE_NAME "Byte-Swapped Little-Endian" +#define BOOST_ENDIAN_LITTLE_WORD_NAME "Word-Swapped Little-Endian" + +#endif + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_ENDIAN_BIG_BYTE,BOOST_ENDIAN_BIG_BYTE_NAME) + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_ENDIAN_BIG_WORD,BOOST_ENDIAN_BIG_WORD_NAME) + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_ENDIAN_LITTLE_BYTE,BOOST_ENDIAN_LITTLE_BYTE_NAME) + +#include +BOOST_PREDEF_DECLARE_TEST(BOOST_ENDIAN_LITTLE_WORD,BOOST_ENDIAN_LITTLE_WORD_NAME) diff --git a/ros/vision_opencv/cv_bridge/src/boost/predef/version_number.h b/ros/vision_opencv/cv_bridge/src/boost/predef/version_number.h new file mode 100644 index 0000000..3903a36 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/boost/predef/version_number.h @@ -0,0 +1,53 @@ +/* +Copyright Rene Rivera 2005, 2008-2013 +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or copy at +http://www.boost.org/LICENSE_1_0.txt) +*/ + +#ifndef BOOST_PREDEF_VERSION_NUMBER_H +#define BOOST_PREDEF_VERSION_NUMBER_H + +/*` +[heading `BOOST_VERSION_NUMBER`] + +`` +BOOST_VERSION_NUMBER(major,minor,patch) +`` + +Defines standard version numbers, with these properties: + +* Decimal base whole numbers in the range \[0,1000000000). + The number range is designed to allow for a (2,2,5) triplet. + Which fits within a 32 bit value. +* The `major` number can be in the \[0,99\] range. +* The `minor` number can be in the \[0,99\] range. +* The `patch` number can be in the \[0,99999\] range. +* Values can be specified in any base. As the defined value + is an constant expression. +* Value can be directly used in both preprocessor and compiler + expressions for comparison to other similarly defined values. +* The implementation enforces the individual ranges for the + major, minor, and patch numbers. And values over the ranges + are truncated (modulo). + +*/ +#define BOOST_VERSION_NUMBER(major,minor,patch) \ + ( (((major)%100)*10000000) + (((minor)%100)*100000) + ((patch)%100000) ) + +#define BOOST_VERSION_NUMBER_MAX \ + BOOST_VERSION_NUMBER(99,99,99999) + +#define BOOST_VERSION_NUMBER_ZERO \ + BOOST_VERSION_NUMBER(0,0,0) + +#define BOOST_VERSION_NUMBER_MIN \ + BOOST_VERSION_NUMBER(0,0,1) + +#define BOOST_VERSION_NUMBER_AVAILABLE \ + BOOST_VERSION_NUMBER_MIN + +#define BOOST_VERSION_NUMBER_NOT_AVAILABLE \ + BOOST_VERSION_NUMBER_ZERO + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/cv_bridge.cpp b/ros/vision_opencv/cv_bridge/src/cv_bridge.cpp new file mode 100644 index 0000000..d471efb --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/cv_bridge.cpp @@ -0,0 +1,700 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011, Willow Garage, Inc. +* Copyright (c) 2015, Tal Regev. +* 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. +*********************************************************************/ + +#include "boost/endian/conversion.hpp" + +#include + +#include +#include + +#include + +#include + +#include + +#include +#include + +namespace enc = sensor_msgs::image_encodings; + +namespace cv_bridge { + +static int depthStrToInt(const std::string depth) { + if (depth == "8U") { + return 0; + } else if (depth == "8S") { + return 1; + } else if (depth == "16U") { + return 2; + } else if (depth == "16S") { + return 3; + } else if (depth == "32S") { + return 4; + } else if (depth == "32F") { + return 5; + } + return 6; +} + +int getCvType(const std::string& encoding) +{ + // Check for the most common encodings first + if (encoding == enc::BGR8) return CV_8UC3; + if (encoding == enc::MONO8) return CV_8UC1; + if (encoding == enc::RGB8) return CV_8UC3; + if (encoding == enc::MONO16) return CV_16UC1; + if (encoding == enc::BGR16) return CV_16UC3; + if (encoding == enc::RGB16) return CV_16UC3; + if (encoding == enc::BGRA8) return CV_8UC4; + if (encoding == enc::RGBA8) return CV_8UC4; + if (encoding == enc::BGRA16) return CV_16UC4; + if (encoding == enc::RGBA16) return CV_16UC4; + + // For bayer, return one-channel + if (encoding == enc::BAYER_RGGB8) return CV_8UC1; + if (encoding == enc::BAYER_BGGR8) return CV_8UC1; + if (encoding == enc::BAYER_GBRG8) return CV_8UC1; + if (encoding == enc::BAYER_GRBG8) return CV_8UC1; + if (encoding == enc::BAYER_RGGB16) return CV_16UC1; + if (encoding == enc::BAYER_BGGR16) return CV_16UC1; + if (encoding == enc::BAYER_GBRG16) return CV_16UC1; + if (encoding == enc::BAYER_GRBG16) return CV_16UC1; + + // Miscellaneous + if (encoding == enc::YUV422) return CV_8UC2; + + // Check all the generic content encodings + boost::cmatch m; + + if (boost::regex_match(encoding.c_str(), m, + boost::regex("(8U|8S|16U|16S|32S|32F|64F)C([0-9]+)"))) { + return CV_MAKETYPE(depthStrToInt(m[1].str()), atoi(m[2].str().c_str())); + } + + if (boost::regex_match(encoding.c_str(), m, + boost::regex("(8U|8S|16U|16S|32S|32F|64F)"))) { + return CV_MAKETYPE(depthStrToInt(m[1].str()), 1); + } + + throw Exception("Unrecognized image encoding [" + encoding + "]"); +} + +/// @cond DOXYGEN_IGNORE + +enum Encoding { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA, YUV422, BAYER_RGGB, BAYER_BGGR, BAYER_GBRG, BAYER_GRBG}; + +Encoding getEncoding(const std::string& encoding) +{ + if ((encoding == enc::MONO8) || (encoding == enc::MONO16)) return GRAY; + if ((encoding == enc::BGR8) || (encoding == enc::BGR16)) return BGR; + if ((encoding == enc::RGB8) || (encoding == enc::RGB16)) return RGB; + if ((encoding == enc::BGRA8) || (encoding == enc::BGRA16)) return BGRA; + if ((encoding == enc::RGBA8) || (encoding == enc::RGBA16)) return RGBA; + if (encoding == enc::YUV422) return YUV422; + + if ((encoding == enc::BAYER_RGGB8) || (encoding == enc::BAYER_RGGB16)) return BAYER_RGGB; + if ((encoding == enc::BAYER_BGGR8) || (encoding == enc::BAYER_BGGR16)) return BAYER_BGGR; + if ((encoding == enc::BAYER_GBRG8) || (encoding == enc::BAYER_GBRG16)) return BAYER_GBRG; + if ((encoding == enc::BAYER_GRBG8) || (encoding == enc::BAYER_GRBG16)) return BAYER_GRBG; + + // We don't support conversions to/from other types + return INVALID; +} + +static const int SAME_FORMAT = -1; + +/** Return a lit of OpenCV conversion codes to get from one Format to the other + * The key is a pair: and the value a succession of OpenCV code conversion + * It's not efficient code but it is only called once and the structure is small enough + */ +std::map, std::vector > getConversionCodes() { + std::map, std::vector > res; + for(int i=0; i<=5; ++i) + res[std::pair(Encoding(i),Encoding(i))].push_back(SAME_FORMAT); + + res[std::make_pair(GRAY, RGB)].push_back(cv::COLOR_GRAY2RGB); + res[std::make_pair(GRAY, BGR)].push_back(cv::COLOR_GRAY2BGR); + res[std::make_pair(GRAY, RGBA)].push_back(cv::COLOR_GRAY2RGBA); + res[std::make_pair(GRAY, BGRA)].push_back(cv::COLOR_GRAY2BGRA); + + res[std::make_pair(RGB, GRAY)].push_back(cv::COLOR_RGB2GRAY); + res[std::make_pair(RGB, BGR)].push_back(cv::COLOR_RGB2BGR); + res[std::make_pair(RGB, RGBA)].push_back(cv::COLOR_RGB2RGBA); + res[std::make_pair(RGB, BGRA)].push_back(cv::COLOR_RGB2BGRA); + + res[std::make_pair(BGR, GRAY)].push_back(cv::COLOR_BGR2GRAY); + res[std::make_pair(BGR, RGB)].push_back(cv::COLOR_BGR2RGB); + res[std::make_pair(BGR, RGBA)].push_back(cv::COLOR_BGR2RGBA); + res[std::make_pair(BGR, BGRA)].push_back(cv::COLOR_BGR2BGRA); + + res[std::make_pair(RGBA, GRAY)].push_back(cv::COLOR_RGBA2GRAY); + res[std::make_pair(RGBA, RGB)].push_back(cv::COLOR_RGBA2RGB); + res[std::make_pair(RGBA, BGR)].push_back(cv::COLOR_RGBA2BGR); + res[std::make_pair(RGBA, BGRA)].push_back(cv::COLOR_RGBA2BGRA); + + res[std::make_pair(BGRA, GRAY)].push_back(cv::COLOR_BGRA2GRAY); + res[std::make_pair(BGRA, RGB)].push_back(cv::COLOR_BGRA2RGB); + res[std::make_pair(BGRA, BGR)].push_back(cv::COLOR_BGRA2BGR); + res[std::make_pair(BGRA, RGBA)].push_back(cv::COLOR_BGRA2RGBA); + + res[std::make_pair(YUV422, GRAY)].push_back(cv::COLOR_YUV2GRAY_UYVY); + res[std::make_pair(YUV422, RGB)].push_back(cv::COLOR_YUV2RGB_UYVY); + res[std::make_pair(YUV422, BGR)].push_back(cv::COLOR_YUV2BGR_UYVY); + res[std::make_pair(YUV422, RGBA)].push_back(cv::COLOR_YUV2RGBA_UYVY); + res[std::make_pair(YUV422, BGRA)].push_back(cv::COLOR_YUV2BGRA_UYVY); + + // Deal with Bayer + res[std::make_pair(BAYER_RGGB, GRAY)].push_back(cv::COLOR_BayerBG2GRAY); + res[std::make_pair(BAYER_RGGB, RGB)].push_back(cv::COLOR_BayerBG2RGB); + res[std::make_pair(BAYER_RGGB, BGR)].push_back(cv::COLOR_BayerBG2BGR); + + res[std::make_pair(BAYER_BGGR, GRAY)].push_back(cv::COLOR_BayerRG2GRAY); + res[std::make_pair(BAYER_BGGR, RGB)].push_back(cv::COLOR_BayerRG2RGB); + res[std::make_pair(BAYER_BGGR, BGR)].push_back(cv::COLOR_BayerRG2BGR); + + res[std::make_pair(BAYER_GBRG, GRAY)].push_back(cv::COLOR_BayerGR2GRAY); + res[std::make_pair(BAYER_GBRG, RGB)].push_back(cv::COLOR_BayerGR2RGB); + res[std::make_pair(BAYER_GBRG, BGR)].push_back(cv::COLOR_BayerGR2BGR); + + res[std::make_pair(BAYER_GRBG, GRAY)].push_back(cv::COLOR_BayerGB2GRAY); + res[std::make_pair(BAYER_GRBG, RGB)].push_back(cv::COLOR_BayerGB2RGB); + res[std::make_pair(BAYER_GRBG, BGR)].push_back(cv::COLOR_BayerGB2BGR); + + return res; +} + +const std::vector getConversionCode(std::string src_encoding, std::string dst_encoding) +{ + Encoding src_encod = getEncoding(src_encoding); + Encoding dst_encod = getEncoding(dst_encoding); + bool is_src_color_format = enc::isColor(src_encoding) || enc::isMono(src_encoding) || + enc::isBayer(src_encoding) || (src_encoding == enc::YUV422); + bool is_dst_color_format = enc::isColor(dst_encoding) || enc::isMono(dst_encoding) || + enc::isBayer(dst_encoding) || (dst_encoding == enc::YUV422); + bool is_num_channels_the_same = (enc::numChannels(src_encoding) == enc::numChannels(dst_encoding)); + + // If we have no color info in the source, we can only convert to the same format which + // was resolved in the previous condition. Otherwise, fail + if (!is_src_color_format) { + if (is_dst_color_format) + throw Exception("[" + src_encoding + "] is not a color format. but [" + dst_encoding + + "] is. The conversion does not make sense"); + if (!is_num_channels_the_same) + throw Exception("[" + src_encoding + "] and [" + dst_encoding + "] do not have the same number of channel"); + return std::vector(1, SAME_FORMAT); + } + + // If we are converting from a color type to a non color type, we can only do so if we stick + // to the number of channels + if (!is_dst_color_format) { + if (!is_num_channels_the_same) + throw Exception("[" + src_encoding + "] is a color format but [" + dst_encoding + "] " + + "is not so they must have the same OpenCV type, CV_8UC3, CV16UC1 ...."); + return std::vector(1, SAME_FORMAT); + } + + // If we are converting from a color type to another type, then everything is fine + static const std::map, std::vector > CONVERSION_CODES = getConversionCodes(); + + std::pair key(src_encod, dst_encod); + std::map, std::vector >::const_iterator val = CONVERSION_CODES.find(key); + if (val == CONVERSION_CODES.end()) + throw Exception("Unsupported conversion from [" + src_encoding + + "] to [" + dst_encoding + "]"); + + // And deal with depth differences if the colors are different + std::vector res = val->second; + if ((enc::bitDepth(src_encoding) != enc::bitDepth(dst_encoding)) && (getEncoding(src_encoding) != getEncoding(dst_encoding))) + res.push_back(SAME_FORMAT); + + return res; +} + +/////////////////////////////////////// Image /////////////////////////////////////////// + +// Converts a ROS Image to a cv::Mat by sharing the data or chaning its endianness if needed +cv::Mat matFromImage(const sensor_msgs::Image& source) +{ + int source_type = getCvType(source.encoding); + int byte_depth = enc::bitDepth(source.encoding) / 8; + int num_channels = enc::numChannels(source.encoding); + + if (source.step < source.width * byte_depth * num_channels) + { + std::stringstream ss; + ss << "Image is wrongly formed: step < width * byte_depth * num_channels or " << source.step << " != " << + source.width << " * " << byte_depth << " * " << num_channels; + throw Exception(ss.str()); + } + + if (source.height * source.step != source.data.size()) + { + std::stringstream ss; + ss << "Image is wrongly formed: height * step != size or " << source.height << " * " << + source.step << " != " << source.data.size(); + throw Exception(ss.str()); + } + + // If the endianness is the same as locally, share the data + cv::Mat mat(source.height, source.width, source_type, const_cast(&source.data[0]), source.step); + if ((boost::endian::order::native == boost::endian::order::big && source.is_bigendian) || + (boost::endian::order::native == boost::endian::order::little && !source.is_bigendian) || + byte_depth == 1) + return mat; + + // Otherwise, reinterpret the data as bytes and switch the channels accordingly + mat = cv::Mat(source.height, source.width, CV_MAKETYPE(CV_8U, num_channels*byte_depth), + const_cast(&source.data[0]), source.step); + cv::Mat mat_swap(source.height, source.width, mat.type()); + + std::vector fromTo; + fromTo.reserve(num_channels*byte_depth); + for(int i = 0; i < num_channels; ++i) + for(int j = 0; j < byte_depth; ++j) + { + fromTo.push_back(byte_depth*i + j); + fromTo.push_back(byte_depth*i + byte_depth - 1 - j); + } + cv::mixChannels(std::vector(1, mat), std::vector(1, mat_swap), fromTo); + + // Interpret mat_swap back as the proper type + mat_swap = cv::Mat(source.height, source.width, source_type, mat_swap.data, mat_swap.step); + + return mat_swap; +} + +// Internal, used by toCvCopy and cvtColor +CvImagePtr toCvCopyImpl(const cv::Mat& source, + const std_msgs::Header& src_header, + const std::string& src_encoding, + const std::string& dst_encoding) +{ + // Copy metadata + CvImagePtr ptr = boost::make_shared(); + ptr->header = src_header; + + // Copy to new buffer if same encoding requested + if (dst_encoding.empty() || dst_encoding == src_encoding) + { + ptr->encoding = src_encoding; + source.copyTo(ptr->image); + } + else + { + // Convert the source data to the desired encoding + const std::vector conversion_codes = getConversionCode(src_encoding, dst_encoding); + cv::Mat image1 = source; + cv::Mat image2; + for(size_t i=0; iimage = image2; + ptr->encoding = dst_encoding; + } + + return ptr; +} + +/// @endcond + +sensor_msgs::ImagePtr CvImage::toImageMsg() const +{ + sensor_msgs::ImagePtr ptr = boost::make_shared(); + toImageMsg(*ptr); + return ptr; +} + +void CvImage::toImageMsg(sensor_msgs::Image& ros_image) const +{ + ros_image.header = header; + ros_image.height = image.rows; + ros_image.width = image.cols; + ros_image.encoding = encoding; + ros_image.is_bigendian = (boost::endian::order::native == boost::endian::order::big); + ros_image.step = image.cols * image.elemSize(); + size_t size = ros_image.step * image.rows; + ros_image.data.resize(size); + + if (image.isContinuous()) + { + memcpy((char*)(&ros_image.data[0]), image.data, size); + } + else + { + // Copy by row by row + uchar* ros_data_ptr = (uchar*)(&ros_image.data[0]); + uchar* cv_data_ptr = image.data; + for (int i = 0; i < image.rows; ++i) + { + memcpy(ros_data_ptr, cv_data_ptr, ros_image.step); + ros_data_ptr += ros_image.step; + cv_data_ptr += image.step; + } + } +} + +// Deep copy data, returnee is mutable +CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source, + const std::string& encoding) +{ + return toCvCopy(*source, encoding); +} + +CvImagePtr toCvCopy(const sensor_msgs::Image& source, + const std::string& encoding) +{ + // Construct matrix pointing to source data + return toCvCopyImpl(matFromImage(source), source.header, source.encoding, encoding); +} + +// Share const data, returnee is immutable +CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source, + const std::string& encoding) +{ + return toCvShare(*source, source, encoding); +} + +CvImageConstPtr toCvShare(const sensor_msgs::Image& source, + const boost::shared_ptr& tracked_object, + const std::string& encoding) +{ + // If the encoding different or the endianness different, you have to copy + if ((!encoding.empty() && source.encoding != encoding) || (source.is_bigendian && + (boost::endian::order::native != boost::endian::order::big))) + return toCvCopy(source, encoding); + + CvImagePtr ptr = boost::make_shared(); + ptr->header = source.header; + ptr->encoding = source.encoding; + ptr->tracked_object_ = tracked_object; + ptr->image = matFromImage(source); + return ptr; +} + +CvImagePtr cvtColor(const CvImageConstPtr& source, + const std::string& encoding) +{ + return toCvCopyImpl(source->image, source->header, source->encoding, encoding); +} + +/////////////////////////////////////// CompressedImage /////////////////////////////////////////// + +sensor_msgs::CompressedImagePtr CvImage::toCompressedImageMsg(const Format dst_format) const +{ + sensor_msgs::CompressedImagePtr ptr = boost::make_shared(); + toCompressedImageMsg(*ptr,dst_format); + return ptr; +} + +std::string getFormat(Format format) { + + switch (format) { + case DIB: + return "dib"; + case BMP: + return "bmp"; + case JPG: + return "jpg"; + case JPEG: + return "jpeg"; + case JPE: + return "jpe"; + case JP2: + return "jp2"; + case PNG: + return "png"; + case PBM: + return "pbm"; + case PGM: + return "pgm"; + case PPM: + return "ppm"; + case RAS: + return "ras"; + case SR: + return "sr"; + case TIF: + return "tif"; + case TIFF: + return "tiff"; + } + + throw Exception("Unrecognized image format"); +} + +void CvImage::toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format) const +{ + ros_image.header = header; + cv::Mat image; + if (encoding == enc::BGR8 || encoding == enc::BGRA8) + { + image = this->image; + } + else + { + CvImagePtr tempThis = boost::make_shared(*this); + CvImagePtr temp; + if (enc::hasAlpha(encoding)) + { + temp = cvtColor(tempThis, enc::BGRA8); + } + else + { + temp = cvtColor(tempThis, enc::BGR8); + } + image = temp->image; + } + + std::string format = getFormat(dst_format); + ros_image.format = format; + cv::imencode("." + format, image, ros_image.data); +} + +// Deep copy data, returnee is mutable +CvImagePtr toCvCopy(const sensor_msgs::CompressedImageConstPtr& source, + const std::string& encoding) +{ + return toCvCopy(*source, encoding); +} + +CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source, const std::string& encoding) +{ + // Construct matrix pointing to source data + const cv::Mat_ in(1, source.data.size(), const_cast(&source.data[0])); + // Loads as BGR or BGRA. + const cv::Mat rgb_a = cv::imdecode(in, cv::IMREAD_UNCHANGED); + + switch (rgb_a.channels()) + { + case 4: + return toCvCopyImpl(rgb_a, source.header, enc::BGRA8, encoding); + break; + case 3: + return toCvCopyImpl(rgb_a, source.header, enc::BGR8, encoding); + break; + case 1: + return toCvCopyImpl(rgb_a, source.header, enc::MONO8, encoding); + break; + default: + return CvImagePtr(); + } +} + +CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source, + const std::string& encoding_out, + const CvtColorForDisplayOptions options) +{ + double min_image_value = options.min_image_value; + double max_image_value = options.max_image_value; + + if (!source) + throw Exception("cv_bridge.cvtColorForDisplay() called with empty image."); + // let's figure out what to do with the empty encoding + std::string encoding = encoding_out; + if (encoding.empty()) + { + try + { + // Let's decide upon an output format + if (enc::numChannels(source->encoding) == 1) + { + if ((source->encoding == enc::TYPE_32SC1) || + (enc::bitDepth(source->encoding) == 8) || + (enc::bitDepth(source->encoding) == 16) || + (enc::bitDepth(source->encoding) == 32)) + encoding = enc::BGR8; + else + throw std::runtime_error("Unsupported depth of the source encoding " + encoding); + } + else + { + // We choose BGR by default here as we assume people will use OpenCV + if ((enc::bitDepth(source->encoding) == 8) || + (enc::bitDepth(source->encoding) == 16)) + encoding = enc::BGR8; + else + throw std::runtime_error("Unsupported depth of the source encoding " + encoding); + } + } + // We could have cv_bridge exception or std_runtime_error from sensor_msgs::image_codings routines + catch (const std::runtime_error& e) + { + throw Exception("cv_bridge.cvtColorForDisplay() output encoding is empty and cannot be guessed."); + } + } + else + { + if ((!enc::isColor(encoding_out) && !enc::isMono(encoding_out)) || + (enc::bitDepth(encoding) != 8)) + throw Exception("cv_bridge.cvtColorForDisplay() does not have an output encoding that is color or mono, and has is bit in depth"); + + } + + // Convert label to bgr image + if (encoding == sensor_msgs::image_encodings::BGR8 && + source->encoding == enc::TYPE_32SC1) + { + CvImagePtr result(new CvImage()); + result->header = source->header; + result->encoding = encoding; + result->image = cv::Mat(source->image.rows, source->image.cols, CV_8UC3); + for (size_t j = 0; j < source->image.rows; ++j) { + for (size_t i = 0; i < source->image.cols; ++i) { + int label = source->image.at(j, i); + if (label == options.bg_label) { // background label + result->image.at(j, i) = cv::Vec3b(0, 0, 0); + } + else + { + cv::Vec3d rgb = rgb_colors::getRGBColor(label); + // result image should be BGR + result->image.at(j, i) = cv::Vec3b(int(rgb[2] * 255), int(rgb[1] * 255), int(rgb[0] * 255)); + } + } + } + return result; + } + + // Perform scaling if asked for + if (options.do_dynamic_scaling) + { + cv::minMaxLoc(source->image, &min_image_value, &max_image_value); + if (min_image_value == max_image_value) + { + CvImagePtr result(new CvImage()); + result->header = source->header; + result->encoding = encoding; + if (enc::bitDepth(encoding) == 1) + { + result->image = cv::Mat(source->image.size(), CV_8UC1); + result->image.setTo(255./2.); + } else { + result->image = cv::Mat(source->image.size(), CV_8UC3); + result->image.setTo(cv::Scalar(1., 1., 1.)*255./2.); + } + return result; + } + } + + if (min_image_value != max_image_value) + { + if (enc::numChannels(source->encoding) != 1) + throw Exception("cv_bridge.cvtColorForDisplay() scaling for images with more than one channel is unsupported"); + CvImagePtr img_scaled(new CvImage()); + img_scaled->header = source->header; + if (options.colormap == -1) { + img_scaled->encoding = enc::MONO8; + cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC1, 255.0 / + (max_image_value - min_image_value)); + } else { + img_scaled->encoding = enc::BGR8; + cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC3, 255.0 / + (max_image_value - min_image_value)); + cv::applyColorMap(img_scaled->image, img_scaled->image, options.colormap); + // Fill black color to the nan region. + if (source->encoding == enc::TYPE_32FC1) { + for (size_t j = 0; j < source->image.rows; ++j) { + for (size_t i = 0; i < source->image.cols; ++i) { + float float_value = source->image.at(j, i); + if (std::isnan(float_value)) { + img_scaled->image.at(j, i) = cv::Vec3b(0, 0, 0); + } + } + } + } + } + return cvtColor(img_scaled, encoding); + } + + // If no color conversion is possible, we must "guess" the input format + CvImagePtr source_typed(new CvImage()); + source_typed->image = source->image; + source_typed->header = source->header; + source_typed->encoding = source->encoding; + + // If we get the OpenCV format, if we have 1,3 or 4 channels, we are most likely in mono, BGR or BGRA modes + if (source->encoding == "CV_8UC1") + source_typed->encoding = enc::MONO8; + else if (source->encoding == "16UC1") + source_typed->encoding = enc::MONO16; + else if (source->encoding == "CV_8UC3") + source_typed->encoding = enc::BGR8; + else if (source->encoding == "CV_8UC4") + source_typed->encoding = enc::BGRA8; + else if (source->encoding == "CV_16UC3") + source_typed->encoding = enc::BGR8; + else if (source->encoding == "CV_16UC4") + source_typed->encoding = enc::BGRA8; + + // If no conversion is needed, don't convert + if (source_typed->encoding == encoding) + return source; + + try + { + // Now that the output is a proper color format, try to see if any conversion is possible + return cvtColor(source_typed, encoding); + } + catch (cv_bridge::Exception& e) + { + throw Exception("cv_bridge.cvtColorForDisplay() while trying to convert image from '" + source->encoding + "' to '" + encoding + "' an exception was thrown (" + e.what() + ")"); + } +} + +} //namespace cv_bridge diff --git a/ros/vision_opencv/cv_bridge/src/module.cpp b/ros/vision_opencv/cv_bridge/src/module.cpp new file mode 100644 index 0000000..c123198 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/module.cpp @@ -0,0 +1,110 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2012, 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 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. +*********************************************************************/ + +#include "module.hpp" + +PyObject *mod_opencv; + +bp::object +cvtColor2Wrap(bp::object obj_in, const std::string & encoding_in, const std::string & encoding_out) { + // Convert the Python input to an image + cv::Mat mat_in; + convert_to_CvMat2(obj_in.ptr(), mat_in); + + // Call cv_bridge for color conversion + cv_bridge::CvImagePtr cv_image(new cv_bridge::CvImage(std_msgs::Header(), encoding_in, mat_in)); + + cv::Mat mat = cv_bridge::cvtColor(cv_image, encoding_out)->image; + + return bp::object(boost::python::handle<>(pyopencv_from(mat))); +} + +bp::object +cvtColorForDisplayWrap(bp::object obj_in, + const std::string & encoding_in, + const std::string & encoding_out, + bool do_dynamic_scaling = false, + double min_image_value = 0.0, + double max_image_value = 0.0) { + // Convert the Python input to an image + cv::Mat mat_in; + convert_to_CvMat2(obj_in.ptr(), mat_in); + + cv_bridge::CvImagePtr cv_image(new cv_bridge::CvImage(std_msgs::Header(), encoding_in, mat_in)); + + cv_bridge::CvtColorForDisplayOptions options; + options.do_dynamic_scaling = do_dynamic_scaling; + options.min_image_value = min_image_value; + options.max_image_value = max_image_value; + cv::Mat mat = cv_bridge::cvtColorForDisplay(/*source=*/cv_image, + /*encoding_out=*/encoding_out, + /*options=*/options)->image; + + return bp::object(boost::python::handle<>(pyopencv_from(mat))); +} + +BOOST_PYTHON_FUNCTION_OVERLOADS(cvtColorForDisplayWrap_overloads, cvtColorForDisplayWrap, 3, 6) + +int CV_MAT_CNWrap(int i) { + return CV_MAT_CN(i); +} + +int CV_MAT_DEPTHWrap(int i) { + return CV_MAT_DEPTH(i); +} + +BOOST_PYTHON_MODULE(cv_bridge_boost) +{ + do_numpy_import(); + mod_opencv = PyImport_ImportModule("cv2"); + + // Wrap the function to get encodings as OpenCV types + boost::python::def("getCvType", cv_bridge::getCvType); + boost::python::def("cvtColor2", cvtColor2Wrap); + boost::python::def("CV_MAT_CNWrap", CV_MAT_CNWrap); + boost::python::def("CV_MAT_DEPTHWrap", CV_MAT_DEPTHWrap); + boost::python::def("cvtColorForDisplay", cvtColorForDisplayWrap, + cvtColorForDisplayWrap_overloads( + boost::python::args("source", "encoding_in", "encoding_out", "do_dynamic_scaling", + "min_image_value", "max_image_value"), + "Convert image to display with specified encodings.\n\n" + "Args:\n" + " - source (numpy.ndarray): input image\n" + " - encoding_in (str): input image encoding\n" + " - encoding_out (str): encoding to which the image conveted\n" + " - do_dynamic_scaling (bool): flag to do dynamic scaling with min/max value\n" + " - min_image_value (float): minimum pixel value for dynamic scaling\n" + " - max_image_value (float): maximum pixel value for dynamic scaling\n" + )); +} diff --git a/ros/vision_opencv/cv_bridge/src/module.hpp b/ros/vision_opencv/cv_bridge/src/module.hpp new file mode 100644 index 0000000..92153ac --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/module.hpp @@ -0,0 +1,48 @@ +/* + * Copyright 2014 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. + */ + +#ifndef CV_BRIDGE_MODULE_HPP_ +#define CV_BRIDGE_MODULE_HPP_ + +#include +#include +#include +#include + +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#include + +#include + +namespace bp = boost::python; + +int convert_to_CvMat2(const PyObject* o, cv::Mat& m); + +PyObject* pyopencv_from(const cv::Mat& m); + +#if PYTHON3 +static int do_numpy_import( ) +{ + import_array( ); +} +#else +static void do_numpy_import( ) +{ + import_array( ); +} +#endif + +#endif diff --git a/ros/vision_opencv/cv_bridge/src/module_opencv2.cpp b/ros/vision_opencv/cv_bridge/src/module_opencv2.cpp new file mode 100644 index 0000000..9f0752b --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/module_opencv2.cpp @@ -0,0 +1,262 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2012, 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 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. +*********************************************************************/ + +#include "module.hpp" + +using namespace cv; + +// These are sucky, sketchy versions of the real things in OpenCV Python, +// inferior in every way. + +static int failmsg(const char *fmt, ...) +{ + char str[1000]; + + va_list ap; + va_start(ap, fmt); + vsnprintf(str, sizeof(str), fmt, ap); + va_end(ap); + + PyErr_SetString(PyExc_TypeError, str); + return 0; +} + +static PyObject* opencv_error = 0; + +class PyAllowThreads +{ +public: + PyAllowThreads() : _state(PyEval_SaveThread()) {} + ~PyAllowThreads() + { + PyEval_RestoreThread(_state); + } +private: + PyThreadState* _state; +}; + +#define ERRWRAP2(expr) \ +try \ +{ \ + PyAllowThreads allowThreads; \ + expr; \ +} \ +catch (const cv::Exception &e) \ +{ \ + PyErr_SetString(opencv_error, e.what()); \ + return 0; \ +} + +// Taken from http://stackoverflow.com/questions/19136944/call-c-opencv-functions-from-python-send-a-cv-mat-to-c-dll-which-is-usi + + +static size_t REFCOUNT_OFFSET = ( size_t )&((( PyObject* )0)->ob_refcnt ) + +( 0x12345678 != *( const size_t* )"\x78\x56\x34\x12\0\0\0\0\0" )*sizeof( int ); + + +static inline PyObject* pyObjectFromRefcount( const int* refcount ) +{ +return ( PyObject* )(( size_t )refcount - REFCOUNT_OFFSET ); +} + +static inline int* refcountFromPyObject( const PyObject* obj ) +{ +return ( int* )(( size_t )obj + REFCOUNT_OFFSET ); +} + +class NumpyAllocator : public cv::MatAllocator +{ +public: +NumpyAllocator( ) { } +~NumpyAllocator( ) { } + +void allocate( int dims, const int* sizes, int type, int*& refcount, +uchar*& datastart, uchar*& data, size_t* step ); + +void deallocate( int* refcount, uchar* datastart, uchar* data ); +}; + +void NumpyAllocator::allocate( int dims, const int* sizes, int type, int*& refcount, uchar*& datastart, uchar*& data, size_t* step ) +{ + int depth = CV_MAT_DEPTH( type ); + int cn = CV_MAT_CN( type ); + const int f = ( int )( sizeof( size_t )/8 ); + int typenum = depth == CV_8U ? NPY_UBYTE : depth == CV_8S ? NPY_BYTE : + depth == CV_16U ? NPY_USHORT : depth == CV_16S ? NPY_SHORT : + depth == CV_32S ? NPY_INT : depth == CV_32F ? NPY_FLOAT : + depth == CV_64F ? NPY_DOUBLE : f*NPY_ULONGLONG + (f^1)*NPY_UINT; + int i; + npy_intp _sizes[CV_MAX_DIM+1]; + for( i = 0; i < dims; i++ ) + _sizes[i] = sizes[i]; + if( cn > 1 ) + { + /*if( _sizes[dims-1] == 1 ) + _sizes[dims-1] = cn; + else*/ + _sizes[dims++] = cn; + } + PyObject* o = PyArray_SimpleNew( dims, _sizes, typenum ); + if( !o ) + CV_Error_(CV_StsError, ("The numpy array of typenum=%d, ndims=%d can not be created", typenum, dims)); + refcount = refcountFromPyObject(o); + npy_intp* _strides = PyArray_STRIDES((PyArrayObject*) o); + for( i = 0; i < dims - (cn > 1); i++ ) + step[i] = (size_t)_strides[i]; + datastart = data = (uchar*)PyArray_DATA((PyArrayObject*)o); + +} + +void NumpyAllocator::deallocate( int* refcount, uchar* datastart, uchar* data ) +{ + if( !refcount ) + return; + PyObject* o = pyObjectFromRefcount(refcount); + Py_INCREF(o); + Py_DECREF(o); +} + +// Declare the object +NumpyAllocator g_numpyAllocator; + +int convert_to_CvMat2(const PyObject* o, cv::Mat& m) +{ + // to avoid PyArray_Check() to crash even with valid array + do_numpy_import(); + + if(!o || o == Py_None) + { + if( !m.data ) + m.allocator = &g_numpyAllocator; + return true; + } + + if( !PyArray_Check(o) ) + { + failmsg("Not a numpy array"); + return false; + } + + // NPY_LONG (64 bit) is converted to CV_32S (32 bit) + int typenum = PyArray_TYPE((PyArrayObject*) o); + int type = typenum == NPY_UBYTE ? CV_8U : typenum == NPY_BYTE ? CV_8S : + typenum == NPY_USHORT ? CV_16U : typenum == NPY_SHORT ? CV_16S : + typenum == NPY_INT || typenum == NPY_LONG ? CV_32S : + typenum == NPY_FLOAT ? CV_32F : + typenum == NPY_DOUBLE ? CV_64F : -1; + + if( type < 0 ) + { + failmsg("data type = %d is not supported", typenum); + return false; + } + + int ndims = PyArray_NDIM((PyArrayObject*) o); + if(ndims >= CV_MAX_DIM) + { + failmsg("dimensionality (=%d) is too high", ndims); + return false; + } + + int size[CV_MAX_DIM+1]; + size_t step[CV_MAX_DIM+1], elemsize = CV_ELEM_SIZE1(type); + const npy_intp* _sizes = PyArray_DIMS((PyArrayObject*) o); + const npy_intp* _strides = PyArray_STRIDES((PyArrayObject*) o); + bool transposed = false; + + for(int i = 0; i < ndims; i++) + { + size[i] = (int)_sizes[i]; + step[i] = (size_t)_strides[i]; + } + + if( ndims == 0 || step[ndims-1] > elemsize ) { + size[ndims] = 1; + step[ndims] = elemsize; + ndims++; + } + + if( ndims >= 2 && step[0] < step[1] ) + { + std::swap(size[0], size[1]); + std::swap(step[0], step[1]); + transposed = true; + } + + if( ndims == 3 && size[2] <= CV_CN_MAX && step[1] == elemsize*size[2] ) + { + ndims--; + type |= CV_MAKETYPE(0, size[2]); + } + + if( ndims > 2 ) + { + failmsg("more than 2 dimensions"); + return false; + } + + m = cv::Mat(ndims, size, type, PyArray_DATA((PyArrayObject*) o), step); + + if( m.data ) + { + m.refcount = refcountFromPyObject(o); + m.addref(); // protect the original numpy array from deallocation + // (since Mat destructor will decrement the reference counter) + }; + m.allocator = &g_numpyAllocator; + + if( transposed ) + { + cv::Mat tmp; + tmp.allocator = &g_numpyAllocator; + transpose(m, tmp); + m = tmp; + } + return true; +} + +PyObject* pyopencv_from(const Mat& m) +{ + if( !m.data ) + Py_RETURN_NONE; + Mat temp, *p = (Mat*)&m; + if(!p->refcount || p->allocator != &g_numpyAllocator) + { + temp.allocator = &g_numpyAllocator; + ERRWRAP2(m.copyTo(temp)); + p = &temp; + } + p->addref(); + return pyObjectFromRefcount(p->refcount); +} diff --git a/ros/vision_opencv/cv_bridge/src/module_opencv3.cpp b/ros/vision_opencv/cv_bridge/src/module_opencv3.cpp new file mode 100644 index 0000000..68c1b20 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/module_opencv3.cpp @@ -0,0 +1,367 @@ +// Taken from opencv/modules/python/src2/cv2.cpp + +#include "module.hpp" + +#include "opencv2/core/types_c.h" + +#include "opencv2/opencv_modules.hpp" + +#include "pycompat.hpp" + +static PyObject* opencv_error = 0; + +static int failmsg(const char *fmt, ...) +{ + char str[1000]; + + va_list ap; + va_start(ap, fmt); + vsnprintf(str, sizeof(str), fmt, ap); + va_end(ap); + + PyErr_SetString(PyExc_TypeError, str); + return 0; +} + +struct ArgInfo +{ + const char * name; + bool outputarg; + // more fields may be added if necessary + + ArgInfo(const char * name_, bool outputarg_) + : name(name_) + , outputarg(outputarg_) {} + + // to match with older pyopencv_to function signature + operator const char *() const { return name; } +}; + +class PyAllowThreads +{ +public: + PyAllowThreads() : _state(PyEval_SaveThread()) {} + ~PyAllowThreads() + { + PyEval_RestoreThread(_state); + } +private: + PyThreadState* _state; +}; + +class PyEnsureGIL +{ +public: + PyEnsureGIL() : _state(PyGILState_Ensure()) {} + ~PyEnsureGIL() + { + PyGILState_Release(_state); + } +private: + PyGILState_STATE _state; +}; + +#define ERRWRAP2(expr) \ +try \ +{ \ + PyAllowThreads allowThreads; \ + expr; \ +} \ +catch (const cv::Exception &e) \ +{ \ + PyErr_SetString(opencv_error, e.what()); \ + return 0; \ +} + +using namespace cv; + +static PyObject* failmsgp(const char *fmt, ...) +{ + char str[1000]; + + va_list ap; + va_start(ap, fmt); + vsnprintf(str, sizeof(str), fmt, ap); + va_end(ap); + + PyErr_SetString(PyExc_TypeError, str); + return 0; +} + +class NumpyAllocator : public MatAllocator +{ +public: + NumpyAllocator() { stdAllocator = Mat::getStdAllocator(); } + ~NumpyAllocator() {} + + UMatData* allocate(PyObject* o, int dims, const int* sizes, int type, size_t* step) const + { + UMatData* u = new UMatData(this); + u->data = u->origdata = (uchar*)PyArray_DATA((PyArrayObject*) o); + npy_intp* _strides = PyArray_STRIDES((PyArrayObject*) o); + for( int i = 0; i < dims - 1; i++ ) + step[i] = (size_t)_strides[i]; + step[dims-1] = CV_ELEM_SIZE(type); + u->size = sizes[0]*step[0]; + u->userdata = o; + return u; + } + + UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, int flags, UMatUsageFlags usageFlags) const + { + if( data != 0 ) + { + CV_Error(Error::StsAssert, "The data should normally be NULL!"); + // probably this is safe to do in such extreme case + return stdAllocator->allocate(dims0, sizes, type, data, step, flags, usageFlags); + } + PyEnsureGIL gil; + + int depth = CV_MAT_DEPTH(type); + int cn = CV_MAT_CN(type); + const int f = (int)(sizeof(size_t)/8); + int typenum = depth == CV_8U ? NPY_UBYTE : depth == CV_8S ? NPY_BYTE : + depth == CV_16U ? NPY_USHORT : depth == CV_16S ? NPY_SHORT : + depth == CV_32S ? NPY_INT : depth == CV_32F ? NPY_FLOAT : + depth == CV_64F ? NPY_DOUBLE : f*NPY_ULONGLONG + (f^1)*NPY_UINT; + int i, dims = dims0; + cv::AutoBuffer _sizes(dims + 1); + for( i = 0; i < dims; i++ ) + _sizes[i] = sizes[i]; + if( cn > 1 ) + _sizes[dims++] = cn; + PyObject* o = PyArray_SimpleNew(dims, _sizes, typenum); + if(!o) + CV_Error_(Error::StsError, ("The numpy array of typenum=%d, ndims=%d can not be created", typenum, dims)); + return allocate(o, dims0, sizes, type, step); + } + + bool allocate(UMatData* u, int accessFlags, UMatUsageFlags usageFlags) const + { + return stdAllocator->allocate(u, accessFlags, usageFlags); + } + + void deallocate(UMatData* u) const + { + if(u) + { + PyEnsureGIL gil; + PyObject* o = (PyObject*)u->userdata; + Py_XDECREF(o); + delete u; + } + } + + const MatAllocator* stdAllocator; +}; + +NumpyAllocator g_numpyAllocator; + + +template static +bool pyopencv_to(PyObject* obj, T& p, const char* name = ""); + +template static +PyObject* pyopencv_from(const T& src); + +enum { ARG_NONE = 0, ARG_MAT = 1, ARG_SCALAR = 2 }; + +// special case, when the convertor needs full ArgInfo structure +static bool pyopencv_to(PyObject* o, Mat& m, const ArgInfo info) +{ + // to avoid PyArray_Check() to crash even with valid array + do_numpy_import( ); + + + bool allowND = true; + if(!o || o == Py_None) + { + if( !m.data ) + m.allocator = &g_numpyAllocator; + return true; + } + + if( PyInt_Check(o) ) + { + double v[] = {(double)PyInt_AsLong((PyObject*)o), 0., 0., 0.}; + m = Mat(4, 1, CV_64F, v).clone(); + return true; + } + if( PyFloat_Check(o) ) + { + double v[] = {PyFloat_AsDouble((PyObject*)o), 0., 0., 0.}; + m = Mat(4, 1, CV_64F, v).clone(); + return true; + } + if( PyTuple_Check(o) ) + { + int i, sz = (int)PyTuple_Size((PyObject*)o); + m = Mat(sz, 1, CV_64F); + for( i = 0; i < sz; i++ ) + { + PyObject* oi = PyTuple_GET_ITEM(o, i); + if( PyInt_Check(oi) ) + m.at(i) = (double)PyInt_AsLong(oi); + else if( PyFloat_Check(oi) ) + m.at(i) = (double)PyFloat_AsDouble(oi); + else + { + failmsg("%s is not a numerical tuple", info.name); + m.release(); + return false; + } + } + return true; + } + + if( !PyArray_Check(o) ) + { + failmsg("%s is not a numpy array, neither a scalar", info.name); + return false; + } + + PyArrayObject* oarr = (PyArrayObject*) o; + + bool needcopy = false, needcast = false; + int typenum = PyArray_TYPE(oarr), new_typenum = typenum; + int type = typenum == NPY_UBYTE ? CV_8U : + typenum == NPY_BYTE ? CV_8S : + typenum == NPY_USHORT ? CV_16U : + typenum == NPY_SHORT ? CV_16S : + typenum == NPY_INT ? CV_32S : + typenum == NPY_INT32 ? CV_32S : + typenum == NPY_FLOAT ? CV_32F : + typenum == NPY_DOUBLE ? CV_64F : -1; + + if( type < 0 ) + { + if( typenum == NPY_INT64 || typenum == NPY_UINT64 || type == NPY_LONG ) + { + needcopy = needcast = true; + new_typenum = NPY_INT; + type = CV_32S; + } + else + { + failmsg("%s data type = %d is not supported", info.name, typenum); + return false; + } + } + +#ifndef CV_MAX_DIM + const int CV_MAX_DIM = 32; +#endif + + int ndims = PyArray_NDIM(oarr); + if(ndims >= CV_MAX_DIM) + { + failmsg("%s dimensionality (=%d) is too high", info.name, ndims); + return false; + } + + int size[CV_MAX_DIM+1]; + size_t step[CV_MAX_DIM+1]; + size_t elemsize = CV_ELEM_SIZE1(type); + const npy_intp* _sizes = PyArray_DIMS(oarr); + const npy_intp* _strides = PyArray_STRIDES(oarr); + bool ismultichannel = ndims == 3 && _sizes[2] <= CV_CN_MAX; + + for( int i = ndims-1; i >= 0 && !needcopy; i-- ) + { + // these checks handle cases of + // a) multi-dimensional (ndims > 2) arrays, as well as simpler 1- and 2-dimensional cases + // b) transposed arrays, where _strides[] elements go in non-descending order + // c) flipped arrays, where some of _strides[] elements are negative + if( (i == ndims-1 && (size_t)_strides[i] != elemsize) || + (i < ndims-1 && _strides[i] < _strides[i+1]) ) + needcopy = true; + } + + if( ismultichannel && _strides[1] != (npy_intp)elemsize*_sizes[2] ) + needcopy = true; + + if (needcopy) + { + if (info.outputarg) + { + failmsg("Layout of the output array %s is incompatible with cv::Mat (step[ndims-1] != elemsize or step[1] != elemsize*nchannels)", info.name); + return false; + } + + if( needcast ) { + o = PyArray_Cast(oarr, new_typenum); + oarr = (PyArrayObject*) o; + } + else { + oarr = PyArray_GETCONTIGUOUS(oarr); + o = (PyObject*) oarr; + } + + _strides = PyArray_STRIDES(oarr); + } + + for(int i = 0; i < ndims; i++) + { + size[i] = (int)_sizes[i]; + step[i] = (size_t)_strides[i]; + } + + // handle degenerate case + if( ndims == 0) { + size[ndims] = 1; + step[ndims] = elemsize; + ndims++; + } + + if( ismultichannel ) + { + ndims--; + type |= CV_MAKETYPE(0, size[2]); + } + + if( ndims > 2 && !allowND ) + { + failmsg("%s has more than 2 dimensions", info.name); + return false; + } + + m = Mat(ndims, size, type, PyArray_DATA(oarr), step); + m.u = g_numpyAllocator.allocate(o, ndims, size, type, step); + m.addref(); + + if( !needcopy ) + { + Py_INCREF(o); + } + m.allocator = &g_numpyAllocator; + + return true; +} + +template<> +bool pyopencv_to(PyObject* o, Mat& m, const char* name) +{ + return pyopencv_to(o, m, ArgInfo(name, 0)); +} + +PyObject* pyopencv_from(const Mat& m) +{ + if( !m.data ) + Py_RETURN_NONE; + Mat temp, *p = (Mat*)&m; + if(!p->u || p->allocator != &g_numpyAllocator) + { + temp.allocator = &g_numpyAllocator; + ERRWRAP2(m.copyTo(temp)); + p = &temp; + } + PyObject* o = (PyObject*)p->u->userdata; + Py_INCREF(o); + return o; +} + +int convert_to_CvMat2(const PyObject* o, cv::Mat& m) +{ + pyopencv_to(const_cast(o), m, "unknown"); + return 0; +} diff --git a/ros/vision_opencv/cv_bridge/src/pycompat.hpp b/ros/vision_opencv/cv_bridge/src/pycompat.hpp new file mode 100644 index 0000000..f4ebea6 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/pycompat.hpp @@ -0,0 +1,70 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's 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. +// +// * The name of the copyright holders may not 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 Intel Corporation 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. +// +//M*/ + +// Defines for Python 2/3 compatibility. +#ifndef __PYCOMPAT_HPP__ +#define __PYCOMPAT_HPP__ + +#if PY_MAJOR_VERSION >= 3 +// Python3 treats all ints as longs, PyInt_X functions have been removed. +#define PyInt_Check PyLong_Check +#define PyInt_CheckExact PyLong_CheckExact +#define PyInt_AsLong PyLong_AsLong +#define PyInt_AS_LONG PyLong_AS_LONG +#define PyInt_FromLong PyLong_FromLong +#define PyNumber_Int PyNumber_Long + +// Python3 strings are unicode, these defines mimic the Python2 functionality. +#define PyString_Check PyUnicode_Check +#define PyString_FromString PyUnicode_FromString +#define PyString_FromStringAndSize PyUnicode_FromStringAndSize +#define PyString_Size PyUnicode_GET_SIZE + +// PyUnicode_AsUTF8 isn't available until Python 3.3 +#if (PY_VERSION_HEX < 0x03030000) +#define PyString_AsString _PyUnicode_AsString +#else +#define PyString_AsString PyUnicode_AsUTF8 +#endif +#endif + +#endif // END HEADER GUARD diff --git a/ros/vision_opencv/cv_bridge/src/rgb_colors.cpp b/ros/vision_opencv/cv_bridge/src/rgb_colors.cpp new file mode 100644 index 0000000..ca2eaaa --- /dev/null +++ b/ros/vision_opencv/cv_bridge/src/rgb_colors.cpp @@ -0,0 +1,202 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Original color definition is at scikit-image distributed with + * following license disclaimer: + * + * Copyright (C) 2011, the scikit-image team + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name of skimage 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 AUTHOR ``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 AUTHOR 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. + *********************************************************************/ + +#include "cv_bridge/rgb_colors.h" +#include + + +namespace cv_bridge +{ + +namespace rgb_colors +{ + + cv::Vec3d getRGBColor(const int color) + { + cv::Vec3d c; + switch (color % 146) { + case ALICEBLUE: c = cv::Vec3d(0.941, 0.973, 1); break; + case ANTIQUEWHITE: c = cv::Vec3d(0.98, 0.922, 0.843); break; + case AQUA: c = cv::Vec3d(0, 1, 1); break; + case AQUAMARINE: c = cv::Vec3d(0.498, 1, 0.831); break; + case AZURE: c = cv::Vec3d(0.941, 1, 1); break; + case BEIGE: c = cv::Vec3d(0.961, 0.961, 0.863); break; + case BISQUE: c = cv::Vec3d(1, 0.894, 0.769); break; + case BLACK: c = cv::Vec3d(0, 0, 0); break; + case BLANCHEDALMOND: c = cv::Vec3d(1, 0.922, 0.804); break; + case BLUE: c = cv::Vec3d(0, 0, 1); break; + case BLUEVIOLET: c = cv::Vec3d(0.541, 0.169, 0.886); break; + case BROWN: c = cv::Vec3d(0.647, 0.165, 0.165); break; + case BURLYWOOD: c = cv::Vec3d(0.871, 0.722, 0.529); break; + case CADETBLUE: c = cv::Vec3d(0.373, 0.62, 0.627); break; + case CHARTREUSE: c = cv::Vec3d(0.498, 1, 0); break; + case CHOCOLATE: c = cv::Vec3d(0.824, 0.412, 0.118); break; + case CORAL: c = cv::Vec3d(1, 0.498, 0.314); break; + case CORNFLOWERBLUE: c = cv::Vec3d(0.392, 0.584, 0.929); break; + case CORNSILK: c = cv::Vec3d(1, 0.973, 0.863); break; + case CRIMSON: c = cv::Vec3d(0.863, 0.0784, 0.235); break; + case CYAN: c = cv::Vec3d(0, 1, 1); break; + case DARKBLUE: c = cv::Vec3d(0, 0, 0.545); break; + case DARKCYAN: c = cv::Vec3d(0, 0.545, 0.545); break; + case DARKGOLDENROD: c = cv::Vec3d(0.722, 0.525, 0.0431); break; + case DARKGRAY: c = cv::Vec3d(0.663, 0.663, 0.663); break; + case DARKGREEN: c = cv::Vec3d(0, 0.392, 0); break; + case DARKGREY: c = cv::Vec3d(0.663, 0.663, 0.663); break; + case DARKKHAKI: c = cv::Vec3d(0.741, 0.718, 0.42); break; + case DARKMAGENTA: c = cv::Vec3d(0.545, 0, 0.545); break; + case DARKOLIVEGREEN: c = cv::Vec3d(0.333, 0.42, 0.184); break; + case DARKORANGE: c = cv::Vec3d(1, 0.549, 0); break; + case DARKORCHID: c = cv::Vec3d(0.6, 0.196, 0.8); break; + case DARKRED: c = cv::Vec3d(0.545, 0, 0); break; + case DARKSALMON: c = cv::Vec3d(0.914, 0.588, 0.478); break; + case DARKSEAGREEN: c = cv::Vec3d(0.561, 0.737, 0.561); break; + case DARKSLATEBLUE: c = cv::Vec3d(0.282, 0.239, 0.545); break; + case DARKSLATEGRAY: c = cv::Vec3d(0.184, 0.31, 0.31); break; + case DARKSLATEGREY: c = cv::Vec3d(0.184, 0.31, 0.31); break; + case DARKTURQUOISE: c = cv::Vec3d(0, 0.808, 0.82); break; + case DARKVIOLET: c = cv::Vec3d(0.58, 0, 0.827); break; + case DEEPPINK: c = cv::Vec3d(1, 0.0784, 0.576); break; + case DEEPSKYBLUE: c = cv::Vec3d(0, 0.749, 1); break; + case DIMGRAY: c = cv::Vec3d(0.412, 0.412, 0.412); break; + case DIMGREY: c = cv::Vec3d(0.412, 0.412, 0.412); break; + case DODGERBLUE: c = cv::Vec3d(0.118, 0.565, 1); break; + case FIREBRICK: c = cv::Vec3d(0.698, 0.133, 0.133); break; + case FLORALWHITE: c = cv::Vec3d(1, 0.98, 0.941); break; + case FORESTGREEN: c = cv::Vec3d(0.133, 0.545, 0.133); break; + case FUCHSIA: c = cv::Vec3d(1, 0, 1); break; + case GAINSBORO: c = cv::Vec3d(0.863, 0.863, 0.863); break; + case GHOSTWHITE: c = cv::Vec3d(0.973, 0.973, 1); break; + case GOLD: c = cv::Vec3d(1, 0.843, 0); break; + case GOLDENROD: c = cv::Vec3d(0.855, 0.647, 0.125); break; + case GRAY: c = cv::Vec3d(0.502, 0.502, 0.502); break; + case GREEN: c = cv::Vec3d(0, 0.502, 0); break; + case GREENYELLOW: c = cv::Vec3d(0.678, 1, 0.184); break; + case GREY: c = cv::Vec3d(0.502, 0.502, 0.502); break; + case HONEYDEW: c = cv::Vec3d(0.941, 1, 0.941); break; + case HOTPINK: c = cv::Vec3d(1, 0.412, 0.706); break; + case INDIANRED: c = cv::Vec3d(0.804, 0.361, 0.361); break; + case INDIGO: c = cv::Vec3d(0.294, 0, 0.51); break; + case IVORY: c = cv::Vec3d(1, 1, 0.941); break; + case KHAKI: c = cv::Vec3d(0.941, 0.902, 0.549); break; + case LAVENDER: c = cv::Vec3d(0.902, 0.902, 0.98); break; + case LAVENDERBLUSH: c = cv::Vec3d(1, 0.941, 0.961); break; + case LAWNGREEN: c = cv::Vec3d(0.486, 0.988, 0); break; + case LEMONCHIFFON: c = cv::Vec3d(1, 0.98, 0.804); break; + case LIGHTBLUE: c = cv::Vec3d(0.678, 0.847, 0.902); break; + case LIGHTCORAL: c = cv::Vec3d(0.941, 0.502, 0.502); break; + case LIGHTCYAN: c = cv::Vec3d(0.878, 1, 1); break; + case LIGHTGOLDENRODYELLOW: c = cv::Vec3d(0.98, 0.98, 0.824); break; + case LIGHTGRAY: c = cv::Vec3d(0.827, 0.827, 0.827); break; + case LIGHTGREEN: c = cv::Vec3d(0.565, 0.933, 0.565); break; + case LIGHTGREY: c = cv::Vec3d(0.827, 0.827, 0.827); break; + case LIGHTPINK: c = cv::Vec3d(1, 0.714, 0.757); break; + case LIGHTSALMON: c = cv::Vec3d(1, 0.627, 0.478); break; + case LIGHTSEAGREEN: c = cv::Vec3d(0.125, 0.698, 0.667); break; + case LIGHTSKYBLUE: c = cv::Vec3d(0.529, 0.808, 0.98); break; + case LIGHTSLATEGRAY: c = cv::Vec3d(0.467, 0.533, 0.6); break; + case LIGHTSLATEGREY: c = cv::Vec3d(0.467, 0.533, 0.6); break; + case LIGHTSTEELBLUE: c = cv::Vec3d(0.69, 0.769, 0.871); break; + case LIGHTYELLOW: c = cv::Vec3d(1, 1, 0.878); break; + case LIME: c = cv::Vec3d(0, 1, 0); break; + case LIMEGREEN: c = cv::Vec3d(0.196, 0.804, 0.196); break; + case LINEN: c = cv::Vec3d(0.98, 0.941, 0.902); break; + case MAGENTA: c = cv::Vec3d(1, 0, 1); break; + case MAROON: c = cv::Vec3d(0.502, 0, 0); break; + case MEDIUMAQUAMARINE: c = cv::Vec3d(0.4, 0.804, 0.667); break; + case MEDIUMBLUE: c = cv::Vec3d(0, 0, 0.804); break; + case MEDIUMORCHID: c = cv::Vec3d(0.729, 0.333, 0.827); break; + case MEDIUMPURPLE: c = cv::Vec3d(0.576, 0.439, 0.859); break; + case MEDIUMSEAGREEN: c = cv::Vec3d(0.235, 0.702, 0.443); break; + case MEDIUMSLATEBLUE: c = cv::Vec3d(0.482, 0.408, 0.933); break; + case MEDIUMSPRINGGREEN: c = cv::Vec3d(0, 0.98, 0.604); break; + case MEDIUMTURQUOISE: c = cv::Vec3d(0.282, 0.82, 0.8); break; + case MEDIUMVIOLETRED: c = cv::Vec3d(0.78, 0.0824, 0.522); break; + case MIDNIGHTBLUE: c = cv::Vec3d(0.098, 0.098, 0.439); break; + case MINTCREAM: c = cv::Vec3d(0.961, 1, 0.98); break; + case MISTYROSE: c = cv::Vec3d(1, 0.894, 0.882); break; + case MOCCASIN: c = cv::Vec3d(1, 0.894, 0.71); break; + case NAVAJOWHITE: c = cv::Vec3d(1, 0.871, 0.678); break; + case NAVY: c = cv::Vec3d(0, 0, 0.502); break; + case OLDLACE: c = cv::Vec3d(0.992, 0.961, 0.902); break; + case OLIVE: c = cv::Vec3d(0.502, 0.502, 0); break; + case OLIVEDRAB: c = cv::Vec3d(0.42, 0.557, 0.137); break; + case ORANGE: c = cv::Vec3d(1, 0.647, 0); break; + case ORANGERED: c = cv::Vec3d(1, 0.271, 0); break; + case ORCHID: c = cv::Vec3d(0.855, 0.439, 0.839); break; + case PALEGOLDENROD: c = cv::Vec3d(0.933, 0.91, 0.667); break; + case PALEGREEN: c = cv::Vec3d(0.596, 0.984, 0.596); break; + case PALEVIOLETRED: c = cv::Vec3d(0.686, 0.933, 0.933); break; + case PAPAYAWHIP: c = cv::Vec3d(1, 0.937, 0.835); break; + case PEACHPUFF: c = cv::Vec3d(1, 0.855, 0.725); break; + case PERU: c = cv::Vec3d(0.804, 0.522, 0.247); break; + case PINK: c = cv::Vec3d(1, 0.753, 0.796); break; + case PLUM: c = cv::Vec3d(0.867, 0.627, 0.867); break; + case POWDERBLUE: c = cv::Vec3d(0.69, 0.878, 0.902); break; + case PURPLE: c = cv::Vec3d(0.502, 0, 0.502); break; + case RED: c = cv::Vec3d(1, 0, 0); break; + case ROSYBROWN: c = cv::Vec3d(0.737, 0.561, 0.561); break; + case ROYALBLUE: c = cv::Vec3d(0.255, 0.412, 0.882); break; + case SADDLEBROWN: c = cv::Vec3d(0.545, 0.271, 0.0745); break; + case SALMON: c = cv::Vec3d(0.98, 0.502, 0.447); break; + case SANDYBROWN: c = cv::Vec3d(0.98, 0.643, 0.376); break; + case SEAGREEN: c = cv::Vec3d(0.18, 0.545, 0.341); break; + case SEASHELL: c = cv::Vec3d(1, 0.961, 0.933); break; + case SIENNA: c = cv::Vec3d(0.627, 0.322, 0.176); break; + case SILVER: c = cv::Vec3d(0.753, 0.753, 0.753); break; + case SKYBLUE: c = cv::Vec3d(0.529, 0.808, 0.922); break; + case SLATEBLUE: c = cv::Vec3d(0.416, 0.353, 0.804); break; + case SLATEGRAY: c = cv::Vec3d(0.439, 0.502, 0.565); break; + case SLATEGREY: c = cv::Vec3d(0.439, 0.502, 0.565); break; + case SNOW: c = cv::Vec3d(1, 0.98, 0.98); break; + case SPRINGGREEN: c = cv::Vec3d(0, 1, 0.498); break; + case STEELBLUE: c = cv::Vec3d(0.275, 0.51, 0.706); break; + case TAN: c = cv::Vec3d(0.824, 0.706, 0.549); break; + case TEAL: c = cv::Vec3d(0, 0.502, 0.502); break; + case THISTLE: c = cv::Vec3d(0.847, 0.749, 0.847); break; + case TOMATO: c = cv::Vec3d(1, 0.388, 0.278); break; + case TURQUOISE: c = cv::Vec3d(0.251, 0.878, 0.816); break; + case VIOLET: c = cv::Vec3d(0.933, 0.51, 0.933); break; + case WHEAT: c = cv::Vec3d(0.961, 0.871, 0.702); break; + case WHITE: c = cv::Vec3d(1, 1, 1); break; + case WHITESMOKE: c = cv::Vec3d(0.961, 0.961, 0.961); break; + case YELLOW: c = cv::Vec3d(1, 1, 0); break; + case YELLOWGREEN: c = cv::Vec3d(0.604, 0.804, 0.196); break; + } // switch + return c; + } + +} // namespace rgb_colors + +} // namespace cv_bridge diff --git a/ros/vision_opencv/cv_bridge/test/CMakeLists.txt b/ros/vision_opencv/cv_bridge/test/CMakeLists.txt new file mode 100644 index 0000000..7497d45 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/CMakeLists.txt @@ -0,0 +1,15 @@ +# add the tests + +# add boost directories for now +include_directories("../src") + +catkin_add_gtest(${PROJECT_NAME}-utest test_endian.cpp test_compression.cpp utest.cpp utest2.cpp test_rgb_colors.cpp) +target_link_libraries(${PROJECT_NAME}-utest + ${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ${catkin_LIBRARIES} +) + +catkin_add_nosetests(enumerants.py) +catkin_add_nosetests(conversions.py) +catkin_add_nosetests(python_bindings.py) diff --git a/ros/vision_opencv/cv_bridge/test/conversions.py b/ros/vision_opencv/cv_bridge/test/conversions.py new file mode 100644 index 0000000..ab20cd7 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/conversions.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python +import rostest +import unittest + +import numpy as np + +import sensor_msgs.msg + +from cv_bridge import CvBridge, CvBridgeError + +class TestConversions(unittest.TestCase): + + def test_mono16_cv2(self): + import numpy as np + br = CvBridge() + im = np.uint8(np.random.randint(0, 255, size=(480, 640, 3))) + self.assertRaises(CvBridgeError, lambda: br.imgmsg_to_cv2(br.cv2_to_imgmsg(im), "mono16")) + br.imgmsg_to_cv2(br.cv2_to_imgmsg(im,"rgb8"), "mono16") + + def test_encode_decode_cv2(self): + import cv2 + import numpy as np + fmts = [cv2.CV_8U, cv2.CV_8S, cv2.CV_16U, cv2.CV_16S, cv2.CV_32S, cv2.CV_32F, cv2.CV_64F] + + cvb_en = CvBridge() + cvb_de = CvBridge() + + for w in range(100, 800, 100): + for h in range(100, 800, 100): + for f in fmts: + for channels in ([], 1, 2, 3, 4, 5): + if channels == []: + original = np.uint8(np.random.randint(0, 255, size=(h, w))) + else: + original = np.uint8(np.random.randint(0, 255, size=(h, w, channels))) + rosmsg = cvb_en.cv2_to_imgmsg(original) + newimg = cvb_de.imgmsg_to_cv2(rosmsg) + + self.assert_(original.dtype == newimg.dtype) + if channels == 1: + # in that case, a gray image has a shape of size 2 + self.assert_(original.shape[:2] == newimg.shape[:2]) + else: + self.assert_(original.shape == newimg.shape) + self.assert_(len(original.tostring()) == len(newimg.tostring())) + + def test_encode_decode_cv2_compressed(self): + import numpy as np + # from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) + formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm", + "jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support + + cvb_en = CvBridge() + cvb_de = CvBridge() + + for w in range(100, 800, 100): + for h in range(100, 800, 100): + for f in formats: + for channels in ([], 1, 3): + if channels == []: + original = np.uint8(np.random.randint(0, 255, size=(h, w))) + else: + original = np.uint8(np.random.randint(0, 255, size=(h, w, channels))) + compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f) + newimg = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg) + self.assert_(original.dtype == newimg.dtype) + if channels == 1: + # in that case, a gray image has a shape of size 2 + self.assert_(original.shape[:2] == newimg.shape[:2]) + else: + self.assert_(original.shape == newimg.shape) + self.assert_(len(original.tostring()) == len(newimg.tostring())) + + def test_endianness(self): + br = CvBridge() + dtype = np.dtype('int32') + # Set to big endian. + dtype = dtype.newbyteorder('>') + img = np.random.randint(0, 255, size=(30, 40)) + msg = br.cv2_to_imgmsg(img.astype(dtype)) + self.assert_(msg.is_bigendian == True) + self.assert_((br.imgmsg_to_cv2(msg) == img).all()) + +if __name__ == '__main__': + rosunit.unitrun('opencv_tests', 'conversions', TestConversions) diff --git a/ros/vision_opencv/cv_bridge/test/enumerants.py b/ros/vision_opencv/cv_bridge/test/enumerants.py new file mode 100644 index 0000000..bdcc7a8 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/enumerants.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +import rostest +import unittest + +import numpy as np +import cv2 + +import sensor_msgs.msg + +from cv_bridge import CvBridge, CvBridgeError, getCvType + +class TestEnumerants(unittest.TestCase): + + def test_enumerants_cv2(self): + img_msg = sensor_msgs.msg.Image() + img_msg.width = 640 + img_msg.height = 480 + img_msg.encoding = "rgba8" + img_msg.step = 640*4 + img_msg.data = (640 * 480) * "1234" + + bridge_ = CvBridge() + cvim = bridge_.imgmsg_to_cv2(img_msg, "rgb8") + import sys + self.assertRaises(sys.getrefcount(cvim) == 2) + + # A 3 channel image cannot be sent as an rgba8 + self.assertRaises(CvBridgeError, lambda: bridge_.cv2_to_imgmsg(cvim, "rgba8")) + + # but it can be sent as rgb8 and bgr8 + bridge_.cv2_to_imgmsg(cvim, "rgb8") + bridge_.cv2_to_imgmsg(cvim, "bgr8") + + self.assertRaises(getCvType("32FC4") == cv2.CV_8UC4) + self.assertRaises(getCvType("8UC1") == cv2.CV_8UC1) + self.assertRaises(getCvType("8U") == cv2.CV_8UC1) + + def test_numpy_types(self): + import cv2 + import numpy as np + bridge_ = CvBridge() + self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(1, "rgba8")) + if hasattr(cv2, 'cv'): + self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(cv2.cv(), "rgba8")) + +if __name__ == '__main__': + rosunit.unitrun('opencv_tests', 'enumerants', TestEnumerants) diff --git a/ros/vision_opencv/cv_bridge/test/python_bindings.py b/ros/vision_opencv/cv_bridge/test/python_bindings.py new file mode 100644 index 0000000..6dd3209 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/python_bindings.py @@ -0,0 +1,35 @@ +from nose.tools import assert_equal +import numpy as np + +import cv_bridge + + +def test_cvtColorForDisplay(): + # convert label image to display + label = np.zeros((480, 640), dtype=np.int32) + height, width = label.shape[:2] + label_value = 0 + grid_num_y, grid_num_x = 3, 4 + for grid_row in xrange(grid_num_y): + grid_size_y = height / grid_num_y + min_y = grid_size_y * grid_row + max_y = min_y + grid_size_y + for grid_col in xrange(grid_num_x): + grid_size_x = width / grid_num_x + min_x = grid_size_x * grid_col + max_x = min_x + grid_size_x + label[min_y:max_y, min_x:max_x] = label_value + label_value += 1 + label_viz = cv_bridge.cvtColorForDisplay(label, '32SC1', 'bgr8') + assert_equal(label_viz.dtype, np.uint8) + assert_equal(label_viz.min(), 0) + assert_equal(label_viz.max(), 255) + + # Check that mono8 conversion returns the right shape. + bridge = cv_bridge.CvBridge() + mono = np.random.random((100, 100)) * 255 + mono = mono.astype(np.uint8) + + input_msg = bridge.cv2_to_imgmsg(mono, encoding='mono8') + output = bridge.imgmsg_to_cv2(input_msg, desired_encoding='mono8') + assert_equal(output.shape, (100,100)) diff --git a/ros/vision_opencv/cv_bridge/test/test_compression.cpp b/ros/vision_opencv/cv_bridge/test/test_compression.cpp new file mode 100644 index 0000000..5be403e --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/test_compression.cpp @@ -0,0 +1,36 @@ +#include +#include +#include + +TEST(CvBridgeTest, compression) +{ + cv::RNG rng(0); + std_msgs::Header header; + + // Test 3 channel images. + for (int i = 0; i < 2; ++i) + { + const std::string format = (i == 0) ? "bgr8" : "rgb8"; + cv::Mat_ in(10, 10); + rng.fill(in, cv::RNG::UNIFORM, 0, 256); + + sensor_msgs::CompressedImagePtr msg = cv_bridge::CvImage(header, format, in).toCompressedImageMsg(cv_bridge::PNG); + const cv_bridge::CvImageConstPtr out = cv_bridge::toCvCopy(msg, format); + + EXPECT_EQ(out->image.channels(), 3); + EXPECT_EQ(cv::norm(out->image, in), 0); + } + + // Test 4 channel images. + for (int i = 0; i < 2; ++i) + { + const std::string format = (i == 0) ? "bgra8" : "rgba8"; + cv::Mat_ in(10, 10); + rng.fill(in, cv::RNG::UNIFORM, 0, 256); + + sensor_msgs::CompressedImagePtr msg = cv_bridge::CvImage(header, format, in).toCompressedImageMsg(cv_bridge::PNG); + const cv_bridge::CvImageConstPtr out = cv_bridge::toCvCopy(msg, format); + EXPECT_EQ(out->image.channels(), 4); + EXPECT_EQ(cv::norm(out->image, in), 0); + } +} diff --git a/ros/vision_opencv/cv_bridge/test/test_endian.cpp b/ros/vision_opencv/cv_bridge/test/test_endian.cpp new file mode 100644 index 0000000..58a1f50 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/test_endian.cpp @@ -0,0 +1,38 @@ +#include "boost/endian/conversion.hpp" +#include +#include +#include + +TEST(CvBridgeTest, endianness) +{ + using namespace boost::endian; + + // Create an image of the type opposite to the platform + sensor_msgs::Image msg; + msg.height = 1; + msg.width = 1; + msg.encoding = "32SC2"; + msg.step = 8; + + msg.data.resize(msg.step); + int32_t* data = reinterpret_cast(&msg.data[0]); + + // Write 1 and 2 in order, but with an endianness opposite to the platform + if (order::native == order::little) + { + msg.is_bigendian = true; + *(data++) = native_to_big(static_cast(1)); + *data = native_to_big(static_cast(2)); + } else { + msg.is_bigendian = false; + *(data++) = native_to_little(static_cast(1)); + *data = native_to_little(static_cast(2)); + } + + // Make sure the values are still the same + cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(boost::make_shared(msg)); + EXPECT_EQ(img->image.at(0, 0)[0], 1); + EXPECT_EQ(img->image.at(0, 0)[1], 2); + // Make sure we cannot share data + EXPECT_NE(img->image.data, &msg.data[0]); +} diff --git a/ros/vision_opencv/cv_bridge/test/test_rgb_colors.cpp b/ros/vision_opencv/cv_bridge/test/test_rgb_colors.cpp new file mode 100644 index 0000000..090e667 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/test_rgb_colors.cpp @@ -0,0 +1,19 @@ +#include "cv_bridge/rgb_colors.h" +#include +#include + + +TEST(RGBColors, testGetRGBColor) +{ + cv::Vec3d color; + // red + color = cv_bridge::rgb_colors::getRGBColor(cv_bridge::rgb_colors::RED); + EXPECT_EQ(1, color[0]); + EXPECT_EQ(0, color[1]); + EXPECT_EQ(0, color[2]); + // gray + color = cv_bridge::rgb_colors::getRGBColor(cv_bridge::rgb_colors::GRAY); + EXPECT_EQ(0.502, color[0]); + EXPECT_EQ(0.502, color[1]); + EXPECT_EQ(0.502, color[2]); +} diff --git a/ros/vision_opencv/cv_bridge/test/utest.cpp b/ros/vision_opencv/cv_bridge/test/utest.cpp new file mode 100644 index 0000000..1d8255c --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/utest.cpp @@ -0,0 +1,112 @@ +#include "cv_bridge/cv_bridge.h" +#include +#include + + +// Tests conversion of non-continuous cv::Mat. #5206 +TEST(CvBridgeTest, NonContinuous) +{ + cv::Mat full = cv::Mat::eye(8, 8, CV_16U); + cv::Mat partial = full.colRange(2, 5); + + cv_bridge::CvImage cvi; + cvi.encoding = sensor_msgs::image_encodings::MONO16; + cvi.image = partial; + + sensor_msgs::ImagePtr msg = cvi.toImageMsg(); + EXPECT_EQ(msg->height, 8); + EXPECT_EQ(msg->width, 3); + EXPECT_EQ(msg->encoding, cvi.encoding); + EXPECT_EQ(msg->step, 6); +} + +TEST(CvBridgeTest, ChannelOrder) +{ + cv::Mat_ mat(200, 200); + mat.setTo(cv::Scalar(1000,0,0,0)); + sensor_msgs::ImagePtr image(new sensor_msgs::Image()); + + image = cv_bridge::CvImage(image->header, sensor_msgs::image_encodings::MONO16, mat).toImageMsg(); + + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image); + + cv_bridge::CvImagePtr res = cv_bridge::cvtColor(cv_ptr, sensor_msgs::image_encodings::BGR8); + EXPECT_EQ(res->encoding, sensor_msgs::image_encodings::BGR8); + EXPECT_EQ(res->image.type(), cv_bridge::getCvType(res->encoding)); + EXPECT_EQ(res->image.channels(), sensor_msgs::image_encodings::numChannels(res->encoding)); + EXPECT_EQ(res->image.depth(), CV_8U); + + // The matrix should be the following + cv::Mat_ gt(200, 200); + gt.setTo(cv::Scalar(1, 1, 1)*1000.*255./65535.); + + ASSERT_EQ(res->image.type(), gt.type()); + EXPECT_EQ(cv::norm(res->image, gt, cv::NORM_INF), 0); +} + +TEST(CvBridgeTest, initialization) +{ + sensor_msgs::Image image; + cv_bridge::CvImagePtr cv_ptr; + + image.encoding = "bgr8"; + image.height = 200; + image.width = 200; + + try { + cv_ptr = cv_bridge::toCvCopy(image, "mono8"); + // Before the fix, it would never get here, as it would segfault + EXPECT_EQ(1, 0); + } catch (cv_bridge::Exception& e) { + EXPECT_EQ(1, 1); + } + + // Check some normal images with different ratios + for(int height = 100; height <= 300; ++height) { + image.encoding = sensor_msgs::image_encodings::RGB8; + image.step = image.width*3; + image.data.resize(image.height*image.step); + cv_ptr = cv_bridge::toCvCopy(image, "mono8"); + } +} + +TEST(CvBridgeTest, imageMessageStep) +{ + // Test 1: image step is padded + sensor_msgs::Image image; + cv_bridge::CvImagePtr cv_ptr; + + image.encoding = "mono8"; + image.height = 220; + image.width = 200; + image.is_bigendian = false; + image.step = 208; + + image.data.resize(image.height*image.step); + + ASSERT_NO_THROW(cv_ptr = cv_bridge::toCvCopy(image, "mono8")); + ASSERT_EQ(220, cv_ptr->image.rows); + ASSERT_EQ(200, cv_ptr->image.cols); + //OpenCV copyTo argument removes the stride + ASSERT_EQ(200, cv_ptr->image.step[0]); + + //Test 2: image step is invalid + image.step = 199; + + ASSERT_THROW(cv_ptr = cv_bridge::toCvCopy(image, "mono8"), cv_bridge::Exception); + + //Test 3: image step == image.width * element size * number of channels + image.step = 200; + image.data.resize(image.height*image.step); + + ASSERT_NO_THROW(cv_ptr = cv_bridge::toCvCopy(image, "mono8")); + ASSERT_EQ(220, cv_ptr->image.rows); + ASSERT_EQ(200, cv_ptr->image.cols); + ASSERT_EQ(200, cv_ptr->image.step[0]); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros/vision_opencv/cv_bridge/test/utest2.cpp b/ros/vision_opencv/cv_bridge/test/utest2.cpp new file mode 100644 index 0000000..d41c976 --- /dev/null +++ b/ros/vision_opencv/cv_bridge/test/utest2.cpp @@ -0,0 +1,150 @@ +/********************************************************************* +* 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 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. +*********************************************************************/ + +#include +#include +#include + +#include "opencv2/core/core.hpp" + +#include "cv_bridge/cv_bridge.h" +#include +#include + +using namespace sensor_msgs::image_encodings; + +bool isUnsigned(const std::string & encoding) { + return encoding == RGB8 || encoding == RGBA8 || encoding == RGB16 || encoding == RGBA16 || encoding == BGR8 || encoding == BGRA8 || encoding == BGR16 || encoding == BGRA16 || encoding == MONO8 || encoding == MONO16 || + encoding == MONO8 || encoding == MONO16 || encoding == TYPE_8UC1 || encoding == TYPE_8UC2 || encoding == TYPE_8UC3 || encoding == TYPE_8UC4 || + encoding == TYPE_16UC1 || encoding == TYPE_16UC2 || encoding == TYPE_16UC3 || encoding == TYPE_16UC4; + //BAYER_RGGB8, BAYER_BGGR8, BAYER_GBRG8, BAYER_GRBG8, BAYER_RGGB16, BAYER_BGGR16, BAYER_GBRG16, BAYER_GRBG16, + //YUV422 +} +std::vector +getEncodings() { +// TODO for Groovy, the following types should be uncommented +std::string encodings[] = { RGB8, RGBA8, RGB16, RGBA16, BGR8, BGRA8, BGR16, BGRA16, MONO8, MONO16, + TYPE_8UC1, /*TYPE_8UC2,*/ TYPE_8UC3, TYPE_8UC4, + TYPE_8SC1, /*TYPE_8SC2,*/ TYPE_8SC3, TYPE_8SC4, + TYPE_16UC1, /*TYPE_16UC2,*/ TYPE_16UC3, TYPE_16UC4, + TYPE_16SC1, /*TYPE_16SC2,*/ TYPE_16SC3, TYPE_16SC4, + TYPE_32SC1, /*TYPE_32SC2,*/ TYPE_32SC3, TYPE_32SC4, + TYPE_32FC1, /*TYPE_32FC2,*/ TYPE_32FC3, TYPE_32FC4, + TYPE_64FC1, /*TYPE_64FC2,*/ TYPE_64FC3, TYPE_64FC4, + //BAYER_RGGB8, BAYER_BGGR8, BAYER_GBRG8, BAYER_GRBG8, BAYER_RGGB16, BAYER_BGGR16, BAYER_GBRG16, BAYER_GRBG16, + YUV422 + }; +return std::vector(encodings, encodings+47-8-7); +} + +TEST(OpencvTests, testCase_encode_decode) +{ + std::vector encodings = getEncodings(); + for(size_t i=0; iimage, cv_bridge::Exception); + continue; + } + // We do not support conversion to YUV422 for now, except from YUV422 + if ((dst_encoding == YUV422) && (src_encoding != YUV422)) { + EXPECT_THROW(cv_bridge::toCvShare(image_msg, dst_encoding), cv_bridge::Exception); + continue; + } + + cv_image = cv_bridge::toCvShare(image_msg, dst_encoding); + + // We do not support conversion to YUV422 for now, except from YUV422 + if ((src_encoding == YUV422) && (dst_encoding != YUV422)) { + EXPECT_THROW(cvtColor(cv_image, src_encoding)->image, cv_bridge::Exception); + continue; + } + } + // And convert back to a cv::Mat + image_back = cvtColor(cv_image, src_encoding)->image; + + // If the number of channels,s different some information got lost at some point, so no possible test + if (!is_num_channels_the_same) + continue; + if (bitDepth(src_encoding) >= 32) { + // In the case where the input has floats, we will lose precision but no more than 1 + EXPECT_LT(cv::norm(image_original, image_back, cv::NORM_INF), 1) << "problem converting from " << src_encoding << " to " << dst_encoding << " and back."; + } else if ((bitDepth(src_encoding) == 16) && (bitDepth(dst_encoding) == 8)) { + // In the case where the input has floats, we will lose precision but no more than 1 * max(127) + EXPECT_LT(cv::norm(image_original, image_back, cv::NORM_INF), 128) << "problem converting from " << src_encoding << " to " << dst_encoding << " and back."; + } else { + EXPECT_EQ(cv::norm(image_original, image_back, cv::NORM_INF), 0) << "problem converting from " << src_encoding << " to " << dst_encoding << " and back."; + } + } + } +} diff --git a/ros/vision_opencv/image_geometry/CHANGELOG.rst b/ros/vision_opencv/image_geometry/CHANGELOG.rst new file mode 100644 index 0000000..809822a --- /dev/null +++ b/ros/vision_opencv/image_geometry/CHANGELOG.rst @@ -0,0 +1,320 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package image_geometry +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.15 (2017-01-29) +-------------------- +* Import using __future_\_ for python 3 compatibility. +* Contributors: Hans Gaiser + +1.11.14 (2016-09-24) +-------------------- +* Fix "stdlib.h: No such file or directory" errors in GCC-6 + Including '-isystem /usr/include' breaks building with GCC-6. + See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129 +* remap with nan border if mat value is float or double +* remap with nan border if mat value is float or double +* Contributors: Hodorgasm, YuOhara + +1.11.13 (2016-07-11) +-------------------- +* Add fullResolution getter to PinholeCameraModel +* add a missing dependency when building the doc +* fix sphinx doc path +* Contributors: Jacob Panikulam, Vincent Rabaud + +1.11.12 (2016-03-10) +-------------------- +* issue `#117 `_ pull request `#118 `_ check all distortion coefficients to see if rectification ought to be done +* Contributors: Lucas Walter + +1.11.11 (2016-01-31) +-------------------- +* clean up the doc files +* fix a few warnings in doc jobs +* Contributors: Vincent Rabaud + +1.11.10 (2016-01-16) +-------------------- + +1.11.9 (2015-11-29) +------------------- +* add a condition if D=None +* fix compilation warnings +* Contributors: Vincent Rabaud, YuOhara + +1.11.8 (2015-07-15) +------------------- +* fixes `#62 `_, bug in Python rectifyPoint old opencv1 API +* Simplify some OpenCV3 distinction +* Contributors: Basheer Subei, Vincent Rabaud + +1.11.7 (2014-12-14) +------------------- +* Merge pull request `#53 `_ from carnegieroboticsllc/patch-1 + Update stereo_camera_model.cpp +* Updated inline math for reprojecting a single disparity +* Update stereo_camera_model.cpp + Correct slight error in the Q matrix derivation +* Updated Q matrix to account for cameras with different Fx and Fy values +* Contributors: Carnegie Robotics LLC, Matt Alvarado, Vincent Rabaud + +1.11.6 (2014-11-16) +------------------- +* Fixes in image_geometry for Python cv2 API +* Fixed typo: np -> numpy +* Added missing tfFrame method to Python PinholeCameraModel. +* Removed trailing whitespace. +* Contributors: Daniel Maturana + +1.11.5 (2014-09-21) +------------------- +* get code to work with OpenCV3 + actually fixes `#46 `_ properly +* Contributors: Vincent Rabaud + +1.11.4 (2014-07-27) +------------------- + +1.11.3 (2014-06-08) +------------------- +* pinhole_camera_model: fix implicit shared_ptr cast to bool for C++11 + In C++11 boost::shared_ptr does not provide the implicit bool conversion + operator anymore, so make the cast in pinhole_camera_model.h explicit. + That does not hurt in older C++ standards and makes compilation with C++11 + possible. +* Contributors: Max Schwarz + +1.11.2 (2014-04-28) +------------------- + +1.11.1 (2014-04-16) +------------------- + +1.11.0 (2014-02-15) +------------------- +* remove OpenCV 1 API +* fixes `#6 `_ +* fix OpenCV dependencies +* Contributors: Vincent Rabaud + +1.10.15 (2014-02-07) +-------------------- +* add assignment operator for StereoCameraModel +* fixed Python rectifyImage implementation in PinholeCameraModel +* Added operator= for the PinholeCameraModel. + Added the operator= for the PinholeCameraModel. I am not sure if the + PinholeCameraModel needs to have a destructor, too. To follow the + 'rule of three' it should actually have one. +* Contributors: Tobias Bär, Valsamis Ntouskos, Vincent Rabaud + +1.10.14 (2013-11-23 16:17) +-------------------------- +* Contributors: Vincent Rabaud + +1.10.13 (2013-11-23 09:19) +-------------------------- +* Contributors: Vincent Rabaud + +1.10.12 (2013-11-22) +-------------------- +* "1.10.12" +* Contributors: Vincent Rabaud + +1.10.11 (2013-10-23) +-------------------- +* Contributors: Vincent Rabaud + +1.10.10 (2013-10-19) +-------------------- +* Contributors: Vincent Rabaud + +1.10.9 (2013-10-07) +------------------- +* fixes `#23 `_ +* Contributors: Vincent Rabaud + +1.10.8 (2013-09-09) +------------------- +* check for CATKIN_ENABLE_TESTING +* update email address +* Contributors: Lukas Bulwahn, Vincent Rabaud + +1.10.7 (2013-07-17) +------------------- + +1.10.6 (2013-03-01) +------------------- + +1.10.5 (2013-02-11) +------------------- +* Add dependency on generated messages + Catkin requires explicit enumeration of dependencies on generated messages. + Add this declaration to properly flatten the dependency graph and force Catkin + to generate geometry_msgs before compiling image_geometry. +* Contributors: Adam Hachey + +1.10.4 (2013-02-02) +------------------- + +1.10.3 (2013-01-17) +------------------- + +1.10.2 (2013-01-13) +------------------- +* fix ticket 4253 +* Contributors: Vincent Rabaud + +1.10.1 (2013-01-10) +------------------- + +1.10.0 (2013-01-03) +------------------- + +1.9.15 (2013-01-02) +------------------- + +1.9.14 (2012-12-30) +------------------- +* add feature for https://code.ros.org/trac/ros-pkg/ticket/5592 +* CMake cleanups +* fix a failing test +* Contributors: Vincent Rabaud + +1.9.13 (2012-12-15) +------------------- +* use the catkin macros for the setup.py +* Contributors: Vincent Rabaud + +1.9.12 (2012-12-14) +------------------- +* buildtool_depend catkin fix +* Contributors: William Woodall + +1.9.11 (2012-12-10) +------------------- +* Fixing image_geometry package.xml +* fix https://code.ros.org/trac/ros-pkg/ticket/5570 +* Contributors: Vincent Rabaud, William Woodall + +1.9.10 (2012-10-04) +------------------- + +1.9.9 (2012-10-01) +------------------ +* fix dependencies +* Contributors: Vincent Rabaud + +1.9.8 (2012-09-30) +------------------ +* fix some dependencies +* fix missing Python at install and fix some dependencies +* Contributors: Vincent Rabaud + +1.9.7 (2012-09-28 21:07) +------------------------ +* add missing stuff +* make sure we find catkin +* Contributors: Vincent Rabaud + +1.9.6 (2012-09-28 15:17) +------------------------ +* make all the tests pass +* comply to the new Catkin API +* Contributors: Vincent Rabaud + +1.9.5 (2012-09-15) +------------------ +* remove dependencies to the opencv2 ROS package +* Contributors: Vincent Rabaud + +1.9.4 (2012-09-13) +------------------ +* make sure the include folders are copied to the right place +* Contributors: Vincent Rabaud + +1.9.3 (2012-09-12) +------------------ + +1.9.2 (2012-09-07) +------------------ +* be more compliant to the latest catkin +* added catkin_project() to cv_bridge, image_geometry, and opencv_tests +* Contributors: Jonathan Binney, Vincent Rabaud + +1.9.1 (2012-08-28 22:06) +------------------------ +* remove things that were marked as ROS_DEPRECATED +* Contributors: Vincent Rabaud + +1.9.0 (2012-08-28 14:29) +------------------------ +* catkinized opencv_tests by Jon Binney +* fix ticket 5449 +* use OpenCV's projectPoints +* remove the version check, let's trust OpenCV :) +* revert the removal of opencv2 +* vision_opencv: Export OpenCV flags in manifests for image_geometry, cv_bridge. +* finally get rid of opencv2 as it is a system dependency now +* bump REQUIRED version of OpenCV to 2.3.2, which is what's in ros-fuerte-opencv +* switch rosdep name to opencv2, to refer to ros-fuerte-opencv2 +* Adding a few missing headers so that client code may compile against pinhole camera model. +* Adding opencv2 to all manifests, so that client packages may + not break when using them. +* baking in opencv debs and attempting a pre-release +* image_geometry: (Python) Adjust K and P for ROI/binning. Also expose full resolution K and P. Add raw_roi property. +* image_geometry: Add Tx, Ty getters (Python). +* image_geometry: Added tf_frame and stamp properties. Only generate undistort maps when rectifyImage is called. +* image_geometry: Fix for when D is empty (Python). +* image_geometry: Take all D coefficients, not just the first 4 (Python). +* image_geometry: Fix rectification in the presence of binning (`#4848 `_). +* image_geometry: Fixed wg-ros-pkg `#5019 `_, error updating StereoCameraModel. Removed manifest dependency on cv_bridge. +* image_geometry: fromCameraInfo() returns bool, true if parameters have changed since last call. +* image_geometry: Accessors for full-res K, P. +* image_geometry: Implemented toFullResolution(), toReducedResolution(). +* image_geometry: Implemented reducedResolution(). +* image_geometry: Implemented rectifiedRoi() with caching. Fixed bug that would cause rectification maps to be regenerated every time. +* image_geometry: Implemented rectifyRoi(). +* image_geometry: Overloads of projection functions that return the output directly instead of through a reference parameter. Implemented unrectifyRoi(). Added fullResolution(), rawRoi(). +* image_geometry: Library-specific exception class. +* image_geometry: PIMPL pattern for cached data, so I can change in patch releases if necessary. Changed projectPixelTo3dRay() to normalize to z=1. +* image_geometry (rep0104): Added binning. Partially fixed ROI (not finding rectified ROI yet). Now interpreting distortion_model. Lots of code cleanup. +* image_geometry (rep0104): Got tests passing again, were issues with D becoming variable-length. +* image_geometry: Fixed swapped width/height in computing ROI undistort maps. Partially fixes `#4206 `_. +* image_geometry: getDelta functions, getZ and getDisparity in C++ and Python. Docs and tests for them. Changed Python fx() and friends to pull values out of P instead of K. +* image_geometry: Added C++ getDeltaU and getDeltaV. +* `#4201 `_, implement/doc/test for getDeltaU getDeltaX getDeltaV getDeltaY +* Added Ubuntu platform tags to manifest +* `#4083 `_, projectPixelTo3dRay implemented +* image_geometry: Added PinholeCameraModel::stamp() returning the time stamp. +* image_geometry: Fixed bugs related to ignoring Tx & Ty in projectPixelTo3dRay and unrectifyPoint. Added Tx() and Ty() accessors. +* image_geometry: Fixed `#4063 `_, PinholeCameraModel ignores Tx term in P matrix. +* image_geometry: Implemented projectDisparityTo3d, `#4019 `_. +* image_geometry: Implemented unrectifyPoint, with unit tests. +* image_geometry: Fixed bug in rectifyPoint due to cv::undistortPoints not accepting double pt data, `#4053 `_. +* image_geometry: Tweaked manifest. +* image_geometry: Better manifest description. +* Removed tfFrame sample +* image_geometry: Doxygen main page, manifest updates. +* image_geometry: Doxygen for StereoCameraModel. +* image_geometry: Made Q calculation match old stereoproc one. +* image_geometry: Tweaked projectDisparityImageTo3D API for handling missing values. +* image_geometry: Added method to project disparity image to 3d. Added ConstPtr version of fromCameraInfo in StereoCameraModel. +* image_geometry: Export linker flags. Fixed bug that could cause rectification maps to not be initialized before use. +* Fixed path-handling on gtest for CMake 2.6 +* image_geometry: Added missing source file. +* image_geometry: Added some C++ docs. +* image_geometry: Minor cleanup of StereoCameraModel, added it to build. Put in copy constructors. +* image_geometry: Switched pinhole_camera_model to use new C++ OpenCV types and functions. +* Remove use of deprecated rosbuild macros +* image_geometry (C++): Unit test for projecting points uv <-> xyz. +* image_geometry (C++): Implemented more projection functions, added beginnings of the unit tests. +* trigger rebuild +* Enable rosdoc.yaml +* docs +* image_geometry: Started C++ API. PinholeCameraModel is in theory (untested) able to track state efficiently and rectify images. +* First stereo test +* Checkpoint +* Skeleton of test +* First cut +* Contributors: Vincent Rabaud, ethanrublee, gerkey, jamesb, mihelich, vrabaud, wheeler diff --git a/ros/vision_opencv/image_geometry/CMakeLists.txt b/ros/vision_opencv/image_geometry/CMakeLists.txt new file mode 100644 index 0000000..9169c04 --- /dev/null +++ b/ros/vision_opencv/image_geometry/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 2.8) +project(image_geometry) + +find_package(catkin REQUIRED sensor_msgs) +find_package(OpenCV REQUIRED) + +catkin_package(CATKIN_DEPENDS sensor_msgs + DEPENDS OpenCV + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +) + +catkin_python_setup() + +include_directories(include) +include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) + +# add a library +add_library(${PROJECT_NAME} src/pinhole_camera_model.cpp src/stereo_camera_model.cpp) +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}/ +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +# add tests +if(CATKIN_ENABLE_TESTING) + add_subdirectory(test) +endif() diff --git a/ros/vision_opencv/image_geometry/doc/conf.py b/ros/vision_opencv/image_geometry/doc/conf.py new file mode 100644 index 0000000..ff05555 --- /dev/null +++ b/ros/vision_opencv/image_geometry/doc/conf.py @@ -0,0 +1,201 @@ +# -*- coding: utf-8 -*- +# +# image_geometry documentation build configuration file, created by +# sphinx-quickstart on Mon Jun 1 14:21:53 2009. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.append(os.path.abspath('.')) + +# -- General configuration ----------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'image_geometry' +copyright = u'2009, Willow Garage, Inc.' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.1' +# The full version, including alpha/beta/rc tags. +release = '0.1.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of documents that shouldn't be included in the build. +#unused_docs = [] + +# List of directories, relative to source directory, that shouldn't be searched +# for source files. +exclude_trees = ['_build'] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. Major themes that come with +# Sphinx are currently 'default' and 'sphinxdoc'. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_use_modindex = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = '' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'image_geometrydoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +# The paper size ('letter' or 'a4'). +#latex_paper_size = 'letter' + +# The font size ('10pt', '11pt' or '12pt'). +#latex_font_size = '10pt' + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'image_geometry.tex', u'stereo\\_utils Documentation', + u'James Bowman', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# Additional stuff for the LaTeX preamble. +#latex_preamble = '' + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_use_modindex = True + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = { + 'http://docs.python.org/': None, + 'http://docs.scipy.org/doc/numpy' : None, + 'http://www.ros.org/doc/api/tf/html/python/' : None + } diff --git a/ros/vision_opencv/image_geometry/doc/index.rst b/ros/vision_opencv/image_geometry/doc/index.rst new file mode 100644 index 0000000..08d0c10 --- /dev/null +++ b/ros/vision_opencv/image_geometry/doc/index.rst @@ -0,0 +1,21 @@ +image_geometry +============== + +image_geometry simplifies interpreting images geometrically using the +parameters from sensor_msgs/CameraInfo. + +.. module:: image_geometry + +.. autoclass:: image_geometry.PinholeCameraModel + :members: fromCameraInfo, rectifyImage, rectifyPoint, tfFrame, project3dToPixel, projectPixelTo3dRay, distortionCoeffs, intrinsicMatrix, projectionMatrix, rotationMatrix, cx, cy, fx, fy + +.. autoclass:: image_geometry.StereoCameraModel + :members: + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` + diff --git a/ros/vision_opencv/image_geometry/doc/mainpage.dox b/ros/vision_opencv/image_geometry/doc/mainpage.dox new file mode 100644 index 0000000..eadc128 --- /dev/null +++ b/ros/vision_opencv/image_geometry/doc/mainpage.dox @@ -0,0 +1,29 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b image_geometry contains camera model classes that simplify interpreting +images geometrically using the calibration parameters from +sensor_msgs/CameraInfo messages. They may be efficiently updated in your +image callback: + +\code +image_geometry::PinholeCameraModel model_; + +void imageCb(const sensor_msgs::ImageConstPtr& raw_image, + const sensor_msgs::CameraInfoConstPtr& cam_info) +{ + // Update the camera model (usually a no-op) + model_.fromCameraInfo(cam_info); + + // Do processing... +} +\endcode + +\section codeapi Code API + +\b image_geometry contains two classes: + - image_geometry::PinholeCameraModel - models a pinhole camera with distortion. + - image_geometry::StereoCameraModel - models a stereo pair of pinhole cameras. + +*/ diff --git a/ros/vision_opencv/image_geometry/include/image_geometry/pinhole_camera_model.h b/ros/vision_opencv/image_geometry/include/image_geometry/pinhole_camera_model.h new file mode 100644 index 0000000..be85904 --- /dev/null +++ b/ros/vision_opencv/image_geometry/include/image_geometry/pinhole_camera_model.h @@ -0,0 +1,338 @@ +#ifndef IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H +#define IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H + +#include +#include +#include +#include +#include + +namespace image_geometry { + +class Exception : public std::runtime_error +{ +public: + Exception(const std::string& description) : std::runtime_error(description) {} +}; + +/** + * \brief Simplifies interpreting images geometrically using the parameters from + * sensor_msgs/CameraInfo. + */ +class PinholeCameraModel +{ +public: + + PinholeCameraModel(); + + PinholeCameraModel(const PinholeCameraModel& other); + + PinholeCameraModel& operator=(const PinholeCameraModel& other); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. + */ + bool fromCameraInfo(const sensor_msgs::CameraInfo& msg); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. + */ + bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg); + + /** + * \brief Get the name of the camera coordinate frame in tf. + */ + std::string tfFrame() const; + + /** + * \brief Get the time stamp associated with this camera model. + */ + ros::Time stamp() const; + + /** + * \brief The resolution at which the camera was calibrated. + * + * The maximum resolution at which the camera can be used with the current + * calibration; normally this is the same as the imager resolution. + */ + cv::Size fullResolution() const; + + /** + * \brief The resolution of the current rectified image. + * + * The size of the rectified image associated with the latest CameraInfo, as + * reduced by binning/ROI and affected by distortion. If binning and ROI are + * not in use, this is the same as fullResolution(). + */ + cv::Size reducedResolution() const; + + cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const; + + cv::Rect toFullResolution(const cv::Rect& roi_reduced) const; + + cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const; + + cv::Rect toReducedResolution(const cv::Rect& roi_full) const; + + /** + * \brief The current raw ROI, as used for capture by the camera driver. + */ + cv::Rect rawRoi() const; + + /** + * \brief The current rectified ROI, which best fits the raw ROI. + */ + cv::Rect rectifiedRoi() const; + + /** + * \brief Project a 3d point to rectified pixel coordinates. + * + * This is the inverse of projectPixelTo3dRay(). + * + * \param xyz 3d point in the camera coordinate frame + * \return (u,v) in rectified pixel coordinates + */ + cv::Point2d project3dToPixel(const cv::Point3d& xyz) const; + + /** + * \brief Project a rectified pixel to a 3d ray. + * + * Returns the unit vector in the camera coordinate frame in the direction of rectified + * pixel (u,v) in the image plane. This is the inverse of project3dToPixel(). + * + * In 1.4.x, the vector has z = 1.0. Previously, this function returned a unit vector. + * + * \param uv_rect Rectified pixel coordinates + * \return 3d ray passing through (u,v) + */ + cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const; + + /** + * \brief Rectify a raw camera image. + */ + void rectifyImage(const cv::Mat& raw, cv::Mat& rectified, + int interpolation = cv::INTER_LINEAR) const; + + /** + * \brief Apply camera distortion to a rectified image. + */ + void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, + int interpolation = cv::INTER_LINEAR) const; + + /** + * \brief Compute the rectified image coordinates of a pixel in the raw image. + */ + cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const; + + /** + * \brief Compute the raw image coordinates of a pixel in the rectified image. + */ + cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const; + + /** + * \brief Compute the rectified ROI best fitting a raw ROI. + */ + cv::Rect rectifyRoi(const cv::Rect& roi_raw) const; + + /** + * \brief Compute the raw ROI best fitting a rectified ROI. + */ + cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const; + + /** + * \brief Returns the camera info message held internally + */ + const sensor_msgs::CameraInfo& cameraInfo() const; + + /** + * \brief Returns the original camera matrix. + */ + const cv::Matx33d& intrinsicMatrix() const; + + /** + * \brief Returns the distortion coefficients. + */ + const cv::Mat_& distortionCoeffs() const; + + /** + * \brief Returns the rotation matrix. + */ + const cv::Matx33d& rotationMatrix() const; + + /** + * \brief Returns the projection matrix. + */ + const cv::Matx34d& projectionMatrix() const; + + /** + * \brief Returns the original camera matrix for full resolution. + */ + const cv::Matx33d& fullIntrinsicMatrix() const; + + /** + * \brief Returns the projection matrix for full resolution. + */ + const cv::Matx34d& fullProjectionMatrix() const; + + /** + * \brief Returns the focal length (pixels) in x direction of the rectified image. + */ + double fx() const; + + /** + * \brief Returns the focal length (pixels) in y direction of the rectified image. + */ + double fy() const; + + /** + * \brief Returns the x coordinate of the optical center. + */ + double cx() const; + + /** + * \brief Returns the y coordinate of the optical center. + */ + double cy() const; + + /** + * \brief Returns the x-translation term of the projection matrix. + */ + double Tx() const; + + /** + * \brief Returns the y-translation term of the projection matrix. + */ + double Ty() const; + + /** + * \brief Returns the number of columns in each bin. + */ + uint32_t binningX() const; + + /** + * \brief Returns the number of rows in each bin. + */ + uint32_t binningY() const; + + /** + * \brief Compute delta u, given Z and delta X in Cartesian space. + * + * For given Z, this is the inverse of getDeltaX(). + * + * \param deltaX Delta X, in Cartesian space + * \param Z Z (depth), in Cartesian space + */ + double getDeltaU(double deltaX, double Z) const; + + /** + * \brief Compute delta v, given Z and delta Y in Cartesian space. + * + * For given Z, this is the inverse of getDeltaY(). + * + * \param deltaY Delta Y, in Cartesian space + * \param Z Z (depth), in Cartesian space + */ + double getDeltaV(double deltaY, double Z) const; + + /** + * \brief Compute delta X, given Z in Cartesian space and delta u in pixels. + * + * For given Z, this is the inverse of getDeltaU(). + * + * \param deltaU Delta u, in pixels + * \param Z Z (depth), in Cartesian space + */ + double getDeltaX(double deltaU, double Z) const; + + /** + * \brief Compute delta Y, given Z in Cartesian space and delta v in pixels. + * + * For given Z, this is the inverse of getDeltaV(). + * + * \param deltaV Delta v, in pixels + * \param Z Z (depth), in Cartesian space + */ + double getDeltaY(double deltaV, double Z) const; + + /** + * \brief Returns true if the camera has been initialized + */ + bool initialized() const { return (bool)cache_; } + +protected: + sensor_msgs::CameraInfo cam_info_; + cv::Mat_ D_; // Unaffected by binning, ROI + cv::Matx33d R_; // Unaffected by binning, ROI + cv::Matx33d K_; // Describe current image (includes binning, ROI) + cv::Matx34d P_; // Describe current image (includes binning, ROI) + cv::Matx33d K_full_; // Describe full-res image, needed for full maps + cv::Matx34d P_full_; // Describe full-res image, needed for full maps + + // Use PIMPL here so we can change internals in patch updates if needed + struct Cache; + boost::shared_ptr cache_; // Holds cached data for internal use + + void initRectificationMaps() const; + + friend class StereoCameraModel; +}; + + +/* Trivial inline functions */ +inline std::string PinholeCameraModel::tfFrame() const +{ + assert( initialized() ); + return cam_info_.header.frame_id; +} + +inline ros::Time PinholeCameraModel::stamp() const +{ + assert( initialized() ); + return cam_info_.header.stamp; +} + +inline const sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; } +inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; } +inline const cv::Mat_& PinholeCameraModel::distortionCoeffs() const { return D_; } +inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; } +inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; } +inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; } +inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; } + +inline double PinholeCameraModel::fx() const { return P_(0,0); } +inline double PinholeCameraModel::fy() const { return P_(1,1); } +inline double PinholeCameraModel::cx() const { return P_(0,2); } +inline double PinholeCameraModel::cy() const { return P_(1,2); } +inline double PinholeCameraModel::Tx() const { return P_(0,3); } +inline double PinholeCameraModel::Ty() const { return P_(1,3); } + +inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; } +inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; } + +inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const +{ + assert( initialized() ); + return fx() * deltaX / Z; +} + +inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const +{ + assert( initialized() ); + return fy() * deltaY / Z; +} + +inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const +{ + assert( initialized() ); + return Z * deltaU / fx(); +} + +inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const +{ + assert( initialized() ); + return Z * deltaV / fy(); +} + +} //namespace image_geometry + +#endif diff --git a/ros/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h b/ros/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h new file mode 100644 index 0000000..a4f5880 --- /dev/null +++ b/ros/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h @@ -0,0 +1,130 @@ +#ifndef IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H +#define IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H + +#include "image_geometry/pinhole_camera_model.h" + +namespace image_geometry { + +/** + * \brief Simplifies interpreting stereo image pairs geometrically using the + * parameters from the left and right sensor_msgs/CameraInfo. + */ +class StereoCameraModel +{ +public: + StereoCameraModel(); + + StereoCameraModel(const StereoCameraModel& other); + + StereoCameraModel& operator=(const StereoCameraModel& other); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo messages. + */ + bool fromCameraInfo(const sensor_msgs::CameraInfo& left, + const sensor_msgs::CameraInfo& right); + + /** + * \brief Set the camera parameters from the sensor_msgs/CameraInfo messages. + */ + bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left, + const sensor_msgs::CameraInfoConstPtr& right); + + /** + * \brief Get the left monocular camera model. + */ + const PinholeCameraModel& left() const; + + /** + * \brief Get the right monocular camera model. + */ + const PinholeCameraModel& right() const; + + /** + * \brief Get the name of the camera coordinate frame in tf. + * + * For stereo cameras, both the left and right CameraInfo should be in the left + * optical frame. + */ + std::string tfFrame() const; + + /** + * \brief Project a rectified pixel with disparity to a 3d point. + */ + void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const; + + /** + * \brief Project a disparity image to a 3d point cloud. + * + * If handleMissingValues = true, all points with minimal disparity (outliers) have + * Z set to MISSING_Z (currently 10000.0). + */ + void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud, + bool handleMissingValues = false) const; + static const double MISSING_Z; + + /** + * \brief Returns the disparity reprojection matrix. + */ + const cv::Matx44d& reprojectionMatrix() const; + + /** + * \brief Returns the horizontal baseline in world coordinates. + */ + double baseline() const; + + /** + * \brief Returns the depth at which a point is observed with a given disparity. + * + * This is the inverse of getDisparity(). + */ + double getZ(double disparity) const; + + /** + * \brief Returns the disparity observed for a point at depth Z. + * + * This is the inverse of getZ(). + */ + double getDisparity(double Z) const; + + /** + * \brief Returns true if the camera has been initialized + */ + bool initialized() const { return left_.initialized() && right_.initialized(); } +protected: + PinholeCameraModel left_, right_; + cv::Matx44d Q_; + + void updateQ(); +}; + + +/* Trivial inline functions */ +inline const PinholeCameraModel& StereoCameraModel::left() const { return left_; } +inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; } + +inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); } + +inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return Q_; } + +inline double StereoCameraModel::baseline() const +{ + /// @todo Currently assuming horizontal baseline + return -right_.Tx() / right_.fx(); +} + +inline double StereoCameraModel::getZ(double disparity) const +{ + assert( initialized() ); + return -right_.Tx() / (disparity - (left().cx() - right().cx())); +} + +inline double StereoCameraModel::getDisparity(double Z) const +{ + assert( initialized() ); + return -right_.Tx() / Z + (left().cx() - right().cx()); ; +} + +} //namespace image_geometry + +#endif diff --git a/ros/vision_opencv/image_geometry/package.xml b/ros/vision_opencv/image_geometry/package.xml new file mode 100644 index 0000000..6ae2e93 --- /dev/null +++ b/ros/vision_opencv/image_geometry/package.xml @@ -0,0 +1,31 @@ + + image_geometry + 1.11.15 + + `image_geometry` contains C++ and Python libraries for interpreting images + geometrically. It interfaces the calibration parameters in sensor_msgs/CameraInfo + messages with OpenCV functions such as image rectification, much as cv_bridge + interfaces ROS sensor_msgs/Image with OpenCV data types. + + Patrick Mihelich + Vincent Rabaud + BSD + http://www.ros.org/wiki/image_geometry + + + + + + catkin + + libopencv-dev + python-opencv + sensor_msgs + + libopencv-dev + python-opencv + sensor_msgs + + dvipng + texlive-latex-extra + diff --git a/ros/vision_opencv/image_geometry/rosdoc.yaml b/ros/vision_opencv/image_geometry/rosdoc.yaml new file mode 100644 index 0000000..615dc7e --- /dev/null +++ b/ros/vision_opencv/image_geometry/rosdoc.yaml @@ -0,0 +1,8 @@ + - builder: sphinx + name: Python API + output_dir: python + sphinx_root_dir: doc + - builder: doxygen + name: C++ API + output_dir: c++ + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' diff --git a/ros/vision_opencv/image_geometry/setup.py b/ros/vision_opencv/image_geometry/setup.py new file mode 100644 index 0000000..407ce1e --- /dev/null +++ b/ros/vision_opencv/image_geometry/setup.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup() + +d['packages'] = ['image_geometry'] +d['package_dir'] = {'' : 'src'} + +setup(**d) diff --git a/ros/vision_opencv/image_geometry/src/image_geometry/__init__.py b/ros/vision_opencv/image_geometry/src/image_geometry/__init__.py new file mode 100644 index 0000000..f1cbda0 --- /dev/null +++ b/ros/vision_opencv/image_geometry/src/image_geometry/__init__.py @@ -0,0 +1,2 @@ +from __future__ import absolute_import +from .cameramodels import PinholeCameraModel, StereoCameraModel diff --git a/ros/vision_opencv/image_geometry/src/image_geometry/cameramodels.py b/ros/vision_opencv/image_geometry/src/image_geometry/cameramodels.py new file mode 100644 index 0000000..139c95c --- /dev/null +++ b/ros/vision_opencv/image_geometry/src/image_geometry/cameramodels.py @@ -0,0 +1,373 @@ +import array + +import cv2 +import sensor_msgs.msg +import math +import copy +import numpy + +def mkmat(rows, cols, L): + mat = numpy.matrix(L, dtype='float64') + mat.resize((rows,cols)) + return mat + +class PinholeCameraModel: + + """ + A pinhole camera is an idealized monocular camera. + """ + + def __init__(self): + self.K = None + self.D = None + self.R = None + self.P = None + self.full_K = None + self.full_P = None + self.width = None + self.height = None + self.binning_x = None + self.binning_y = None + self.raw_roi = None + self.tf_frame = None + self.stamp = None + + def fromCameraInfo(self, msg): + """ + :param msg: camera parameters + :type msg: sensor_msgs.msg.CameraInfo + + Set the camera parameters from the :class:`sensor_msgs.msg.CameraInfo` message. + """ + self.K = mkmat(3, 3, msg.K) + if msg.D: + self.D = mkmat(len(msg.D), 1, msg.D) + else: + self.D = None + self.R = mkmat(3, 3, msg.R) + self.P = mkmat(3, 4, msg.P) + self.full_K = mkmat(3, 3, msg.K) + self.full_P = mkmat(3, 4, msg.P) + self.width = msg.width + self.height = msg.height + self.binning_x = max(1, msg.binning_x) + self.binning_y = max(1, msg.binning_y) + self.resolution = (msg.width, msg.height) + + self.raw_roi = copy.copy(msg.roi) + # ROI all zeros is considered the same as full resolution + if (self.raw_roi.x_offset == 0 and self.raw_roi.y_offset == 0 and + self.raw_roi.width == 0 and self.raw_roi.height == 0): + self.raw_roi.width = self.width + self.raw_roi.height = self.height + self.tf_frame = msg.header.frame_id + self.stamp = msg.header.stamp + + # Adjust K and P for binning and ROI + self.K[0,0] /= self.binning_x + self.K[1,1] /= self.binning_y + self.K[0,2] = (self.K[0,2] - self.raw_roi.x_offset) / self.binning_x + self.K[1,2] = (self.K[1,2] - self.raw_roi.y_offset) / self.binning_y + self.P[0,0] /= self.binning_x + self.P[1,1] /= self.binning_y + self.P[0,2] = (self.P[0,2] - self.raw_roi.x_offset) / self.binning_x + self.P[1,2] = (self.P[1,2] - self.raw_roi.y_offset) / self.binning_y + + def rectifyImage(self, raw, rectified): + """ + :param raw: input image + :type raw: :class:`CvMat` or :class:`IplImage` + :param rectified: rectified output image + :type rectified: :class:`CvMat` or :class:`IplImage` + + Applies the rectification specified by camera parameters :math:`K` and and :math:`D` to image `raw` and writes the resulting image `rectified`. + """ + + self.mapx = numpy.ndarray(shape=(self.height, self.width, 1), + dtype='float32') + self.mapy = numpy.ndarray(shape=(self.height, self.width, 1), + dtype='float32') + cv2.initUndistortRectifyMap(self.K, self.D, self.R, self.P, + (self.width, self.height), cv2.CV_32FC1, self.mapx, self.mapy) + cv2.remap(raw, self.mapx, self.mapy, cv2.INTER_CUBIC, rectified) + + def rectifyPoint(self, uv_raw): + """ + :param uv_raw: pixel coordinates + :type uv_raw: (u, v) + + Applies the rectification specified by camera parameters + :math:`K` and and :math:`D` to point (u, v) and returns the + pixel coordinates of the rectified point. + """ + + src = mkmat(1, 2, list(uv_raw)) + src.resize((1,1,2)) + dst = cv2.undistortPoints(src, self.K, self.D, R=self.R, P=self.P) + return dst[0,0] + + def project3dToPixel(self, point): + """ + :param point: 3D point + :type point: (x, y, z) + + Returns the rectified pixel coordinates (u, v) of the 3D point, + using the camera :math:`P` matrix. + This is the inverse of :meth:`projectPixelTo3dRay`. + """ + src = mkmat(4, 1, [point[0], point[1], point[2], 1.0]) + dst = self.P * src + x = dst[0,0] + y = dst[1,0] + w = dst[2,0] + if w != 0: + return (x / w, y / w) + else: + return (float('nan'), float('nan')) + + def projectPixelTo3dRay(self, uv): + """ + :param uv: rectified pixel coordinates + :type uv: (u, v) + + Returns the unit vector which passes from the camera center to through rectified pixel (u, v), + using the camera :math:`P` matrix. + This is the inverse of :meth:`project3dToPixel`. + """ + x = (uv[0] - self.cx()) / self.fx() + y = (uv[1] - self.cy()) / self.fy() + norm = math.sqrt(x*x + y*y + 1) + x /= norm + y /= norm + z = 1.0 / norm + return (x, y, z) + + def getDeltaU(self, deltaX, Z): + """ + :param deltaX: delta X, in cartesian space + :type deltaX: float + :param Z: Z, in cartesian space + :type Z: float + :rtype: float + + Compute delta u, given Z and delta X in Cartesian space. + For given Z, this is the inverse of :meth:`getDeltaX`. + """ + fx = self.P[0, 0] + if Z == 0: + return float('inf') + else: + return fx * deltaX / Z + + def getDeltaV(self, deltaY, Z): + """ + :param deltaY: delta Y, in cartesian space + :type deltaY: float + :param Z: Z, in cartesian space + :type Z: float + :rtype: float + + Compute delta v, given Z and delta Y in Cartesian space. + For given Z, this is the inverse of :meth:`getDeltaY`. + """ + fy = self.P[1, 1] + if Z == 0: + return float('inf') + else: + return fy * deltaY / Z + + def getDeltaX(self, deltaU, Z): + """ + :param deltaU: delta u in pixels + :type deltaU: float + :param Z: Z, in cartesian space + :type Z: float + :rtype: float + + Compute delta X, given Z in cartesian space and delta u in pixels. + For given Z, this is the inverse of :meth:`getDeltaU`. + """ + fx = self.P[0, 0] + return Z * deltaU / fx + + def getDeltaY(self, deltaV, Z): + """ + :param deltaV: delta v in pixels + :type deltaV: float + :param Z: Z, in cartesian space + :type Z: float + :rtype: float + + Compute delta Y, given Z in cartesian space and delta v in pixels. + For given Z, this is the inverse of :meth:`getDeltaV`. + """ + fy = self.P[1, 1] + return Z * deltaV / fy + + def fullResolution(self): + """Returns the full resolution of the camera""" + return self.resolution + + def intrinsicMatrix(self): + """ Returns :math:`K`, also called camera_matrix in cv docs """ + return self.K + def distortionCoeffs(self): + """ Returns :math:`D` """ + return self.D + def rotationMatrix(self): + """ Returns :math:`R` """ + return self.R + def projectionMatrix(self): + """ Returns :math:`P` """ + return self.P + def fullIntrinsicMatrix(self): + """ Return the original camera matrix for full resolution """ + return self.full_K + def fullProjectionMatrix(self): + """ Return the projection matrix for full resolution """ + return self.full_P + + def cx(self): + """ Returns x center """ + return self.P[0,2] + def cy(self): + """ Returns y center """ + return self.P[1,2] + def fx(self): + """ Returns x focal length """ + return self.P[0,0] + def fy(self): + """ Returns y focal length """ + return self.P[1,1] + + def Tx(self): + """ Return the x-translation term of the projection matrix """ + return self.P[0,3] + + def Ty(self): + """ Return the y-translation term of the projection matrix """ + return self.P[1,3] + + def tfFrame(self): + """ Returns the tf frame name - a string - of the camera. + This is the frame of the :class:`sensor_msgs.msg.CameraInfo` message. + """ + return self.tf_frame + +class StereoCameraModel: + """ + An idealized stereo camera. + """ + def __init__(self): + self.left = PinholeCameraModel() + self.right = PinholeCameraModel() + + def fromCameraInfo(self, left_msg, right_msg): + """ + :param left_msg: left camera parameters + :type left_msg: sensor_msgs.msg.CameraInfo + :param right_msg: right camera parameters + :type right_msg: sensor_msgs.msg.CameraInfo + + Set the camera parameters from the :class:`sensor_msgs.msg.CameraInfo` messages. + """ + self.left.fromCameraInfo(left_msg) + self.right.fromCameraInfo(right_msg) + + # [ Fx, 0, Cx, Fx*-Tx ] + # [ 0, Fy, Cy, 0 ] + # [ 0, 0, 1, 0 ] + + fx = self.right.P[0, 0] + fy = self.right.P[1, 1] + cx = self.right.P[0, 2] + cy = self.right.P[1, 2] + tx = -self.right.P[0, 3] / fx + + # Q is: + # [ 1, 0, 0, -Clx ] + # [ 0, 1, 0, -Cy ] + # [ 0, 0, 0, Fx ] + # [ 0, 0, 1 / Tx, (Crx-Clx)/Tx ] + + self.Q = numpy.zeros((4, 4), dtype='float64') + self.Q[0, 0] = 1.0 + self.Q[0, 3] = -cx + self.Q[1, 1] = 1.0 + self.Q[1, 3] = -cy + self.Q[2, 3] = fx + self.Q[3, 2] = 1 / tx + + def tfFrame(self): + """ + Returns the tf frame name - a string - of the 3d points. This is + the frame of the :class:`sensor_msgs.msg.CameraInfo` message. It + may be used as a source frame in :class:`tf.TransformListener`. + """ + + return self.left.tfFrame() + + def project3dToPixel(self, point): + """ + :param point: 3D point + :type point: (x, y, z) + + Returns the rectified pixel coordinates (u, v) of the 3D point, for each camera, as ((u_left, v_left), (u_right, v_right)) + using the cameras' :math:`P` matrices. + This is the inverse of :meth:`projectPixelTo3d`. + """ + l = self.left.project3dToPixel(point) + r = self.right.project3dToPixel(point) + return (l, r) + + def projectPixelTo3d(self, left_uv, disparity): + """ + :param left_uv: rectified pixel coordinates + :type left_uv: (u, v) + :param disparity: disparity, in pixels + :type disparity: float + + Returns the 3D point (x, y, z) for the given pixel position, + using the cameras' :math:`P` matrices. + This is the inverse of :meth:`project3dToPixel`. + + Note that a disparity of zero implies that the 3D point is at infinity. + """ + src = mkmat(4, 1, [left_uv[0], left_uv[1], disparity, 1.0]) + dst = self.Q * src + x = dst[0, 0] + y = dst[1, 0] + z = dst[2, 0] + w = dst[3, 0] + if w != 0: + return (x / w, y / w, z / w) + else: + return (0.0, 0.0, 0.0) + + def getZ(self, disparity): + """ + :param disparity: disparity, in pixels + :type disparity: float + + Returns the depth at which a point is observed with a given disparity. + This is the inverse of :meth:`getDisparity`. + + Note that a disparity of zero implies Z is infinite. + """ + if disparity == 0: + return float('inf') + Tx = -self.right.P[0, 3] + return Tx / disparity + + def getDisparity(self, Z): + """ + :param Z: Z (depth), in cartesian space + :type Z: float + + Returns the disparity observed for a point at depth Z. + This is the inverse of :meth:`getZ`. + """ + if Z == 0: + return float('inf') + Tx = -self.right.P[0, 3] + return Tx / Z diff --git a/ros/vision_opencv/image_geometry/src/pinhole_camera_model.cpp b/ros/vision_opencv/image_geometry/src/pinhole_camera_model.cpp new file mode 100644 index 0000000..b0387e9 --- /dev/null +++ b/ros/vision_opencv/image_geometry/src/pinhole_camera_model.cpp @@ -0,0 +1,478 @@ +#include "image_geometry/pinhole_camera_model.h" +#include +#include + +namespace image_geometry { + +enum DistortionState { NONE, CALIBRATED, UNKNOWN }; + +struct PinholeCameraModel::Cache +{ + DistortionState distortion_state; + + cv::Mat_ K_binned, P_binned; // Binning applied, but not cropping + + mutable bool full_maps_dirty; + mutable cv::Mat full_map1, full_map2; + + mutable bool reduced_maps_dirty; + mutable cv::Mat reduced_map1, reduced_map2; + + mutable bool rectified_roi_dirty; + mutable cv::Rect rectified_roi; + + Cache() + : full_maps_dirty(true), + reduced_maps_dirty(true), + rectified_roi_dirty(true) + { + } +}; + +PinholeCameraModel::PinholeCameraModel() +{ +} + +PinholeCameraModel& PinholeCameraModel::operator=(const PinholeCameraModel& other) +{ + if (other.initialized()) + this->fromCameraInfo(other.cameraInfo()); + return *this; +} + +PinholeCameraModel::PinholeCameraModel(const PinholeCameraModel& other) +{ + if (other.initialized()) + fromCameraInfo(other.cam_info_); +} + +// For uint32_t, string, bool... +template +bool update(const T& new_val, T& my_val) +{ + if (my_val == new_val) + return false; + my_val = new_val; + return true; +} + +// For boost::array, std::vector +template +bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_& cv_mat, int rows, int cols) +{ + if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols)) + return false; + my_mat = new_mat; + // D may be empty if camera is uncalibrated or distortion model is non-standard + cv_mat = (my_mat.size() == 0) ? cv::Mat_() : cv::Mat_(rows, cols, &my_mat[0]); + return true; +} + +template +bool updateMat(const MatT& new_mat, MatT& my_mat, MatU& cv_mat) +{ + if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols)) + return false; + my_mat = new_mat; + // D may be empty if camera is uncalibrated or distortion model is non-standard + cv_mat = MatU(&my_mat[0]); + return true; +} + +bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg) +{ + // Create our repository of cached data (rectification maps, etc.) + if (!cache_) + cache_ = boost::make_shared(); + + // Binning = 0 is considered the same as binning = 1 (no binning). + uint32_t binning_x = msg.binning_x ? msg.binning_x : 1; + uint32_t binning_y = msg.binning_y ? msg.binning_y : 1; + + // ROI all zeros is considered the same as full resolution. + sensor_msgs::RegionOfInterest roi = msg.roi; + if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) { + roi.width = msg.width; + roi.height = msg.height; + } + + // Update time stamp (and frame_id if that changes for some reason) + cam_info_.header = msg.header; + + // Update any parameters that have changed. The full rectification maps are + // invalidated by any change in the calibration parameters OR binning. + bool &full_dirty = cache_->full_maps_dirty; + full_dirty |= update(msg.height, cam_info_.height); + full_dirty |= update(msg.width, cam_info_.width); + full_dirty |= update(msg.distortion_model, cam_info_.distortion_model); + full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size()); + full_dirty |= updateMat(msg.K, cam_info_.K, K_full_); + full_dirty |= updateMat(msg.R, cam_info_.R, R_); + full_dirty |= updateMat(msg.P, cam_info_.P, P_full_); + full_dirty |= update(binning_x, cam_info_.binning_x); + full_dirty |= update(binning_y, cam_info_.binning_y); + + // The reduced rectification maps are invalidated by any of the above or a + // change in ROI. + bool &reduced_dirty = cache_->reduced_maps_dirty; + reduced_dirty = full_dirty; + reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset); + reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset); + reduced_dirty |= update(roi.height, cam_info_.roi.height); + reduced_dirty |= update(roi.width, cam_info_.roi.width); + reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify); + // As is the rectified ROI + cache_->rectified_roi_dirty = reduced_dirty; + + // Figure out how to handle the distortion + if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB || + cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) { + // If any distortion coefficient is non-zero, then need to apply the distortion + cache_->distortion_state = NONE; + for (size_t i = 0; i < cam_info_.D.size(); ++i) + { + if (cam_info_.D[i] != 0) + { + cache_->distortion_state = CALIBRATED; + break; + } + } + } + else + cache_->distortion_state = UNKNOWN; + + // If necessary, create new K_ and P_ adjusted for binning and ROI + /// @todo Calculate and use rectified ROI + bool adjust_binning = (binning_x > 1) || (binning_y > 1); + bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0); + + if (!adjust_binning && !adjust_roi) { + K_ = K_full_; + P_ = P_full_; + } + else { + K_ = K_full_; + P_ = P_full_; + + // ROI is in full image coordinates, so change it first + if (adjust_roi) { + // Move principal point by the offset + /// @todo Adjust P by rectified ROI instead + K_(0,2) -= roi.x_offset; + K_(1,2) -= roi.y_offset; + P_(0,2) -= roi.x_offset; + P_(1,2) -= roi.y_offset; + } + + if (binning_x > 1) { + double scale_x = 1.0 / binning_x; + K_(0,0) *= scale_x; + K_(0,2) *= scale_x; + P_(0,0) *= scale_x; + P_(0,2) *= scale_x; + P_(0,3) *= scale_x; + } + if (binning_y > 1) { + double scale_y = 1.0 / binning_y; + K_(1,1) *= scale_y; + K_(1,2) *= scale_y; + P_(1,1) *= scale_y; + P_(1,2) *= scale_y; + P_(1,3) *= scale_y; + } + } + + return reduced_dirty; +} + +bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg) +{ + return fromCameraInfo(*msg); +} + +cv::Size PinholeCameraModel::fullResolution() const +{ + assert( initialized() ); + return cv::Size(cam_info_.width, cam_info_.height); +} + +cv::Size PinholeCameraModel::reducedResolution() const +{ + assert( initialized() ); + + cv::Rect roi = rectifiedRoi(); + return cv::Size(roi.width / binningX(), roi.height / binningY()); +} + +cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Point2d(uv_reduced.x * binningX() + roi.x, + uv_reduced.y * binningY() + roi.y); +} + +cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Rect(roi_reduced.x * binningX() + roi.x, + roi_reduced.y * binningY() + roi.y, + roi_reduced.width * binningX(), + roi_reduced.height * binningY()); +} + +cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Point2d((uv_full.x - roi.x) / binningX(), + (uv_full.y - roi.y) / binningY()); +} + +cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Rect((roi_full.x - roi.x) / binningX(), + (roi_full.y - roi.y) / binningY(), + roi_full.width / binningX(), + roi_full.height / binningY()); +} + +cv::Rect PinholeCameraModel::rawRoi() const +{ + assert( initialized() ); + + return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset, + cam_info_.roi.width, cam_info_.roi.height); +} + +cv::Rect PinholeCameraModel::rectifiedRoi() const +{ + assert( initialized() ); + + if (cache_->rectified_roi_dirty) + { + if (!cam_info_.roi.do_rectify) + cache_->rectified_roi = rawRoi(); + else + cache_->rectified_roi = rectifyRoi(rawRoi()); + cache_->rectified_roi_dirty = false; + } + return cache_->rectified_roi; +} + +cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const +{ + assert( initialized() ); + assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane + + // [U V W]^T = P * [X Y Z 1]^T + // u = U/W + // v = V/W + cv::Point2d uv_rect; + uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx(); + uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy(); + return uv_rect; +} + +cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const +{ + assert( initialized() ); + + cv::Point3d ray; + ray.x = (uv_rect.x - cx() - Tx()) / fx(); + ray.y = (uv_rect.y - cy() - Ty()) / fy(); + ray.z = 1.0; + return ray; +} + +void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const +{ + assert( initialized() ); + + switch (cache_->distortion_state) { + case NONE: + raw.copyTo(rectified); + break; + case CALIBRATED: + initRectificationMaps(); + if (raw.depth() == CV_32F || raw.depth() == CV_64F) + { + cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits::quiet_NaN()); + } + else { + cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation); + } + break; + default: + assert(cache_->distortion_state == UNKNOWN); + throw Exception("Cannot call rectifyImage when distortion is unknown."); + } +} + +void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const +{ + assert( initialized() ); + + throw Exception("PinholeCameraModel::unrectifyImage is unimplemented."); + /// @todo Implement unrectifyImage() + // Similar to rectifyImage, but need to build separate set of inverse maps (raw->rectified)... + // - Build src_pt Mat with all the raw pixel coordinates (or do it one row at a time) + // - Do cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_) + // - Use convertMaps() to convert dst_pt to fast fixed-point maps + // - cv::remap(rectified, raw, ...) + // Need interpolation argument. Same caching behavior? +} + +cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const +{ + assert( initialized() ); + + if (cache_->distortion_state == NONE) + return uv_raw; + if (cache_->distortion_state == UNKNOWN) + throw Exception("Cannot call rectifyPoint when distortion is unknown."); + assert(cache_->distortion_state == CALIBRATED); + + /// @todo cv::undistortPoints requires the point data to be float, should allow double + cv::Point2f raw32 = uv_raw, rect32; + const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x); + cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x); + cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_); + return rect32; +} + +cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const +{ + assert( initialized() ); + + if (cache_->distortion_state == NONE) + return uv_rect; + if (cache_->distortion_state == UNKNOWN) + throw Exception("Cannot call unrectifyPoint when distortion is unknown."); + assert(cache_->distortion_state == CALIBRATED); + + // Convert to a ray + cv::Point3d ray = projectPixelTo3dRay(uv_rect); + + // Project the ray on the image + cv::Mat r_vec, t_vec = cv::Mat_::zeros(3, 1); + cv::Rodrigues(R_.t(), r_vec); + std::vector image_point; + cv::projectPoints(std::vector(1, ray), r_vec, t_vec, K_, D_, image_point); + + return image_point[0]; +} + +cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const +{ + assert( initialized() ); + + /// @todo Actually implement "best fit" as described by REP 104. + + // For now, just unrectify the four corners and take the bounding box. + cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y)); + cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y)); + cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, + roi_raw.y + roi_raw.height)); + cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height)); + + cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)), + std::ceil (std::min(rect_tl.y, rect_tr.y))); + cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)), + std::floor(std::max(rect_bl.y, rect_br.y))); + + return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y); +} + +cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const +{ + assert( initialized() ); + + /// @todo Actually implement "best fit" as described by REP 104. + + // For now, just unrectify the four corners and take the bounding box. + cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y)); + cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y)); + cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, + roi_rect.y + roi_rect.height)); + cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height)); + + cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)), + std::floor(std::min(raw_tl.y, raw_tr.y))); + cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)), + std::ceil (std::max(raw_bl.y, raw_br.y))); + + return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y); +} + +void PinholeCameraModel::initRectificationMaps() const +{ + /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary. + /// Make sure we're handling that 100% correctly. + + if (cache_->full_maps_dirty) { + // Create the full-size map at the binned resolution + /// @todo Should binned resolution, K, P be part of public API? + cv::Size binned_resolution = fullResolution(); + binned_resolution.width /= binningX(); + binned_resolution.height /= binningY(); + + cv::Matx33d K_binned; + cv::Matx34d P_binned; + if (binningX() == 1 && binningY() == 1) { + K_binned = K_full_; + P_binned = P_full_; + } + else { + K_binned = K_full_; + P_binned = P_full_; + if (binningX() > 1) { + double scale_x = 1.0 / binningX(); + K_binned(0,0) *= scale_x; + K_binned(0,2) *= scale_x; + P_binned(0,0) *= scale_x; + P_binned(0,2) *= scale_x; + P_binned(0,3) *= scale_x; + } + if (binningY() > 1) { + double scale_y = 1.0 / binningY(); + K_binned(1,1) *= scale_y; + K_binned(1,2) *= scale_y; + P_binned(1,1) *= scale_y; + P_binned(1,2) *= scale_y; + P_binned(1,3) *= scale_y; + } + } + + // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap) + cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution, + CV_16SC2, cache_->full_map1, cache_->full_map2); + cache_->full_maps_dirty = false; + } + + if (cache_->reduced_maps_dirty) { + /// @todo Use rectified ROI + cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset, + cam_info_.roi.width, cam_info_.roi.height); + if (roi.x != 0 || roi.y != 0 || + roi.height != (int)cam_info_.height || + roi.width != (int)cam_info_.width) { + + // map1 contains integer (x,y) offsets, which we adjust by the ROI offset + // map2 contains LUT index for subpixel interpolation, which we can leave as-is + roi.x /= binningX(); + roi.y /= binningY(); + roi.width /= binningX(); + roi.height /= binningY(); + cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y); + cache_->reduced_map2 = cache_->full_map2(roi); + } + else { + // Otherwise we're rectifying the full image + cache_->reduced_map1 = cache_->full_map1; + cache_->reduced_map2 = cache_->full_map2; + } + cache_->reduced_maps_dirty = false; + } +} + +} //namespace image_geometry diff --git a/ros/vision_opencv/image_geometry/src/stereo_camera_model.cpp b/ros/vision_opencv/image_geometry/src/stereo_camera_model.cpp new file mode 100644 index 0000000..062edb7 --- /dev/null +++ b/ros/vision_opencv/image_geometry/src/stereo_camera_model.cpp @@ -0,0 +1,140 @@ +#include "image_geometry/stereo_camera_model.h" + +namespace image_geometry { + +StereoCameraModel::StereoCameraModel() + : Q_(0.0) +{ + Q_(0,0) = Q_(1,1) = 1.0; +} + +StereoCameraModel::StereoCameraModel(const StereoCameraModel& other) + : left_(other.left_), right_(other.right_), + Q_(0.0) +{ + Q_(0,0) = Q_(1,1) = 1.0; + if (other.initialized()) + updateQ(); +} + +StereoCameraModel& StereoCameraModel::operator=(const StereoCameraModel& other) +{ + if (other.initialized()) + this->fromCameraInfo(other.left_.cameraInfo(), other.right_.cameraInfo()); + return *this; +} + +bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left, + const sensor_msgs::CameraInfo& right) +{ + bool changed_left = left_.fromCameraInfo(left); + bool changed_right = right_.fromCameraInfo(right); + bool changed = changed_left || changed_right; + + // Note: don't require identical time stamps to allow imperfectly synced stereo. + assert( left_.tfFrame() == right_.tfFrame() ); + assert( left_.fx() == right_.fx() ); + assert( left_.fy() == right_.fy() ); + assert( left_.cy() == right_.cy() ); + // cx may differ for verged cameras + + if (changed) + updateQ(); + + return changed; +} + +bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left, + const sensor_msgs::CameraInfoConstPtr& right) +{ + return fromCameraInfo(*left, *right); +} + +void StereoCameraModel::updateQ() +{ + // Update variable fields of reprojection matrix + /* + From Springer Handbook of Robotics, p. 524: + + [ Fx 0 Cx 0 ] + P = [ 0 Fy Cy 0 ] + [ 0 0 1 0 ] + + [ Fx 0 Cx' FxTx ] + P' = [ 0 Fy Cy 0 ] + [ 0 0 1 0 ] + where primed parameters are from the left projection matrix, unprimed from the right. + + [u v 1]^T = P * [x y z 1]^T + [u-d v 1]^T = P' * [x y z 1]^T + + Combining the two equations above results in the following equation + + [u v u-d 1]^T = [ Fx 0 Cx 0 ] * [ x y z 1]^T + [ 0 Fy Cy 0 ] + [ Fx 0 Cx' FxTx ] + [ 0 0 1 0 ] + + Subtracting the 3rd from from the first and inverting the expression + results in the following equation. + + [x y z 1]^T = Q * [u v d 1]^T + + Where Q is defined as + + Q = [ FyTx 0 0 -FyCxTx ] + [ 0 FxTx 0 -FxCyTx ] + [ 0 0 0 FxFyTx ] + [ 0 0 -Fy Fy(Cx-Cx') ] + + Using the assumption Fx = Fy Q can be simplified to the following. But for + compatibility with stereo cameras with different focal lengths we will use + the full Q matrix. + + [ 1 0 0 -Cx ] + Q = [ 0 1 0 -Cy ] + [ 0 0 0 Fx ] + [ 0 0 -1/Tx (Cx-Cx')/Tx ] + + Disparity = x_left - x_right + + For compatibility with stereo cameras with different focal lengths we will use + the full Q matrix. + + */ + double Tx = -baseline(); // The baseline member negates our Tx. Undo this negation + Q_(0,0) = left_.fy() * Tx; + Q_(0,3) = -left_.fy() * left_.cx() * Tx; + Q_(1,1) = left_.fx() * Tx; + Q_(1,3) = -left_.fx() * left_.cy() * Tx; + Q_(2,3) = left_.fx() * left_.fy() * Tx; + Q_(3,2) = -left_.fy(); + Q_(3,3) = left_.fy() * (left_.cx() - right_.cx()); // zero when disparities are pre-adjusted +} + +void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, + cv::Point3d& xyz) const +{ + assert( initialized() ); + + // Do the math inline: + // [X Y Z W]^T = Q * [u v d 1]^T + // Point = (X/W, Y/W, Z/W) + // cv::perspectiveTransform could be used but with more overhead. + double u = left_uv_rect.x, v = left_uv_rect.y; + cv::Point3d XYZ( (Q_(0,0) * u) + Q_(0,3), (Q_(1,1) * v) + Q_(1,3), Q_(2,3)); + double W = Q_(3,2)*disparity + Q_(3,3); + xyz = XYZ * (1.0/W); +} + +const double StereoCameraModel::MISSING_Z = 10000.; + +void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud, + bool handleMissingValues) const +{ + assert( initialized() ); + + cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues); +} + +} //namespace image_geometry diff --git a/ros/vision_opencv/image_geometry/test/CMakeLists.txt b/ros/vision_opencv/image_geometry/test/CMakeLists.txt new file mode 100644 index 0000000..6dba1e5 --- /dev/null +++ b/ros/vision_opencv/image_geometry/test/CMakeLists.txt @@ -0,0 +1,4 @@ +catkin_add_nosetests(directed.py) + +catkin_add_gtest(${PROJECT_NAME}-utest utest.cpp) +target_link_libraries(${PROJECT_NAME}-utest ${PROJECT_NAME} ${OpenCV_LIBS}) diff --git a/ros/vision_opencv/image_geometry/test/directed.py b/ros/vision_opencv/image_geometry/test/directed.py new file mode 100644 index 0000000..79ee137 --- /dev/null +++ b/ros/vision_opencv/image_geometry/test/directed.py @@ -0,0 +1,75 @@ +import rostest +import rospy +import unittest +import sensor_msgs.msg + +from image_geometry import PinholeCameraModel, StereoCameraModel + +class TestDirected(unittest.TestCase): + + def setUp(self): + pass + + def test_monocular(self): + ci = sensor_msgs.msg.CameraInfo() + ci.width = 640 + ci.height = 480 + print ci + cam = PinholeCameraModel() + cam.fromCameraInfo(ci) + print cam.rectifyPoint((0, 0)) + + print cam.project3dToPixel((0,0,0)) + + def test_stereo(self): + lmsg = sensor_msgs.msg.CameraInfo() + rmsg = sensor_msgs.msg.CameraInfo() + for m in (lmsg, rmsg): + m.width = 640 + m.height = 480 + + # These parameters taken from a real camera calibration + lmsg.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0] + lmsg.K = [430.15433020105519, 0.0, 311.71339830549732, 0.0, 430.60920415473657, 221.06824942698509, 0.0, 0.0, 1.0] + lmsg.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516] + lmsg.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0] + + rmsg.D = [-0.3560641041112021, 0.15647260261553159, -0.00016442960757099968, -0.00093175810713916221] + rmsg.K = [428.38163131344191, 0.0, 327.95553847249192, 0.0, 428.85728580588329, 217.54828640915309, 0.0, 0.0, 1.0] + rmsg.R = [0.9982082576219119, 0.0067433328293516528, 0.059454199832973849, -0.0068433268864187356, 0.99997549128605434, 0.0014784127772287513, -0.059442773257581252, -0.0018826283666309878, 0.99822993965212292] + rmsg.P = [295.53402059708782, 0.0, 285.55760765075684, -26.507895206214123, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0] + + cam = StereoCameraModel() + cam.fromCameraInfo(lmsg, rmsg) + + for x in (16, 320, m.width - 16): + for y in (16, 240, m.height - 16): + for d in range(1, 10): + pt3d = cam.projectPixelTo3d((x, y), d) + ((lx, ly), (rx, ry)) = cam.project3dToPixel(pt3d) + self.assertAlmostEqual(y, ly, 3) + self.assertAlmostEqual(y, ry, 3) + self.assertAlmostEqual(x, lx, 3) + self.assertAlmostEqual(x, rx + d, 3) + + u = 100.0 + v = 200.0 + du = 17.0 + dv = 23.0 + Z = 2.0 + xyz0 = cam.left.projectPixelTo3dRay((u, v)) + xyz0 = (xyz0[0] * (Z / xyz0[2]), xyz0[1] * (Z / xyz0[2]), Z) + xyz1 = cam.left.projectPixelTo3dRay((u + du, v + dv)) + xyz1 = (xyz1[0] * (Z / xyz1[2]), xyz1[1] * (Z / xyz1[2]), Z) + self.assertAlmostEqual(cam.left.getDeltaU(xyz1[0] - xyz0[0], Z), du, 3) + self.assertAlmostEqual(cam.left.getDeltaV(xyz1[1] - xyz0[1], Z), dv, 3) + self.assertAlmostEqual(cam.left.getDeltaX(du, Z), xyz1[0] - xyz0[0], 3) + self.assertAlmostEqual(cam.left.getDeltaY(dv, Z), xyz1[1] - xyz0[1], 3) + +if __name__ == '__main__': + if 1: + rostest.unitrun('image_geometry', 'directed', TestDirected) + else: + suite = unittest.TestSuite() + suite.addTest(TestDirected('test_stereo')) + unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/ros/vision_opencv/image_geometry/test/utest.cpp b/ros/vision_opencv/image_geometry/test/utest.cpp new file mode 100644 index 0000000..2589019 --- /dev/null +++ b/ros/vision_opencv/image_geometry/test/utest.cpp @@ -0,0 +1,259 @@ +#include "image_geometry/pinhole_camera_model.h" +#include +#include + +/// @todo Tests with simple values (R = identity, D = 0, P = K or simple scaling) +/// @todo Test projection functions for right stereo values, P(:,3) != 0 +/// @todo Tests for [un]rectifyImage +/// @todo Tests using ROI, needs support from PinholeCameraModel +/// @todo Tests for StereoCameraModel + +class PinholeTest : public testing::Test +{ +protected: + virtual void SetUp() + { + /// @todo Just load these from file + // These parameters taken from a real camera calibration + double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0}; + double K[] = {430.15433020105519, 0.0, 311.71339830549732, + 0.0, 430.60920415473657, 221.06824942698509, + 0.0, 0.0, 1.0}; + double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, + -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, + -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516}; + double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0, + 0.0, 295.53402059708782, 223.29617881774902, 0.0, + 0.0, 0.0, 1.0, 0.0}; + + cam_info_.header.frame_id = "tf_frame"; + cam_info_.height = 480; + cam_info_.width = 640; + // No ROI + cam_info_.D.resize(5); + std::copy(D, D+5, cam_info_.D.begin()); + std::copy(K, K+9, cam_info_.K.begin()); + std::copy(R, R+9, cam_info_.R.begin()); + std::copy(P, P+12, cam_info_.P.begin()); + cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + + model_.fromCameraInfo(cam_info_); + } + + sensor_msgs::CameraInfo cam_info_; + image_geometry::PinholeCameraModel model_; +}; + +TEST_F(PinholeTest, accessorsCorrect) +{ + EXPECT_STREQ("tf_frame", model_.tfFrame().c_str()); + EXPECT_EQ(cam_info_.P[0], model_.fx()); + EXPECT_EQ(cam_info_.P[5], model_.fy()); + EXPECT_EQ(cam_info_.P[2], model_.cx()); + EXPECT_EQ(cam_info_.P[6], model_.cy()); +} + +TEST_F(PinholeTest, projectPoint) +{ + // Spot test an arbitrary point. + { + cv::Point2d uv(100, 100); + cv::Point3d xyz = model_.projectPixelTo3dRay(uv); + EXPECT_NEAR(-0.62787224048135637, xyz.x, 1e-8); + EXPECT_NEAR(-0.41719792045817677, xyz.y, 1e-8); + EXPECT_DOUBLE_EQ(1.0, xyz.z); + } + + // Principal point should project straight out. + { + cv::Point2d uv(model_.cx(), model_.cy()); + cv::Point3d xyz = model_.projectPixelTo3dRay(uv); + EXPECT_DOUBLE_EQ(0.0, xyz.x); + EXPECT_DOUBLE_EQ(0.0, xyz.y); + EXPECT_DOUBLE_EQ(1.0, xyz.z); + } + + // Check projecting to 3d and back over entire image is accurate. + const size_t step = 10; + for (size_t row = 0; row <= cam_info_.height; row += step) { + for (size_t col = 0; col <= cam_info_.width; col += step) { + cv::Point2d uv(row, col), uv_back; + cv::Point3d xyz = model_.projectPixelTo3dRay(uv); + uv_back = model_.project3dToPixel(xyz); + // Measured max error at 1.13687e-13 + EXPECT_NEAR(uv.x, uv_back.x, 1.14e-13) << "at (" << row << ", " << col << ")"; + EXPECT_NEAR(uv.y, uv_back.y, 1.14e-13) << "at (" << row << ", " << col << ")"; + } + } +} + +TEST_F(PinholeTest, rectifyPoint) +{ + // Spot test an arbitrary point. + { + cv::Point2d uv_raw(100, 100), uv_rect; + uv_rect = model_.rectifyPoint(uv_raw); + EXPECT_DOUBLE_EQ(142.30311584472656, uv_rect.x); + EXPECT_DOUBLE_EQ(132.061065673828, uv_rect.y); + } + + /// @todo Need R = identity for the principal point tests. +#if 0 + // Test rectifyPoint takes (c'x, c'y) [from K] -> (cx, cy) [from P]. + double cxp = model_.intrinsicMatrix()(0,2), cyp = model_.intrinsicMatrix()(1,2); + { + cv::Point2d uv_raw(cxp, cyp), uv_rect; + model_.rectifyPoint(uv_raw, uv_rect); + EXPECT_NEAR(uv_rect.x, model_.cx(), 1e-4); + EXPECT_NEAR(uv_rect.y, model_.cy(), 1e-4); + } + + // Test unrectifyPoint takes (cx, cy) [from P] -> (c'x, c'y) [from K]. + { + cv::Point2d uv_rect(model_.cx(), model_.cy()), uv_raw; + model_.unrectifyPoint(uv_rect, uv_raw); + EXPECT_NEAR(uv_raw.x, cxp, 1e-4); + EXPECT_NEAR(uv_raw.y, cyp, 1e-4); + } +#endif + + // Check rectifying then unrectifying over most of the image is accurate. + const size_t step = 5; + const size_t border = 65; // Expect bad accuracy far from the center of the image. + for (size_t row = border; row <= cam_info_.height - border; row += step) { + for (size_t col = border; col <= cam_info_.width - border; col += step) { + cv::Point2d uv_raw(row, col), uv_rect, uv_unrect; + uv_rect = model_.rectifyPoint(uv_raw); + uv_unrect = model_.unrectifyPoint(uv_rect); + // Check that we're at least within a pixel... + EXPECT_NEAR(uv_raw.x, uv_unrect.x, 1.0); + EXPECT_NEAR(uv_raw.y, uv_unrect.y, 1.0); + } + } +} + +TEST_F(PinholeTest, getDeltas) +{ + double u = 100.0, v = 200.0, du = 17.0, dv = 23.0, Z = 2.0; + cv::Point2d uv0(u, v), uv1(u + du, v + dv); + cv::Point3d xyz0, xyz1; + xyz0 = model_.projectPixelTo3dRay(uv0); + xyz0 *= (Z / xyz0.z); + xyz1 = model_.projectPixelTo3dRay(uv1); + xyz1 *= (Z / xyz1.z); + + EXPECT_NEAR(model_.getDeltaU(xyz1.x - xyz0.x, Z), du, 1e-4); + EXPECT_NEAR(model_.getDeltaV(xyz1.y - xyz0.y, Z), dv, 1e-4); + EXPECT_NEAR(model_.getDeltaX(du, Z), xyz1.x - xyz0.x, 1e-4); + EXPECT_NEAR(model_.getDeltaY(dv, Z), xyz1.y - xyz0.y, 1e-4); +} + +TEST_F(PinholeTest, initialization) +{ + + sensor_msgs::CameraInfo info; + image_geometry::PinholeCameraModel camera; + + camera.fromCameraInfo(info); + + EXPECT_EQ(camera.initialized(), 1); + EXPECT_EQ(camera.projectionMatrix().rows, 3); + EXPECT_EQ(camera.projectionMatrix().cols, 4); +} + +TEST_F(PinholeTest, rectifyIfCalibrated) +{ + /// @todo use forward distortion for a better test + // Ideally this test would have two images stored on disk + // one which is distorted and the other which is rectified, + // and then rectification would take place here and the output + // image compared to the one on disk (which would mean if + // the distortion coefficients above can't change once paired with + // an image). + + // Later could incorporate distort code + // (https://github.com/lucasw/vimjay/blob/master/src/standalone/distort_image.cpp) + // to take any image distort it, then undistort with rectifyImage, + // and given the distortion coefficients are consistent the input image + // and final output image should be mostly the same (though some + // interpolation error + // creeps in), except for outside a masked region where information was lost. + // The masked region can be generated with a pure white image that + // goes through the same process (if it comes out completely black + // then the distortion parameters are problematic). + + // For now generate an image and pass the test simply if + // the rectified image does not match the distorted image. + // Then zero out the first distortion coefficient and run + // the test again. + // Then zero out all the distortion coefficients and test + // that the output image is the same as the input. + cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0)); + + // draw a grid + const cv::Scalar color = cv::Scalar(255, 255, 255); + // draw the lines thick so the proportion of error due to + // interpolation is reduced + const int thickness = 7; + const int type = 8; + for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10) + { + cv::line(distorted_image, + cv::Point(0, y), cv::Point(cam_info_.width, y), + color, type, thickness); + } + for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10) + { + // draw the lines thick so the prorportion of interpolation error is reduced + cv::line(distorted_image, + cv::Point(x, 0), cv::Point(x, cam_info_.height), + color, type, thickness); + } + + cv::Mat rectified_image; + // Just making this number up, maybe ought to be larger + // since a completely different image would be on the order of + // width * height * 255 = 78e6 + const double diff_threshold = 10000.0; + double error; + + // Test that rectified image is sufficiently different + // using default distortion + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + // Just making this number up, maybe ought to be larger + EXPECT_GT(error, diff_threshold); + + // Test that rectified image is sufficiently different + // using default distortion but with first element zeroed + // out. + sensor_msgs::CameraInfo cam_info_2 = cam_info_; + cam_info_2.D[0] = 0.0; + model_.fromCameraInfo(cam_info_2); + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + EXPECT_GT(error, diff_threshold); + + // Test that rectified image is the same using zero distortion + cam_info_2.D.assign(cam_info_2.D.size(), 0); + model_.fromCameraInfo(cam_info_2); + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + EXPECT_EQ(error, 0); + + // Test that rectified image is the same using empty distortion + cam_info_2.D.clear(); + model_.fromCameraInfo(cam_info_2); + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + EXPECT_EQ(error, 0); + + // restore original distortion + model_.fromCameraInfo(cam_info_); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/ros/vision_opencv/opencv_tests/CHANGELOG.rst b/ros/vision_opencv/opencv_tests/CHANGELOG.rst new file mode 100644 index 0000000..8db47bc --- /dev/null +++ b/ros/vision_opencv/opencv_tests/CHANGELOG.rst @@ -0,0 +1,257 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package opencv_tests +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.15 (2017-01-29) +-------------------- + +1.11.14 (2016-09-24) +-------------------- + +1.11.13 (2016-07-11) +-------------------- +* Support compressed Images messages in python for indigo + - Add cv2_to_comprssed_imgmsg: Convert from cv2 image to compressed image ros msg. + - Add comprssed_imgmsg_to_cv2: Convert the compress message to a new image. + - Add compressed image tests. + - Add time to msgs (compressed and regular). + add enumerants test for compressed image. + merge the compressed tests with the regular ones. + better comment explanation. I will squash this commit. + Fix indentation + fix typo mistage: from .imgmsg_to_compressed_cv2 to .compressed_imgmsg_to_cv2. + remove cv2.CV_8UC1 + remove rospy and time depndency. + change from IMREAD_COLOR to IMREAD_ANYCOLOR. + - make indentaion of 4. + - remove space trailer. + - remove space from empty lines. + - another set of for loops, it will make things easier to track. In that new set, just have the number of channels in ([],1,3,4) (ignore two for jpg). from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721943 + - keep the OpenCV error message. from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721013 + add debug print for test. + add case for 4 channels in test. + remove 4 channels case from compressed test. + add debug print for test. + change typo of format. + fix typo in format. change from dip to dib. + change to IMREAD_ANYCOLOR as python code. (as it should). + rename TIFF to tiff + Sperate the tests one for regular images and one for compressed. + update comment +* Contributors: talregev + +1.11.12 (2016-03-10) +-------------------- + +1.11.11 (2016-01-31) +-------------------- +* fix a few warnings in doc jobs +* Contributors: Vincent Rabaud + +1.11.10 (2016-01-16) +-------------------- + +1.11.9 (2015-11-29) +------------------- + +1.11.8 (2015-07-15) +------------------- +* simplify dependencies +* Contributors: Vincent Rabaud + +1.11.7 (2014-12-14) +------------------- + +1.11.6 (2014-11-16) +------------------- + +1.11.5 (2014-09-21) +------------------- + +1.11.4 (2014-07-27) +------------------- + +1.11.3 (2014-06-08) +------------------- +* remove file whose functinality is now in cv_bridge +* remove references to cv (use cv2) +* Correct dependency from non-existent package to cv_bridge +* Contributors: Isaac Isao Saito, Vincent Rabaud + +1.11.2 (2014-04-28) +------------------- + +1.11.1 (2014-04-16) +------------------- + +1.11.0 (2014-02-15) +------------------- + +1.10.15 (2014-02-07) +-------------------- + +1.10.14 (2013-11-23 16:17) +-------------------------- +* Contributors: Vincent Rabaud + +1.10.13 (2013-11-23 09:19) +-------------------------- +* Contributors: Vincent Rabaud + +1.10.12 (2013-11-22) +-------------------- +* Contributors: Vincent Rabaud + +1.10.11 (2013-10-23) +-------------------- +* Contributors: Vincent Rabaud + +1.10.10 (2013-10-19) +-------------------- +* Contributors: Vincent Rabaud + +1.10.9 (2013-10-07) +------------------- +* Contributors: Vincent Rabaud + +1.10.8 (2013-09-09) +------------------- +* update email address +* Contributors: Vincent Rabaud + +1.10.7 (2013-07-17) +------------------- + +1.10.6 (2013-03-01) +------------------- + +1.10.5 (2013-02-11) +------------------- + +1.10.4 (2013-02-02) +------------------- + +1.10.3 (2013-01-17) +------------------- + +1.10.2 (2013-01-13) +------------------- + +1.10.1 (2013-01-10) +------------------- +* fixes `#5 `_ by removing the logic from Python and using wrapped C++ and adding a test for it +* Contributors: Vincent Rabaud + +1.10.0 (2013-01-03) +------------------- + +1.9.15 (2013-01-02) +------------------- + +1.9.14 (2012-12-30) +------------------- + +1.9.13 (2012-12-15) +------------------- + +1.9.12 (2012-12-14) +------------------- +* Removed brief tag + Conflicts: + opencv_tests/package.xml +* buildtool_depend catkin fix +* Contributors: William Woodall + +1.9.11 (2012-12-10) +------------------- + +1.9.10 (2012-10-04) +------------------- + +1.9.9 (2012-10-01) +------------------ + +1.9.8 (2012-09-30) +------------------ + +1.9.7 (2012-09-28 21:07) +------------------------ +* add missing stuff +* make sure we find catkin +* Contributors: Vincent Rabaud + +1.9.6 (2012-09-28 15:17) +------------------------ +* move the test to where it belongs +* fix the tests and the API to not handle conversion from CV_TYPE to Color type (does not make sense) +* make all the tests pass +* comply to the new Catkin API +* backport the C++ test from Fuerte +* Contributors: Vincent Rabaud + +1.9.5 (2012-09-15) +------------------ +* remove dependencies to the opencv2 ROS package +* Contributors: Vincent Rabaud + +1.9.4 (2012-09-13) +------------------ + +1.9.3 (2012-09-12) +------------------ +* update to nosetests +* Contributors: Vincent Rabaud + +1.9.2 (2012-09-07) +------------------ +* be more compliant to the latest catkin +* added catkin_project() to cv_bridge, image_geometry, and opencv_tests +* Contributors: Jonathan Binney, Vincent Rabaud + +1.9.1 (2012-08-28 22:06) +------------------------ +* remove a deprecated header +* Contributors: Vincent Rabaud + +1.9.0 (2012-08-28 14:29) +------------------------ +* cleanup by Jon Binney +* catkinized opencv_tests by Jon Binney +* remove the version check, let's trust OpenCV :) +* revert the removal of opencv2 +* finally get rid of opencv2 as it is a system dependency now +* bump REQUIRED version of OpenCV to 2.3.2, which is what's in ros-fuerte-opencv +* switch rosdep name to opencv2, to refer to ros-fuerte-opencv2 +* Fixing link lines for gtest against opencv. +* Adding opencv2 to all manifests, so that client packages may + not break when using them. +* baking in opencv debs and attempting a pre-release +* Another hack for prerelease to quiet test failures. +* Dissable a dubious opencv test. Temporary HACK. +* Changing to expect for more verbose failure. +* Minor change to test. +* Making this depend on libopencv-2.3-dev debian available in ros-shadow. +* mono16 -> bgr conversion tested and fixed in C +* Added Ubuntu platform tags to manifest +* Tuned for parc loop +* Demo of ROS node face detecton +* mono16 support, ticket `#2890 `_ +* Remove use of deprecated rosbuild macros +* cv_bridge split from opencv2 +* Name changes for opencv -> vision_opencv +* Validation for image message encoding +* utest changed to reflect rosimgtocv change to imgmsgtocv +* Add opencvpython as empty package +* New methods for cv image conversion +* Disabling tests on OSX, `#2769 `_ +* New Python CvBridge, rewrote C CvBridge, regression test for C and Python CvBridge +* Fix underscore problem, test 8UC3->BGR8, fix 8UC3->BGR8 +* New image format +* Image message and CvBridge change +* Rename rows,cols to height,width in Image message +* New node bbc for image testing +* Make executable +* Pong demo +* Missing utest.cpp +* New sensor_msgs::Image message +* Contributors: Vincent Rabaud, ethanrublee, gerkey, jamesb, jamesbowman, pantofaru, vrabaud, wheeler diff --git a/ros/vision_opencv/opencv_tests/CMakeLists.txt b/ros/vision_opencv/opencv_tests/CMakeLists.txt new file mode 100644 index 0000000..b45ba8c --- /dev/null +++ b/ros/vision_opencv/opencv_tests/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 2.8) +project(opencv_tests) + +find_package(catkin REQUIRED) +catkin_package() diff --git a/ros/vision_opencv/opencv_tests/launch/pong.launch b/ros/vision_opencv/opencv_tests/launch/pong.launch new file mode 100644 index 0000000..c571243 --- /dev/null +++ b/ros/vision_opencv/opencv_tests/launch/pong.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros/vision_opencv/opencv_tests/mainpage.dox b/ros/vision_opencv/opencv_tests/mainpage.dox new file mode 100644 index 0000000..ea72319 --- /dev/null +++ b/ros/vision_opencv/opencv_tests/mainpage.dox @@ -0,0 +1,119 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b opencv_tests is ... + + + + +\section codeapi Code API + + + +\section rosapi ROS API + + + + + + + + +*/ \ No newline at end of file diff --git a/ros/vision_opencv/opencv_tests/nodes/broadcast.py b/ros/vision_opencv/opencv_tests/nodes/broadcast.py new file mode 100755 index 0000000..0df824d --- /dev/null +++ b/ros/vision_opencv/opencv_tests/nodes/broadcast.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# Copyright (c) 2016, Tal Regev. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +import time +import math +import rospy +import cv2 + +import sensor_msgs.msg +from cv_bridge import CvBridge + + +# Send each image by iterate it from given array of files names to a given topic, +# as a regular and compressed ROS Images msgs. +class Source: + + def __init__(self, topic, filenames): + self.pub = rospy.Publisher(topic, sensor_msgs.msg.Image) + self.pub_compressed = rospy.Publisher(topic + "/compressed", sensor_msgs.msg.CompressedImage) + self.filenames = filenames + + def spin(self): + time.sleep(1.0) + cvb = CvBridge() + while not rospy.core.is_shutdown(): + cvim = cv2.imload(self.filenames[0]) + self.pub.publish(cvb.cv2_to_imgmsg(cvim)) + self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) + self.filenames = self.filenames[1:] + [self.filenames[0]] + time.sleep(1) + + +def main(args): + s = Source(args[1], args[2:]) + rospy.init_node('Source') + try: + s.spin() + rospy.spin() + outcome = 'test completed' + except KeyboardInterrupt: + print "shutting down" + outcome = 'keyboard interrupt' + rospy.core.signal_shutdown(outcome) + +if __name__ == '__main__': + main(sys.argv) diff --git a/ros/vision_opencv/opencv_tests/nodes/rosfacedetect.py b/ros/vision_opencv/opencv_tests/nodes/rosfacedetect.py new file mode 100755 index 0000000..9b072d7 --- /dev/null +++ b/ros/vision_opencv/opencv_tests/nodes/rosfacedetect.py @@ -0,0 +1,105 @@ +#!/usr/bin/python +""" +This program is demonstration for face and object detection using haar-like features. +The program finds faces in a camera image or video stream and displays a red box around them. + +Original C implementation by: ? +Python implementation by: Roman Stanchak, James Bowman +Updated: Copyright (c) 2016, Tal Regev. +""" + +import sys +import os +from optparse import OptionParser + +import rospy +import sensor_msgs.msg +from cv_bridge import CvBridge +import cv2 +import numpy + +# Parameters for haar detection +# From the API: +# The default parameters (scale_factor=2, min_neighbors=3, flags=0) are tuned +# for accurate yet slow object detection. For a faster operation on real video +# images the settings are: +# scale_factor=1.2, min_neighbors=2, flags=CV_HAAR_DO_CANNY_PRUNING, +# min_size= + opencv_tests + 1.11.15 + + Tests the enumerants of the ROS Image message, and functionally tests the Python and C++ implementations of CvBridge. + + James Bowman + Vincent Rabaud + BSD + http://wiki.ros.org/opencv_tests + + catkin + + cv_bridge + diff --git a/ros/vision_opencv/vision_opencv/CHANGELOG.rst b/ros/vision_opencv/vision_opencv/CHANGELOG.rst new file mode 100644 index 0000000..42732d1 --- /dev/null +++ b/ros/vision_opencv/vision_opencv/CHANGELOG.rst @@ -0,0 +1,165 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package vision_opencv +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.11.15 (2017-01-29) +-------------------- + +1.11.14 (2016-09-24) +-------------------- + +1.11.13 (2016-07-11) +-------------------- +* move opencv_apps to its own repo +* Contributors: Vincent Rabaud + +1.11.12 (2016-03-10) +-------------------- + +1.11.11 (2016-01-31) +-------------------- + +1.11.10 (2016-01-16) +-------------------- + +1.11.9 (2015-11-29) +------------------- +* Add opencv_apps to vision_opencv dependency +* Contributors: Ryohei Ueda + +1.11.8 (2015-07-15) +------------------- + +1.11.7 (2014-12-14) +------------------- + +1.11.6 (2014-11-16) +------------------- + +1.11.5 (2014-09-21) +------------------- + +1.11.4 (2014-07-27) +------------------- + +1.11.3 (2014-06-08) +------------------- + +1.11.2 (2014-04-28) +------------------- + +1.11.1 (2014-04-16) +------------------- + +1.11.0 (2014-02-15) +------------------- + +1.10.15 (2014-02-07) +-------------------- + +1.10.14 (2013-11-23 16:17) +-------------------------- +* Contributors: Vincent Rabaud + +1.10.13 (2013-11-23 09:19) +-------------------------- +* Contributors: Vincent Rabaud + +1.10.12 (2013-11-22) +-------------------- +* Contributors: Vincent Rabaud + +1.10.11 (2013-10-23) +-------------------- +* Contributors: Vincent Rabaud + +1.10.10 (2013-10-19) +-------------------- +* Contributors: Vincent Rabaud + +1.10.9 (2013-10-07) +------------------- +* Contributors: Vincent Rabaud + +1.10.8 (2013-09-09) +------------------- +* update email address +* Contributors: Vincent Rabaud + +1.10.7 (2013-07-17) +------------------- +* update to REP 0127 +* Contributors: Vincent Rabaud + +1.10.6 (2013-03-01) +------------------- + +1.10.5 (2013-02-11) +------------------- + +1.10.4 (2013-02-02) +------------------- + +1.10.3 (2013-01-17) +------------------- + +1.10.2 (2013-01-13) +------------------- + +1.10.1 (2013-01-10) +------------------- + +1.10.0 (2013-01-03) +------------------- + +1.9.15 (2013-01-02) +------------------- + +1.9.14 (2012-12-30) +------------------- + +1.9.13 (2012-12-15) +------------------- + +1.9.12 (2012-12-14) +------------------- + +1.9.11 (2012-12-10) +------------------- + +1.9.10 (2012-10-04) +------------------- +* the CMake file is useless +* add the missing CMake file +* re-add the meta-package +* Contributors: Vincent Rabaud + +1.9.9 (2012-10-01) +------------------ + +1.9.8 (2012-09-30) +------------------ + +1.9.7 (2012-09-28 21:07) +------------------------ + +1.9.6 (2012-09-28 15:17) +------------------------ + +1.9.5 (2012-09-15) +------------------ + +1.9.4 (2012-09-13) +------------------ + +1.9.3 (2012-09-12) +------------------ + +1.9.2 (2012-09-07) +------------------ + +1.9.1 (2012-08-28 22:06) +------------------------ + +1.9.0 (2012-08-28 14:29) +------------------------ diff --git a/ros/vision_opencv/vision_opencv/CMakeLists.txt b/ros/vision_opencv/vision_opencv/CMakeLists.txt new file mode 100644 index 0000000..8f1965d --- /dev/null +++ b/ros/vision_opencv/vision_opencv/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(vision_opencv) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/ros/vision_opencv/vision_opencv/package.xml b/ros/vision_opencv/vision_opencv/package.xml new file mode 100644 index 0000000..f26e49d --- /dev/null +++ b/ros/vision_opencv/vision_opencv/package.xml @@ -0,0 +1,23 @@ + + vision_opencv + 1.11.15 + Packages for interfacing ROS with OpenCV, a library of programming functions for real time computer vision. + Patrick Mihelich + James Bowman + Vincent Rabaud + BSD + + http://www.ros.org/wiki/vision_opencv + https://github.com/ros-perception/vision_opencv/issues + https://github.com/ros-perception/vision_opencv + + catkin + + + cv_bridge + image_geometry + + + + +