From 9898c0d969e78fc090e5717c86bbf27cb666dc78 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jul 2023 10:18:03 -0400 Subject: [PATCH 01/11] removed flir_camera_driver meta package --- flir_camera_driver/CHANGELOG.rst | 49 ------------------------------- flir_camera_driver/CMakeLists.txt | 4 --- flir_camera_driver/package.xml | 19 ------------ 3 files changed, 72 deletions(-) delete mode 100644 flir_camera_driver/CHANGELOG.rst delete mode 100644 flir_camera_driver/CMakeLists.txt delete mode 100644 flir_camera_driver/package.xml diff --git a/flir_camera_driver/CHANGELOG.rst b/flir_camera_driver/CHANGELOG.rst deleted file mode 100644 index ce444f71..00000000 --- a/flir_camera_driver/CHANGELOG.rst +++ /dev/null @@ -1,49 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package flir_camera_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.2.2 (2022-03-28) ------------------- - -0.2.1 (2022-03-21) ------------------- - -0.2.0 (2022-03-11) ------------------- -* Bump CMake version to avoid CMP0048 warning. -* Changes. -* Include flir_camera_description as dependency in metapackage -* Contributors: Joey Yang, Tony Baltovski - -0.2.5 (2023-01-06) ------------------- - -0.2.4 (2023-01-06) ------------------- - -0.2.3 (2022-04-19) ------------------- -* 0.2.2 -* Changes. -* 0.2.1 -* Changes. -* 0.2.0 -* Changes. -* Bump CMake version to avoid CMP0048 warning. -* Changes. -* Include flir_camera_description as dependency in metapackage -* Contributors: Joey Yang, Tony Baltovski - -0.1.3 (2018-09-25) ------------------- - -0.1.2 (2018-07-27) ------------------- - -0.1.1 (2018-07-25) ------------------- - -0.1.0 (2018-07-24) ------------------- -* Initial Release -* Contributors: Michael Hosmar diff --git a/flir_camera_driver/CMakeLists.txt b/flir_camera_driver/CMakeLists.txt deleted file mode 100644 index 5a3517a0..00000000 --- a/flir_camera_driver/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(flir_camera_driver) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/flir_camera_driver/package.xml b/flir_camera_driver/package.xml deleted file mode 100644 index 1225a2a4..00000000 --- a/flir_camera_driver/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - flir_camera_driver - 0.2.5 - A set of drivers for Flir cameras based on the Spinnaker SDK. - - Michael Hosmar - - BSD - - catkin - - spinnaker_camera_driver - flir_camera_description - - - - - From 60ae143f9fca7ddac5e67c95464b2576a13d8bfb Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Mon, 17 Jul 2023 10:22:08 -0400 Subject: [PATCH 02/11] deleted spinnaker ros2 driver, to be replaced by new version --- spinnaker_camera_driver/CHANGELOG.rst | 182 ----- spinnaker_camera_driver/CMakeLists.txt | 129 ---- spinnaker_camera_driver/cfg/Spinnaker.cfg | 313 -------- .../cmake/DownloadSpinnaker.cmake | 21 - .../cmake/TargetArch.cmake | 155 ---- .../cmake/download_spinnaker | 155 ---- spinnaker_camera_driver/cmake/install_files | 1 - .../cmake/modules/FindSpinnaker.cmake | 22 - spinnaker_camera_driver/debian/udev | 1 - .../spinnaker_camera_driver/SpinnakerCamera.h | 211 ----- .../include/spinnaker_camera_driver/camera.h | 170 ---- .../camera_exceptions.h | 82 -- .../include/spinnaker_camera_driver/cm3.h | 43 -- .../spinnaker_camera_driver/diagnostics.h | 154 ---- .../include/spinnaker_camera_driver/gh3.h | 43 -- .../spinnaker_camera_driver/set_property.h | 269 ------- spinnaker_camera_driver/launch/camera.launch | 113 --- .../launch/diagnostics.launch | 117 --- spinnaker_camera_driver/launch/stereo.launch | 101 --- .../launch/test_spinnaker.launch | 28 - spinnaker_camera_driver/nodelet_plugins.xml | 30 - spinnaker_camera_driver/package.xml | 65 -- spinnaker_camera_driver/params/analyzer.yml | 6 - .../src/SpinnakerCamera.cpp | 567 -------------- spinnaker_camera_driver/src/camera.cpp | 333 -------- spinnaker_camera_driver/src/cm3.cpp | 207 ----- spinnaker_camera_driver/src/diagnostics.cpp | 275 ------- spinnaker_camera_driver/src/gh3.cpp | 207 ----- spinnaker_camera_driver/src/node.cpp | 61 -- spinnaker_camera_driver/src/nodelet.cpp | 729 ------------------ .../src/spinnaker_test_node.cpp | 84 -- 31 files changed, 4874 deletions(-) delete mode 100644 spinnaker_camera_driver/CHANGELOG.rst delete mode 100644 spinnaker_camera_driver/CMakeLists.txt delete mode 100755 spinnaker_camera_driver/cfg/Spinnaker.cfg delete mode 100644 spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake delete mode 100644 spinnaker_camera_driver/cmake/TargetArch.cmake delete mode 100755 spinnaker_camera_driver/cmake/download_spinnaker delete mode 100644 spinnaker_camera_driver/cmake/install_files delete mode 100644 spinnaker_camera_driver/cmake/modules/FindSpinnaker.cmake delete mode 100644 spinnaker_camera_driver/debian/udev delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/camera_exceptions.h delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/cm3.h delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/gh3.h delete mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/set_property.h delete mode 100644 spinnaker_camera_driver/launch/camera.launch delete mode 100644 spinnaker_camera_driver/launch/diagnostics.launch delete mode 100644 spinnaker_camera_driver/launch/stereo.launch delete mode 100644 spinnaker_camera_driver/launch/test_spinnaker.launch delete mode 100644 spinnaker_camera_driver/nodelet_plugins.xml delete mode 100644 spinnaker_camera_driver/package.xml delete mode 100644 spinnaker_camera_driver/params/analyzer.yml delete mode 100644 spinnaker_camera_driver/src/SpinnakerCamera.cpp delete mode 100644 spinnaker_camera_driver/src/camera.cpp delete mode 100644 spinnaker_camera_driver/src/cm3.cpp delete mode 100644 spinnaker_camera_driver/src/diagnostics.cpp delete mode 100644 spinnaker_camera_driver/src/gh3.cpp delete mode 100644 spinnaker_camera_driver/src/node.cpp delete mode 100644 spinnaker_camera_driver/src/nodelet.cpp delete mode 100644 spinnaker_camera_driver/src/spinnaker_test_node.cpp diff --git a/spinnaker_camera_driver/CHANGELOG.rst b/spinnaker_camera_driver/CHANGELOG.rst deleted file mode 100644 index e9f70b34..00000000 --- a/spinnaker_camera_driver/CHANGELOG.rst +++ /dev/null @@ -1,182 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package spinnaker_camera_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.2.2 (2022-03-28) ------------------- -* Added new-line at EOF -* Spinnaker libraries are now all copied to usr/lib -* Reordered definitions to prevent compiler warnings -* Contributors: Luis Camero - -0.2.1 (2022-03-21) ------------------- -* Removed check for build/usr/lib which would cause build to skip Spinnaker SDK install -* Contributors: Luis Camero - -0.2.0 (2022-03-11) ------------------- -* Changes. -* Merge pull request `#91 `_ from luis-camero/noetic-devel - ROS Industrial CI -* Fixed all issues reported by roslint -* Updated file paths to /opt/spinnaker instead of /usr/spinnaker -* Updated download_spinnaker look-up table -* Merge pull request `#88 `_ from luis-camero/noetic-devel - Add readable check to SDK parameters -* Add readable check to SDK parameters -* URDF Description, Diagnostics, ISP Enable, and Launch Files (`#81 `_) - * Changes required to use GigE Blackfly S version - * Added blackfly mesh - * Added URDF of blackflys and CHANGELOG - * Added new_line at end of flir_blackflys.urdf.xacro - * Added DiagnosticAnalyzers and more detailed diagnostic messages - * Added ISP enable and disable config and updated camera launch file to be more descriptive - * Switched order of configuration to put ISP enable next to color encoding - * Updated config to include enumeration for Off, Once, Continuous parameters, and udpated diagnostics.launch - * Handled issue where no namespace prevents diagnostics_agg from loading from analyzer paramaters -* Branch to Support GigE Cameras (`#79 `_) - * Changes required to use GigE Blackfly S version - * Update SpinnakerCamera.cpp -* Add new parameter to apply an offset to image time stamps (`#56 `_) -* Fixes SpinnakerCamera teardown (`#16 `_) - * fixes error on destroying SpinnakerCamera with multiple cameras - * adds clarifying comment -* Add /opt/spinnaker to spinnaker discovery options (`#63 `_) -* increase maximum value of exposure_time/auto_exposure_time_upper_limit (`#55 `_) -* add option to set queue_size for ros publisher (`#54 `_) -* Added support for Grasshopper3. Identical to Chameleon3, split into separate files for clarity. (`#26 `_) -* Feature: horizontal and vertical image reverse (`#41 `_) - * Add horizontal/vertical inverse to reconfigure cfg - * Add ReverseX/ReverseY with setProperty - Co-authored-by: Fabian Schilling -* Update Spinnaker.cfg (`#50 `_) - Fix for correct spelling with capital letter for bool type -* Add auto exposure ROI parameters (`#52 `_) - * spinnaker_camera_driver: setProperty: report available enum values - Only done on failure. This helps to figure out which enum values are - available on a particular camera model. - * spinnaker_camera_driver: expose AE ROI parameters - This is highly useful when using fisheye lenses, which illuminate only - a circle in the center of the image. The AE gets confused by the black - regions around it and overexposes the image. - This also exposes the "AutoExposureLightingMode" parameter, which allows - the user to choose a lighting preset (front/back/normal). -* Fix/frame rate params (`#20 `_) - * [spinnaker_camera_driver] Fixed naming of frame rate control params - * [spinnaker_camera_driver] Format of mono and stereo launchfiles - * [spinnaker_camera_driver] Updated diagnostics launchfile -* Removed opencv as depend. (`#46 `_) -* Changed the download script to check for destination folder and moved unpack directory. (`#44 `_) -* Merge pull request `#42 `_ from civerachb-cpr/rpsw-185 - Fix Flycap & Spinnaker endpoints -* Create the directory if it doesn't exist -* Remove an unnecessary deb -* Spinnaker driver now successfully downloads & builds -* Start overhauling the spinnaker download script so it works with the correct endpoint & matches the general structure of the pointgrey_camera_driver -* Contributors: Adam Romlein, Chris I-B, Evan Bretl, Fabian Schilling, Ferdinand, Joseph Curtis, Luis Camero, Max Schwarz, Stephan, Tony Baltovski, Yoshua Nava, Yuki Furuta, luis-camero - -0.2.5 (2023-01-06) ------------------- -* Fixed arm64 folder name. -* Contributors: Tony Baltovski - -0.2.4 (2023-01-06) ------------------- -* Fixed typo in arm64 arch. -* Contributors: Tony Baltovski - -0.2.3 (2022-04-19) ------------------- -* Only install necessary libraries -* 0.2.2 -* Changes. -* Added new-line at EOF -* Spinnaker libraries are now all copied to usr/lib -* Reordered definitions to prevent compiler warnings -* 0.2.1 -* Changes. -* Removed check for build/usr/lib which would cause build to skip Spinnaker SDK install -* 0.2.0 -* Changes. -* Changes. -* Fixed all issues reported by roslint -* Updated file paths to /opt/spinnaker instead of /usr/spinnaker -* Updated download_spinnaker look-up table -* Add readable check to SDK parameters -* URDF Description, Diagnostics, ISP Enable, and Launch Files (`#81 `_) - * Changes required to use GigE Blackfly S version - * Added blackfly mesh - * Added URDF of blackflys and CHANGELOG - * Added new_line at end of flir_blackflys.urdf.xacro - * Added DiagnosticAnalyzers and more detailed diagnostic messages - * Added ISP enable and disable config and updated camera launch file to be more descriptive - * Switched order of configuration to put ISP enable next to color encoding - * Updated config to include enumeration for Off, Once, Continuous parameters, and udpated diagnostics.launch - * Handled issue where no namespace prevents diagnostics_agg from loading from analyzer paramaters -* Branch to Support GigE Cameras (`#79 `_) - * Changes required to use GigE Blackfly S version - * Update SpinnakerCamera.cpp -* Add new parameter to apply an offset to image time stamps (`#56 `_) -* Fixes SpinnakerCamera teardown (`#16 `_) - * fixes error on destroying SpinnakerCamera with multiple cameras - * adds clarifying comment -* Add /opt/spinnaker to spinnaker discovery options (`#63 `_) -* increase maximum value of exposure_time/auto_exposure_time_upper_limit (`#55 `_) -* add option to set queue_size for ros publisher (`#54 `_) -* Added support for Grasshopper3. Identical to Chameleon3, split into separate files for clarity. (`#26 `_) -* Feature: horizontal and vertical image reverse (`#41 `_) - * Add horizontal/vertical inverse to reconfigure cfg - * Add ReverseX/ReverseY with setProperty - Co-authored-by: Fabian Schilling -* Update Spinnaker.cfg (`#50 `_) - Fix for correct spelling with capital letter for bool type -* Add auto exposure ROI parameters (`#52 `_) - * spinnaker_camera_driver: setProperty: report available enum values - Only done on failure. This helps to figure out which enum values are - available on a particular camera model. - * spinnaker_camera_driver: expose AE ROI parameters - This is highly useful when using fisheye lenses, which illuminate only - a circle in the center of the image. The AE gets confused by the black - regions around it and overexposes the image. - This also exposes the "AutoExposureLightingMode" parameter, which allows - the user to choose a lighting preset (front/back/normal). -* Fix/frame rate params (`#20 `_) - * [spinnaker_camera_driver] Fixed naming of frame rate control params - * [spinnaker_camera_driver] Format of mono and stereo launchfiles - * [spinnaker_camera_driver] Updated diagnostics launchfile -* Removed opencv as depend. (`#46 `_) -* Changed the download script to check for destination folder and moved unpack directory. (`#44 `_) -* Create the directory if it doesn't exist -* Remove an unnecessary deb -* Spinnaker driver now successfully downloads & builds -* Start overhauling the spinnaker download script so it works with the correct endpoint & matches the general structure of the pointgrey_camera_driver -* Contributors: Adam Romlein, Chris I-B, Evan Bretl, Fabian Schilling, Ferdinand, Joseph Curtis, Luis Camero, Max Schwarz, Stephan, Tony Baltovski, Yoshua Nava, Yuki Furuta, luis-camero - -0.1.3 (2018-09-25) ------------------- -* Fix install targets when Spinnaker is installed locally. Tabs in FindSpinnaker. -* Add missing target (Cm3) and switch to find_package script. (`#11 `_) - * Add missing target (Cm3) and switch to find_package script. - * Clean up message. -* Adding support of feeding some camera diagnostics to the diagnostic a… (`#4 `_) - * Adding support of feeding some camera diagnostics to the diagnostic aggregator - * Creating a seperate diagnostics launch example -* Fix null conversion and unsigned comparison Warnings. -* Contributors: Helen Oleynikova, Michael Hosmar, mlowe-ascent - -0.1.2 (2018-07-27) ------------------- -* Add ARM Build Support (`#3 `_) - * Added ARM Build Support. -* Contributors: Michael Hosmar - -0.1.1 (2018-07-25) ------------------- -* Add opencv3 as build dependency. -* Contributors: Michael Hosmar - -0.1.0 (2018-07-24) ------------------- -* Initial Release -* Contributors: Michael Hosmar diff --git a/spinnaker_camera_driver/CMakeLists.txt b/spinnaker_camera_driver/CMakeLists.txt deleted file mode 100644 index f4ad7e6f..00000000 --- a/spinnaker_camera_driver/CMakeLists.txt +++ /dev/null @@ -1,129 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) - -set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Wextra") - - -project(spinnaker_camera_driver) - -find_package(catkin REQUIRED COMPONENTS - camera_info_manager diagnostic_updater dynamic_reconfigure - image_exposure_msgs image_transport nodelet roscpp sensor_msgs - wfov_camera_msgs -) - -generate_dynamic_reconfigure_options( - cfg/Spinnaker.cfg -) - -catkin_package(CATKIN_DEPENDS - image_exposure_msgs nodelet roscpp sensor_msgs wfov_camera_msgs -) - - -# If Spinnacker is already present, use the found version. If not, download it. -# We can't resolve this dependency using the usual rosdep means because -# the Point Grey EULA prohibits redistributing the headers or the packages which -# contains them. We work around this by downloading the archive directly from -# their website during this step in the build process. -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules") -find_package(Spinnaker QUIET) -if(NOT Spinnaker_FOUND) - message(STATUS "libSpinnaker not found in system library path") - include(cmake/DownloadSpinnaker.cmake) - download_spinnaker(Spinnaker_LIBRARIES Spinnaker_INCLUDE_DIRS) -endif() - -message(STATUS "libSpinnaker library: ${Spinnaker_LIBRARIES}") -message(STATUS "libSpinnaker include: ${Spinnaker_INCLUDE_DIRS}") - - -include_directories(SYSTEM - ${Spinnaker_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS}) -include_directories(include) - -add_library(SpinnakerCameraLib src/SpinnakerCamera.cpp) - -# Include the Spinnaker Libs -target_link_libraries(SpinnakerCameraLib - Camera - ${Spinnaker_LIBRARIES} - ${catkin_LIBRARIES}) - - -# Test Executable for Spinnaker -add_executable(spinnaker_test_node - src/spinnaker_test_node.cpp -) - -target_link_libraries(spinnaker_test_node - SpinnakerCameraLib - ${catkin_LIBRARIES} -) - - -add_dependencies(SpinnakerCameraLib ${PROJECT_NAME}_gencfg) - - -add_library(Camera src/camera.cpp) -target_link_libraries(Camera ${catkin_LIBRARIES}) -add_dependencies(Camera ${PROJECT_NAME}_gencfg) - -add_library(Cm3 src/cm3.cpp) -target_link_libraries(Cm3 Camera ${catkin_LIBRARIES}) -add_dependencies(Cm3 ${PROJECT_NAME}_gencfg) - -add_library(Gh3 src/gh3.cpp) -target_link_libraries(Gh3 Camera ${catkin_LIBRARIES}) -add_dependencies(Gh3 ${PROJECT_NAME}_gencfg) - -add_library(Diagnostics src/diagnostics.cpp) -target_link_libraries(Diagnostics Camera SpinnakerCameraLib ${catkin_LIBRARIES}) -add_dependencies(Diagnostics ${PROJECT_NAME}_gencfg) - -add_library(SpinnakerCameraNodelet src/nodelet.cpp) -target_link_libraries(SpinnakerCameraNodelet Diagnostics SpinnakerCameraLib Camera Gh3 Cm3 ${catkin_LIBRARIES}) - -add_executable(spinnaker_camera_node src/node.cpp) -target_link_libraries(spinnaker_camera_node SpinnakerCameraLib ${catkin_LIBRARIES}) -set_target_properties(spinnaker_camera_node PROPERTIES OUTPUT_NAME camera_node PREFIX "") - -install(TARGETS - SpinnakerCameraLib - SpinnakerCameraNodelet - Camera - Cm3 - Gh3 - Diagnostics - spinnaker_camera_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Redistributing the flycapture .so file is permitted by the SDK EULA: -# http://www.ptgrey.com/support/kb/data/PGR-FlyCap-SDK-LA.pdf -if(EXISTS ${CMAKE_CURRENT_BINARY_DIR}/usr/lib) - install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/usr/lib/ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - PATTERN libQt* EXCLUDE) -else() - get_filename_component(Spinnaker_LIB_DIR ${Spinnaker_LIBRARIES} DIRECTORY) - file(READ ${PROJECT_SOURCE_DIR}/cmake/install_files filenames) - foreach(filename ${filenames}) - install(FILES ${Spinnaker_LIB_DIR}/${filename} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - endforeach() -endif() - -install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -if (CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(launch/camera.launch) - - find_package(roslint REQUIRED) - set(ROSLINT_CPP_OPTS "--filter=-build/c++11") - roslint_cpp() - roslint_add_test() -endif() diff --git a/spinnaker_camera_driver/cfg/Spinnaker.cfg b/spinnaker_camera_driver/cfg/Spinnaker.cfg deleted file mode 100755 index ab976ec0..00000000 --- a/spinnaker_camera_driver/cfg/Spinnaker.cfg +++ /dev/null @@ -1,313 +0,0 @@ -#! /usr/bin/env python - -''' -This code was developed by the National Robotics Engineering Center (NREC), -part of Carnegie Mellon University's Robotics Institute. Its development was -funded by DARPA under the LS3 program and submitted for public release -on June 7th, 2012. Release was granted on August, 21st 2012 with Distribution Statement A -(Approved for Public Release, Distribution Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, National Robotics Engineering Center (NREC) -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 National Robotics Engineering Center (NREC) 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 HOLDER 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. -''' - -''' - @file Camera.cfg - @author Chad Rockey - @date July 11, 2011 - @brief Interface to Point Grey cameras - - @attention Copyright (C) 2011 - @attention National Robotics Engineering Center - @attention Carnegie Mellon University - @attention Adapted from ROS Camera1394.cfg by Jack O' Quin -''' - -#* Copyright (c) 2010, Jack O'Quin -#* All rights reserved. BSD. -#* -#* 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 the names of 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. -#*********************************************************** - -PACKAGE='spinnaker_camera_driver' - -from dynamic_reconfigure.parameter_generator_catkin import * - -class SensorLevels: - RECONFIGURE_RUNNING = 0 - RECONFIGURE_STOP = 1 - -gen = ParameterGenerator() - - -# Name Type Reconfiguration level Description Default Min Max -gen.add("acquisition_frame_rate", double_t, SensorLevels.RECONFIGURE_RUNNING, "User controlled acquisition frame rate in Hertz (frames per second).", 10, 0, 120) -gen.add("acquisition_frame_rate_enable", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables manual (True) and automatic (False) control of the aquisition frame rate", False) - -# Set Exposure -# Note: For the Auto Exposure feature, gain and/or exposure time must be set to Once or Continuous. -capture_modes = gen.enum([gen.const("AutoOff", str_t, "Off", ""), - gen.const("AutoOnce", str_t, "Once", ""), - gen.const("AutoContinuous", str_t, "Continuous", "")], - "Automatic mode: Off, Once, or Continuous.") -gen.add("exposure_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Sets the operation mode of the Exposure (Timed or TriggerWidth).", "Timed") -gen.add("exposure_auto", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the automatic exposure mode to: 'Off', 'Once' or 'Continuous'", "Continuous", edit_method=capture_modes) -gen.add("exposure_time", double_t, SensorLevels.RECONFIGURE_RUNNING, "Exposure time in microseconds when Exposure Mode is Timed and Exposure Auto is not Continuous.", 100.0, 0.0, 3000000.0) -gen.add("auto_exposure_time_upper_limit", double_t, SensorLevels.RECONFIGURE_RUNNING, "Upper Limit on Shutter Speed.", 5000, 0.0, 3000000.0) - - -# Gain Settings -gen.add("gain_selector", str_t, SensorLevels.RECONFIGURE_RUNNING, "Selects which gain to control. The All selection is a total amplification across all channels.", "All") -gen.add("auto_gain", str_t, SensorLevels.RECONFIGURE_RUNNING, "Gain state control. (Off, Once, Continuous)", "Continuous", edit_method=capture_modes) -gen.add("gain", double_t, SensorLevels.RECONFIGURE_RUNNING, "Controls the amplification of the video signal in dB.", 0, -10, 30) - -# Pan and Tilt not in Spinnaker Driver -# gen.add("pan", int_t, SensorLevels.RECONFIGURE_RUNNING, "Controls camera pan.", 0, -1000, 1000) -# gen.add("tilt", int_t, SensorLevels.RECONFIGURE_RUNNING, "Controls camera tilt.", 0, -1000, 1000) - - -# Brightness = Black Level -gen.add("brightness", double_t, SensorLevels.RECONFIGURE_RUNNING, "Also known as Black level offset. Refers to the output of the camera when not illuminated.", 1.7, 0.0, 10.0) - -gen.add("sharpening_enable", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables/disables the sharpening feature. Sharpening is disabled by default.", False) -gen.add("auto_sharpness", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables/disables the auto sharpening feature.", True) -gen.add("sharpness", double_t, SensorLevels.RECONFIGURE_RUNNING, "Controls the amount to sharpen a signal.", 1024.0, 0.0, 4095.0) -gen.add("sharpening_threshold", double_t, SensorLevels.RECONFIGURE_RUNNING, "Controls the minimum intensity gradient change to invoke sharpening. ", 0.1, 0.0, 0.25) - -gen.add("saturation_enable", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables/disables Saturation adjustment..", False) -gen.add("saturation", double_t, SensorLevels.RECONFIGURE_RUNNING, "Saturation.", 100.0, 0.0, 399.0) - -gen.add("gamma_enable", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables/disables gamma correction.", True) -gen.add("gamma", double_t, SensorLevels.RECONFIGURE_RUNNING, "Controls the gamma correction of pixel intensity.", 1.0, 0.5, 4.0) - - -# White Balance -gen.add("auto_white_balance", str_t, SensorLevels.RECONFIGURE_RUNNING, "White Balance compensates for color shifts caused by different lighting conditions.", "Off", edit_method=capture_modes) -gen.add("white_balance_blue_ratio", double_t, SensorLevels.RECONFIGURE_RUNNING, "White balance blue component.", 800, 0, 1023) -gen.add("white_balance_red_ratio", double_t, SensorLevels.RECONFIGURE_RUNNING, "White balance red component.", 550, 0, 1023) - - -# Image Format Control parameters -gen.add("image_format_roi_width", int_t, SensorLevels.RECONFIGURE_STOP, "Width of the image provided by the device (in pixels).", 0, 0, 65535) -gen.add("image_format_roi_height", int_t, SensorLevels.RECONFIGURE_STOP, "Height of the image provided by the device (in pixels).", 0, 0, 65535) -gen.add("image_format_x_offset", int_t, SensorLevels.RECONFIGURE_STOP, "Horizontal offset from the origin to the ROI (in pixels).", 0, 0, 65535) -gen.add("image_format_y_offset", int_t, SensorLevels.RECONFIGURE_STOP, "Vertical offset from the origin to the ROI (in pixels).", 0, 0, 65535) -gen.add("image_format_x_binning", int_t, SensorLevels.RECONFIGURE_STOP, "Horizontal Binning.", 1, 1, 8) -gen.add("image_format_y_binning", int_t, SensorLevels.RECONFIGURE_STOP, "Vertical Binning.", 1, 1, 8) -gen.add("image_format_x_decimation", int_t, SensorLevels.RECONFIGURE_STOP, "Horizontal Decimation.", 1, 1, 8) -gen.add("image_format_y_decimation", int_t, SensorLevels.RECONFIGURE_STOP, "Vertical Decimation.", 1, 1, 8) -gen.add("image_format_x_reverse", bool_t, SensorLevels.RECONFIGURE_STOP, "Horizontal Reverse.", False) -gen.add("image_format_y_reverse", bool_t, SensorLevels.RECONFIGURE_STOP, "Vertical Reverse.", False) - -# Image Color Coding: Format of the pixel provided by the camera. -codings = gen.enum([gen.const("Mono8", str_t, "Mono8", ""), - gen.const("Mono16", str_t, "Mono16", ""), - - gen.const("RGB8Packed", str_t, "RGB8Packed", ""), - - gen.const("BayerGR8", str_t, "BayerGR8", ""), - gen.const("BayerRG8", str_t, "BayerRG8", ""), - gen.const("BayerGB8", str_t, "BayerGB8", ""), - gen.const("BayerBG8", str_t, "BayerBG8", ""), - - gen.const("BayerGR16", str_t, "BayerGR16", ""), - gen.const("BayerRG16", str_t, "BayerRG16", ""), - gen.const("BayerGB16", str_t, "BayerGB16", ""), - gen.const("BayerBG16", str_t, "BayerBG16", ""), - - gen.const("Mono12Packed", str_t, "Mono12Packed", ""), - - gen.const("BayerGR12Packed", str_t, "BayerGR12Packed", ""), - gen.const("BayerRG12Packed", str_t, "BayerRG12Packed", ""), - gen.const("BayerGB12Packed", str_t, "BayerGB12Packed", ""), - gen.const("BayerBG12Packed", str_t, "BayerBG12Packed", ""), - - gen.const("YUV411Packed", str_t, "YUV411Packed", ""), - gen.const("YUV422Packed", str_t, "YUV422Packed", ""), - gen.const("YUV444Packed", str_t, "YUV444Packed", ""), - - gen.const("Mono12p", str_t, "Mono12p", ""), - - gen.const("BayerGR12p", str_t, "BayerGR12p", ""), - gen.const("BayerRG12p", str_t, "BayerRG12p", ""), - gen.const("BayerGB12p", str_t, "BayerGB12p", ""), - gen.const("BayerBG12p", str_t, "BayerBG12p", ""), - - - gen.const("YCbCr8", str_t, "YCbCr8", ""), - gen.const("YCbCr422_8", str_t, "YCbCr422_8", ""), - gen.const("YCbCr411_8", str_t, "YCbCr411_8", ""), - - gen.const("BGR8", str_t, "BGR8", ""), - gen.const("BGRa8", str_t, "BGRa8", "")], - - "Image Color Coding: Format of the pixel provided by the camera.") - -gen.add("image_format_color_coding", str_t, SensorLevels.RECONFIGURE_STOP, "Image Color coding", "Mono8", edit_method = codings) -gen.add("isp_enable", bool_t, SensorLevels.RECONFIGURE_STOP, "Controls whether the image processing core is used for optional pixel format mode", True) - - -# Trigger parameters -# enable_trigger specified by "TriggerMode" in Spinnaker: Controls whether or not trigger is active. -gen.add("enable_trigger", str_t, SensorLevels.RECONFIGURE_RUNNING, "Enable the external triggering mode.", "Off") - - - - - -trigger_selector_options = gen.enum([gen.const("AcquisitionStart", str_t, "AcquisitionStart", ""), - gen.const("FrameStart", str_t, "FrameStart", ""), - gen.const("FrameBurstStart", str_t, "FrameBurstStart", "")], - "Trigger Types") - -gen.add("trigger_selector", str_t, SensorLevels.RECONFIGURE_RUNNING, "Selects the type of trigger to configure.", "FrameStart", edit_method = trigger_selector_options) - - -# trigger_modes specified by "TriggerActivation" in Spinnaker: Specifies the activation mode of the trigger. -trigger_modes = gen.enum([gen.const("LevelLow", str_t, "LevelLow", ""), - gen.const("LevelHigh", str_t, "LevelHigh", ""), - gen.const("FallingEdge", str_t, "FallingEdge", ""), - gen.const("RisingEdge", str_t, "RisingEdge", ""), - gen.const("AnyEdge", str_t, "AnyEdge", "")], - "Trigger Activation Modes") - -gen.add("trigger_activation_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Trigger Activiation Modes", "FallingEdge", edit_method = trigger_modes) - -trigger_sources = gen.enum([gen.const("Software", str_t, "Software", ""), - gen.const("Line0", str_t, "Line0", ""), - gen.const("Line1", str_t, "Line1", ""), - gen.const("Line2", str_t, "Line2", ""), - gen.const("Line3", str_t, "Line3", ""), - - gen.const("UserOutput0", str_t, "UserOutput0", ""), - gen.const("UserOutput1", str_t, "UserOutput1", ""), - gen.const("UserOutput2", str_t, "UserOutput2", ""), - gen.const("UserOutput3", str_t, "UserOutput3", ""), - - gen.const("Counter0Start", str_t, "Counter0Start", ""), - gen.const("Counter1Start", str_t, "Counter1Start", ""), - gen.const("Counter0End", str_t, "Counter0End", ""), - gen.const("Counter1End", str_t, "Counter1End", ""), - - gen.const("LogicBlock0", str_t, "LogicBlock0", ""), - gen.const("LogicBlock1", str_t, "LogicBlock1", ""), - gen.const("Action0", str_t, "Action0", "")], - - "Software and Hardware Trigger Sources") - -# Trigger Source: Specifies the internal signal or physical input line to use as the trigger source. -gen.add("trigger_source", str_t, SensorLevels.RECONFIGURE_RUNNING, "Trigger Sources", "Line0", edit_method = trigger_sources) - - - - -# Trigger Overlap: Specifies the overlap mode of the trigger. -trigger_overlap_modes = gen.enum([gen.const("Off", str_t, "Off", ""), - gen.const("ReadOut", str_t, "ReadOut", ""), - gen.const("PreviousFrame", str_t, "PreviousFrame", "")], - "Trigger Overlap Modes") - -gen.add("trigger_overlap_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Trigger Overlap Modes", "ReadOut", edit_method = trigger_overlap_modes) - - - -# gen.add("enable_trigger_delay", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Whether Trigger Delay is active.", False) -# gen.add("trigger_delay", double_t, SensorLevels.RECONFIGURE_RUNNING, "The trigger delay to wait once triggered (in seconds).", 0.0, 0.0, 1.0) -# gen.add("trigger_parameter", int_t, SensorLevels.RECONFIGURE_RUNNING, "Trigger mode parameter. Varies based on mode.", 0, -32768, 32767) - -line_sources = gen.enum([gen.const("LineSource_Off", str_t, "Off", ""), - - gen.const("LineSource_Line0", str_t, "Line0", ""), - gen.const("LineSource_Line1", str_t, "Line1", ""), - gen.const("LineSource_Line2", str_t, "Line2", ""), - gen.const("LineSource_Line3", str_t, "Line3", ""), - - gen.const("LineSource_UserOutput0", str_t, "UserOutput0", ""), - gen.const("LineSource_UserOutput1", str_t, "UserOutput1", ""), - gen.const("LineSource_UserOutput2", str_t, "UserOutput2", ""), - gen.const("LineSource_UserOutput3", str_t, "UserOutput3", ""), - - gen.const("LineSource_Counter0Active", str_t, "Counter0Active", ""), - gen.const("LineSource_Counter1Active", str_t, "Counter1Active", ""), - gen.const("LineSource_LogicBlock0", str_t, "LogicBlock0", ""), - gen.const("LineSource_LogicBlock1", str_t, "LogicBlock1", ""), - - gen.const("LineSource_ExposureActive", str_t, "ExposureActive", ""), - gen.const("LineSource_FrameTriggerWait", str_t, "FrameTriggerWait", ""), - gen.const("LineSource_SerialPort0", str_t, "SerialPort0", ""), - gen.const("LineSource_PPSSignal", str_t, "PPSSignal", ""), - gen.const("LineSource_AllPixel", str_t, "AllPixel", ""), - gen.const("LineSource_AnyPixel", str_t, "AnyPixel", "")], - - "Selects which internal acquisition or I/O source signal to output on the selected line. LineMode must be Output") - -gen.add("line_source", str_t, SensorLevels.RECONFIGURE_RUNNING, "Line Sources", "Off", edit_method = line_sources) - -trigger_sources = gen.enum([gen.const("LineSelector_Line0", str_t, "Line0", ""), - gen.const("LineSelector_Line1", str_t, "Line1", ""), - gen.const("LineSelector_Line2", str_t, "Line2", ""), - gen.const("LineSelector_Line3", str_t, "Line3", "")], - - "Selects the physical line (or pin) of the external device connector to configure.") - -gen.add("line_selector", str_t, SensorLevels.RECONFIGURE_RUNNING, "Line Selector", "Line0", edit_method = trigger_sources) - -line_modes = gen.enum([gen.const("Input", str_t, "Input", ""), - gen.const("Output", str_t, "Output", "")], - "Line Mode") - -gen.add("line_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Line Mode", "Input", edit_method = line_modes) - - -# Auto algorithm parameters -gen.add("auto_exposure_roi_offset_x", int_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure ROI X offset.", 0, 0, 65535) -gen.add("auto_exposure_roi_offset_y", int_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure ROI Y offset.", 0, 0, 65535) -gen.add("auto_exposure_roi_width", int_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure ROI width.", 0, 0, 65535) -gen.add("auto_exposure_roi_height", int_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure ROI height.", 0, 0, 65535) - -auto_lighting_mode = gen.enum([ - gen.const("Normal", str_t, "Normal", "Normal"), - gen.const("Frontlight", str_t, "Frontlight", "Front Lighting"), - gen.const("Backlight", str_t, "Backlight", "Back Lighting") -], "Auto algorithms lighting modes") -gen.add("auto_exposure_lighting_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure lighting mode.", "Normal", edit_method=auto_lighting_mode) - -# Other -gen.add("time_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "Time offset to add to image time stamps.", 0.0, -5.0, 5.0) - - -exit(gen.generate(PACKAGE, "spinnaker_camera_driver", "Spinnaker")) diff --git a/spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake b/spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake deleted file mode 100644 index 93e8efeb..00000000 --- a/spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake +++ /dev/null @@ -1,21 +0,0 @@ -function(download_spinnaker FLIR_LIB_VAR FLIR_INCLUDE_DIR_VAR) - if(NOT UNIX) - message(FATAL_ERROR "Downloading libSpinnaker for non-linux systems not supported") - endif() - - execute_process( - COMMAND lsb_release -cs - OUTPUT_VARIABLE OS_CODE_NAME - OUTPUT_STRIP_TRAILING_WHITESPACE) - - include(cmake/TargetArch.cmake) - target_architecture(FLIR_ARCH) - set(FLIR_DIR ${CMAKE_CURRENT_BINARY_DIR}/usr/lib) - set(DOWNLOAD_SCRIPT "${PROJECT_SOURCE_DIR}/cmake/download_spinnaker") - - message(STATUS "Running download_spinnaker script with arguments: ${FLIR_ARCH} ${FLIR_DIR} ${OS_CODE_NAME}") - execute_process( - COMMAND ${DOWNLOAD_SCRIPT} ${FLIR_ARCH} "${FLIR_DIR}" ${OS_CODE_NAME}) - set(${FLIR_LIB_VAR} "${CMAKE_BINARY_DIR}/usr/lib/libSpinnaker.so" PARENT_SCOPE) - set(${FLIR_INCLUDE_DIR_VAR} "${CMAKE_BINARY_DIR}/opt/spinnaker/include/" PARENT_SCOPE) -endfunction() diff --git a/spinnaker_camera_driver/cmake/TargetArch.cmake b/spinnaker_camera_driver/cmake/TargetArch.cmake deleted file mode 100644 index 78b1c50a..00000000 --- a/spinnaker_camera_driver/cmake/TargetArch.cmake +++ /dev/null @@ -1,155 +0,0 @@ -# Source: https://raw.github.com/petroules/solar-cmake/master/TargetArch.cmake - -# Copyright (c) 2012 Petroules Corporation. 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. 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. - -# Based on the Qt 5 processor detection code, so should be very accurate -# https://qt.gitorious.org/qt/qtbase/blobs/master/src/corelib/global/qprocessordetection.h -# Currently handles arm (v5, v6, v7), x86 (32/64), ia64, and ppc (32/64) - -# Regarding POWER/PowerPC, just as is noted in the Qt source, -# "There are many more known variants/revisions that we do not handle/detect." - -set(archdetect_c_code " -#if defined(__aarch64__) - #error cmake_ARCH armv8 -#elif defined(__arm__) || defined(__TARGET_ARCH_ARM) - #if defined(__ARM_ARCH_7__) \\ - || defined(__ARM_ARCH_7A__) \\ - || defined(__ARM_ARCH_7R__) \\ - || defined(__ARM_ARCH_7M__) \\ - || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7) - #error cmake_ARCH armv7 - #elif defined(__ARM_ARCH_6__) \\ - || defined(__ARM_ARCH_6J__) \\ - || defined(__ARM_ARCH_6T2__) \\ - || defined(__ARM_ARCH_6Z__) \\ - || defined(__ARM_ARCH_6K__) \\ - || defined(__ARM_ARCH_6ZK__) \\ - || defined(__ARM_ARCH_6M__) \\ - || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6) - #error cmake_ARCH armv6 - #elif defined(__ARM_ARCH_5TEJ__) \\ - || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5) - #error cmake_ARCH armv5 - #else - #error cmake_ARCH arm - #endif -#elif defined(__i386) || defined(__i386__) || defined(_M_IX86) - #error cmake_ARCH i386 -#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64) - #error cmake_ARCH x86_64 -#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64) - #error cmake_ARCH ia64 -#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\ - || defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\ - || defined(_M_MPPC) || defined(_M_PPC) - #if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__) - #error cmake_ARCH ppc64 - #else - #error cmake_ARCH ppc - #endif -#endif - -#error cmake_ARCH unknown -") - -# Set ppc_support to TRUE before including this file or ppc and ppc64 -# will be treated as invalid architectures since they are no longer supported by Apple - -function(target_architecture output_var) - if(APPLE AND CMAKE_OSX_ARCHITECTURES) - # On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set - # First let's normalize the order of the values - - # Note that it's not possible to compile PowerPC applications if you are using - # the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we - # disable it by default - # See this page for more information: - # http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4 - - # Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime. - # On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise. - - foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES}) - if("${osx_arch}" STREQUAL "ppc" AND ppc_support) - set(osx_arch_ppc TRUE) - elseif("${osx_arch}" STREQUAL "i386") - set(osx_arch_i386 TRUE) - elseif("${osx_arch}" STREQUAL "x86_64") - set(osx_arch_x86_64 TRUE) - elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support) - set(osx_arch_ppc64 TRUE) - else() - message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}") - endif() - endforeach() - - # Now add all the architectures in our normalized order - if(osx_arch_ppc) - list(APPEND ARCH ppc) - endif() - - if(osx_arch_i386) - list(APPEND ARCH i386) - endif() - - if(osx_arch_x86_64) - list(APPEND ARCH x86_64) - endif() - - if(osx_arch_ppc64) - list(APPEND ARCH ppc64) - endif() - else() - file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}") - - enable_language(C) - - # Detect the architecture in a rather creative way... - # This compiles a small C program which is a series of ifdefs that selects a - # particular #error preprocessor directive whose message string contains the - # target architecture. The program will always fail to compile (both because - # file is not a valid C program, and obviously because of the presence of the - # #error preprocessor directives... but by exploiting the preprocessor in this - # way, we can detect the correct target architecture even when cross-compiling, - # since the program itself never needs to be run (only the compiler/preprocessor) - try_run( - run_result_unused - compile_result_unused - "${CMAKE_BINARY_DIR}" - "${CMAKE_BINARY_DIR}/arch.c" - COMPILE_OUTPUT_VARIABLE ARCH - CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES} - ) - - # Parse the architecture name from the compiler output - string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}") - - # Get rid of the value marker leaving just the architecture name - string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}") - - # If we are compiling with an unknown architecture this variable should - # already be set to "unknown" but in the case that it's empty (i.e. due - # to a typo in the code), then set it to unknown - if (NOT ARCH) - set(ARCH unknown) - endif() - endif() - - set(${output_var} "${ARCH}" PARENT_SCOPE) -endfunction() diff --git a/spinnaker_camera_driver/cmake/download_spinnaker b/spinnaker_camera_driver/cmake/download_spinnaker deleted file mode 100755 index 8020608b..00000000 --- a/spinnaker_camera_driver/cmake/download_spinnaker +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python3 -# -# Software License Agreement (BSD) -# -# @author Mike Purvis -# @author Chris Iverach-Brereton -# @copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. -# @usage download_spinnaker {arch} {dir} {os-code-name} -# e.g. download_spinnaker x86_64 /path/to/somewhere xenial -# -# 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 Clearpath Robotics 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 HOLDER 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 http.cookiejar -import io -import logging -import shutil -import subprocess -import sys -import tarfile -import urllib -import os -import glob - -logging.basicConfig(level=logging.INFO) - -URL_TEMPLATE = URL_TEMPLATES = { - 'deb': 'https://packages.clearpathrobotics.com/stable/flir/Spinnaker/Ubuntu{version}/spinnaker-2.2.0.48-Ubuntu{version}-{arch}-pkg.tar.gz', - 'src': None # if we ever have non-deb archives that require manual extraction, those package URLS will go here. -} - -ARCHS = { - 'x86_64': { - 'linux_arch': 'amd64', - 'current': '20.04', - 'type': 'deb', - 'folder_name': 'spinnaker-2.2.0.48-amd64', - 'shared_library': 'opt/spinnaker/lib/', - 'debs':[ - 'libgentl_2.2.0.48_amd64.deb', - 'libspinnaker_2.2.0.48_amd64.deb', - 'libspinnaker-c_2.2.0.48_amd64.deb', - 'libspinnaker-c-dev_2.2.0.48_amd64.deb', - 'libspinnaker-dev_2.2.0.48_amd64.deb', - 'libspinvideo_2.2.0.48_amd64.deb', - 'libspinvideo-c_2.2.0.48_amd64.deb', - 'libspinvideo-c-dev_2.2.0.48_amd64.deb', - 'libspinvideo-dev_2.2.0.48_amd64.deb', - 'spinnaker_2.2.0.48_amd64.deb', - 'spinnaker-doc_2.2.0.48_amd64.deb', - 'spinupdate_2.2.0.48_amd64.deb', - 'spinupdate-dev_2.2.0.48_amd64.deb', - 'spinview-qt_2.2.0.48_amd64.deb', - 'spinview-qt-dev_2.2.0.48_amd64.deb' - ] - }, - 'armv8': { - 'linux_arch': 'arm64', - 'current': '20.04', - 'type': 'deb', - 'shared_library': 'opt/spinnaker/lib/', - 'folder_name': 'spinnaker-2.2.0.48-arm64', - 'debs': [ - 'libgentl_2.2.0.48_arm64.deb', - 'libspinnaker_2.2.0.48_arm64.deb', - 'libspinnaker-c_2.2.0.48_arm64.deb', - 'libspinnaker-c-dev_2.2.0.48_arm64.deb', - 'libspinnaker-dev_2.2.0.48_arm64.deb', - 'libspinvideo_2.2.0.48_arm64.deb', - 'libspinvideo-c_2.2.0.48_arm64.deb', - 'libspinvideo-c-dev_2.2.0.48_arm64.deb', - 'libspinvideo-dev_2.2.0.48_arm64.deb', - 'spinnaker_2.2.0.48_arm64.deb', - 'spinnaker-doc_2.2.0.48_arm64.deb', - 'spinupdate_2.2.0.48_arm64.deb', - 'spinupdate-dev_2.2.0.48_arm64.deb', - 'spinview-qt_2.2.0.48_arm64.deb', - 'spinview-qt-dev_2.2.0.48_arm64.deb' - ] - } -} - - -OS_LIBRARY_VERSION = { - 'focal': 'current' -} - -arch = sys.argv[1] -destination_folder = sys.argv[2] -os_code_name = sys.argv[3] - -os_version = OS_LIBRARY_VERSION[os_code_name] -archive_url = URL_TEMPLATES[ARCHS[arch]['type']].format( - arch=ARCHS[arch]['linux_arch'], - version=ARCHS[arch][os_version]) -folder_name = ARCHS[arch]['folder_name'] -shared_library = ARCHS[arch]['shared_library'] - - -logging.info("CPU architecture is %s", arch) -logging.info("OS code name is %s", os_code_name) -logging.info("Destination folder is %s", destination_folder) - -if not os.path.exists(os.path.join(os.getcwd(), folder_name)): - cj = http.cookiejar.CookieJar() - opener = urllib.request.build_opener(urllib.request.HTTPCookieProcessor(cj)) - opener.addheaders = [ - ('User-agent', 'Mozilla/5.0'), - ('Referer', 'https://www.ptgrey.com')] - - logging.info("Downloading SDK archive from {0}...".format(archive_url)) - resp = opener.open(archive_url) - - logging.info("Unpacking tarball.") - with tarfile.open(mode="r:gz", fileobj=io.BytesIO(resp.read())) as tar: - tar.extractall() - - logging.info("Unpacking debs.") - debs = glob.glob(os.path.join(os.getcwd(), folder_name, "*.deb")) - unpack_dir = os.path.join(os.getcwd()) - if not os.path.exists(unpack_dir): - os.makedirs(unpack_dir) - for deb in debs: - logging.info('Extracting {0}'.format(deb)) - subprocess.call(['dpkg', '--extract', deb, unpack_dir]) - - # Copy the shared libraries into the output folder - logging.info('Copying libraries to {0}'.format(destination_folder)) - if not os.path.exists(destination_folder): - os.makedirs(destination_folder) - libs = glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libSpinnaker*.so*")) - libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libG*.so*")) - libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "lib*Parser*.so*")) - libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libLog*.so*")) - libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libNodeMapData*.so*")) - for lib in libs: - logging.info('Copying: {}'.format(os.path.basename(lib))) - shutil.copyfile(lib, os.path.join(destination_folder, os.path.basename(lib))) -else: - logging.info("Previous installation found, skipping...") diff --git a/spinnaker_camera_driver/cmake/install_files b/spinnaker_camera_driver/cmake/install_files deleted file mode 100644 index 94fdfa4f..00000000 --- a/spinnaker_camera_driver/cmake/install_files +++ /dev/null @@ -1 +0,0 @@ -libGCBase_gcc540_v3_0.so;libGenApi_gcc540_v3_0.so;liblog4cpp_gcc540_v3_0.so;libLog_gcc540_v3_0.so;libMathParser_gcc540_v3_0.so;libNodeMapData_gcc540_v3_0.so;libSpinnaker.so;libSpinnaker.so.1;libSpinnaker.so.1.13.0.31;libXmlParser_gcc540_v3_0.so;SFNC_GenTLDataStream_GigE_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLDataStream_Usb3_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLDevice_GigE_Version_1_0_0_Schema_1_1.xml;;SFNC_GenTLDevice_Reference_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLDevice_Usb3_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLInterface_GigE_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLInterface_Reference_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLInterface_Usb3_Version_1_0_0_Schema_1_1.xml; \ No newline at end of file diff --git a/spinnaker_camera_driver/cmake/modules/FindSpinnaker.cmake b/spinnaker_camera_driver/cmake/modules/FindSpinnaker.cmake deleted file mode 100644 index a1bede6b..00000000 --- a/spinnaker_camera_driver/cmake/modules/FindSpinnaker.cmake +++ /dev/null @@ -1,22 +0,0 @@ -unset(Spinnaker_FOUND) -unset(Spinnaker_INCLUDE_DIRS) -unset(Spinnaker_LIBRARIES) - -find_path(Spinnaker_INCLUDE_DIRS NAMES - Spinnaker.h - PATHS - /usr/include/spinnaker/ - /usr/local/include/spinnaker/ - /opt/spinnaker/include/ -) - -find_library(Spinnaker_LIBRARIES NAMES Spinnaker - PATHS - /usr/lib - /usr/local/lib - /opt/spinnaker/lib -) - -if (Spinnaker_INCLUDE_DIRS AND Spinnaker_LIBRARIES) - set(Spinnaker_FOUND 1) -endif (Spinnaker_INCLUDE_DIRS AND Spinnaker_LIBRARIES) diff --git a/spinnaker_camera_driver/debian/udev b/spinnaker_camera_driver/debian/udev deleted file mode 100644 index 0bf6b908..00000000 --- a/spinnaker_camera_driver/debian/udev +++ /dev/null @@ -1 +0,0 @@ -SUBSYSTEM=="usb", ATTRS{idVendor}=="1e10", MODE="0666", GROUP="flirimaging" diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h deleted file mode 100644 index 4cd9251b..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h +++ /dev/null @@ -1,211 +0,0 @@ -/* -This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie -Mellon University. -Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012. -Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution -Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, Carnegie Mellon University. All rights reserved. -Copyright (c) 2018, Clearpath Robotics, 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 Carnegie Mellon University 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 HOLDER 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. -*/ - -/*-*-C++-*-*/ -/** - @file SpinnakerCamera.h - @author Chad Rockey - @date July 11, 2011 - @brief Interface to Point Grey cameras - - @attention Copyright (C) 2011 - @attention National Robotics Engineering Center - @attention Carnegie Mellon University -*/ - -#ifndef SPINNAKER_CAMERA_DRIVER_SPINNAKERCAMERA_H -#define SPINNAKER_CAMERA_DRIVER_SPINNAKERCAMERA_H - -#include // ROS message header for Image -#include // ROS header for the different supported image encoding types -#include -#include - -#include -#include -#include -#include - -// Header generated by dynamic_reconfigure -#include -#include "spinnaker_camera_driver/camera.h" -#include "spinnaker_camera_driver/cm3.h" -#include "spinnaker_camera_driver/gh3.h" -#include "spinnaker_camera_driver/set_property.h" - -// Spinnaker SDK -#include "Spinnaker.h" -#include "SpinGenApi/SpinnakerGenApi.h" - -namespace spinnaker_camera_driver -{ -class SpinnakerCamera -{ -public: - SpinnakerCamera(); - ~SpinnakerCamera(); - - /*! - * \brief Function that allows reconfiguration of the camera. - * - * This function handles a reference of a camera_library::CameraConfig object and - * configures the camera as close to the given values as possible. As a function for - * dynamic_reconfigure, values that are not valid are changed by the driver and can - * be inspected after this function ends. - * This function will stop and restart the camera when called on a SensorLevels::RECONFIGURE_STOP level. - * \param config camera_library::CameraConfig object passed by reference. Values will be changed to those the driver - * is currently using. - * \param level Reconfiguration level. See constants below for details. - * - * \return Returns true when the configuration could be applied without modification. - */ - void setNewConfiguration(const spinnaker_camera_driver::SpinnakerConfig& config, const uint32_t& level); - - /** Parameters that need a sensor to be stopped completely when changed. */ - static const uint8_t LEVEL_RECONFIGURE_CLOSE = 3; - - /** Parameters that need a sensor to stop streaming when changed. */ - static const uint8_t LEVEL_RECONFIGURE_STOP = 1; - - /** Parameters that can be changed while a sensor is streaming. */ - static const uint8_t LEVEL_RECONFIGURE_RUNNING = 0; - - /*! - * \brief Function that connects to a specified camera. - * - * Will connect to the camera specified in the setDesiredCamera(std::string id) call. If setDesiredCamera is not - * called first - * this will connect to the first camera. Connecting to the first camera is not recommended for multi-camera or - * production systems. - * This function must be called before setNewConfiguration() or start()! - */ - void connect(); - - /*! - * \brief Disconnects from the camera. - * - * Disconnects the camera and frees it. - */ - void disconnect(); - - /*! - * \brief Starts the camera loading data into its buffer. - * - * This function will start the camera capturing images and loading them into the buffer. To retrieve images, - * grabImage must be called. - */ - void start(); - - /*! - * \brief Stops the camera loading data into its buffer. - * - * This function will stop the camera capturing images and loading them into the buffer. - */ - void stop(); - - /*! - * \brief Loads the raw data from the cameras buffer. - * - * This function will load the raw data from the buffer and place it into a sensor_msgs::Image. - * \param image sensor_msgs::Image that will be filled with the image currently in the buffer. - * \param frame_id The name of the optical frame of the camera. - */ - void grabImage(sensor_msgs::Image* image, const std::string& frame_id); - - /*! - * \brief Will set grabImage timeout for the camera. - * - * This function will set the time required for grabCamera to throw a timeout exception. Must be called after - * connect(). - * \param timeout The desired timeout value (in seconds) - * - */ - // TODO(mhosmar): Implement later - void setTimeout(const double& timeout); - - /*! - * \brief Used to set the serial number for the camera you wish to connect to. - * - * Sets the desired serial number. If this value is not set, the driver will try to connect to the first camera on the - * bus. - * This function should be called before connect(). - * \param id serial number for the camera. Should be something like 10491081. - */ - void setDesiredCamera(const uint32_t& id); - - void setGain(const float& gain); - int getHeightMax(); - int getWidthMax(); - bool readableProperty(const Spinnaker::GenICam::gcstring property_name); - Spinnaker::GenApi::CNodePtr readProperty(const Spinnaker::GenICam::gcstring property_name); - - uint32_t getSerial() - { - return serial_; - } - -private: - uint32_t serial_; ///< A variable to hold the serial number of the desired camera. - - Spinnaker::SystemPtr system_; - Spinnaker::CameraList camList_; - Spinnaker::CameraPtr pCam_; - - // TODO(mhosmar) use std::shared_ptr - Spinnaker::GenApi::INodeMap* node_map_; - std::shared_ptr camera_; - - Spinnaker::ChunkData image_metadata_; - - std::mutex mutex_; ///< A mutex to make sure that we don't try to grabImages while reconfiguring or vice versa. - volatile bool captureRunning_; ///< A status boolean that checks if the camera has been started and is loading images - /// into its buffer. - - /// If true, camera is currently running in color mode, otherwise camera is running in mono mode - bool isColor_; - - // For GigE cameras: - /// If true, GigE packet size is automatically determined, otherwise packet_size_ is used: - bool auto_packet_size_; - /// GigE packet size: - unsigned int packet_size_; - /// GigE packet delay: - unsigned int packet_delay_; - - uint64_t timeout_; - - // This function configures the camera to add chunk data to each image. It does - // this by enabling each type of chunk data before enabling chunk data mode. - // When chunk data is turned on, the data is made available in both the nodemap - // and each image. - void ConfigureChunkData(const Spinnaker::GenApi::INodeMap& nodeMap); -}; -} // namespace spinnaker_camera_driver -#endif // SPINNAKER_CAMERA_DRIVER_SPINNAKERCAMERA_H diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h deleted file mode 100644 index 5454588d..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h +++ /dev/null @@ -1,170 +0,0 @@ -/** -Software License Agreement (BSD) - -\file camera.h -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 SPINNAKER_CAMERA_DRIVER_CAMERA_H -#define SPINNAKER_CAMERA_DRIVER_CAMERA_H - -#include - -// Header generated by dynamic_reconfigure -#include -#include "spinnaker_camera_driver/set_property.h" - -// Spinnaker SDK -#include "Spinnaker.h" -#include "SpinGenApi/SpinnakerGenApi.h" - -//******************************************* -// This Class contains camera control functions. -// This base Class is based on the BlackFly S. -// Different cameras can extend/ override -// this class for any specific differences. -//******************************************* - -namespace spinnaker_camera_driver -{ -class Camera -{ -public: - explicit Camera(Spinnaker::GenApi::INodeMap* node_map); - ~Camera() - { - } - virtual void setNewConfiguration(const spinnaker_camera_driver::SpinnakerConfig& config, const uint32_t& level); - - /** Parameters that need a sensor to be stopped completely when changed. */ - static const uint8_t LEVEL_RECONFIGURE_CLOSE = 3; - - /** Parameters that need a sensor to stop streaming when changed. */ - static const uint8_t LEVEL_RECONFIGURE_STOP = 1; - - /** Parameters that can be changed while a sensor is streaming. */ - static const uint8_t LEVEL_RECONFIGURE_RUNNING = 0; - - virtual void setGain(const float& gain); - int getHeightMax(); - int getWidthMax(); - - Spinnaker::GenApi::CNodePtr - readProperty(const Spinnaker::GenICam::gcstring property_name); - bool - readableProperty(const Spinnaker::GenICam::gcstring property_name); - -protected: - Spinnaker::GenApi::INodeMap* node_map_; - - virtual void init(); - - int height_max_; - int width_max_; - - /*! - * \brief Changes the video mode of the connected camera. - * - * This function will change the camera to the desired videomode and allow up the maximum framerate for that mode. - * \param videoMode string of desired video mode, will be changed if unsupported. - */ - virtual void setFrameRate(const float frame_rate); - virtual void setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config); - /*! - * \brief Set parameters relative to GigE cameras. - * - * \param auto_packet_size Flag stating if packet size should be automatically determined or not. - * \param packet_size The packet size value to use if auto_packet_size is false. - */ - // TODO(mhosmar): Implement later - // void setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay); - - /*! - * \brief Will autoconfigure the packet size of the GigECamera with the given GUID. - * - * Note that this is expected only to work for GigE cameras, and only if the camera - * is not connected. - * - * \param guid the camera to autoconfigure - */ - // TODO(mhosmar): Implement later - // void setupGigEPacketSize(FlyCapture2::PGRGuid & guid); - - /*! - * \brief Will configure the packet size of the GigECamera with the given GUID to a given value. - * - * Note that this is expected only to work for GigE cameras, and only if the camera - * is not connected. - * - * \param guid the camera to autoconfigure - * \param packet_size The packet size value to use. - */ - // TODO(mhosmar): Implement later - // void setupGigEPacketSize(FlyCapture2::PGRGuid & guid, unsigned int packet_size); - - /*! - * \brief Will configure the packet delay of the GigECamera with the given GUID to a given value. - * - * Note that this is expected only to work for GigE cameras, and only if the camera - * is not connected. - * - * \param guid the camera to autoconfigure - * \param packet_delay The packet delay value to use. - */ - // TODO(mhosmar): Implement later - // void setupGigEPacketDelay(FlyCapture2::PGRGuid & guid, unsigned int packet_delay); - - /*! - * \brief Gets the current frame rate. - * - * Gets the camera's current reported frame rate. - * - * \return The reported frame rate. - */ - // TODO(mhosmar): Implement later - // float getCameraFrameRate(); - - /*! - * \brief Gets the current operating temperature. - * - * Gets the camera's current reported operating temperature. - * - * \return The reported temperature in Celsius. - */ - // TODO(mhosmar): Implement later - // float getCameraTemperature(); - - // TODO(mhosmar): Implement the following methods later - // void setBRWhiteBalance(bool auto_white_balance, uint16_t &blue, uint16_t &red); - - // uint getGain(); - - // uint getShutter(); - - // uint getBrightness(); - - // uint getExposure(); - - // uint getWhiteBalance(); - - // uint getROIPosition(); -}; -} // namespace spinnaker_camera_driver -#endif // SPINNAKER_CAMERA_DRIVER_CAMERA_H diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_exceptions.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_exceptions.h deleted file mode 100644 index ce1747a0..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_exceptions.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie -Mellon University. -Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012. -Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution -Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, Carnegie Mellon University. All rights reserved. -Copyright (c) 2018, Clearpath Robotics, 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 Carnegie Mellon University 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 HOLDER 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. -*/ - -/*-*-C++-*-*/ -/** - @file camera_exceptions.h - @author Chad Rockey - @date July 20, 2011 - @brief Interface to Point Grey cameras - - @attention Copyright (C) 2011 - @attention National Robotics Engineering Center - @attention Carnegie Mellon University -*/ - -#ifndef SPINNAKER_CAMERA_DRIVER_CAMERA_EXCEPTIONS_H -#define SPINNAKER_CAMERA_DRIVER_CAMERA_EXCEPTIONS_H - -#include -#include - -class CameraTimeoutException : public std::runtime_error -{ -public: - CameraTimeoutException() : runtime_error("Image not found within timeout.") - { - } - explicit CameraTimeoutException(const std::string& msg) : runtime_error(msg.c_str()) - { - } -}; - -class CameraNotRunningException : public std::runtime_error -{ -public: - CameraNotRunningException() : runtime_error("Camera is currently not running. Please start the capture.") - { - } - explicit CameraNotRunningException(const std::string& msg) : runtime_error(msg.c_str()) - { - } -}; - -class CameraImageNotReadyException : public std::runtime_error -{ -public: - CameraImageNotReadyException() : runtime_error("Image is currently not ready.") - { - } - explicit CameraImageNotReadyException(const std::string& msg) : runtime_error(msg.c_str()) - { - } -}; - -#endif // SPINNAKER_CAMERA_DRIVER_CAMERA_EXCEPTIONS_H diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/cm3.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/cm3.h deleted file mode 100644 index 38f9b0f5..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/cm3.h +++ /dev/null @@ -1,43 +0,0 @@ -/** -Software License Agreement (BSD) - -\file cm3.h -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 SPINNAKER_CAMERA_DRIVER_CM3_H -#define SPINNAKER_CAMERA_DRIVER_CM3_H -#include "spinnaker_camera_driver/camera.h" - -namespace spinnaker_camera_driver -{ -class Cm3 : public Camera -{ -public: - explicit Cm3(Spinnaker::GenApi::INodeMap* node_map); - ~Cm3(); - void setFrameRate(const float frame_rate); - void setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level); - -private: - void setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config); -}; -} // namespace spinnaker_camera_driver -#endif // SPINNAKER_CAMERA_DRIVER_CM3_H diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h deleted file mode 100644 index a5fd24ed..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h +++ /dev/null @@ -1,154 +0,0 @@ -/** -Software License Agreement (BSD) - -\file diagnostics.cpp -\authors Michael Lowe -\copyright Copyright (c) 2018, Ascent Robotics, 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 Clearpath Robotics 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 HOLDER 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. -*/ - -/*-*-C++-*-*/ -/** - @file diagnostics.h - @author Michael Lowe - @date August 1, 2018 - @brief Class to support ROS Diagnostics Aggregator for Spinnaker Camera - - @attention Copyright (C) 2018 -*/ - -#ifndef SPINNAKER_CAMERA_DRIVER_DIAGNOSTICS_H -#define SPINNAKER_CAMERA_DRIVER_DIAGNOSTICS_H - -#include "spinnaker_camera_driver/SpinnakerCamera.h" -#include "spinnaker_camera_driver/diagnostics.h" -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace spinnaker_camera_driver -{ -class DiagnosticsManager -{ -public: - DiagnosticsManager(const std::string name, const std::string serial, - std::shared_ptr const& pub, - const ros::NodeHandle& nh); - ~DiagnosticsManager(); - - /*! - * \brief Read the property of given parameters and push to aggregator - * - * Take all the collected parameters that were added read the values, then - * publish them to the - * allow the diagnostics aggreagtor to collect them - * \param spinnaker the SpinnakerCamera object used for getting the parameters - * from the spinnaker API - */ - void processDiagnostics(SpinnakerCamera* spinnaker); - - /*! - * \brief Add a diagnostic with name only (no warning checks) - * - * Allows the user to add an integer or float parameter without having to give - * additional information. - * User must specify the type they are getting - * \param name is the name of the parameter as writting in the User Manual - */ - template - void addDiagnostic(const Spinnaker::GenICam::gcstring name); - - /*! - * \brief Add a diagnostic with warning checks - * - * Allows the user to add an integer or float parameter and values to check it - * against. Anything outside - * of these ranges will be considered an error. - * \param name is the name of the parameter as writting in the User Manual - */ - void addDiagnostic(const Spinnaker::GenICam::gcstring name, bool check_ranges = false, - std::pair operational = std::make_pair(0, 0), int lower_bound = 0, int upper_bound = 0); - void addDiagnostic(const Spinnaker::GenICam::gcstring name, bool check_ranges = false, - std::pair operational = std::make_pair(0.0, 0.0), float lower_bound = 0, - float upper_bound = 0); - - void addAnalyzers(); - -private: - /* - * diagnostic_params is aData Structure to represent a parameter and its - * bounds - */ - template - struct diagnostic_params - { - Spinnaker::GenICam::gcstring parameter_name; // This should be the same as written in the User Manual - bool check_ranges; - std::pair operational_range; // Normal operatinal range - T warn_range_lower; - T warn_range_upper; - }; - - /*! - * \brief Function to push the diagnostic to the publisher - * - * Allows the user to add an integer or float parameter without having to give - * additional information. - * User must specify the type they are getting - * \param param is the diagnostic parameter name and boundaries - * \param value is the current value of the parameter requested from the - * device - */ - template - diagnostic_msgs::DiagnosticStatus getDiagStatus(const diagnostic_params& param, const T value); - - // constuctor parameters - std::string camera_name_; - std::string serial_number_; - std::shared_ptr diagnostics_pub_; - ros::NodeHandle nh_; - std::shared_ptr bond_ = nullptr; - - // vectors to keep track of the items to publish - std::vector> integer_params_; - std::vector> float_params_; - // Information about the device model, firmware, etc - // TODO(mlowe): Allow these to be configured - // clang-format off - const std::vector manufacturer_params_ - { - "DeviceVendorName", "DeviceModelName", "SensorDescription", "DeviceFirmwareVersion" - }; - // clang-format on -}; -} // namespace spinnaker_camera_driver - -#endif // SPINNAKER_CAMERA_DRIVER_DIAGNOSTICS_H diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/gh3.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/gh3.h deleted file mode 100644 index 850c4a78..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/gh3.h +++ /dev/null @@ -1,43 +0,0 @@ -/** -Software License Agreement (BSD) - -\file cm3.h -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 SPINNAKER_CAMERA_DRIVER_GH3_H -#define SPINNAKER_CAMERA_DRIVER_GH3_H -#include "spinnaker_camera_driver/camera.h" - -namespace spinnaker_camera_driver -{ -class Gh3 : public Camera -{ -public: - explicit Gh3(Spinnaker::GenApi::INodeMap* node_map); - ~Gh3(); - void setFrameRate(const float frame_rate); - void setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level); - -private: - void setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config); -}; -} // namespace spinnaker_camera_driver -#endif // SPINNAKER_CAMERA_DRIVER_GH3_H diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/set_property.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/set_property.h deleted file mode 100644 index 06208e58..00000000 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/set_property.h +++ /dev/null @@ -1,269 +0,0 @@ -/** -Software License Agreement (BSD) - -\file set_property.h -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 SPINNAKER_CAMERA_DRIVER_SET_PROPERTY_H -#define SPINNAKER_CAMERA_DRIVER_SET_PROPERTY_H - -// Spinnaker SDK -#include "Spinnaker.h" -#include "SpinGenApi/SpinnakerGenApi.h" - -#include - -namespace spinnaker_camera_driver -{ -inline bool setProperty(Spinnaker::GenApi::INodeMap* node_map, const std::string& property_name, - const std::string& entry_name) -{ - // *** NOTES *** - // Enumeration nodes are slightly more complicated to set than other - // nodes. This is because setting an enumeration node requires working - // with two nodes instead of the usual one. - // - // As such, there are a number of steps to setting an enumeration node: - // retrieve the enumeration node from the nodemap, retrieve the desired - // entry node from the enumeration node, retrieve the integer value from - // the entry node, and set the new value of the enumeration node with - // the integer value from the entry node. - Spinnaker::GenApi::CEnumerationPtr enumerationPtr = node_map->GetNode(property_name.c_str()); - - if (!Spinnaker::GenApi::IsImplemented(enumerationPtr)) - { - ROS_ERROR_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Enumeration name " << property_name << " not " - "implemented."); - return false; - } - - if (Spinnaker::GenApi::IsAvailable(enumerationPtr)) - { - if (Spinnaker::GenApi::IsWritable(enumerationPtr)) - { - Spinnaker::GenApi::CEnumEntryPtr enumEmtryPtr = enumerationPtr->GetEntryByName(entry_name.c_str()); - - if (Spinnaker::GenApi::IsAvailable(enumEmtryPtr)) - { - if (Spinnaker::GenApi::IsReadable(enumEmtryPtr)) - { - enumerationPtr->SetIntValue(enumEmtryPtr->GetValue()); - - ROS_INFO_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") " << property_name << " set to " << enumerationPtr->GetCurrentEntry()->GetSymbolic() - << "."); - - return true; - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Entry name " << entry_name << " not writable."); - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Entry name " << entry_name << " not available."); - - ROS_WARN("Available:"); - Spinnaker::GenApi::NodeList_t entries; - enumerationPtr->GetEntries(entries); - for (auto& entry : entries) - { - auto enumEntry = dynamic_cast(entry); - if (enumEntry && Spinnaker::GenApi::IsAvailable(entry)) - ROS_WARN_STREAM(" - " << entry->GetName() << " (display " << entry->GetDisplayName() << ", symbolic " - << enumEntry->GetSymbolic() << ")"); - } - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Enumeration " << property_name << " not writable."); - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Enumeration " << property_name << " not available."); - } - return false; -} - -inline bool setProperty(Spinnaker::GenApi::INodeMap* node_map, const std::string& property_name, const float& value) -{ - Spinnaker::GenApi::CFloatPtr floatPtr = node_map->GetNode(property_name.c_str()); - - if (!Spinnaker::GenApi::IsImplemented(floatPtr)) - { - ROS_ERROR_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature name " << property_name << " not implemented."); - return false; - } - if (Spinnaker::GenApi::IsAvailable(floatPtr)) - { - if (Spinnaker::GenApi::IsWritable(floatPtr)) - { - float temp_value = value; - if (temp_value > floatPtr->GetMax()) - temp_value = floatPtr->GetMax(); - else if (temp_value < floatPtr->GetMin()) - temp_value = floatPtr->GetMin(); - floatPtr->SetValue(temp_value); - ROS_INFO_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() << ") " - << property_name << " set to " << floatPtr->GetValue() << "."); - return true; - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not writable."); - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not available."); - } - return false; -} - -inline bool setProperty(Spinnaker::GenApi::INodeMap* node_map, const std::string& property_name, const bool& value) -{ - Spinnaker::GenApi::CBooleanPtr boolPtr = node_map->GetNode(property_name.c_str()); - if (!Spinnaker::GenApi::IsImplemented(boolPtr)) - { - ROS_ERROR_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature name " << property_name << " not implemented."); - return false; - } - if (Spinnaker::GenApi::IsAvailable(boolPtr)) - { - if (Spinnaker::GenApi::IsWritable(boolPtr)) - { - boolPtr->SetValue(value); - ROS_INFO_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() << ") " - << property_name << " set to " << boolPtr->GetValue() << "."); - return true; - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not writable."); - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not available."); - } - return false; -} - -inline bool setProperty(Spinnaker::GenApi::INodeMap* node_map, const std::string& property_name, const int& value) -{ - Spinnaker::GenApi::CIntegerPtr intPtr = node_map->GetNode(property_name.c_str()); - if (!Spinnaker::GenApi::IsImplemented(intPtr)) - { - ROS_ERROR_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature name " << property_name << " not implemented."); - return false; - } - if (Spinnaker::GenApi::IsAvailable(intPtr)) - { - if (Spinnaker::GenApi::IsWritable(intPtr)) - { - int temp_value = value; - if (temp_value > intPtr->GetMax()) - temp_value = intPtr->GetMax(); - else if (temp_value < intPtr->GetMin()) - temp_value = intPtr->GetMin(); - intPtr->SetValue(temp_value); - ROS_INFO_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() << ") " - << property_name << " set to " << intPtr->GetValue() << "."); - return true; - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not writable."); - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not available."); - } - return false; -} - -inline bool setMaxInt(Spinnaker::GenApi::INodeMap* node_map, const std::string& property_name) -{ - Spinnaker::GenApi::CIntegerPtr intPtr = node_map->GetNode(property_name.c_str()); - - if (Spinnaker::GenApi::IsAvailable(intPtr)) - { - if (Spinnaker::GenApi::IsWritable(intPtr)) - { - intPtr->SetValue(intPtr->GetMax()); - ROS_INFO_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() << ") " - << property_name << " set to " << intPtr->GetValue() << "."); - return true; - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not writable."); - } - } - else - { - ROS_WARN_STREAM("[SpinnakerCamera]: (" - << static_cast(node_map->GetNode("DeviceID"))->GetValue() - << ") Feature " << property_name << " not available."); - } - return false; -} -} // namespace spinnaker_camera_driver -#endif // SPINNAKER_CAMERA_DRIVER_SET_PROPERTY_H diff --git a/spinnaker_camera_driver/launch/camera.launch b/spinnaker_camera_driver/launch/camera.launch deleted file mode 100644 index 9745dc90..00000000 --- a/spinnaker_camera_driver/launch/camera.launch +++ /dev/null @@ -1,113 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - --> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/spinnaker_camera_driver/launch/diagnostics.launch b/spinnaker_camera_driver/launch/diagnostics.launch deleted file mode 100644 index 839d4afc..00000000 --- a/spinnaker_camera_driver/launch/diagnostics.launch +++ /dev/null @@ -1,117 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - --> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/spinnaker_camera_driver/launch/stereo.launch b/spinnaker_camera_driver/launch/stereo.launch deleted file mode 100644 index ce0c2d7d..00000000 --- a/spinnaker_camera_driver/launch/stereo.launch +++ /dev/null @@ -1,101 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/spinnaker_camera_driver/launch/test_spinnaker.launch b/spinnaker_camera_driver/launch/test_spinnaker.launch deleted file mode 100644 index b908f163..00000000 --- a/spinnaker_camera_driver/launch/test_spinnaker.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - diff --git a/spinnaker_camera_driver/nodelet_plugins.xml b/spinnaker_camera_driver/nodelet_plugins.xml deleted file mode 100644 index cdb073e9..00000000 --- a/spinnaker_camera_driver/nodelet_plugins.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - - This is the nodelet for the Point Grey Camera Driver. - - diff --git a/spinnaker_camera_driver/package.xml b/spinnaker_camera_driver/package.xml deleted file mode 100644 index 7d91a989..00000000 --- a/spinnaker_camera_driver/package.xml +++ /dev/null @@ -1,65 +0,0 @@ - - - - spinnaker_camera_driver - 0.2.5 - Spinnaker camera driver based on Spinnaker. - - Mike Hosmar - - BSD - - http://ros.org/wiki/spinnaker_camera_driver - - Chad Rockey - - catkin - - curl - dpkg - - roscpp - nodelet - sensor_msgs - wfov_camera_msgs - image_exposure_msgs - camera_info_manager - image_transport - dynamic_reconfigure - diagnostic_updater - - - libusb-1.0-dev - - image_proc - - roslaunch - roslint - - - - - diff --git a/spinnaker_camera_driver/params/analyzer.yml b/spinnaker_camera_driver/params/analyzer.yml deleted file mode 100644 index e93e6780..00000000 --- a/spinnaker_camera_driver/params/analyzer.yml +++ /dev/null @@ -1,6 +0,0 @@ -pub_rate: 1.0 -analyzers: - cameras: - type: diagnostic_aggregator/GenericAnalyzer - path: Cameras - startswith: 'Spinnaker' diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp deleted file mode 100644 index 3ecaac9f..00000000 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ /dev/null @@ -1,567 +0,0 @@ -/* -This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie -Mellon University. -Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012. -Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution -Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, Carnegie Mellon University. All rights reserved. -Copyright (c) 2018, Clearpath Robotics, 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 Carnegie Mellon University 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 HOLDER 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. -*/ - -/*-*-C++-*-*/ -/** - @file SpinnakerCamera.cpp - @author Chad Rockey - @date July 11, 2011 - @brief Interface to Point Grey cameras - - @attention Copyright (C) 2011 - @attention National Robotics Engineering Center - @attention Carnegie Mellon University -*/ - -#include "spinnaker_camera_driver/SpinnakerCamera.h" - -#include -#include -#include -#include - -#include - -namespace spinnaker_camera_driver -{ -SpinnakerCamera::SpinnakerCamera() - : serial_(0) - , system_(Spinnaker::System::GetInstance()) - , camList_(system_->GetCameras()) - , pCam_(static_cast(NULL)) // Hack to suppress compiler warning. Spinnaker has only one contructor which takes - // an int - , camera_(static_cast(NULL)) - , captureRunning_(false) -{ - unsigned int num_cameras = camList_.GetSize(); - ROS_INFO_STREAM_ONCE("[SpinnakerCamera]: Number of cameras detected: " << num_cameras); -} - -SpinnakerCamera::~SpinnakerCamera() -{ - // @note ebretl Destructors of camList_ and system_ handle teardown -} - -void SpinnakerCamera::setNewConfiguration(const spinnaker_camera_driver::SpinnakerConfig& config, const uint32_t& level) -{ - // Check if camera is connected - if (!pCam_) - { - SpinnakerCamera::connect(); - } - - // Activate mutex to prevent us from grabbing images during this time - std::lock_guard scopedLock(mutex_); - - if (level >= LEVEL_RECONFIGURE_STOP) - { - ROS_DEBUG("SpinnakerCamera::setNewConfiguration: Reconfigure Stop."); - bool capture_was_running = captureRunning_; - start(); // For some reason some params only work after aquisition has be started once. - stop(); - camera_->setNewConfiguration(config, level); - if (capture_was_running) - start(); - } - else - { - camera_->setNewConfiguration(config, level); - } -} // end setNewConfiguration - -void SpinnakerCamera::setGain(const float& gain) -{ - if (camera_) - camera_->setGain(gain); -} - -int SpinnakerCamera::getHeightMax() -{ - if (camera_) - return camera_->getHeightMax(); - else - return 0; -} - -int SpinnakerCamera::getWidthMax() -{ - if (camera_) - return camera_->getWidthMax(); - else - return 0; -} - -bool SpinnakerCamera::readableProperty(const Spinnaker::GenICam::gcstring property_name) -{ - if (camera_) - { - return camera_->readableProperty(property_name); - } - else - { - return 0; - } -} - -Spinnaker::GenApi::CNodePtr SpinnakerCamera::readProperty(const Spinnaker::GenICam::gcstring property_name) -{ - if (camera_) - { - return camera_->readProperty(property_name); - } - else - { - return 0; - } -} - -void SpinnakerCamera::connect() -{ - if (!pCam_) - { - // If we have a specific camera to connect to (specified by a serial number) - if (serial_ != 0) - { - const auto serial_string = std::to_string(serial_); - - try - { - pCam_ = camList_.GetBySerial(serial_string); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Could not find camera with serial number " + - serial_string + ". Is that camera plugged in? Error: " + std::string(e.what())); - } - } - else - { - // Connect to any camera (the first) - try - { - pCam_ = camList_.GetByIndex(0); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Failed to get first connected camera. Error: " + - std::string(e.what())); - } - } - if (!pCam_ || !pCam_->IsValid()) - { - throw std::runtime_error("[SpinnakerCamera::connect] Failed to obtain camera reference."); - } - - try - { - // Check Device type and save serial for reconnecting - Spinnaker::GenApi::INodeMap& genTLNodeMap = pCam_->GetTLDeviceNodeMap(); - - if (serial_ == 0) - { - Spinnaker::GenApi::CStringPtr serial_ptr = - static_cast(genTLNodeMap.GetNode("DeviceID")); - if (IsAvailable(serial_ptr) && IsReadable(serial_ptr)) - { - serial_ = atoi(serial_ptr->GetValue().c_str()); - ROS_INFO("[SpinnakerCamera::connect]: Using Serial: %i", serial_); - } - else - { - throw std::runtime_error("[SpinnakerCamera::connect]: Unable to determine serial number."); - } - } - - Spinnaker::GenApi::CEnumerationPtr device_type_ptr = - static_cast(genTLNodeMap.GetNode("DeviceType")); - - if (IsAvailable(device_type_ptr) && IsReadable(device_type_ptr)) - { - ROS_INFO_STREAM("[SpinnakerCamera::connect]: Detected device type: " << device_type_ptr->ToString()); - - if (device_type_ptr->GetCurrentEntry() == device_type_ptr->GetEntryByName("U3V")) - { - Spinnaker::GenApi::CEnumerationPtr device_speed_ptr = - static_cast(genTLNodeMap.GetNode("DeviceCurrentSpeed")); - if (IsAvailable(device_speed_ptr) && IsReadable(device_speed_ptr)) - { - if (device_speed_ptr->GetCurrentEntry() != device_speed_ptr->GetEntryByName("SuperSpeed")) - ROS_ERROR_STREAM("[SpinnakerCamera::connect]: U3V Device not running at Super-Speed. Check Cables! "); - } - } - // TODO(mhosmar): - check if interface is GigE and connect to GigE cam - } - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Failed to determine device info with error: " + - std::string(e.what())); - } - - try - { - // Initialize Camera - pCam_->Init(); - - // Retrieve GenICam nodemap - node_map_ = &pCam_->GetNodeMap(); - - // detect model and set camera_ accordingly; - Spinnaker::GenApi::CStringPtr model_name = node_map_->GetNode("DeviceModelName"); - std::string model_name_str(model_name->ToString()); - - ROS_INFO("[SpinnakerCamera::connect]: Camera model name: %s", model_name_str.c_str()); - if (model_name_str.find("Blackfly S") != std::string::npos) - camera_.reset(new Camera(node_map_)); - else if (model_name_str.find("Chameleon3") != std::string::npos) - camera_.reset(new Cm3(node_map_)); - else if (model_name_str.find("Grasshopper3") != std::string::npos) - camera_.reset(new Gh3(node_map_)); - else - { - camera_.reset(new Camera(node_map_)); - ROS_WARN("SpinnakerCamera::connect: Could not detect camera model name."); - } - - // Configure chunk data - Enable Metadata - // SpinnakerCamera::ConfigureChunkData(*node_map_); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Failed to connect to camera. Error: " + - std::string(e.what())); - } - catch (const std::runtime_error& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Failed to configure chunk data. Error: " + - std::string(e.what())); - } - } - - // TODO(mhosmar): Get camera info to check if camera is running in color or mono mode - /* - CameraInfo cInfo; - error = cam_.GetCameraInfo(&cInfo); - SpinnakerCamera::handleError("SpinnakerCamera::connect Failed to get camera info.", error); - isColor_ = cInfo.isColorCamera; - */ -} - -void SpinnakerCamera::disconnect() -{ - std::lock_guard scopedLock(mutex_); - captureRunning_ = false; - try - { - // Check if camera is connected - if (pCam_) - { - pCam_->DeInit(); - pCam_ = static_cast(NULL); - camList_.RemoveBySerial(std::to_string(serial_)); - } - Spinnaker::CameraList temp_list = system_->GetCameras(); - camList_.Append(temp_list); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::disconnect] Failed to disconnect camera with error: " + - std::string(e.what())); - } -} - -void SpinnakerCamera::start() -{ - try - { - // Check if camera is connected - if (pCam_ && !captureRunning_) - { - // Start capturing images - pCam_->BeginAcquisition(); - captureRunning_ = true; - } - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::start] Failed to start capture with error: " + std::string(e.what())); - } -} - -void SpinnakerCamera::stop() -{ - if (pCam_ && captureRunning_) - { - // Stop capturing images - try - { - captureRunning_ = false; - pCam_->EndAcquisition(); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::stop] Failed to stop capture with error: " + std::string(e.what())); - } - } -} - -void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& frame_id) -{ - std::lock_guard scopedLock(mutex_); - - // Check if Camera is connected and Running - if (pCam_ && captureRunning_) - { - // Handle "Image Retrieval" Exception - try - { - Spinnaker::ImagePtr image_ptr = pCam_->GetNextImage(timeout_); - // std::string format(image_ptr->GetPixelFormatName()); - // std::printf("\033[100m format: %s \n", format.c_str()); - - // throw std::runtime_error("[SpinnakerCamera::grabImage] Image received from camera " - // + std::to_string(serial_) - // + " is incomplete."); - while (image_ptr->IsIncomplete()) - { - ROS_WARN_STREAM_ONCE("[SpinnakerCamera::grabImage] Image received from camera " - << std::to_string(serial_) - << " is incomplete. Trying again."); - image_ptr = pCam_->GetNextImage(timeout_); - } - - // Set Image Time Stamp - image->header.stamp.sec = image_ptr->GetTimeStamp() * 1e-9; - image->header.stamp.nsec = image_ptr->GetTimeStamp(); - - // Check the bits per pixel. - size_t bitsPerPixel = image_ptr->GetBitsPerPixel(); - - // -------------------------------------------------- - // Set the image encoding - std::string imageEncoding = sensor_msgs::image_encodings::MONO8; - - Spinnaker::GenApi::CEnumerationPtr color_filter_ptr = - static_cast(node_map_->GetNode("PixelColorFilter")); - - Spinnaker::GenICam::gcstring color_filter_str = color_filter_ptr->ToString(); - Spinnaker::GenICam::gcstring bayer_rg_str = "BayerRG"; - Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR"; - Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB"; - Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG"; - - // if(isColor_ && bayer_format != NONE) - if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None")) - { - if (bitsPerPixel == 16) - { - // 16 Bits per Pixel - if (color_filter_str.compare(bayer_rg_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB16; - } - else if (color_filter_str.compare(bayer_gr_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG16; - } - else if (color_filter_str.compare(bayer_gb_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG16; - } - else if (color_filter_str.compare(bayer_bg_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR16; - } - else - { - throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 16-bit format."); - } - } - else - { - // 8 Bits per Pixel - if (color_filter_str.compare(bayer_rg_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8; - } - else if (color_filter_str.compare(bayer_gr_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8; - } - else if (color_filter_str.compare(bayer_gb_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8; - } - else if (color_filter_str.compare(bayer_bg_str) == 0) - { - imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8; - } - else - { - throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 8-bit format."); - } - } - } - else // Mono camera or in pixel binned mode. - { - if (bitsPerPixel == 16) - { - imageEncoding = sensor_msgs::image_encodings::MONO16; - } - else if (bitsPerPixel == 24) - { - imageEncoding = sensor_msgs::image_encodings::RGB8; - } - else - { - imageEncoding = sensor_msgs::image_encodings::MONO8; - } - } - - int width = image_ptr->GetWidth(); - int height = image_ptr->GetHeight(); - int stride = image_ptr->GetStride(); - - // ROS_INFO_ONCE("\033[93m wxh: (%d, %d), stride: %d \n", width, height, stride); - fillImage(*image, imageEncoding, height, width, stride, image_ptr->GetData()); - image->header.frame_id = frame_id; - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::grabImage] Failed to retrieve buffer with error: " + - std::string(e.what())); - } - } - else if (pCam_) - { - throw CameraNotRunningException("[SpinnakerCamera::grabImage] Camera is currently not running. Please start " - "capturing frames first."); - } - else - { - throw std::runtime_error("[SpinnakerCamera::grabImage] Not connected to the camera."); - } -} // end grabImage - -void SpinnakerCamera::setTimeout(const double& timeout) -{ - timeout_ = static_cast(std::round(timeout * 1000)); -} -void SpinnakerCamera::setDesiredCamera(const uint32_t& id) -{ - serial_ = id; -} - -void SpinnakerCamera::ConfigureChunkData(const Spinnaker::GenApi::INodeMap& nodeMap) -{ - ROS_INFO_STREAM("*** CONFIGURING CHUNK DATA ***"); - try - { - // Activate chunk mode - // - // *** NOTES *** - // Once enabled, chunk data will be available at the end of the payload - // of every image captured until it is disabled. Chunk data can also be - // retrieved from the nodemap. - // - Spinnaker::GenApi::CBooleanPtr ptrChunkModeActive = nodeMap.GetNode("ChunkModeActive"); - if (!Spinnaker::GenApi::IsAvailable(ptrChunkModeActive) || !Spinnaker::GenApi::IsWritable(ptrChunkModeActive)) - { - throw std::runtime_error("Unable to activate chunk mode. Aborting..."); - } - ptrChunkModeActive->SetValue(true); - ROS_INFO_STREAM_ONCE("Chunk mode activated..."); - - // Enable all types of chunk data - // - // *** NOTES *** - // Enabling chunk data requires working with nodes: "ChunkSelector" - // is an enumeration selector node and "ChunkEnable" is a boolean. It - // requires retrieving the selector node (which is of enumeration node - // type), selecting the entry of the chunk data to be enabled, retrieving - // the corresponding boolean, and setting it to true. - // - // In this example, all chunk data is enabled, so these steps are - // performed in a loop. Once this is complete, chunk mode still needs to - // be activated. - // - Spinnaker::GenApi::NodeList_t entries; - // Retrieve the selector node - Spinnaker::GenApi::CEnumerationPtr ptrChunkSelector = nodeMap.GetNode("ChunkSelector"); - if (!Spinnaker::GenApi::IsAvailable(ptrChunkSelector) || !Spinnaker::GenApi::IsReadable(ptrChunkSelector)) - { - throw std::runtime_error("Unable to retrieve chunk selector. Aborting..."); - } - // Retrieve entries - ptrChunkSelector->GetEntries(entries); - - ROS_INFO_STREAM("Enabling entries..."); - - for (unsigned int i = 0; i < entries.size(); i++) - { - // Select entry to be enabled - Spinnaker::GenApi::CEnumEntryPtr ptrChunkSelectorEntry = entries.at(i); - // Go to next node if problem occurs - if (!Spinnaker::GenApi::IsAvailable(ptrChunkSelectorEntry) || - !Spinnaker::GenApi::IsReadable(ptrChunkSelectorEntry)) - { - continue; - } - ptrChunkSelector->SetIntValue(ptrChunkSelectorEntry->GetValue()); - - ROS_INFO_STREAM("\t" << ptrChunkSelectorEntry->GetSymbolic() << ": "); - // Retrieve corresponding boolean - Spinnaker::GenApi::CBooleanPtr ptrChunkEnable = nodeMap.GetNode("ChunkEnable"); - // Enable the boolean, thus enabling the corresponding chunk data - if (!Spinnaker::GenApi::IsAvailable(ptrChunkEnable)) - { - ROS_INFO("Node not available"); - } - else if (ptrChunkEnable->GetValue()) - { - ROS_INFO("Enabled"); - } - else if (Spinnaker::GenApi::IsWritable(ptrChunkEnable)) - { - ptrChunkEnable->SetValue(true); - ROS_INFO("Enabled"); - } - else - { - ROS_INFO("Node not writable"); - } - } - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error(e.what()); - } -} -} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp deleted file mode 100644 index c7b4956f..00000000 --- a/spinnaker_camera_driver/src/camera.cpp +++ /dev/null @@ -1,333 +0,0 @@ -/** -Software License Agreement (BSD) - -\file camera.cpp -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 "spinnaker_camera_driver/camera.h" - -#include - -namespace spinnaker_camera_driver -{ -void Camera::init() -{ - Spinnaker::GenApi::CIntegerPtr height_max_ptr = node_map_->GetNode("HeightMax"); - if (!IsAvailable(height_max_ptr) || !IsReadable(height_max_ptr)) - { - throw std::runtime_error("[Camera::init] Unable to read HeightMax"); - } - height_max_ = height_max_ptr->GetValue(); - Spinnaker::GenApi::CIntegerPtr width_max_ptr = node_map_->GetNode("WidthMax"); - if (!IsAvailable(width_max_ptr) || !IsReadable(width_max_ptr)) - { - throw std::runtime_error("[Camera::init] Unable to read WidthMax"); - } - width_max_ = width_max_ptr->GetValue(); - // Set Throughput to maximum - //===================================== - setMaxInt(node_map_, "DeviceLinkThroughputLimit"); -} -void Camera::setFrameRate(const float frame_rate) -{ - // This enables the "AcquisitionFrameRateEnabled" - //====================================== - setProperty(node_map_, "AcquisitionFrameRateEnable", true); - - // This sets the "AcquisitionFrameRate" to X FPS - // ======================================== - - Spinnaker::GenApi::CFloatPtr ptrAcquisitionFrameRate = node_map_->GetNode("AcquisitionFrameRate"); - ROS_DEBUG_STREAM("Minimum Frame Rate: \t " << ptrAcquisitionFrameRate->GetMin()); - ROS_DEBUG_STREAM("Maximum Frame rate: \t " << ptrAcquisitionFrameRate->GetMax()); - - // Finally Set the Frame Rate - setProperty(node_map_, "AcquisitionFrameRate", frame_rate); - - ROS_DEBUG_STREAM("Current Frame rate: \t " << ptrAcquisitionFrameRate->GetValue()); -} - -void Camera::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level) -{ - try - { - if (level >= LEVEL_RECONFIGURE_STOP) - setImageControlFormats(config); - - setFrameRate(static_cast(config.acquisition_frame_rate)); - // Set enable after frame rate encase its false - setProperty(node_map_, "AcquisitionFrameRateEnable", config.acquisition_frame_rate_enable); - - // Set Trigger and Strobe Settings - // NOTE: The trigger must be disabled (i.e. TriggerMode = "Off") in order to configure whether the source is - // software or hardware. - setProperty(node_map_, "TriggerMode", std::string("Off")); - setProperty(node_map_, "TriggerSource", config.trigger_source); - setProperty(node_map_, "TriggerSelector", config.trigger_selector); - setProperty(node_map_, "TriggerActivation", config.trigger_activation_mode); - setProperty(node_map_, "TriggerMode", config.enable_trigger); - - setProperty(node_map_, "LineSelector", config.line_selector); - setProperty(node_map_, "LineMode", config.line_mode); - setProperty(node_map_, "LineSource", config.line_source); - - // Set auto exposure - setProperty(node_map_, "ExposureMode", config.exposure_mode); - setProperty(node_map_, "ExposureAuto", config.exposure_auto); - - // Set sharpness - if (IsAvailable(node_map_->GetNode("SharpeningEnable"))) - { - setProperty(node_map_, "SharpeningEnable", config.sharpening_enable); - if (config.sharpening_enable) - { - setProperty(node_map_, "SharpeningAuto", config.auto_sharpness); - setProperty(node_map_, "Sharpening", static_cast(config.sharpness)); - setProperty(node_map_, "SharpeningThreshold", static_cast(config.sharpening_threshold)); - } - } - - // Set saturation - if (IsAvailable(node_map_->GetNode("SaturationEnable"))) - { - setProperty(node_map_, "SaturationEnable", config.saturation_enable); - if (config.saturation_enable) - { - setProperty(node_map_, "Saturation", static_cast(config.saturation)); - } - } - - // Set shutter time/speed - if (config.exposure_auto.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "ExposureTime", static_cast(config.exposure_time)); - } - else - { - setProperty(node_map_, "AutoExposureExposureTimeUpperLimit", - static_cast(config.auto_exposure_time_upper_limit)); - } - - // Set gain - setProperty(node_map_, "GainSelector", config.gain_selector); - setProperty(node_map_, "GainAuto", config.auto_gain); - if (config.auto_gain.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "Gain", static_cast(config.gain)); - } - - // Set brightness - setProperty(node_map_, "BlackLevel", static_cast(config.brightness)); - - // Set gamma - if (config.gamma_enable) - { - setProperty(node_map_, "GammaEnable", config.gamma_enable); - setProperty(node_map_, "Gamma", static_cast(config.gamma)); - } - - // Set white balance - if (IsAvailable(node_map_->GetNode("BalanceWhiteAuto"))) - { - setProperty(node_map_, "BalanceWhiteAuto", config.auto_white_balance); - if (config.auto_white_balance.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "BalanceRatioSelector", "Blue"); - setProperty(node_map_, "BalanceRatio", static_cast(config.white_balance_blue_ratio)); - setProperty(node_map_, "BalanceRatioSelector", "Red"); - setProperty(node_map_, "BalanceRatio", static_cast(config.white_balance_red_ratio)); - } - } - - // Set Auto exposure/white balance parameters - if (IsAvailable(node_map_->GetNode("AutoAlgorithmSelector"))) - { - setProperty(node_map_, "AutoAlgorithmSelector", std::string("Ae")); - setProperty(node_map_, "AasRoiEnable", true); - if (config.auto_exposure_roi_width != 0 && config.auto_exposure_roi_height != 0) - { - setProperty(node_map_, "AasRoiOffsetX", config.auto_exposure_roi_offset_x); - setProperty(node_map_, "AasRoiOffsetY", config.auto_exposure_roi_offset_y); - setProperty(node_map_, "AasRoiWidth", config.auto_exposure_roi_width); - setProperty(node_map_, "AasRoiHeight", config.auto_exposure_roi_height); - } - } - - // Set Auto exposure lighting mode - if (IsAvailable(node_map_->GetNode("AutoExposureLightingMode"))) - { - setProperty(node_map_, "AutoExposureLightingMode", config.auto_exposure_lighting_mode); - } - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[Camera::setNewConfiguration] Failed to set configuration: " + std::string(e.what())); - } -} - -// Image Size and Pixel Format -void Camera::setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config) -{ - // Set Binning, Decimation, and Reverse - setProperty(node_map_, "BinningHorizontal", config.image_format_x_binning); - setProperty(node_map_, "BinningVertical", config.image_format_y_binning); - setProperty(node_map_, "DecimationHorizontal", config.image_format_x_decimation); - setProperty(node_map_, "DecimationVertical", config.image_format_y_decimation); - setProperty(node_map_, "ReverseX", config.image_format_x_reverse); - setProperty(node_map_, "ReverseY", config.image_format_y_reverse); - - // Grab the Max values after decimation - Spinnaker::GenApi::CIntegerPtr height_max_ptr = node_map_->GetNode("HeightMax"); - if (!IsAvailable(height_max_ptr) || !IsReadable(height_max_ptr)) - { - throw std::runtime_error("[Camera::setImageControlFormats] Unable to read HeightMax"); - } - height_max_ = height_max_ptr->GetValue(); - Spinnaker::GenApi::CIntegerPtr width_max_ptr = node_map_->GetNode("WidthMax"); - if (!IsAvailable(width_max_ptr) || !IsReadable(width_max_ptr)) - { - throw std::runtime_error("[Camera::setImageControlFormats] Unable to read WidthMax"); - } - width_max_ = width_max_ptr->GetValue(); - - // Offset first encase expanding ROI - // Apply offset X - setProperty(node_map_, "OffsetX", 0); - // Apply offset Y - setProperty(node_map_, "OffsetY", 0); - - // Set Width/Height - if (config.image_format_roi_width <= 0 || config.image_format_roi_width > width_max_) - setProperty(node_map_, "Width", width_max_); - else - setProperty(node_map_, "Width", config.image_format_roi_width); - if (config.image_format_roi_height <= 0 || config.image_format_roi_height > height_max_) - setProperty(node_map_, "Height", height_max_); - else - setProperty(node_map_, "Height", config.image_format_roi_height); - - // Apply offset X - setProperty(node_map_, "OffsetX", config.image_format_x_offset); - // Apply offset Y - setProperty(node_map_, "OffsetY", config.image_format_y_offset); - - // Set Pixel Format - setProperty(node_map_, "PixelFormat", config.image_format_color_coding); - - // Set ISP Enable - setProperty(node_map_, "IspEnable", config.isp_enable); -} - -void Camera::setGain(const float& gain) -{ - setProperty(node_map_, "GainAuto", "Off"); - setProperty(node_map_, "Gain", static_cast(gain)); -} - -/* -void Camera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay) -{ -} - -void Camera::setupGigEPacketSize(PGRGuid & guid) -{ -} - -void Camera::setupGigEPacketSize(PGRGuid & guid, unsigned int packet_size) -{ - -} - -void Camera::setupGigEPacketDelay(PGRGuid & guid, unsigned int packet_delay) -{ -} - -*/ - -int Camera::getHeightMax() -{ - return height_max_; -} - -int Camera::getWidthMax() -{ - return width_max_; -} - -// uint SpinnakerCamera::getGain() -// { -// return metadata_.embeddedGain >> 20; -// } - -// uint Camera::getShutter() -// { -// return metadata_.embeddedShutter >> 20; -// } - -// uint Camera::getBrightness() -// { -// return metadata_.embeddedTimeStamp >> 20; -// } - -// uint Camera::getExposure() -// { -// return metadata_.embeddedBrightness >> 20; -// } - -// uint Camera::getWhiteBalance() -// { -// return metadata_.embeddedExposure >> 8; -// } - -// uint Camera::getROIPosition() -// { -// return metadata_.embeddedROIPosition >> 24; -// } - -// float Camera::getCameraTemperature() -//{ -//} - -// float Camera::getCameraFrameRate() -//{ -//} -Spinnaker::GenApi::CNodePtr Camera::readProperty(const Spinnaker::GenICam::gcstring property_name) -{ - Spinnaker::GenApi::CNodePtr ptr = node_map_->GetNode(property_name); - if (!Spinnaker::GenApi::IsAvailable(ptr) || !Spinnaker::GenApi::IsReadable(ptr)) - { - throw std::runtime_error("Unable to get parmeter " + property_name); - } - return ptr; -} - -bool Camera::readableProperty(const Spinnaker::GenICam::gcstring property_name) -{ - Spinnaker::GenApi::CNodePtr ptr = node_map_->GetNode(property_name); - return Spinnaker::GenApi::IsAvailable(ptr) && Spinnaker::GenApi::IsReadable(ptr); -} - -Camera::Camera(Spinnaker::GenApi::INodeMap* node_map) -{ - node_map_ = node_map; - init(); -} -} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/cm3.cpp b/spinnaker_camera_driver/src/cm3.cpp deleted file mode 100644 index 3ff1f6f4..00000000 --- a/spinnaker_camera_driver/src/cm3.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/** -Software License Agreement (BSD) - -\file cm3.cpp -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 "spinnaker_camera_driver/cm3.h" - -#include - -namespace spinnaker_camera_driver -{ -Cm3::Cm3(Spinnaker::GenApi::INodeMap* node_map) : Camera(node_map) -{ -} - -Cm3::~Cm3() -{ -} - -void Cm3::setFrameRate(const float frame_rate) -{ - // This enables the "AcquisitionFrameRateEnabled" - //====================================== - setProperty(node_map_, "AcquisitionFrameRateEnabled", true); // different from Bfly S - - // This sets the "AcquisitionFrameRateAuto" to "Off" - //====================================== - setProperty(node_map_, "AcquisitionFrameRateAuto", static_cast("Off")); // different from Bfly S - - // This sets the "AcquisitionFrameRate" to X FPS - // ======================================== - - Spinnaker::GenApi::CFloatPtr ptrAcquisitionFrameRate = node_map_->GetNode("AcquisitionFrameRate"); - ROS_DEBUG_STREAM("Minimum Frame Rate: \t " << ptrAcquisitionFrameRate->GetMin()); - ROS_DEBUG_STREAM("Maximum Frame rate: \t " << ptrAcquisitionFrameRate->GetMax()); - - // Finally Set the Frame Rate - setProperty(node_map_, "AcquisitionFrameRate", frame_rate); - - ROS_DEBUG_STREAM("Current Frame rate: \t " << ptrAcquisitionFrameRate->GetValue()); -} - -void Cm3::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level) -{ - try - { - if (level >= LEVEL_RECONFIGURE_STOP) - setImageControlFormats(config); - - setFrameRate(static_cast(config.acquisition_frame_rate)); - setProperty(node_map_, "AcquisitionFrameRateEnabled", - config.acquisition_frame_rate_enable); // Set enable after frame rate encase its false - - // Set Trigger and Strobe Settings - // NOTE: The trigger must be disabled (i.e. TriggerMode = "Off") in order to configure whether the source is - // software or hardware. - setProperty(node_map_, "TriggerMode", std::string("Off")); - setProperty(node_map_, "TriggerSource", config.trigger_source); - setProperty(node_map_, "TriggerSelector", config.trigger_selector); - setProperty(node_map_, "TriggerActivation", config.trigger_activation_mode); - setProperty(node_map_, "TriggerMode", config.enable_trigger); - - setProperty(node_map_, "LineSelector", config.line_selector); - setProperty(node_map_, "LineMode", config.line_mode); - // setProperty(node_map_, "LineSource", config.line_source); // Not available in CM3 - - // Set auto exposure - setProperty(node_map_, "ExposureMode", config.exposure_mode); - setProperty(node_map_, "ExposureAuto", config.exposure_auto); - - // Set sharpness - if (IsAvailable(node_map_->GetNode("SharpeningEnable"))) - { - setProperty(node_map_, "SharpeningEnable", config.sharpening_enable); - if (config.sharpening_enable) - { - setProperty(node_map_, "SharpeningAuto", config.auto_sharpness); - setProperty(node_map_, "Sharpening", static_cast(config.sharpness)); - setProperty(node_map_, "SharpeningThreshold", static_cast(config.sharpening_threshold)); - } - } - - // Set saturation - if (IsAvailable(node_map_->GetNode("SaturationEnable"))) - { - setProperty(node_map_, "SaturationEnable", config.saturation_enable); - if (config.saturation_enable) - { - setProperty(node_map_, "Saturation", static_cast(config.saturation)); - } - } - - // Set shutter time/speed - if (config.exposure_auto.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "ExposureTime", static_cast(config.exposure_time)); - } - else - { - setProperty(node_map_, "AutoExposureTimeUpperLimit", - static_cast(config.auto_exposure_time_upper_limit)); // Different than BFly S - } - - // Set gain - // setProperty(node_map_, "GainSelector", config.gain_selector); //Not Writeable for CM3 - setProperty(node_map_, "GainAuto", config.auto_gain); - if (config.auto_gain.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "Gain", static_cast(config.gain)); - } - - // Set brightness - setProperty(node_map_, "BlackLevel", static_cast(config.brightness)); - - // Set gamma - if (config.gamma_enable) - { - setProperty(node_map_, "GammaEnabled", config.gamma_enable); // CM3 includes -ed - setProperty(node_map_, "Gamma", static_cast(config.gamma)); - } - - // Set white balance - if (IsAvailable(node_map_->GetNode("BalanceWhiteAuto"))) - { - setProperty(node_map_, "BalanceWhiteAuto", config.auto_white_balance); - if (config.auto_white_balance.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "BalanceRatioSelector", "Blue"); - setProperty(node_map_, "BalanceRatio", static_cast(config.white_balance_blue_ratio)); - setProperty(node_map_, "BalanceRatioSelector", "Red"); - setProperty(node_map_, "BalanceRatio", static_cast(config.white_balance_red_ratio)); - } - } - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[Cm3::setNewConfiguration] Failed to set configuration: " + std::string(e.what())); - } -} - -// Image Size and Pixel Format -void Cm3::setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config) -{ - // Set Binning and Decimation - // setProperty(node_map_, "BinningHorizontal", config.image_format_x_binning); // Not available on CM3 - setProperty(node_map_, "BinningVertical", config.image_format_y_binning); - // setProperty(node_map_, "DecimationHorizontal", config.image_format_x_decimation); - // setProperty(node_map_, "DecimationVertical", config.image_format_y_decimation); - - // Grab the Max values after decimation - Spinnaker::GenApi::CIntegerPtr height_max_ptr = node_map_->GetNode("HeightMax"); - if (!IsAvailable(height_max_ptr) || !IsReadable(height_max_ptr)) - { - throw std::runtime_error("[Cm3::setImageControlFormats] Unable to read HeightMax"); - } - height_max_ = height_max_ptr->GetValue(); - Spinnaker::GenApi::CIntegerPtr width_max_ptr = node_map_->GetNode("WidthMax"); - if (!IsAvailable(width_max_ptr) || !IsReadable(width_max_ptr)) - { - throw std::runtime_error("[Cm3::setImageControlFormats] Unable to read WidthMax"); - } - width_max_ = width_max_ptr->GetValue(); - - // Offset first encase expanding ROI - // Apply offset X - setProperty(node_map_, "OffsetX", 0); - // Apply offset Y - setProperty(node_map_, "OffsetY", 0); - - // Set Width/Height - if (config.image_format_roi_width <= 0 || config.image_format_roi_width > width_max_) - setProperty(node_map_, "Width", width_max_); - else - setProperty(node_map_, "Width", config.image_format_roi_width); - if (config.image_format_roi_height <= 0 || config.image_format_roi_height > height_max_) - setProperty(node_map_, "Height", height_max_); - else - setProperty(node_map_, "Height", config.image_format_roi_height); - - // Apply offset X - setProperty(node_map_, "OffsetX", config.image_format_x_offset); - // Apply offset Y - setProperty(node_map_, "OffsetY", config.image_format_y_offset); - - // Set Pixel Format - setProperty(node_map_, "PixelFormat", config.image_format_color_coding); -} -} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/diagnostics.cpp b/spinnaker_camera_driver/src/diagnostics.cpp deleted file mode 100644 index ceee0c7f..00000000 --- a/spinnaker_camera_driver/src/diagnostics.cpp +++ /dev/null @@ -1,275 +0,0 @@ -/** -Software License Agreement (BSD) - -\file diagnostics.cpp -\authors Michael Lowe -\copyright Copyright (c) 2018, Ascent Robotics, 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 Clearpath Robotics 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 HOLDER 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 "spinnaker_camera_driver/diagnostics.h" - -#include -#include -#include - -namespace spinnaker_camera_driver -{ -DiagnosticsManager::DiagnosticsManager(const std::string name, const std::string serial, - std::shared_ptr const& pub, - const ros::NodeHandle& nh) - : camera_name_(name), serial_number_(serial), diagnostics_pub_(pub), nh_(nh) -{ -} - -DiagnosticsManager::~DiagnosticsManager() -{ -} - -template -void DiagnosticsManager::addDiagnostic(const Spinnaker::GenICam::gcstring name) -{ - T first = 0; - T second = 0; - // Call the overloaded function (use the pair to determine which one) - addDiagnostic(name, false, std::make_pair(first, second)); -} - -template void DiagnosticsManager::addDiagnostic(const Spinnaker::GenICam::gcstring name); - -template void DiagnosticsManager::addDiagnostic(const Spinnaker::GenICam::gcstring name); - -void DiagnosticsManager::addDiagnostic(const Spinnaker::GenICam::gcstring name, bool check_ranges, - std::pair operational, int lower_bound, int upper_bound) -{ - diagnostic_params param{ name, check_ranges, operational, lower_bound, upper_bound }; - integer_params_.push_back(param); -} - -void DiagnosticsManager::addDiagnostic(const Spinnaker::GenICam::gcstring name, bool check_ranges, - std::pair operational, float lower_bound, float upper_bound) -{ - diagnostic_params param{ name, check_ranges, operational, lower_bound, upper_bound }; - float_params_.push_back(param); -} - -void DiagnosticsManager::addAnalyzers() -{ - // Get Namespace - std::string node_name = ros::this_node::getName().substr(1); - std::string node_namespace = ros::this_node::getNamespace(); - std::string node_prefix = ""; - std::string node_path; - std::string node_id = node_name; - - // Create "Fake" Namespace for Diagnostics - if (node_namespace == "/") - { - node_namespace = ros::this_node::getName(); - node_prefix = ros::this_node::getName() + "/"; - } - - // Sanitize Node ID - size_t pos = node_id.find("/"); - while (pos != std::string::npos) - { - node_id.replace(pos, 1, "_"); - pos = node_id.find("/"); - } - - // Sanitize Node Path - node_path = node_id; - pos = node_path.find("_"); - while (pos != std::string::npos) - { - node_path.replace(pos, 1, " "); - pos = node_path.find("_"); - } - - // GroupAnalyzer Parameters - if (!ros::param::has(node_prefix + "analyzers/spinnaker/path")) - { - ros::param::set(node_prefix + "analyzers/spinnaker/path", "Spinnaker"); - ros::param::set(node_prefix + "analyzers/spinnaker/type", "diagnostic_aggregator/AnalyzerGroup"); - } - - // Analyzer Parameters - std::string analyzerPath = node_prefix + "analyzers/spinnaker/analyzers/" + node_id; - if (!ros::param::has(analyzerPath + "/path")) - { - ros::param::set(analyzerPath + "/path", node_path); - ros::param::set(analyzerPath + "/type", "diagnostic_aggregator/GenericAnalyzer"); - ros::param::set(analyzerPath + "/startswith", node_name); - ros::param::set(analyzerPath + "/remove_prefix", node_name); - } - - // Bond to Diagnostics Aggregator - if (bond_ == nullptr) - { - bond_ = std::shared_ptr(new bond::Bond("/diagnostics_agg/bond" + node_namespace, node_namespace)); - } - else if (!bond_->isBroken()) - { - return; - } - bond_->setConnectTimeout(120); - - // Add Diagnostics - diagnostic_msgs::AddDiagnostics srv; - srv.request.load_namespace = node_namespace; - if (!ros::service::waitForService("/diagnostics_agg/add_diagnostics", 1000)) - { - return; - } - bond_->start(); - ros::service::call("/diagnostics_agg/add_diagnostics", srv); -} - -template -diagnostic_msgs::DiagnosticStatus DiagnosticsManager::getDiagStatus(const diagnostic_params& param, const T value) -{ - std::string node_name = ros::this_node::getName().substr(1); - diagnostic_msgs::KeyValue kv; - kv.key = param.parameter_name; - kv.value = std::to_string(value); - - diagnostic_msgs::DiagnosticStatus diag_status; - diag_status.values.push_back(kv); - diag_status.name = node_name + ":" + std::string(param.parameter_name.c_str()); - diag_status.hardware_id = serial_number_; - - // Determine status level - if (!param.check_ranges || (value > param.operational_range.first && value <= param.operational_range.second)) - { - diag_status.level = 0; - diag_status.message = "OK: " + std::string(param.parameter_name) - + " performing in expected operational range."; - } - else if (value >= param.warn_range_lower && value <= param.warn_range_upper) - { - diag_status.level = 1; - diag_status.message = "WARNING: " + std::string(param.parameter_name.c_str()) - + " is not in expected operational range."; - } - else - { - diag_status.level = 2; - diag_status.message = "ERROR: " + std::string(param.parameter_name.c_str()) - + " is in critical operation range."; - } - // Warning Range - kv.key = "Warning Range"; - kv.value = "[" + std::to_string(param.warn_range_lower) + ", " - + std::to_string(param.warn_range_upper) + "]"; - diag_status.values.push_back(kv); - - // Operational Range - kv.key = "Operational Range"; - kv.value = "[" + std::to_string(param.operational_range.first) + ", " - + std::to_string(param.operational_range.second) + "]"; - diag_status.values.push_back(kv); - - return diag_status; -} - -void DiagnosticsManager::processDiagnostics(SpinnakerCamera* spinnaker) -{ - std::string node_name = ros::this_node::getName().substr(1); - diagnostic_msgs::DiagnosticArray diag_array; - - // Manufacturer Info - diagnostic_msgs::DiagnosticStatus diag_manufacture_info; - diag_manufacture_info.name = node_name + ": Manufacture Info"; - diag_manufacture_info.hardware_id = serial_number_; - - for (const std::string param : manufacturer_params_) - { - // Check if Readable - if (!spinnaker->readableProperty(Spinnaker::GenICam::gcstring(param.c_str()))) - { - diagnostic_msgs::KeyValue kv; - kv.key = param; - kv.value = "Property not Available and Readable"; - diag_manufacture_info.values.push_back(kv); - continue; - } - - // Write if Readable - Spinnaker::GenApi::CStringPtr string_ptr = static_cast( - spinnaker->readProperty(Spinnaker::GenICam::gcstring(param.c_str()))); - - diagnostic_msgs::KeyValue kv; - kv.key = param; - kv.value = string_ptr->GetValue(true); - diag_manufacture_info.values.push_back(kv); - } - - diag_array.status.push_back(diag_manufacture_info); - - // Float based parameters - for (const diagnostic_params& param : float_params_) - { - // Check if Readable - if (!spinnaker->readableProperty(Spinnaker::GenICam::gcstring(param.parameter_name))) - { - diagnostic_msgs::KeyValue kv; - kv.key = param.parameter_name; - kv.value = "Property not Available and Readable"; - diag_manufacture_info.values.push_back(kv); - continue; - } - - Spinnaker::GenApi::CFloatPtr float_ptr = - static_cast(spinnaker->readProperty(param.parameter_name)); - - float float_value = float_ptr->GetValue(true); - - diagnostic_msgs::DiagnosticStatus diag_status = getDiagStatus(param, float_value); - diag_array.status.push_back(diag_status); - } - - // Int based parameters - for (const diagnostic_params& param : integer_params_) - { - // Check if Readable - if (!spinnaker->readableProperty(Spinnaker::GenICam::gcstring(param.parameter_name))) - { - diagnostic_msgs::KeyValue kv; - kv.key = param.parameter_name; - kv.value = "Property not Available and Readable"; - diag_manufacture_info.values.push_back(kv); - continue; - } - - Spinnaker::GenApi::CIntegerPtr integer_ptr = - static_cast(spinnaker->readProperty(param.parameter_name)); - - int int_value = integer_ptr->GetValue(true); - diagnostic_msgs::DiagnosticStatus diag_status = getDiagStatus(param, int_value); - diag_array.status.push_back(diag_status); - } - - diagnostics_pub_->publish(diag_array); -} -} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/gh3.cpp b/spinnaker_camera_driver/src/gh3.cpp deleted file mode 100644 index 82910838..00000000 --- a/spinnaker_camera_driver/src/gh3.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/** -Software License Agreement (BSD) - -\file gh3.cpp -\authors Michael Hosmar -\copyright Copyright (c) 2018, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, 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 "spinnaker_camera_driver/gh3.h" - -#include - -namespace spinnaker_camera_driver -{ -Gh3::Gh3(Spinnaker::GenApi::INodeMap* node_map) : Camera(node_map) -{ -} - -Gh3::~Gh3() -{ -} - -void Gh3::setFrameRate(const float frame_rate) -{ - // This enables the "AcquisitionFrameRateEnabled" - //====================================== - setProperty(node_map_, "AcquisitionFrameRateEnabled", true); // different from Bfly S - - // This sets the "AcquisitionFrameRateAuto" to "Off" - //====================================== - setProperty(node_map_, "AcquisitionFrameRateAuto", static_cast("Off")); // different from Bfly S - - // This sets the "AcquisitionFrameRate" to X FPS - // ======================================== - - Spinnaker::GenApi::CFloatPtr ptrAcquisitionFrameRate = node_map_->GetNode("AcquisitionFrameRate"); - ROS_DEBUG_STREAM("Minimum Frame Rate: \t " << ptrAcquisitionFrameRate->GetMin()); - ROS_DEBUG_STREAM("Maximum Frame rate: \t " << ptrAcquisitionFrameRate->GetMax()); - - // Finally Set the Frame Rate - setProperty(node_map_, "AcquisitionFrameRate", frame_rate); - - ROS_DEBUG_STREAM("Current Frame rate: \t " << ptrAcquisitionFrameRate->GetValue()); -} - -void Gh3::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level) -{ - try - { - if (level >= LEVEL_RECONFIGURE_STOP) - setImageControlFormats(config); - - setFrameRate(static_cast(config.acquisition_frame_rate)); - setProperty(node_map_, "AcquisitionFrameRateEnabled", - config.acquisition_frame_rate_enable); // Set enable after frame rate encase its false - - // Set Trigger and Strobe Settings - // NOTE: The trigger must be disabled (i.e. TriggerMode = "Off") in order to configure whether the source is - // software or hardware. - setProperty(node_map_, "TriggerMode", std::string("Off")); - setProperty(node_map_, "TriggerSource", config.trigger_source); - setProperty(node_map_, "TriggerSelector", config.trigger_selector); - setProperty(node_map_, "TriggerActivation", config.trigger_activation_mode); - setProperty(node_map_, "TriggerMode", config.enable_trigger); - - setProperty(node_map_, "LineSelector", config.line_selector); - setProperty(node_map_, "LineMode", config.line_mode); - // setProperty(node_map_, "LineSource", config.line_source); // Not available in GH3 - - // Set auto exposure - setProperty(node_map_, "ExposureMode", config.exposure_mode); - setProperty(node_map_, "ExposureAuto", config.exposure_auto); - - // Set sharpness - if (IsAvailable(node_map_->GetNode("SharpeningEnable"))) - { - setProperty(node_map_, "SharpeningEnable", config.sharpening_enable); - if (config.sharpening_enable) - { - setProperty(node_map_, "SharpeningAuto", config.auto_sharpness); - setProperty(node_map_, "Sharpening", static_cast(config.sharpness)); - setProperty(node_map_, "SharpeningThreshold", static_cast(config.sharpening_threshold)); - } - } - - // Set saturation - if (IsAvailable(node_map_->GetNode("SaturationEnable"))) - { - setProperty(node_map_, "SaturationEnable", config.saturation_enable); - if (config.saturation_enable) - { - setProperty(node_map_, "Saturation", static_cast(config.saturation)); - } - } - - // Set shutter time/speed - if (config.exposure_auto.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "ExposureTime", static_cast(config.exposure_time)); - } - else - { - setProperty(node_map_, "AutoExposureTimeUpperLimit", - static_cast(config.auto_exposure_time_upper_limit)); // Different than BFly S - } - - // Set gain - // setProperty(node_map_, "GainSelector", config.gain_selector); //Not Writeable for GH3 - setProperty(node_map_, "GainAuto", config.auto_gain); - if (config.auto_gain.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "Gain", static_cast(config.gain)); - } - - // Set brightness - setProperty(node_map_, "BlackLevel", static_cast(config.brightness)); - - // Set gamma - if (config.gamma_enable) - { - setProperty(node_map_, "GammaEnabled", config.gamma_enable); // GH3 includes -ed - setProperty(node_map_, "Gamma", static_cast(config.gamma)); - } - - // Set white balance - if (IsAvailable(node_map_->GetNode("BalanceWhiteAuto"))) - { - setProperty(node_map_, "BalanceWhiteAuto", config.auto_white_balance); - if (config.auto_white_balance.compare(std::string("Off")) == 0) - { - setProperty(node_map_, "BalanceRatioSelector", "Blue"); - setProperty(node_map_, "BalanceRatio", static_cast(config.white_balance_blue_ratio)); - setProperty(node_map_, "BalanceRatioSelector", "Red"); - setProperty(node_map_, "BalanceRatio", static_cast(config.white_balance_red_ratio)); - } - } - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[Gh3::setNewConfiguration] Failed to set configuration: " + std::string(e.what())); - } -} - -// Image Size and Pixel Format -void Gh3::setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config) -{ - // Set Binning and Decimation - // setProperty(node_map_, "BinningHorizontal", config.image_format_x_binning); // Not available on GH3 - setProperty(node_map_, "BinningVertical", config.image_format_y_binning); - // setProperty(node_map_, "DecimationHorizontal", config.image_format_x_decimation); - // setProperty(node_map_, "DecimationVertical", config.image_format_y_decimation); - - // Grab the Max values after decimation - Spinnaker::GenApi::CIntegerPtr height_max_ptr = node_map_->GetNode("HeightMax"); - if (!IsAvailable(height_max_ptr) || !IsReadable(height_max_ptr)) - { - throw std::runtime_error("[Gh3::setImageControlFormats] Unable to read HeightMax"); - } - height_max_ = height_max_ptr->GetValue(); - Spinnaker::GenApi::CIntegerPtr width_max_ptr = node_map_->GetNode("WidthMax"); - if (!IsAvailable(width_max_ptr) || !IsReadable(width_max_ptr)) - { - throw std::runtime_error("[Gh3::setImageControlFormats] Unable to read WidthMax"); - } - width_max_ = width_max_ptr->GetValue(); - - // Offset first encase expanding ROI - // Apply offset X - setProperty(node_map_, "OffsetX", 0); - // Apply offset Y - setProperty(node_map_, "OffsetY", 0); - - // Set Width/Height - if (config.image_format_roi_width <= 0 || config.image_format_roi_width > width_max_) - setProperty(node_map_, "Width", width_max_); - else - setProperty(node_map_, "Width", config.image_format_roi_width); - if (config.image_format_roi_height <= 0 || config.image_format_roi_height > height_max_) - setProperty(node_map_, "Height", height_max_); - else - setProperty(node_map_, "Height", config.image_format_roi_height); - - // Apply offset X - setProperty(node_map_, "OffsetX", config.image_format_x_offset); - // Apply offset Y - setProperty(node_map_, "OffsetY", config.image_format_y_offset); - - // Set Pixel Format - setProperty(node_map_, "PixelFormat", config.image_format_color_coding); -} -} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/node.cpp b/spinnaker_camera_driver/src/node.cpp deleted file mode 100644 index 030a01c5..00000000 --- a/spinnaker_camera_driver/src/node.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* -This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie -Mellon University. -Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012. -Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution -Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, Carnegie Mellon University. All rights reserved. -Copyright (c) 2018, Clearpath Robotics, 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 Carnegie Mellon University 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 HOLDER 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. -*/ - -/** - @file node.cpp - @author Chad Rockey - @date July 13, 2011 - @brief ROS node to wrap the nodelet for standalone rosrun execution - - @attention Copyright (C) 2011 - @attention National Robotics Engineering Center - @attention Carnegie Mellon University -*/ - -#include "ros/ros.h" -#include - -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "spinnaker_camera_node"); - - // This is code based nodelet loading, the preferred nodelet launching is done through roslaunch - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - std::string nodelet_name = ros::this_node::getName(); - nodelet.load(nodelet_name, "spinnaker_camera_driver/SpinnakerCameraNodelet", remap, nargv); - - ros::spin(); - - return 0; -} diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp deleted file mode 100644 index 1c8588d0..00000000 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ /dev/null @@ -1,729 +0,0 @@ -/* -This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie -Mellon University. -Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012. -Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution -Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, Carnegie Mellon University. All rights reserved. -Copyright (c) 2018, Clearpath Robotics, 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 Carnegie Mellon University 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 HOLDER 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. -*/ - -/** - @file nodelet.cpp - @author Chad Rockey - @date July 13, 2011 - @brief ROS nodelet for the Point Grey Chameleon Camera - - @attention Copyright (C) 2011 - @attention National Robotics Engineering Center - @attention Carnegie Mellon University -*/ - -/** - @file nodelet.cpp - @author Teyvonia Thomas - @date August 28, 2017 - @brief ROS nodelet for the Point Grey Chameleon Camera - Updated to use Spinnaker driver insteady of Flycapture -*/ - -// ROS and associated nodelet interface and PLUGINLIB declaration header -#include "ros/ros.h" -#include -#include - -#include "spinnaker_camera_driver/SpinnakerCamera.h" // The actual standalone library for the Spinnakers -#include "spinnaker_camera_driver/diagnostics.h" - -#include // ROS library that allows sending compressed images -#include // ROS library that publishes CameraInfo topics -#include // ROS message header for CameraInfo - -#include -#include // Message type for configuring gain and white balance. - -#include // Headers for publishing diagnostic messages. -#include - -#include // Needed for the nodelet to launch the reading thread. - -#include // Needed for the dynamic_reconfigure gui service to run - -#include -#include -#include -#include - -namespace spinnaker_camera_driver -{ -class SpinnakerCameraNodelet : public nodelet::Nodelet -{ -public: - SpinnakerCameraNodelet() - { - } - - ~SpinnakerCameraNodelet() - { - std::lock_guard scopedLock(connect_mutex_); - - if (diagThread_) - { - diagThread_->interrupt(); - diagThread_->join(); - } - - if (pubThread_) - { - pubThread_->interrupt(); - pubThread_->join(); - - try - { - NODELET_DEBUG_ONCE("Stopping camera capture."); - spinnaker_.stop(); - NODELET_DEBUG_ONCE("Disconnecting from camera."); - spinnaker_.disconnect(); - } - catch (const std::runtime_error& e) - { - NODELET_ERROR("%s", e.what()); - } - } - } - -private: - /*! - * \brief Function that allows reconfiguration of the camera. - * - * This function serves as a callback for the dynamic reconfigure service. It simply passes the configuration object - * to the driver to allow the camera to reconfigure. - * \param config camera_library::CameraConfig object passed by reference. Values will be changed to those the driver - * is currently using. - * \param level driver_base reconfiguration level. See driver_base/SensorLevels.h for more information. - */ - - void paramCallback(const spinnaker_camera_driver::SpinnakerConfig& config, uint32_t level) - { - config_ = config; - - try - { - NODELET_DEBUG_ONCE("Dynamic reconfigure callback with level: %u", level); - spinnaker_.setNewConfiguration(config, level); - - // Store needed parameters for the metadata message - gain_ = config.gain; - wb_blue_ = config.white_balance_blue_ratio; - wb_red_ = config.white_balance_red_ratio; - - // No separate param in CameraInfo for binning/decimation - binning_x_ = config.image_format_x_binning * config.image_format_x_decimation; - binning_y_ = config.image_format_y_binning * config.image_format_y_decimation; - - // Store CameraInfo RegionOfInterest information - // TODO(mhosmar): Not compliant with CameraInfo message: "A particular ROI always denotes the - // same window of pixels on the camera sensor, regardless of binning settings." - // These values are in the post binned frame. - if ((config.image_format_roi_width + config.image_format_roi_height) > 0 && - (config.image_format_roi_width < spinnaker_.getWidthMax() || - config.image_format_roi_height < spinnaker_.getHeightMax())) - { - roi_x_offset_ = config.image_format_x_offset; - roi_y_offset_ = config.image_format_y_offset; - roi_width_ = config.image_format_roi_width; - roi_height_ = config.image_format_roi_height; - do_rectify_ = true; // Set to true if an ROI is used. - } - else - { - // Zeros mean the full resolution was captured. - roi_x_offset_ = 0; - roi_y_offset_ = 0; - roi_height_ = 0; - roi_width_ = 0; - do_rectify_ = false; // Set to false if the whole image is captured. - } - } - catch (std::runtime_error& e) - { - NODELET_ERROR("Reconfigure Callback failed with error: %s", e.what()); - } - } - - void diagCb() - { - if (!diagThread_) // We need to connect - { - // Start the thread to loop through and publish messages - diagThread_.reset( - new boost::thread(boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::diagPoll, this))); - } - } - - /*! - * \brief Connection callback to only do work when someone is listening. - * - * This function will connect/disconnect from the camera depending on who is using the output. - */ - void connectCb() - { - if (!pubThread_) // We need to connect - { - // Start the thread to loop through and publish messages - pubThread_.reset( - new boost::thread(boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::devicePoll, this))); - } - - // @tthomas - removing subscriber check and logic below as it's leading to mutex locks and crashes currently - /* - NODELET_DEBUG_ONCE("Connect callback!"); - std::lock_guard scopedLock(connect_mutex_); // Grab the mutex. Wait until we're done initializing - before letting this function through. - // Check if we should disconnect (there are 0 subscribers to our data) - if(it_pub_.getNumSubscribers() == 0 && pub_->getPublisher().getNumSubscribers() == 0) - { - if (pubThread_) - { - NODELET_DEBUG_ONCE("Disconnecting."); - pubThread_->interrupt(); - scopedLock.unlock(); - pubThread_->join(); - scopedLock.lock(); - pubThread_.reset(); - sub_.shutdown(); - - try - { - NODELET_DEBUG_ONCE("Stopping camera capture."); - spinnaker_.stop(); - } - catch(std::runtime_error& e) - { - NODELET_ERROR("%s", e.what()); - } - - try - { - NODELET_DEBUG_ONCE("Disconnecting from camera."); - spinnaker_.disconnect(); - } - catch(std::runtime_error& e) - { - NODELET_ERROR("%s", e.what()); - } - } - } - else if(!pubThread_) // We need to connect - { - // Start the thread to loop through and publish messages - pubThread_.reset(new boost::thread(boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::devicePoll, - this))); - } - else - { - NODELET_DEBUG_ONCE("Do nothing in callback."); - } - */ - } - - /*! - * \brief Serves as a psuedo constructor for nodelets. - * - * This function needs to do the MINIMUM amount of work to get the nodelet running. Nodelets should not call blocking - * functions here. - */ - void onInit() - { - // Get nodeHandles - ros::NodeHandle& nh = getMTNodeHandle(); - ros::NodeHandle& pnh = getMTPrivateNodeHandle(); - - // Get a serial number through ros - int serial = 0; - - XmlRpc::XmlRpcValue serial_xmlrpc; - pnh.getParam("serial", serial_xmlrpc); - if (serial_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeInt) - { - pnh.param("serial", serial, 0); - } - else if (serial_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString) - { - std::string serial_str; - pnh.param("serial", serial_str, "0"); - std::istringstream(serial_str) >> serial; - } - else - { - NODELET_DEBUG_ONCE("Serial XMLRPC type."); - serial = 0; - } - - std::string camera_serial_path; - pnh.param("camera_serial_path", camera_serial_path, ""); - NODELET_DEBUG_ONCE("Camera serial path %s", camera_serial_path.c_str()); - // If serial has been provided directly as a param, ignore the path - // to read in the serial from. - while (serial == 0 && !camera_serial_path.empty()) - { - serial = readSerialAsHexFromFile(camera_serial_path); - if (serial == 0) - { - NODELET_WARN_ONCE("Waiting for camera serial path to become available"); - ros::Duration(1.0).sleep(); // Sleep for 1 second, wait for serial device path to become available - } - } - - NODELET_DEBUG_ONCE("Using camera serial %d", serial); - - spinnaker_.setDesiredCamera((uint32_t)serial); - - // Get GigE camera parameters: - pnh.param("packet_size", packet_size_, 1400); - pnh.param("auto_packet_size", auto_packet_size_, true); - pnh.param("packet_delay", packet_delay_, 4000); - - // TODO(mhosmar): Set GigE parameters: - // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_); - - // Get the location of our camera config yaml - std::string camera_info_url; - pnh.param("camera_info_url", camera_info_url, ""); - // Get the desired frame_id, set to 'camera' if not found - pnh.param("frame_id", frame_id_, "camera"); - // Do not call the connectCb function until after we are done initializing. - std::lock_guard scopedLock(connect_mutex_); - - // Start up the dynamic_reconfigure service, note that this needs to stick around after this function ends - srv_ = std::make_shared >(pnh); - dynamic_reconfigure::Server::CallbackType f = - boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::paramCallback, this, _1, _2); - - srv_->setCallback(f); - - // queue size of ros publisher - int queue_size; - pnh.param("queue_size", queue_size, 5); - - // Start the camera info manager and attempt to load any configurations - std::stringstream cinfo_name; - cinfo_name << serial; - cinfo_.reset(new camera_info_manager::CameraInfoManager(nh, cinfo_name.str(), camera_info_url)); - - // Publish topics using ImageTransport through camera_info_manager (gives cool things like compression) - it_.reset(new image_transport::ImageTransport(nh)); - image_transport::SubscriberStatusCallback cb = boost::bind(&SpinnakerCameraNodelet::connectCb, this); - it_pub_ = it_->advertiseCamera("image_raw", queue_size, cb, cb); - - // Set up diagnostics - updater_.setHardwareID("spinnaker_camera " + cinfo_name.str()); - - // Set up a diagnosed publisher - double desired_freq; - std::string device_type; - pnh.param("device_type", device_type, "USB3"); - pnh.param("desired_freq", desired_freq, 30.0); - pnh.param("min_freq", min_freq_, desired_freq); - pnh.param("max_freq", max_freq_, desired_freq); - double freq_tolerance; // Tolerance before stating error on publish frequency, fractional percent of desired - // frequencies. - pnh.param("freq_tolerance", freq_tolerance, 0.1); - int window_size; // Number of samples to consider in frequency - pnh.param("window_size", window_size, 30); - double min_acceptable; // The minimum publishing delay (in seconds) before warning. Negative values mean future - // dated messages. - pnh.param("min_acceptable_delay", min_acceptable, 0.0); - double max_acceptable; // The maximum publishing delay (in seconds) before warning. - pnh.param("max_acceptable_delay", max_acceptable, 0.2); - ros::SubscriberStatusCallback cb2 = boost::bind(&SpinnakerCameraNodelet::connectCb, this); - pub_.reset(new diagnostic_updater::DiagnosedPublisher( - nh.advertise("image", queue_size, cb2, cb2), - updater_, - diagnostic_updater::FrequencyStatusParam(&min_freq_, - &max_freq_, - freq_tolerance, - window_size), - diagnostic_updater::TimeStampStatusParam(min_acceptable, - max_acceptable))); - - // Set up diagnostics aggregator publisher and diagnostics manager - ros::SubscriberStatusCallback diag_cb = boost::bind(&SpinnakerCameraNodelet::diagCb, this); - diagnostics_pub_.reset(new ros::Publisher( - nh.advertise("/diagnostics", 1, diag_cb, diag_cb))); - - diag_man = std::unique_ptr(new DiagnosticsManager( - frame_id_, std::to_string(spinnaker_.getSerial()), diagnostics_pub_, nh)); - diag_man->addDiagnostic("DeviceTemperature", true, std::make_pair(0.0f, 90.0f), -10.0f, 95.0f); - diag_man->addDiagnostic("AcquisitionResultingFrameRate", true, std::make_pair(10.0f, 60.0f), 5.0f, 90.0f); - diag_man->addDiagnostic("PowerSupplyVoltage", true, std::make_pair(4.5f, 5.2f), 4.4f, 5.3f); - diag_man->addDiagnostic("PowerSupplyCurrent", true, std::make_pair(0.4f, 0.6f), 0.3f, 1.0f); - diag_man->addDiagnostic("DeviceUptime"); - if (device_type.compare("USB3") == 0 ) - { - diag_man->addDiagnostic("U3VMessageChannelID"); - } - } - - /** - * @brief Reads in the camera serial from a specified file path. - * The format of the serial is expected to be base 16. - * @param camera_serial_path The path of where to read in the serial from. Generally this - * is a USB device path to the serial file. - * @return int The serial number for the given path, 0 if failure. - */ - int readSerialAsHexFromFile(std::string camera_serial_path) - { - NODELET_DEBUG_ONCE("Reading camera serial file from: %s", camera_serial_path.c_str()); - - std::ifstream serial_file(camera_serial_path.c_str()); - std::stringstream buffer; - int serial = 0; - - if (serial_file.is_open()) - { - std::string serial_str((std::istreambuf_iterator(serial_file)), std::istreambuf_iterator()); - NODELET_DEBUG_ONCE("Serial file contents: %s", serial_str.c_str()); - buffer << std::hex << serial_str; - buffer >> serial; - NODELET_DEBUG_ONCE("Serial discovered %d", serial); - - return serial; - } - - NODELET_WARN_ONCE("Unable to open serial path: %s", camera_serial_path.c_str()); - return 0; - } - - void diagPoll() - { - diag_man->addAnalyzers(); - while (!boost::this_thread::interruption_requested()) // Block until we need - // to stop this - // thread. - { - diag_man->processDiagnostics(&spinnaker_); - } - } - - /*! - * \brief Function for the boost::thread to grabImages and publish them. - * - * This function continues until the thread is interupted. Responsible for getting sensor_msgs::Image and publishing - * them. - */ - void devicePoll() - { - ROS_INFO_ONCE("devicePoll"); - - enum State - { - NONE, - ERROR, - STOPPED, - DISCONNECTED, - CONNECTED, - STARTED - }; - - State state = DISCONNECTED; - State previous_state = NONE; - - while (!boost::this_thread::interruption_requested()) // Block until we need to stop this thread. - { - bool state_changed = state != previous_state; - - previous_state = state; - - switch (state) - { - case ERROR: -// Generally there's no need to stop before disconnecting after an -// error. Indeed, stop will usually fail. -#if STOP_ON_ERROR - // Try stopping the camera - { - std::lock_guard scopedLock(connect_mutex_); - sub_.shutdown(); - } - - try - { - NODELET_DEBUG_ONCE("Stopping camera."); - spinnaker_.stop(); - NODELET_DEBUG_ONCE("Stopped camera."); - - state = STOPPED; - } - catch (std::runtime_error& e) - { - if (state_changed) - { - NODELET_ERROR("Failed to stop with error: %s", e.what()); - ros::Duration(1.0).sleep(); // sleep for one second each time - } - state = ERROR; - } - - break; -#endif - case STOPPED: - // Try disconnecting from the camera - try - { - NODELET_DEBUG("Disconnecting from camera."); - spinnaker_.disconnect(); - NODELET_DEBUG("Disconnected from camera."); - - state = DISCONNECTED; - } - catch (std::runtime_error& e) - { - if (state_changed) - { - NODELET_ERROR("Failed to disconnect with error: %s", e.what()); - ros::Duration(1.0).sleep(); // sleep for one second each time - } - state = ERROR; - } - - break; - case DISCONNECTED: - // Try connecting to the camera - try - { - NODELET_DEBUG("Connecting to camera."); - - spinnaker_.connect(); - - NODELET_DEBUG("Connected to camera."); - - // Set last configuration, forcing the reconfigure level to stop - spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP); - - // Set the timeout for grabbing images. - try - { - double timeout; - getMTPrivateNodeHandle().param("timeout", timeout, 1.0); - - NODELET_DEBUG_ONCE("Setting timeout to: %f.", timeout); - spinnaker_.setTimeout(timeout); - } - catch (const std::runtime_error& e) - { - NODELET_ERROR("%s", e.what()); - } - - // Subscribe to gain and white balance changes - { - std::lock_guard scopedLock(connect_mutex_); - sub_ = - getMTNodeHandle().subscribe("image_exposure_sequence", 10, - &spinnaker_camera_driver::SpinnakerCameraNodelet::gainWBCallback, this); - } - state = CONNECTED; - } - catch (const std::runtime_error& e) - { - if (state_changed) - { - NODELET_ERROR("Failed to connect with error: %s", e.what()); - ros::Duration(1.0).sleep(); // sleep for one second each time - } - state = ERROR; - } - - break; - case CONNECTED: - // Try starting the camera - try - { - NODELET_DEBUG("Starting camera."); - spinnaker_.start(); - NODELET_DEBUG("Started camera."); - NODELET_DEBUG("Attention: if nothing subscribes to the camera topic, the camera_info is not published " - "on the correspondent topic."); - state = STARTED; - } - catch (std::runtime_error& e) - { - if (state_changed) - { - NODELET_ERROR("Failed to start with error: %s", e.what()); - ros::Duration(1.0).sleep(); // sleep for one second each time - } - state = ERROR; - } - - break; - case STARTED: - try - { - wfov_camera_msgs::WFOVImagePtr wfov_image(new wfov_camera_msgs::WFOVImage); - // Get the image from the camera library - NODELET_DEBUG_ONCE("Starting a new grab from camera with serial {%d}.", spinnaker_.getSerial()); - spinnaker_.grabImage(&wfov_image->image, frame_id_); - - // Set other values - wfov_image->header.frame_id = frame_id_; - - wfov_image->gain = gain_; - wfov_image->white_balance_blue = wb_blue_; - wfov_image->white_balance_red = wb_red_; - - // wfov_image->temperature = spinnaker_.getCameraTemperature(); - - ros::Time time = ros::Time::now() + ros::Duration(config_.time_offset); - wfov_image->header.stamp = time; - wfov_image->image.header.stamp = time; - - // Set the CameraInfo message - ci_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo())); - ci_->header.stamp = wfov_image->image.header.stamp; - ci_->header.frame_id = wfov_image->header.frame_id; - // The height, width, distortion model, and parameters are all filled in by camera info manager. - ci_->binning_x = binning_x_; - ci_->binning_y = binning_y_; - ci_->roi.x_offset = roi_x_offset_; - ci_->roi.y_offset = roi_y_offset_; - ci_->roi.height = roi_height_; - ci_->roi.width = roi_width_; - ci_->roi.do_rectify = do_rectify_; - - wfov_image->info = *ci_; - - // Publish the full message - pub_->publish(wfov_image); - - // Publish the message using standard image transport - if (it_pub_.getNumSubscribers() > 0) - { - sensor_msgs::ImagePtr image(new sensor_msgs::Image(wfov_image->image)); - it_pub_.publish(image, ci_); - } - } - catch (CameraTimeoutException& e) - { - NODELET_WARN("%s", e.what()); - } - - catch (std::runtime_error& e) - { - NODELET_ERROR("%s", e.what()); - state = ERROR; - } - - break; - default: - NODELET_ERROR("Unknown camera state %d!", state); - } - - // Update diagnostics - updater_.update(); - } - NODELET_DEBUG_ONCE("Leaving thread."); - } - - void gainWBCallback(const image_exposure_msgs::ExposureSequence& msg) - { - try - { - NODELET_DEBUG_ONCE("Gain callback: Setting gain to %f and white balances to %u, %u", msg.gain, - msg.white_balance_blue, msg.white_balance_red); - gain_ = msg.gain; - - spinnaker_.setGain(static_cast(gain_)); - wb_blue_ = msg.white_balance_blue; - wb_red_ = msg.white_balance_red; - - // TODO(mhosmar): - // spinnaker_.setBRWhiteBalance(false, wb_blue_, wb_red_); - } - catch (std::runtime_error& e) - { - NODELET_ERROR("gainWBCallback failed with error: %s", e.what()); - } - } - - /* Class Fields */ - std::shared_ptr > srv_; ///< Needed to - /// initialize - /// and keep the - /// dynamic_reconfigure::Server - /// in scope. - std::shared_ptr it_; ///< Needed to initialize and keep the ImageTransport in - /// scope. - std::shared_ptr cinfo_; ///< Needed to initialize and keep the - /// CameraInfoManager in scope. - image_transport::CameraPublisher it_pub_; ///< CameraInfoManager ROS publisher - std::shared_ptr > pub_; ///< Diagnosed - std::shared_ptr diagnostics_pub_; - /// publisher, has to be - /// a pointer because of - /// constructor - /// requirements - ros::Subscriber sub_; ///< Subscriber for gain and white balance changes. - - std::mutex connect_mutex_; - - diagnostic_updater::Updater updater_; ///< Handles publishing diagnostics messages. - double min_freq_; - double max_freq_; - - SpinnakerCamera spinnaker_; ///< Instance of the SpinnakerCamera library, used to interface with the hardware. - sensor_msgs::CameraInfoPtr ci_; ///< Camera Info message. - std::string frame_id_; ///< Frame id for the camera messages, defaults to 'camera' - std::shared_ptr pubThread_; ///< The thread that reads and publishes the images. - std::shared_ptr diagThread_; ///< The thread that reads and publishes the diagnostics. - - std::unique_ptr diag_man; - - double gain_; - uint16_t wb_blue_; - uint16_t wb_red_; - - // Parameters for cameraInfo - size_t binning_x_; ///< Camera Info pixel binning along the image x axis. - size_t binning_y_; ///< Camera Info pixel binning along the image y axis. - size_t roi_x_offset_; ///< Camera Info ROI x offset - size_t roi_y_offset_; ///< Camera Info ROI y offset - size_t roi_height_; ///< Camera Info ROI height - size_t roi_width_; ///< Camera Info ROI width - bool do_rectify_; ///< Whether or not to rectify as if part of an image. Set to false if whole image, and true if in - /// ROI mode. - - // For GigE cameras: - /// If true, GigE packet size is automatically determined, otherwise packet_size_ is used: - bool auto_packet_size_; - /// GigE packet size: - int packet_size_; - /// GigE packet delay: - int packet_delay_; - - /// Configuration: - spinnaker_camera_driver::SpinnakerConfig config_; -}; - -PLUGINLIB_EXPORT_CLASS(spinnaker_camera_driver::SpinnakerCameraNodelet, - nodelet::Nodelet) // Needed for Nodelet declaration -} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/spinnaker_test_node.cpp b/spinnaker_camera_driver/src/spinnaker_test_node.cpp deleted file mode 100644 index 1fd6c511..00000000 --- a/spinnaker_camera_driver/src/spinnaker_test_node.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/** -Software License Agreement (proprietary) - -\file spinnaker_test_node.cpp -\authors Teyvonia Thomas -\copyright Copyright (c) 2017, Clearpath Robotics, Inc., All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, is not permitted without the express -permission of Clearpath Robotics. -*/ - -// ROS Includes -#include "ros/ros.h" - -// Spinnaker Includes -#include "Spinnaker.h" -// #include "SpinGenApi/SpinnakerGenApi.h" - -namespace spinnaker_camera_driver -{ -class SpinnakerTestNode -{ -public: - SpinnakerTestNode(); - - void test(); -}; - -SpinnakerTestNode::SpinnakerTestNode() -{ - test(); -} - -void SpinnakerTestNode::test() -{ - Spinnaker::SystemPtr system = Spinnaker::System::GetInstance(); - - Spinnaker::InterfaceList interfaceList = system->GetInterfaces(); - unsigned int numInterfaces = interfaceList.GetSize(); - std::printf("\033[93m[Spinnaker] Number of interfaces detected: %d \n", numInterfaces); - - Spinnaker::CameraList camList = system->GetCameras(); - unsigned int numCameras = camList.GetSize(); - - std::printf("\033[93m[Spinnaker] # of connected cameras: %d \n", numCameras); - - // Finish if there are no cameras - if (numCameras == 0) - { - std::printf("\033[91mNO Cameras Connected! \n\n"); - // Clear camera list before releasing system - camList.Clear(); - interfaceList.Clear(); - // Release system - system->ReleaseInstance(); - - return; - } - else - { - for (unsigned int i = 0; i < numCameras; i++) - { - Spinnaker::CameraPtr pCam = camList[i]; - Spinnaker::GenApi::INodeMap& nodeMapTLDevice = pCam->GetTLDeviceNodeMap(); - Spinnaker::GenApi::CStringPtr ptrDeviceSerialNumber = nodeMapTLDevice.GetNode("DeviceSerialNumber"); - if (Spinnaker::GenApi::IsAvailable(ptrDeviceSerialNumber) && Spinnaker::GenApi::IsReadable(ptrDeviceSerialNumber)) - { - std::cout << "\033[92m[" << i << "]\t" << ptrDeviceSerialNumber->ToString() << std::endl; - } - } - } - camList.Clear(); - interfaceList.Clear(); - system->ReleaseInstance(); -} -} // namespace spinnaker_camera_driver - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "spinnaker_test_node"); - spinnaker_camera_driver::SpinnakerTestNode node; - ros::spin(); - return 0; -} From 4b7e5e18fe10caafb57516bd6c3343d47673e88a Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:15:27 -0400 Subject: [PATCH 03/11] modify the description package for ROS2, add documentation --- flir_camera_description/CHANGELOG.rst | 65 ------------- flir_camera_description/CMakeLists.txt | 22 +++-- flir_camera_description/LICENSE | 26 +++++ flir_camera_description/README.md | 17 ++++ .../images/blackfly_s_description.png | Bin 0 -> 39099 bytes flir_camera_description/launch/demo.launch.py | 92 ++++++++++++++++++ ...flir_blackflys.stl => flir_blackfly_s.stl} | Bin flir_camera_description/package.xml | 25 +++-- flir_camera_description/urdf/demo.urdf.xacro | 20 ++++ ....urdf.xacro => flir_blackfly_s.urdf.xacro} | 6 +- 10 files changed, 190 insertions(+), 83 deletions(-) delete mode 100644 flir_camera_description/CHANGELOG.rst create mode 100644 flir_camera_description/LICENSE create mode 100644 flir_camera_description/README.md create mode 100644 flir_camera_description/images/blackfly_s_description.png create mode 100644 flir_camera_description/launch/demo.launch.py rename flir_camera_description/meshes/{flir_blackflys.stl => flir_blackfly_s.stl} (100%) create mode 100644 flir_camera_description/urdf/demo.urdf.xacro rename flir_camera_description/urdf/{flir_blackflys.urdf.xacro => flir_blackfly_s.urdf.xacro} (97%) diff --git a/flir_camera_description/CHANGELOG.rst b/flir_camera_description/CHANGELOG.rst deleted file mode 100644 index 14f3e9d4..00000000 --- a/flir_camera_description/CHANGELOG.rst +++ /dev/null @@ -1,65 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package pointgrey_camera_description -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.2.2 (2022-03-28) ------------------- - -0.2.1 (2022-03-21) ------------------- - -0.2.0 (2022-03-11) ------------------- -* Bump CMake version to avoid CMP0048 warning. -* Bumped flir_camera_description verison. -* Changes. -* Merge pull request `#91 `_ from luis-camero/noetic-devel - ROS Industrial CI -* Removed launch and rviz folders from CMakeLists -* URDF Description, Diagnostics, ISP Enable, and Launch Files (`#81 `_) - * Changes required to use GigE Blackfly S version - * Added blackfly mesh - * Added URDF of blackflys and CHANGELOG - * Added new_line at end of flir_blackflys.urdf.xacro - * Added DiagnosticAnalyzers and more detailed diagnostic messages - * Added ISP enable and disable config and updated camera launch file to be more descriptive - * Switched order of configuration to put ISP enable next to color encoding - * Updated config to include enumeration for Off, Once, Continuous parameters, and udpated diagnostics.launch - * Handled issue where no namespace prevents diagnostics_agg from loading from analyzer paramaters -* Contributors: Tony Baltovski, luis-camero - -0.2.5 (2023-01-06) ------------------- - -0.2.4 (2023-01-06) ------------------- - -0.2.3 (2022-04-19) ------------------- -* 0.2.2 -* Changes. -* 0.2.1 -* Changes. -* 0.2.0 -* Changes. -* Bump CMake version to avoid CMP0048 warning. -* Bumped flir_camera_description verison. -* Changes. -* Removed launch and rviz folders from CMakeLists -* URDF Description, Diagnostics, ISP Enable, and Launch Files (`#81 `_) - * Changes required to use GigE Blackfly S version - * Added blackfly mesh - * Added URDF of blackflys and CHANGELOG - * Added new_line at end of flir_blackflys.urdf.xacro - * Added DiagnosticAnalyzers and more detailed diagnostic messages - * Added ISP enable and disable config and updated camera launch file to be more descriptive - * Switched order of configuration to put ISP enable next to color encoding - * Updated config to include enumeration for Off, Once, Continuous parameters, and udpated diagnostics.launch - * Handled issue where no namespace prevents diagnostics_agg from loading from analyzer paramaters -* Contributors: Tony Baltovski, luis-camero - -0.1.0 (2021-11-10) -------------------- -* Initial Release -* Contributors: Luis Camero - diff --git a/flir_camera_description/CMakeLists.txt b/flir_camera_description/CMakeLists.txt index 5d4674f4..5eddd1be 100644 --- a/flir_camera_description/CMakeLists.txt +++ b/flir_camera_description/CMakeLists.txt @@ -1,10 +1,20 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(flir_camera_description) -find_package(catkin REQUIRED) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(urdf REQUIRED) +find_package(xacro REQUIRED) +find_package(robot_state_publisher REQUIRED) -catkin_package() - -install(DIRECTORY meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/flir_camera_description/LICENSE b/flir_camera_description/LICENSE new file mode 100644 index 00000000..92f0fdf6 --- /dev/null +++ b/flir_camera_description/LICENSE @@ -0,0 +1,26 @@ +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 copyright holder 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. diff --git a/flir_camera_description/README.md b/flir_camera_description/README.md new file mode 100644 index 00000000..ec69f18e --- /dev/null +++ b/flir_camera_description/README.md @@ -0,0 +1,17 @@ +# flir_camera_description + +This package contains urdf files and meshes for FLIR cameras. +![banner image](images/blackfly_s_description.png) + +## How to use +Install this package like any other ROS package, then launch the demo: +``` +ros2 launch flir_camera_description demo.launch.py +ros2 run rviz2 rviz2 +``` +In rviz, add a "RobotModel" and set the topic to "robot_description". + + +## License: +Licensed under BSD License. + diff --git a/flir_camera_description/images/blackfly_s_description.png b/flir_camera_description/images/blackfly_s_description.png new file mode 100644 index 0000000000000000000000000000000000000000..82528751ba17f2297b3085e778613151fc111176 GIT binary patch literal 39099 zcmdSAWmr^i)IN%+fHX*`q$4E_Dh#bSgn)DjLr4q4P|}?P$RI5(C?Jf0h%hwLUD6Um zcXxXp{JsBk-t*~vK3u@Q81{Z*JuB{YueHNemE}kX?-AnQ;E=qKhpFS>T>pcEgPTQw z3qHwldPD#&x9sI#IpN@teZTs-w&IgogM-72^8)rv(=BysCD zTH~Mn{4EOw1`E!-JzWY9Sp|~x?tW=AUO$6Z&-TA=3cJm&oOURu5T?g0N`~SbZyg2g)iw}BUgt-q%>q$ob)XO$6GUP1% zIS}07&g;55U3`szZx)J!6CvU0QlLF_3u17f$MI{n zF8wAi$MSGj3~~@rQnakhb&`a3$+deiAC5$7%J{h0e|OnR-;*O^h9T#0aCC)f@{pRm zwbl0>4|Z=BcEJ}87WkOLQ*AJ1HIr$zP`{6_A6`j%n(k`)viJ*09Gpx!dVHK;m1{ms zIDTKR1U{XfiN(iZUcMTTyKn6vwW@fpnf4kvdy@kWPHM~!IHDjjus9r}L76X_tH<7V zaJ*!}7&tgy&##6ew77?BfAi`FoNbb;%eX=WFaP!|2Tsu4t3P{xo`Y%q|J_^3Q%8pL zbdbFD8qRie($%!&|4qB1n@XB)c$h$R^N)^S`GfxtJ^0@D8vd6w-IW0UUysE7yxNf0)lzhI z4uIHT|HSb>Qt9PiCja&L>Kup$t)J_UJgU$(fGK37YUZwyv;MEu{LiA+A6{uymLHf4 z>@4Kplr}WHu?`m;I53L;CGCj9BA0`N&HXWJ;hW5&SCf~U8DDNlXPWz5!*RIU{{I}* z|2Z$EqLDoyCD^~i@l?3l1_$&1Bk*bDeiqT=+}fUn9s^M-ocq#X1mnyf1Hx|W`y{vo zTvt2zABCc^<#9jB z`(97>e#Rl^zY^!$kCS*D8;xo>1XAf@I5_+R)&i2^eL}(**cZKmaQO;2MYKsV2V}#g zYV0jizQ#~WMC9<`&b3EZ&ra|I#{`ivx1-3@Msqqv@VZ1G6{dRAC8#N=YlVd|nzGo* z8i%t(1_}{TkL<$Zd{{lAy|BJZPJ9wNq#NK#3Vmu(NN)k+NAkz z*`){eMb%2g*+z0NdvGJh{ema1cZTyyDvSi)$E0p~<0Rk%lB*Bg9w=nn39Cwm(p^3L zjd$+o8aI0g_=uZ3Bf_o{a*bi{$^gG-RxjgTA{b$L{!Zy8o7jUoml_o|rqxK*CMd%zZV>vtoWHqU_DK*X|rw)3|-g6Z7V-B#m{? z-n1xDD;f#zVt5qSYx>_Q`J7>Vb4S*O)+{$%Qb=!q{N%;xekJIxf-q#;J4?jA)J5*! zuM3>i`GapLsk>e!70N#)qrIBuvd=L-xuO_@;d?}KYgOMbY|O8aQL(|f`NNgMY!9#Z z{Zf&1x1I4QDauuhk{e`46im7Qy_9n-jwgQAQE7;Hp0?~foKvgCQJwTAwlmGcLy7S> zqoR@y@l7y1vr7WKvHIi6$QZPo{EXMDnyHqZop%&@Lxd5$RZk8M4qb9Jt;6Iq?HSwO z_w4rhT`4i7GxNg1PSnSEe0(ySS7l~mV(itzv~lm@kWFcW3=&Hjt&c&CjMTkb?!ygV z$n?K5pdfvMqgMCgj`#wqo;ljg+S@O}s@Y?xUKG0NoIMUsKG?0;E`Co0 zZO(OUShUo(QQrF9zR>6wZLXChIYl&?Od)cRuSW^UEip&u{0Rj`UKpGrFT8rw zIlYw=)tjs%$(P7=wB;MuyCjy@<0dXCB7z#Y-o*pN{nRh$`=D(!?HH<|0*U+VhD4rUHoF$VYwhPCO%ZKe6bCMw?=t%wFZiFExYTh{xL=&U%iPY9%eYJ!y3!BH z?d(AY;hl%67`=jsoo$Vro~|yqF~1Y)tF4AFBCubgM zL3LK(Xqz95ny~#0V_2<*dGow2awWX%YD9FnM_v^-q-0=+Mu58tFSz!R+f-IfU*Raa z(2Izxpp+ze%(WTKj^O%OXmu4Rjc;Y!X_Od{TL1^D*!ntfCQG8!L`7O1S~O%$yOrGa zvN=5)V@+5d+&+$#Qpw@!xr#n;$^&)G)@lUbEZT~?r@+kl<;Q!Pxarjey1J5y5w3xh zIiaZxKupL73X^C7_>()&_BxWT@unIXEjEOZ$Q+mpzj4&R5rdIg9H3^cTyO&V1G}5O z`HMhf;TXC;;G})q8iSvnKEX`j`U`x%)I=lNgKV4txN_eIrqXIp?pS|v=AtX_8GQ;~Y z%p>#9SVd)t)4}qxR;gaLW5qXR_NO+yf!zGTd*=AlU7}9Y9J~+Qt1MTaQUBf*0b&Yh zR?xvTkD09oD=5(<{87<=?iy@>}vnxc;8V%c)rHLrw#t@7SN*CRMq5E`PmNU92Ju{I-3*?qPfC(et zGj`{2pns|)Q$DysrLagq+SpJ*_`L48na}#kM#|6$X696vx+!R`^Lf}DPS*Q^=``tf z`ua?==P4=r;4W>)LxLM@s`O%M+sB;O3#s7Jpm@@<} zh$y8e8@_+}ro6t!kt41pQ9&S|gA1;}kn#G$(r+gwP~y@LZI(B!~ONK=;B z)@-xkCQcLWSmEWo&&fH1*H5(@Q?WBh%Gb=|j?v@MZy!G~A`f;N1*o;530MRnm*7&c zwwMzF@%Qo~Doe{5X4d@4td^3iT^~YGODmA%W6J0@_4q4vRL)EnYAL}1qN4w=V#8JK z+>M$hNuP6hB4N(Ny#~MOy;?Iv^s9%TdA$Ul*vJ_YwtFfRXgk@N+fHVVCkd6_{H(CF z;23=HNfkNghXCfnr=z|afuic9;Em=>Qej2(OGq_6a_4Io|4bL9^)O!4j=qD+TQt|6 z6aX5#y_~@s(eMR=q7SY~5g)44940rZ^=a9AQbj(Pt3pFT49|s`S}`{}2Bq1sI*qIK z{@NH%vKcviF~!SeaQWM-@3pE@wv!8U_^ny!_n_}_l{&f9om$Z$CL+?U6)_bvgQ%GP zq|UHkw}ZDluGFclFyDx>&w0@b{)-eQ0Kz^o>@!%wfx;)l>l_IWssjei4QZp7dV9xh zrE)>2O;~Z|xtT7VS$ZrjSu7|Yy|iTC?-7ffL-LgcVB~A?RJrADxQ&|Iy}Rym*#v%;D;MoFfDq?sfCqh2#DPg}MU9V=8X zb2w7*V?A)kCTs4?D#G^N1EDD7r@@W8-pH}B3O!z(D#`Uk(C!Dc3COFVJE9=~RFBX6Y;jH?VZbrFK7><|Qx zxo-U-vaFiaj+$Q^!M?%hi;3wJOqxM+Tws%xoBNvY#B1xAxq_Y2D>sS_&kd1K6m!-f z1LV}?WMkab#T5Z|YyXi2A>chT$oeq_#iaFNgs3GjK27^`JBZ(+r43K=v7TtEI$8*- zQ6;YOb5LZ^G~ampIIL*I>6Cxwi#FOk^Cg@nJ59Ko6CjHrvWGB^)RYuXp9s5!Y-rjG z!0*<%;Q1 z1WLQZQM$$IsNlMe9PATDv^wXz31}RDGG{6}HJ@;aGXCIGh$((^?kuwdeP`xiWVR}I z=sW)xQyN`?X_?1mY@Xk=(PO7KSKfG^d)W&lCiKYYK*=VVW$<+jMNv>v)40kR?(l^^ zBI{k3TSUSV_LKWnJ89o$q27;a4Fcu*@E%rH?*4XY+R^!z!n#dII5Bf@Sn3dV~QE3Ntu>;Y>g$6E^Q!Asu!NH=Fm5 ze73GnMf?%EL;PcG-I)JQR4arD>ekt8!WAu#Y%>#d*tVE!{BF;?x&D-@aP~;LX=nL6 zfa63#tU*zluo$0c^5)&XByW*8T^0nsxYzO1GcPH&M_g|Q8eD}^w2T%VMJU=WN)lUD z?Yb+jH7ooPQD&j!M6pxe;)%Z9ZCfHJ-+y|$cBZ4VO?-&F^?8W8aF(1AH}+ptjyG}d z14f$72g!e*)zZF$a_01kK+@C>B__R(=iLtejCjwEo_8eK_}Q@_{n!;X!z>fqQ1N2# z#eTXia_(Ao^CnQc7i_VsKWEh0XSB%G1ga)tV=|BAr+8B?``-+N?kmoks0)tB8NM^& z^h#NC7FAwP^6+t(ZLU^SV#p<2fbD5@(e!jKboBHLVNVPGS_s}f@~H8u-JWfBx@Ei9 z{sTGZ^Fw`gYIGDTAraZ$Nb#YwgaV-ck}A*7V{e-(43`;Wj-=C2u7O`47zt*OU=1DI zzJ(9>L125-T{Ly$8;AY<=p8pWug;&1;hr9?ijUiDYQDS>l;`W_On=TedWM?tLsN>6 z(!~?cSP*h!>%rf9zMC-?!V7=r4||yuw3|_{RSqoHeag$*M&!?li5buxDLlrswR9-P zqV|?+XvkX`<_vW(9HR;e&_eClJsFY z4H#=N2dRjn!rIf9+2E0YEbi>q3v1r<( zP49{hwKuGLCdJF%yu!DzjGm|aEB*Z!5e6bJeV*Nn4%qC>EHr|{E|p=hRWUJNkGU3q zy4Jg~R)rGW3yzmAe7xwy>j$CahYcb_4yN@JOmardEJ)_`eV~foUUh`lQ<4>n$2@k3 z@i*apdmzLMTB;f_^T%(k&PG(fo>0)_ttwS!j?r>zEDoHt|7lNtDPJ_+dDYW;bq)>Z!fdw-pyX`B}-^9BTh54W1LW_vomIy zVU=Cq1|$C!@}0Ib&%L#G;*AH_4>ds}hn`PcC=`nKN4dz~wWBZ?B(~kG>hwm-bFz5H-&D znD#FojU~m!4sLE=r*QKaWWC=nDi>4}Ghc{l^?Reteq4_}YM^cXtg~A7QlddqZF{B> zzyC7h<~P-cFp2R~U2b;wMpsU!I*)_h{rys(Qgf=3<8Lu5uA#D)q9;eXyoq}!)tvsc z5zTO3;RFA`Qy!{_=F-yAy*;OjV|YV}731OT5$FDjI5c=~x@AWJ_oQFC#X44K|azDJuiy^?25TaDKO zr0XJ@R+}ATgt;G0S;;<(*G`@mpsNiOU2RhQ4+Sdlj@ZwuR!-UpCUt=+l`Q!0d>L4= z1f+)VCEVN9+q(wN>smFbp7;1(q+`Xz@Nfc>CANL5_N;Ima3qtOobl~u`oq^e`^_t9eNiM(N?z&5i3iixdeX|uuuWyo~#CCPcW{>Zg3np2Yp6;|VVJqL+@ZWha``}&D z5!Uzb$==fH!|WkzEzWqb7-e)~l*OB0Mm-4L+aHWbP+dh4UYsf?*fBSJV?iYS-KRc`3?i2`b0B+ls+QQvRZv7y zn-^sgP`0KTH_DZoJJfb$Q9?1Ard&)&EeO_N#m~*8t1`7{A=Y^kdkq&o_Vy2F5J1Oa zbkrSTi*_aIfJYGThF7-@NaL-EQ9ugW5Vxs1XM*y&T;KG~458!I-!MapjOq@zKV)@J zNskXVhdpW~WeGwqri0W0TkE}axFP0cNJIPn8O-=3zPekDJ5-S~-XTwuf}%7=w{3xR z&yLu9pS)WFP!{Z{RsvJwJB_4xNsYbB0=fUT&J6RQp z$hn<{J1{pk=l1bbZEnEAoS!rqwr^4|3CWn1jJ*{?FYMZ3&?K6EnRLQ-ah4?6W!lklbLocpBxND0Apmo&6-#%ju* znkO-Wpn!XEc*My2?^#h#4`=scdw>68sv=LDh?p4rg5$usVBdq}s{0OGQxbJi*J{0y`5qkyN}HkElQW-JEU0 zKr*m(e=Aj&0Rg7^A)>&yD`|e$OVTJ$vkZe7vJ-4KQ@8y+wRMys>HTNLIl0?HiEyK4 z3gIj&4BJG?mz)$L=@|@YE3)dkdU|MAVHM{FncIrdOBEBjQ%u$@6x}+{-iZ(22_(49 z)~@e7BgWP~-nZ`9Ywl|DoMm((q-J2{t1s=MV?nc*P$F4q<69vcU7Owo#|pR|l%A1+ zA-=QUX!>KPcXFpU=dK3DDE?8M z?!De)xfz`sgMm-A#LRCn15D!klB%vloWW z!aN zvi!Nm&T;!cc_=zgD#FDIc!?&j3J9xjwP76#j=+QD4uAlg*qP^?orZ8TIB5F4;UWWk z^t-Cb%bUO5=U=sBOGfSy;~Dv!WV_1UrPOQ{h&)KUCf6+nAC)H%<+{+yr*SIgk7+hr z$&Kk;P{@u!nhD;eZXE!{fV3VE^mZ%-@A9C)FK_nu2_+!BN+O1=OWz8K-66&c^uL!T znWUuokfGn0&ljzjA8NxHPZ_a-xEh{T4F3#*xBii~oP-D2YGLXw>RIB-bN+VAbJ)|CN-+g)t zXeUD>qphQ}Kidfvrd>wnZj zDIMr7bg?UUV_YY!eBli#Q#g;p=WHMtKBEo{w6Hk6v)O88sp!0;2-#L@GxF1^_Iy{- zmhKu*&pFxL0k!(dk+qtmDbH1974W63oZKxuz6`JPG2qGue5y7&ULm_>ZU^}Bki5r_$tT&!C%bkXE3%Sx zk8cy*>x`titc$V!Oj+>WEXm;XWWN12h5PP&`{m`OnczbzLQ6}_VCiQ=4Ty^5%(5f8bzL)3c=WwHQpPgA5$qbcj^+`LO$bNuS@^4Re|MeVQdU;8M$G|)LH8m+Uc`J-MhU$wRi-cCoGMM~qQblbfB1e4PusV`E9Dz_9{VdjPHhS;vYotc?9Ms8@cxq2Kl~q&P-6`UgV&8POX@^i|hR%vCH$5Jpkzdg|yU@ zR!~p?aBGqhP(>FTUT%oAc>ynflG0D+ z9>;)JO%A%*-eo>Vn-VR|%C@F2c0(R;Bvip_&Vd7~H)e*cVuQI2yJ+?H0f=4Qw}%d@ zs|hBhDzSa;mv1(I=WiyM=NSLut@@0Tjrod8)zZH&5cX-K#auVP6%AzauqlTC}_$UicWnG@t2_ocEXsTAN!! zqE(%m*ki-)!M~E}>nUYemNmo2f0>(_K1l2YsK{000I_#Z9R35Wpr3%Mbq|C5s$#k< zQ|#Z)`Tb#xDS3CSaLLPQqKmgp9yL7=>=T@3&UoTs5zQu&8B9@J-HX0wE4kL6I@ZnN zxf1Kvky(BZ_S;CvMF1_9vaD~nc7G~&1*PQb}INkNzv8P%eL$uqmt*x!0p`nTfth>m$ z?|g%%@X?avUmP2r)F)5JABW8Eo^b=6tZdUzx&WS?HtuHNbA&4IiF%t7gPyO{~)4}i@wRpnVIv(;a8ORKF(@}TON6FpVD zYhR1`MwC);Obym$h`WR5D?4~#pr+0>HZf7@eWv{DHb@fGC^3;i>!sd&W~^Sl8t4lZ z$nHvR-8+X6eTVAt4G;`Qij2_x{->vy@ITg(Ir&cL0X7GPYys*&`|aHE3V9VA`6)70 z4qW|SzyrQI@$x|+RK~^FKv)x50E)yJ%fP_!>f}vRWJ2A-F3rcDMT7u@e^%|213wp6SXggSB>Mm`LGPxR&<$p>L+a zH|?{uh}u0x&H5?;B$8P2+X~(}q6MZLqnovC!nJXQxovZJk-_H@=1}_lyTS1a=HiSt zVpC}Uu*RA1BWrhCo^`MPR-x!eY>4!(th^CFxqD;y`E2zaX0M`INc7*Gsi`-mr|Kl+gUL!Tos;2vwOYS_ zSNfR!@04uG=(Q#0R-Tb*sngXtYdq2+}`eliVvC8*3{I1C^k6) zkUU>{OU?MhcB7pxtj5n!kE+QWbD8J#p^e&a;rHFIqX{%EEA#@FWshm9Y_XU5LmhlZ zWeVAHB{TYJtoF8A3z|1ZOfWR{-ET%-W~&2L9h*+?Tp$dV7ERN(Wy$x~9oh|T%uLR< z+F2^v;5A!lP?Nq)&&I}uCs2bvhMw_>i9eGy#rDV7+v)71j+z~Xb4bEN)7Yn=5-%U$ z!&)0zv)WgT_Q^5T^DH7Zb~&AFbfOpZwR5bhS^y)U)u~eta!<5E1@ddP-Nli~s0rg; z#(6}<1Jw!LfE5C7dKI=3lb!$^5KP%2f7^h z1}FnR)R|k*o-z+eBGhaZ3$)kfqNEPbKAp<}3GTO_b;NV&>yJmu&NW9YN$=u${ivwW zD$v$>m=NmSZH`kqzOlEr=YFDZv^~uJ4xmBF@fAhn!gtr^E0{rASHOBg;QQO zp6hT>WhHm&v}1`d(-bxj2%6Cp9CjR3gr}v1O%B0O(lhSiYAx^0v69>`_P>l)A8}C# z@Fi4LE~+<-9d1w#o!ze-<)KyNX|_8~r!BRt<4UYD|LLh_`{<9O$~z0uv|*tGDvBpI z+1M@Nly09osV(CKYXLF~UL7g=1F2cWWdd`l-?%nsd`{2F0-x_I^pKax$7yC%V+?iG z%Ya88>HT~vENcvX4_33w@7B(}Ixj06StG%_9?`DPG&XdLC+XE)a=SaV?6haD*4YlR z`l^BLTEZtFa046n&Q=vMj z9SgJVzg1$-3wSJ^R6|ZYUs+@F@*b_|P|Uh@xz{zs2#QRzV#bvTu%!1QWmk{Pqh1^j zdnm6j{2MITF&-xOS*;F>DAm&zy67Lh(+(jm{c(fESMh6tv8c3nrJ$$eO-Gl7_Yr>U zuTPG5K6}*ER(YBt5E(F1Xiv63yYx8bY|vRcBGZf0%jT9o(lDOBz?%Px8)+)8GA9-EO?fbC1Bzy#yo( z+DXg{{YB>ZaP%z%Ls^?v;M@CqOZSUx5esD)sNS*_BIPmT8_uGg!)>|H(kj(YyBJu_ z&E`t?A%|0~uc*)z0Q`^Bs!8&w3PZ#{v|Zy`77r`SYYxE2#|NHbe9Y3tFr2VSd0{K> z;#qEOYO?3ov?4>hmN|z^)wZR*#u-(f!IHGLAi2{595E2RYlh1wRo zPA@i#2s@7$xA1LP4HX$zWfazIeQwI>9wA?>ORU#Dj@QDB8$y7M&SjMmxHsItbp+7r z{?a$~zd|bGA&WBGubNE+K;ZFEEg7RH#^V}pJK!Gf98yt#Y4G?YFTCKKr`RZ9h3der zdpok*=}ymhi`@&vdQ7ZAnbxUAZW{AnQ9S-LO|n`?5AYUXiI0d%Ng z$ry9=15yI4gR!~#fpnzoa|v$!4idUlj&(0mQKxg#tFe0Gt8bZc_X;%aAt_kXi-Wo!h%YyO_R!S@PNPTM3Sp4dU+j?!no0 zgLj(7FS67skiO<>cfF@g{OrGN!8CHGGu?ljtL5=Mh0S9lUO_AbP+4J_gmV{2@+r#~b0b zdqLX_wswy*MkOh^c({ZrRf+j>dNeU*Z{sqQ*Z`*F$pg$IpzMI?(L)0R1KD<#uevQX zQvTehEnw3~yzhI~S$~tP|2e&|MND;bDam~15wKK{<$&A|TjRM^(kr=hW>2p1)4MfP zxSRd(4ugNDbTWjQ!}=(ZZ~+!Ugs1`s?O~Tje!|-;iNC^dm7C zA3DVt*I#)DX<#CN7GBKTUqLC5!V}JjsRj&qnMCQ)i}2k|Nihh1dg<7Ka^vqby@7I1a5>ON6Vao>(={u{qw* zC>nX>&ggTn8lQ-N`|16NIhL)D14Ub2hwBgPtgOU83dtXnCVvM@iB-;(`y9R6#NUNQ zvmhkKAHsXj*c_SP2hg>?nEEKk>e#~Ea(6~71TyAjq5|-L&*{E+x2MH$j`t(%X{0?~p;o!Ai0i5T2k}c~8Rb?L+)e|uLxpuHJt_py3m)|R}T4rNSiGD=Zv8nGBl%}RO8(*Z8! z+`JN)?8yf*H?e8Qiu2QhwaCcG*x1{0n>OCwlhNb;N6VUO^bvC!GSXe2KGMY(%9%>Su?k zDsg(}O`EJGpf=@Ly z7mgKx$22Mz-G=ef*4@HBd%hRO#}*s*I@LfRSjE6w$VJ6Koy$K3`xcNnOM4(ej#ntv zE3hbWcXa%5n%v`x^>+32)IW2C(*H)$a{#6RI1vrY!!vK{)_6d~Q-HetA3Kaz!~3C# z#?U`$jCBOUQU@1AacX z-7HUY_2|sT#-^}v1;(E@RCDSyxOOnt8U?yL^M|Y_6c)pzjstyVy6K{wL5T#=%b`-H zh`vLTU037CR@25>+nT&DfMWU<`X-i!if-^ZN18GjUf@-Jb?<~z7^=kp9C5yqpPw6` z1qLmd)0A5~v{iQ=Cb3_xNL_4wKK+ZgzOk{j4mbDCCHd(cxEQg+HN5pJA<0EFIqk7_nmA_h#+lFKOh0ifacT4Z=aQ zu5I0DL%yNXS@p|wL3&aEV%@}*v(>GaUzh3`u%Z$bSnvS0>A)cUvS3nX5VZk++b(`gk_}cAIUVx4+VdC6gQ(~{lg@__tPFt z4oy8k)m9Hq|CN<4pzOOtCFK?~a`X1ryV?1_B*^2fZaEmndemV@rhe;i#)I$^5Mk6B zhTQQh$M^D9%RS_aii-H? zTGI>|2wHEx4%O6AT>KNRw9)+~(z`xv4p}scdd5vfxbkz)_l(7{HM;I~uba5GH?!eX zUX2LPg^yH5FQA+Nv83(4CVW@sMru0ff+0wRf@VFIfVJYd+aLbf`Hxwwy6&=EUb#Qa zj;@>CUK<=msqE&UXup@FRFV>5aDy>FR7B*><9n^GisIBN8`mDwlLo4yf*1cKPvkTu zKQ40TDBw}6&}|FgqFb0N)0%_tod|OHG_tPF6}=T&TW^!Pu*e|2j06_uEGoemrR5J) zSc5t0{dZ!&3>`r}8bv-saUCu}>OJbDGBpPLsqn))FtQ)zAuv1SIZ5Kz^MYiFeF;v$ z|24zcR=>h{upq+2Akm!*n*P<==vUalxA~$yj8FZ6(bb-=a@9qdys_p0VOj^UdAx~Y zphH{sbNLa4=#ay70YTKWeZdH#Cyo@ECpUOOaW79(;X-UnFi+EZ47z#v)sZJ)trx`0 zQ(X5_JzLB;RojDK(e3e5AJY`SE+EM8O2}LN75{^9@B9e6YGOS;}@UwaQ=4R9invPoX1 zrj12aC>IF3Z=;sb?h$vR54DlP^rCv6CIFq;>g{HrF#WUNO5F3XtAiR8MK1u-^nOvI z@JZZ#m&Gbp>unUCa7EgX;-Y5jHPh(3fy>9poora1tn%KiiJaY|24L=17o3N96ndCh z9vd;|aG?6#3yz0nHym9KIJ4>_GdX$^y7ExhiM|RTuX;_f_$HyPJacf$9+L_sp6pi6nT6*a3m5z6=7^U>>}9i?c0y7I|m2vbWmi<_U}4g^FJuR&&3Kl zf4>K{ecRfPXbnqUyzSOLb^PQx>tio~`zlKzRf>2xt?mmj}9;D2+UU722do(kGFM%-JCO& zdjy5F0V7}nTqvFZl>^*@^56YIdl=yfj!M3vq5$J$;f21V{^@!u)YW|fLU;n)4saN# zaDXmPZCreD@@yKaI=8-4lWe1GP0kAMJNB>}Az9^|kBi2hZr?N`Hrb|Y&2_Fa7&fif zVtfl@B19jd-3I(>73v6avxclG>I*|*A;CZ6i}5-(nG0s<=WdZRSfYtKHfol^k3J~b~}gmLwP=M4=eYDLpREFqAXydhCY z5n@tLUoZ|WcrsaaRBeRKqk)AG-Eollbf#6UG81wH1?0u&Xqr~gzZ;(dDrm|~ z4AbC6gxO6aNza6DL;HxC2*?@aN%V6HiZRMDQK%b>pq^x$@8KA`UD1{%dT=Pm9HP!m zQ+IjFCA`^YOVw6P&T4;Dp4?!IE%#XZH1%~rl94yAB~hAQ&g;UX=!mYhm5x?sC(n|U znwX5Qc7j$!5_+sY|0gCB9{H>qG~Ymf)@n$xDx?hkc`G!?2|7`zTUN_enL{adF?9Pk z9dp&o@#jdLDgbyfGOCRwCt#a}nyij^*7)uYjdMC`vG6&vWiiE7^8Dc0UCweaSDYV# z)rvzuu->llA)W;ZXM({XVA8Vrd~8m}?Y)&URdrWet}iTfCJG~__Rlz`b5#*ACWz4Q zlDdLHjXZUI4KSUA&KakmIvIq@y7bA`*>;93%;zXwG$Ph0`<9kFK%NMhCBn z9dxr>$E#OO4u@ljK6@><lJSMbZooskzA*OCsH&gC%chcpz?L#;cdCJT6awQ z*R;lGc% z7bUgyq;R=&J~*N!-g11LPG5fv-m|?)9D$XSD?*_RZ4q08)^eoE@LQg5;35(dz>$Aj zAr{LaZ+NBsr(?u7D#L33Y`_c{Dw~xn%qbKcniu5U)3InJO{S{TnhOs!CI3v( z8k`wA1G^;-2e@bZObh7cGU=0)?%$G(gi0cCZJ1&8wFsZB^>18)`o^zn92QRM>o@;y zpKaB8I_1SDat4FgMa|yAEX#&^w9ik~hTZ;U^;kXXr$Y8`00uYaK-e02ex3eyG%ey+ zC93VIS(&g1PYl)FQCs*Jt^2`%)a9e-t5;vDTd1FdWEM0cl}<`BMz%PR+}y=6A-rEL zd@mBT_ryP2*+)I7jsfqfO$TPS<;lA)o_~>7ZsH%(mJzb(nV{)cY1a;HCj+4$K9X+N z53pHscMD)8K;E5LQ%ITgyF80<-iPTX3uxSNtWeMUSyraaeZ>{?CO(Le3ruhbR60N- z{|ktKnK(*aD1fb?`)Iie%vJ$xW_yn>~jJ!NfV&Y5r$HWdc?QN?-RJz-x~ zNZD=orolDvg5iA>03f1iyzvTpH$WVh(>p6a1+{dR*p_~C{*N2NL1cGC?XEk}@2FRw zKYkt4Ml${q=yQYMNo`wvU?6^mmkD3ozqiDA=sG?NJNe;O2wUvA7V9cytHbP%LE9F< zr=Tne%9iDg%M$(9fQe6tpO+eCSYO*hriOQw(X+`ADhd^PsT^fn!`|j`KA9WptKsM_%;y1*jzeD@B({b zb-9;?+B|+Dd!bced(q}5@4HdZ!vwyETbXGHZ|)HVQ9aO3ZIIn3VB^}tAGB7cP}3lN zl+8BFxZ0+^jSu}RR`!+%B6ubLLV zJmo>>B%TvfWNkY>cZPm6IN@dmK~em#tb04%tt%#44iAU)UcxNsbOwoGjV;u%-C`tk zI*0Efxo#TFb+il*s|U6(nEdSli8JUQxRcNhs`u_IPnv)Xm|ORbtR8om3C`_6K_#i= zRim8P(2k#RN2WxR!;Rz@JNn#!^d@t0l`#Wv^L!F21pQmfo0FcNo@W9|^Hgor3eyD4 zIwOJkbUJTRvQz6@&~fl&>|EclA-e<`({1r$9(iXLdOokdGoJOipx|~i z_UlkMgeQ79ReQIg3;V)xp(WlsdM+@KPevE}#z2X1{w*vDXV;i9>%_oniw5+qoK|tf zE7&nnPT!q{ia!x^QH3RU%hnjWih1=W&pS3wPO2zm8%aP%DWXLR<~w_y{+ue`wSgEF zmeuZo_bV7By{0dsdT4WVb1UGyG0(}8q>-O?EEp>eJk=VtU@V|O8Hvu}eY)lLlz*X* zaTW?UgSx0rypb%n{aJh2U7#;072TQD5})4R-D3kEqjCRY13xi{mUE%g(R>3h*bSz(bLLmV}9D<>6O#Yn)v>m&6dt4Jz#6&+q zLlK=U|8jm&iK}9U7*^_@d(c8%F;NcT5-$8Ic%nW+ShT%K2)mT+`>HF>7&Qb6%`;OE z2BP((jY&=9ut@m?Nco(ZZsc6t6_(*HbI<*5aTKG<7^bek<_u zf@Rdt>$tk+B5-K~;LN!(GYEWT(|*P+Z-dpg-!lZHyv}xA>RRO;3l3x4f)YL zl*$P6Y80llE?0m=%d^F9xadnQe<=Ycy03X+JA0g8^xPv2Ek4~J?!*=0nI>Q>pn#u`Cbj~8O1EbuCjd9P{<^5_EZ_;p zR}1FfL{FvuGt2iQu099Ukz6Hko(}^70l&i4ZzP^!sA1h8F;k`P6aH`=!dfVYvQ zk0K4n@)rMAurtNW(zS!XGUs;>dMBDD`&ljTPA!?5I7C3B$s!*fu&{s+_5sq)Kr={7j_h(n*3 z-pv)R3N?8J!tG55aiztA>Te@!K?qEq_AWw!wRlJ&{pV1*IylH##U~%n`|Inu@M70K zQndj+-@eHiAg0UUy5egs&d46y4*Ej$T>aXZQTVi`_}AOM$9wHLUX}@5S6h5sNG0QA z@6He6j6h)fJJ59mcpbfg7niae-z1~?KhnRED>e~|7!YH;LCjzT*74Lz@>cMe_HKe2 zS4BHj4r1Hykc5k`(WY73aj#np zoj?b&CDL?T1mdmjtGh~!ualHQxKW?NTyD_Hr8E>wBqBkN7YLN{EEXx&c-I@r7-k;k z&=&?kHyOgQYClZ4Dxm%4KUg8Okz8f??_s|{vrFy(=veY$BADe}r z1e6dU4z!3r@;<66c-ED^t1M;&X4msG#udh?{4=IAwSHbo3R>>5p_eLEQ!r%xALb;N zt5ssz|5&oYL4+{l43Imm+>;HV-VIlr_nZSY1HJZK+=POBHnPh|Iwc7Ac}rO zLGciIzM1+k9A|?3s{_a8t6~Q8F`GAu!gU%tp`Wk&+((VKjy~hyDvOCIXoxZk5y4`y;~;^ts0*iEda0UdV{#U%2G?_RerZ+5Kaw3X3RDy>vUsMPctk{MM! zK}~S2DXx(kR50iYtG^tR@ZBwo(a-Sz2o!ub>T(fb*ix$G(juwroXkWlg4?zO7Y@Z*c??I1->07W}qCh zkA@LZOnCl1+{Rv(9QYpEEf@&f!ENv{JL3T8?W6E-gX3P(rx&Id8+|LZdLTMOM`7`w zgp*t1PnLCVi9yP@F6Y-(=9N+j7tk>K8qk`r#MU}jEE)Gm`)2>N+SRihk29DPi1pk5 zFQ&c%DC+Nf8x#d;1SyeNP*Pgy5Gm=-1(Xz!Sh_(%5Co(_N?N3(gr!y_mhKSgSa7Ag z+4ti2_n�vBup`+w&$;#xAsddReWUIU$7LndYQOsN@g{?h;!wq1Q56=#VEMvb z_eJ~Dx4;YTIIvO0S-GB$X;3H-tJ~}4fSwW9XOn9xjpNK#(`tfGGxv&`3e>v=gqe{B}vj^>e*mBD4ND$j$N>v(JNvB z+s$ivrwE=+E3W+}>J7qM?<}EBij1s{nDQ8GAvd)sy+P9hm(c#d!W~XO*x#G-6xaj* z8{btv{BSd=du_bSIL;9|H9rqFpD|UD+A`X1d43bqCN zA%)F&w!;*tg-WQ)n=CgN!PJrB6^Kjyh0Y$BqNBFlGcIDa_vLAMjnU(!`jm);R|iqY zfv=n$50}i^iU`X>P#RY?yG4+_yq;KD%R0-GxU7dpm=S>U0kwVaehwAAXM#<|F#;Z> zc)?>+hIVjH4S2a^kMhO#K(x|#V*jb*`NFi3;TR0 zNaH?r!Mmo?D*|**;|?{-CA?PUoB>HBr56?1vGiIH#6ReRX;y@Fb$5BjmCxSFodjLI zl#>vt6MeyKthez2M}5tDF7H*B0X~FCh2_Hssf$gZqx)+F9pe9%#yiKa7_Ypj%8NVY zKH+VEAQz@*`=ASxwX?ThUu>%|YxX()I}Wf5v*?;jNfySfPJ4TsQ@^c^QWgm0W*m83 zewR)ST_nruSXTmsJ#IK-nkR}ywxN2B;ei!L(y00Hf+!Q%ZT}Hk2=mS zp74yApMm$uYE-_WJPTjZuxfWyDfk+`z~F<2Kxo7uG;s~$TT)l~r`;arzK#nCXKNJ39T2Vq|^9`~jOzH;o?hs|%Eda3~Ypbyuso&dbeRlr4cIRpNPzAp-f-(V* zUj0z!^3~Gf%o>5}-qP#^M@|z<6}MImLk~fi%)b=6+0;p#etr4GOZ-vHdC9CJ`M=kY z`QgLqt9@rFvb?WWPk)-TnOT+DtZLy#QTC+g z6-uVCa+AmHdI2KRSPB3ijG_T;$-f#1yk~`JbFNtz8K9`sae;_HF}w3#=C7lYKjA&Y zp!>dde=yr73vqoxs_;pl0I`ds>BTyZME&YLT#T==7kG%aPSNGfPH;p?=f< zE}iWuSwYBYht)f_{M~>{?NNdb#}hhY<1#$}YxtWL{H~1&He;rqCBA!33JDe_4($U0 zP@mJoY@wT)8&AtaYgFhr1MWS#Yi<~LFu*_)*fyoX9VK&`RA(=NxF2{tzEDCm#a7hw zTW+j$7qliG#Q-K5=pfZ5CJ1lwyo(V7u(!}y5FbtpkUlhu<{VzvX9{lnzUX`cJF*Dj zuB%I__gbPhdd+F(j}=uOB1eGghgD~Alz0x?2@IH4=vPcmPE15Bx3&(!a3A{-5Hd!{ zudmO~G#s8RER;t?cG?yFjZRbckd4n=VryF+AGyz3V%0 zjT%>WJNl5lIKRZCjsCs^WZBzp}-ANsu7 z(0@gTua`3+3)(E!)y48SU+Y{Udvd1)wr1jH)aAH=zMRQo2Bjn~E5A?dH|~1khJ>ga zXK%51R3*Ftn55(LYh~}qTtgz3rRxoy9tx$sr9SOU`gTx4+FF^r==^05 zEVLfG-x0rABEsuDEJ=e~vh1)latxKeC4?Ee4FCB}gPN*vNqF2akPP`imJU}&y*A!+ zaNsG=K&8-4zK8suT&Z4V*8`Hqjr+*=3kGqNtu_RU@3=c)9Vr^z%5RtsIClRs`tO)7 zHs`arrU}`P(NwNo?S%yDT5k$SCQp3@T!+gYut*&aKnnNo^p;;kt{=`=X&Xel$2Mj$TWd}Om z1ot*77L1Ld{2pIXr`psZ2Tpc*z5s9)-qaWc>sUY!5 z);`>ojGNW3I@+C1Pr;}k$$gjo3bAJhIB~OI@72BHnfZo5n6af!o^1@#wua>_7`*+I z3+5iotKfP9*rzB%CT3&B#%b_3Iayq$i2g51vQbTxk`YVwYaO`t+Ruco-X5~IUBrB{ zG#Cc1Ds{4Ob9(Y&a2q?o;XAi(TaZQwI2h=yW0b^Yg&^>l>6F zl&H8hiEzlKBAVGIiaU^yy**U#P^b&v=1beCn%A73z1MuBVJw06ii|`@XApwHM)Vu~|SM za<6BUvM`CW^pKEaslFPR6C9e(T{W8-J5{^vI@8}9GGuW?ZJ@FH`CeCTz`e(Tz`isr z9OtHj$P6Gxk9d~8_#f=A4)U0c6>D&}lpRut8t4H8;(rZyh}QK5i^nrE2MlDhq!`|J zE#~<6yO$DJ(ZJ7-G1;I11!z5u+bzmC;mkjQVbB1xkmO*`nFvra#_&e!k|p6N1}bd4 za8sU#fV-(qOma@OHWjn*9Z*SDWM%Uq@y)_yjFzxc$cJV&f;3AQ*;J3y<^F|K;GEwT zI!b+B{JA)#x42Z|q^uoeI_j)Lj0$69%c=jEo8US8x37?tP1)M_t%ID4wS5fN+TL<4 z18}rTPYu+yBkp}&mi}|-Q>?~1Zuii>cC5o2;1;=_1gH1BUy-e%BD*L{7c$%0#yD}# zR3!i;O7#7)wr?5Vu2&`&34lR6e&eGB^W-%XCCx;ro}Wr^9G9}9Aw5TJjwCR73y|F7 z=5g{1tti0{5U1HPEc<4Z}L(C3yuntepK zLjugs#Wy$Y)coIZ<6u>q_MQ|jO_iTcGGWOk6ZVEnI zoH{EAw&q#n6O*Ucz}n7v=7a!*pE+nqrEqZp$$n1pw$> z+|=Nzdn-(Q!sunfwgkmtFuM?YEDXz75Ruu9A)Ug-=+Rh1?r zBow=rQvEz!G`=j^exyJDb#lFmeZH`z*-dr(IlR7hLR)5BuRH*LR=5Mm1k{9fwZC5r zjs)by0KNyP5mWT8f&Ekad#D8j%)Ep^PH&Dv8)}b}QUIwp{k$aHrkl$WKW*t_0k9W` z-g@|pfNkP0a=-OZUz2J3u-Gl$ay#K(JA6SmejI4Sp->($m)F=zVcn8R4TmIpSZ$hH_$ z3WXY@ve8j(K8{dztF7Yprxn5FCT{>le?G23x>(1^*jSn6Zolk4Rlgaup@=cu<&LNE zsSZ9MGiIvU@KOUX)Kl7MDEHdrTTQNPFrP1(@*6h=tbq*570Y7qg?WSw7~qw_;%>}^ zQ@}|@;Ggz>(I8=Zt%F?QlHu21*u&3u91J+ed)9KJ7PM1zT<_wByp_xY*%-j<>;rF2s=3k6Vda*tB|JsgoSpP^@DeK&1_pDl9F2yNMR z>j#FsBz1sZ=F{|~sQfR%=P-;HT!Xt@RgOY`!Y8uM0aeW|o~Cl-(~MQP!+wU!I`p{& zGc~rE_h?~syp;L>z4b;}M~0Ic&g8{Z;788@2&DD>GNJw*VIR$=-2?P!v1{R{E~0Bv zpNV)8{-O(6KVVw?qwxaDt^pUwOaB#fLFGNU;|cO|dC<&XQsV1OoJJO9w^z%Aw)m{f z>%YeZkNZJ=3lvkL<(aQ1Clt#GkyCs%nES`PD~&|8!xkQ2>kJwedbdyBnaq&1^$BLJ zXm1!(t~Q71zWkX@rPtz#ONtD6Z%V-fc%XTC%y2sND$1!NpS1i+cMZ(gQ!1!xX~|M~ z1PH#6I6%<^P7owe(%AXyKhgD5tBIt7C{0_jjGBs4M+Kr)${& zb_!<|r#H3VyCd*RK8ZCNnCDyKCb0De^v3L{Y(cqY>hV~O#7 zOaT{+89Id2LuaGdi~lkP8XsE2!nk6p(*>m6G=Kgebk1wBb7g8eKbe`yfSU=V_a8lu zG$uzp*d;_&t?b3-f2a}Pu&u~{+x>n*Gv4}v6%1B6{JO8dABf9I!H(n$VMQ=JOV+s} z?Cx(!eWNem6MvjZI15@j@DlSU_R3^lj0PsY_$N0M+0oI_+WKrhB!=?mLlYB|=0FUn z5C_=W+FDt4K*`LYP#{sO?XaR}Gk<|6y3Z__1Krv_98-vQ4DURU*gO)!ugKq&*Ac#31nNb!h;I`TV3u=}m<(>v#fQm^;7-RCg3k2<9M zG*5)WXv#-b+hHIaYHQ1g1I$eTlPN1Y5;nmL#9a9ojlvrmKxp591t4=3v&i3c+_P3=jaH2FifD!Ns=T`axg+hVpQsT#$u%Q3`?YYQ< z)sO~wd&BGN0R|5Neisr-0RSj;jY!M4bmCd1MZH*h2|Gbru&`IP;W^OO?ZXV}9)=7F zgNDXLvB9a9q133+>xCGmY@v))kaIp2jB^eqBWqeW%SIzkvwZ@XemL5RI+eK=2{!(4L&Y~6tjJY(~a zj=sk?`zU`mRAWw;+o`Oyl$(b~!KX3<%yGM8NFXP>L!y4ZPJVeXe~4L`6TTWIVQ{$g zG}bnGcOJ1^&KMjXSsTOuS7g&;nwK!z6b}KI{q^;Lk6x_Pvk_)&QFodFhr{_4yc*V# zQdhfsd*KeT^8x1$zU7zu+ktcYEOqipoPxd*Pjd9s*y?)L2*4zchUFAg-FjO}cyWRwJzT?-qgra=^{AkC_=FKlPCx7WNev#|-<+;G;m zw1k1z9e1;4&maO3=QWPc9*XywIb3QlXCH;``P&A*VXIKQ=bJNgfy(OFaNNSJt)3L% zv_ftjaw*9vvjskEro}@g$C8*nV9R5TW{vMcHJDHd)%i;L9_i{(_8LA=FsL7siBk?Y z?+F3w@GGjA=FwfdPk2|S1y}10EcXZTuevGd%Fjig6CV2^yg$RCrdfv4XiS0>KYx0wYh zBBoL#u57@>Dl*yUutDUU;k*>kDwW>a(#aGC2Ft` z?(9U03^!*I5AEFOttmFGV4$ZD@NqwkH7e^wS()KGQAEwZ7?*h^ne1cN!T2b^(gIVPq-x2Lsud*_fmi9-Vv=K{t9R>*a+;hf)*24FBkFqQm8A#? zc2|v&puWA!jh~qjIM&ybO8)8YelI_!xnT_Kv5ARy=m8ps9+zbTt0Fm8o=)CFRU_LT0`@+B|<;XF6rk3mR{F^%xUha(DGg~j% zgwn|T0x?$brhD<5S*0;;PaNwsC|V$Zk=M%lbTOkWIChU8vp1gIOxq&LZM#Es)`Zzy zn?m>}U+MZRc)xwoRrOQ8Mi^+an#Jes~Wbqy^sYPXxwE zns={n&VXnzEMtlZr#DG-{^V~~sJcCmINpk#%hs5ah#wU+p%WsygTmj%N=Dvf75QMU zh^X@ueUun=yI0t{TKr>&OAm*0X8~m-^nt<`wv7%Q4KnehPl!#Q^??Wa>Xr1VpK{d> z968GLIIaKfdqNS|_uJ|-ZkmdJIge(?^i2_0r!5Z?afQD^^X_vURoCa&Bvv-nxvg{i zKXBV;ctTSQ8-3}W%L+*}-q=P6-sft_G~6;~i~TLe8cf;)w3{uMQ-%BvYQfFNRs~1l z?vUJ_$2>dp|K`3_RBw9Ho{DwhCik*~qvKU?IA+fiaXvdw ze^sW>i8Aq!@gD9@FB{LOVYEHC{N89VN7WN0#EFV0HhqxFiOOv?e5;5JYtx*-Z3$SSJj=?YUk-Gv2U)Ba zTQY7@-3nsc#WQNkIgx}8V5j3XaVGo7@7#NdQ}=1NpKH{kUaNMfQIchxejFcP^Li<9 z{qHaj%iSsC9n+M*lV3Q3R9UIIxcnrV$iu_a=&Cw0KCb&Oerc$$_rhFRo1M=#dV+V@ z-Gw!qW@l#yx?IV6Kj?cf;};v115L0)9T%F=ZVSLVMQZnG2py0A+0u4)0&-I1GEeDa zitWgSNEaMTxq*rme-!#4*i#N<-U7=0P^ZTXldMU)3#4S?eK%%OY|R!7n|cXR6VC;2 zrnR`ZyB}U%o&)X}AkC>R#)Bz4i;Fq0sDUj7aV(%d#Jj*V$`A5I9oQxbeI-n~m%u*3 zA3ZTJMKiievoToCZo!)`Yjl2>P7c}_f^9)+12?-@ipT8)%xX#=-xJbSq!Xh{c({L( zp$$C_^NwLS2x7VV2o^)E8N`=Fsh-30Fdo+Uz%+o|&>VkTfGN>_BPZ)oQI20H5!WVJ ztmkw1mn7exWgRX{UMiuDVYx%w9XcD#_2XP=JhQ~ZcO*4Ic$6Au2aLg$%F_%rp~Un& z{go*txFJ4>RX~%_bK7H{$Vtl!u(1lX^0d4d1QNGRH?!s>Ht(QJxZmy3q;w)NVjX=x zt|PSswR=nEe6Dd!iN=SIlWnREMP|0TKeyrCpOA^u!EXr5RRpwaBeqXv5qI+qJ0Dyv zE%F?&)*=3Eh^4wN9+AxdHNo7JI`7R^J4VJStc8GoPY2gr~ET>{koj9w*ax+TX|=Mek*dvqeW@VOJe;aw^El}NesV| zPE|eEK4~T~I|Ek5b5#{~9G}Lpd-n_W`@r*#xvW|wZ?PMY@j8)% znXPI744zi?6x6`L0Avid=C-LHjM%}{9P6YmH@AaV4}M+TESp@PrP%5)I`Mk0*MsuY zIo;SN3KZ}d_P_Y%nvlmFD(GS)LX^A<0%cCl68|U^-)TCX9Ft>?mky4nCr@F-g23Ka z$+`C{{vGIRA!BJTj2QtN+w5CcQ)5}^tUee`^6^Qcl-~(27!d?&o2P!JT2IrTV9L{s z1-9p!@4v*~b|vrOEW+K_4%=hsKUy~XZU6Jv9TeDeT!G-V{V*`V%pqA}$3dV+{B;gn zDUdTbmA358lxik(b&BDG^}^D4UZBW4^ymDl*7yjJhM`T3| zElb8}tYsT7%PCyJRbDb|j~N|E&3E49a-MBL%e5-DSy1zmC$aaGOlGXdfjOp;x&1Lk zY~;e;OP)gO`}pN)%w8Tpg*pa)8y%hf-u0_d@!c>#u?pShbkl?|Jw4}lH&%#~#gEN) zWOFJN{bt6&ab~H zn_*h2zX<|G8B-i8GV!OtUjc!&#;KSbw68U)_2II8_3G6`i&Ar)*j%7pR?UzvSD22k z4(Au%Q4%@+_BUpQj_Yg%}^e6j{=3 z&rdN-{^-vdbNx#UrW1sy{?U-{2^x}*T*YSB+3ir(keK8B!mwO6idH$C*ap8A#$uh{ zg3R^#@5wIq_FQ^SIclfPl(JsjC(pVc07^U0%x)}I^Jq60IKKe04wCzLS^RLC9x3|( zYp(l3IT!QFn4c5J#MZkcJdGd030|{OqLPWdojUF1?`vJiYxiu)kOe#6H7JVlwkCur zrX({>Wt~;;wVs~{n%$y)q;}3fZj3+afX^Cip#i3mniP0cc&Kk@(O+!7y|Uc9ahl3C z_r&r`Z@b1^Vlr!(OWl!=l2N?TSkL+F)a)$L8EtS}7YH%$hv$BVcyT1P2WojRdmYCK zIhiVdYKW?8X!!g3?S<79szl1y-POq?;kz%wFL2>L$h3B<5f^FI$_ECI67HOeFQ>jY zd?0uIl8HYTyU^L-0a9@DT8zXwC~pM^cD5WL>#i4olP zTdO(0P)c*dDHjt_GLBrDGsqx{n^W@8gJcU>2gZgqsr2OJ>SFrUQRJmCED}J4UR& z)Sc}pj@Y*d$B&L&2(3(5PbLP!Q&)bfV z3BahN1F!n|zf4SA<5}UEPOqRhC?AE%V-)FIT9aOe$EojbqpP@%T zE-c9&EM{^er6k;(L)4cK)hR1O)>e<4W zy$jPKsk=-E^)SN&yCq5CXZ}LKI9=!A5y+gbS2z_fkg@=;oH8W#l&VW$vodNi6NdeMI+hQE#Wu+yUuQBn9J#tAi3(z!w&&v;%=~+bjrw?s0CAy!tH_-&P5RTDLt~t z1wCTQu6dupCwS2(o4phT&OdVR_ei8gvfZ!U6G4l@YxgwTciCz*C^DNkKw3zF*SAjTWV+Mo`@|s_|D6v`jW>KFOFwSZ=~4BIfr?% z)kiFhzJG>O$4_WsYx}d<9)0vH>xIoiT-epB*`{XM=ivR4khLWv1v2mi^a6?VDsoN2 zL-9IEUmXcw?)p>|EpZvI6|8rPow;TkqojLU&tC8J7n7meh3!Ar1$h4Zpsqr5Il~6n?@QAuG3Q4`k z%~_wIoE1%9btpM<9KLx`Bh}Rr;=(5wmi`Hu_p)Trxs8rQU-CP0IN55cYj=I@%tJ2n zqGC$trbtlgEfH za?BKH(#@1#c#cmlpItv?~7`+Fcpbls(7W`Jk^&W@UjDwFKon z<^>}}S(73&Yj@ZvTko~N*j)bYxj#e{2+#LN^jULP$vb^1_Q9?|?q3_=7scE-vGrox z{`4@>Oo^cUor`LDQuwRNneeiV?E|KWvaI{rU1#@0vB-hG^B2ff1B3f#RN_K`Su&F$XCA$)4^SBGL&V7%_N1d z#B*)VV;lDiX0(E1lTJSK3D>DHv3k+uH$3{N_im$+$55GVuLfzyi!nuv-NuE>FH*!m zGbXD#_{HV?ropF?D&HLOo>~`@WiO6HU$pXzI&7bwlwFV$@gQ^hB(KGZhDd$~XR9kC zuIx6vm~1gbgt;zB;Ep>55GY93lurA*F=zfDxyrI6-$qeWt}I;+`w@nGOndjSoa1%y z$JDApF&PaEOAT{+3o~c=zb`Iz7_y}#G-o~1m+{uevqN2F^qi`RGBnC>xf94~rYf>Q zPTV3EvS{n1uRQwdh-OSy(J|`G$pnkQ1hf38*FMS_x~V;U<<;!(sX;)-fK zQxCXgIo8h#b#Yqu%Gq%!*h59|oRPQA_Rr%B1_$bM70TkIv_oYAr|dpXK1k7}ohE8n z&TDu1x40`bBKKg)nkX0diX!>3p2-4rpO^~qlf00nKx3|puOH8>>^9Wh`?AMjvq!p3 z+`-{~pc)*taIDQff7K)tnZx#$xvw6=_RJRDVboelSzV*Z!%UU4 zj*tggK+4iarWHe#NE~DxCU^o?UXK zgPP~s^#x6N@!!)Vwnu?{)@@La@;G>HyEOUiB>jc|y;zWykK;M+yZMl{Dli2rOn$t5 zijLSiS#ZKyrd(!?Lba&8*z{-|w|kPVsugntKlSzX1COeu1U{@@UG;vh zAeN?Ep>$~^V3tqnT?V)@C#Q_Jg9Lik{xf+nHvH!}gO-$|=v$C_Y9e}VU}})n-?vJ6kZ_QymulQ zU^7|_fy^fLXTKezpT{MPqCqwplEb!ZO)W1Jqdr7a-^vxb`*NT-JO=5@1pThOczzDX z-j>-6wup*~vLxPg?(NbvlHW@-G3OE}r!;ZnU{)H2$wW9}W#WO#5Cki&PmFF*b({Ls#4 z0^{G>7hWkQ$u@V+kJ`=Oxx7rVPWQ91N%6gf#RJSR^d|g5Attb4y#sbiiF|yxTX*wt z7Z^4fCg%5ej*wuBTu?#MEjv=NEmNoxb96+h{FJG;F3A%%f3&x`Sv2(~*~$gWwEhk% zrV!$xDmJmxU6HMFnrU$=^ygch1u2b!6{18Plc=!e&PF z(OFu*ZF=xbxdM3JSnQSu%hVn^bIjHiiGr-$U* z$(<-yI)>IHJV`-f7P9A>q^Z>})bZLbJ0UL0%Q+lj>PXSsFsE(rp_H49tB&$X^ACV= zGBxa2=i<(ur`Il?r;1YNOnQPYtNA1TKa0W5M^wnsSWHDVdOroFOo7B?Y}gH2LKlMQ+r0v5epG-^+`+}Y&rjw;N?Y3 zXl_^pvSML~XmkSvbLzFuwv{EJsq^RwelsglGQS)0A{F}nA&htaXv$wZb zK_MJFe{Q;^#}#ryE$b}XIi1!reNw!LY0Y?LKpSht?k75NnVGnSb;6#~yLlDcSj3}# zs1vRATx(JPR$y>F*OO(eyUySg3RZQ{#1Pb_{NnVAdD#CjZGgDeHu3&rqtuBVsJD*Fv>qtk_|(*~-g#f7z7e+5PN3Ji{Tq zAJUwd6R|AG6wLfy^qHA7DO=cJ43Hg2|Au5~g%jbC);n-;Py{?wG1$DsKohl^aIF*7 zR4jRGZ#zBO>dG>*288k{wzz?CM?mbKU35e_ImkUR|91ih}n-+q?L#!5-9LReT-;XyI0+@sjM}!M@ zKJun?dwO_GfekgNKG1si$;{L=>r!_KjL(rCQ>_$KV;9djI?Ad>kV|Xypvsbxbm0$f zt)=%r@=tmu>-(lwpZ(6@Co&Pd|%Rn>?=0> zk97X&!t7zl_wIzbo&@?>@onp-JsSJ*cSiu8^rop|W`U1n#jyvHht(4qolQIUpwHHE zDW8Jg$r@q|9tOmYH1uGAPSOKs3?!30rjh(pkONmOuGP}n1M{OeCJ=S z)Uu3eKc|#wzOu%o`zL+omXmeH(z6H95IA&zm#~c&Os*&z1+!B?MGj2D+N--+2Q-33 z3AAE=gEy9diGRdi`EVOImwVQk|B2s7v2+hn;4fZ6m>o%B2e|eTWEfl<6Fr5U?D~IIM;ftLWStoY<_30})4J8MXTX#}Mz#O+L{40n*s$6K zh9yEq_k)z6+Hn`h8k~Z{IP%l=b0O!u_FbBD4)6f+kK9+sJA1&nKO9NoJ}{OHxJpZP zbuU;WnTF;>pG&`gUS>Il3Fg=E?@{8*J$&zKG}i^XpudT2&B0`T$ol#Y0tJ?p86#M- zAPh_ZjiXJjkL0b`^%f26Ln=hpkJOB=NCoqxnPes=c_thn*tl99f8r6#ZnfHKRTU09(Mntjz z3-kB=vcKycp{Mj0;h1m8OT3_7N|wB1vW8_ma)CHtXLeLwz(e)}^J`Et9^Al>MHbJ; zpR^oFuk7-~fGYp_u_@8F{+W404hR0*WIH+|*B=H3;=vONt4{0{EXJw1T9M#a)NfF- zjHTydAkv$$+ERv^%_|!5Sh<)lKEuMImUtoiOy|pK1>&kv(=6Q)OC$(KNoYjCKX4v9 z^Sz;KI0;v9&!@Zuq;U9 z=nk}?HbP(~CtXkleE_v3Xp3pHhbI_g&8PLQh>10BY|BF3uT3k{vl%&J4}HEMx*yau z%-SO5e=1p&3H%aob?;onV_`NoC4d=T3O9``qHBkpabOi%7CmaQW^TW!`Qu8h{dO}& zVBb{W*HfF1gH+;;)L3N6j`9R~>P&S_Jh+eyp1AsMR>fJJ|m zH<~^OPnH&(1QqMzZEt0zi?d6b{wO92EK6R-oCosU?KXWH!=D&qlGU0fF#I1#61NEh zDdfYmP)}hE0hz)N%d79yJb7+m1>u+zLT1J{3&z&hfFTNLp;LKk_3t6kC0O5jsA<$2 zma`i%@zP&X!MPdk+;>F^<)4Y?7RD|g=wcDVxFnJ-i!(n|G17RL)y_H=X_@vv!TtF| z^mbevm2;@6(wHg(l17^NvwA}u{b?;9b7$j(&-07t=*_-K9N9>?An+!?#)y_c#EG-} z=aXcNvfa~ku#i{_kacR=NCb75sZvHC1rkWTF_3!V()B4K;$!T>_A50)Ii82f){+0V zPrB$hS-|U@=81U|;{zWTIqB52b1~%t)lNp411)ohLSgQwZa2OuX2Kh$t9D+M&GHfg zGx7GHeNpG~VXqVd{Cr?W0Bf5T4VTd9)j0p2&F#c0~Dl!TKn&NT_x@F|p2XvwTdbE*#L zZ4Ul><^S`JAw#go|27^Z!Hrl!iyLF@3a2x*C0I)!o@SDLM(0sab1eZ;Ja~-pb4HSm zDx!D33>rCgpccdR?dzsNfbi=WtNs;t!uG~S6+*z^Kx-j_z!lj@ElrFi4UQpiueLGc zOP>im^?PMw(gni%FA5>U&oM|1)MVoc6YV z4^m#29DBby69?yqZ#P{?*V|8ccSyk08d2&o+*<{vNu<$jdBfNb^jcMYa$kln#1peAo*(v8v6yP zuhx}xA&NY=UadM@yK?En%2}9`lMGfNSrK2vEMtJ18}%WEJA!j?{>~Iw2aQVIj4KvK zYuX!&q=$SF;PvihAz`jqB4F1Xp`uN@6poF{x6nfgoHuwQ|3!uOb1pN%$3$EuFFsgT zr}a0?GTkC|DXL>o5jS^j-BPA~*Gz`Of?5sGdmn9-xOj8^v8Q3p<^hMo0%0th90X3u z!)aT@kS=aP!C`cxzJMg&0$D70{JTH5 z_ltoMjc0UE{&c~z23pF+I(th=*nL024*p*prg+A!;VD;=y2B;dA<>dHSsR3ycv=rV@*-^043>>Oa`nejjpjO3{6b=sX`=2yAD+P~i1yk-ge z!I5f4J-NOHT<@+YmjwNhm(7S!jN0D2SKpGzBO)DnP~#=@f&2ZYPt#Ge4Ac}VbQR-v ztFN%JmN;e=15YeFnU>(wBoo%Y53mAgM0q+I_r=~S5`a03L3Glwr$CXg$m0GNb#;F6 zX8)IUkFJ7r>duXHM<7R7+OMxuKgYd&tdm~R`W;YQK{g?dH@89Y`EAF4S|y#)TWme)F_?iVKia|6fyA9tdUr#@}Ha}Kj$z6X#c0-j z{GJ*4{eJ%Rj@LWy_w#+epU>y{KHsPKpRAi=86s&T{l&=aQ@Z%lK3;FCa$b~O<_ zuHr=Sh=sviXeBWH!%{klMpE^7ESz zo&N?w`;XqA82QxF{o>FQS>*I9_XLT5{aUcNEWTt`S~j>cGT6tC{_T6crMxMqo}MjI z7QXT~P((ffWiIk;gN!HV^{>44svUGJ&Q0`1F=M_LfFoO?sn*-fl7X&LOA}LB|6_`UjDchGEY!0*yjkd*PL+CEY)>b-?2Z!qtLn*1=tLry&o zn+2jhtL>m58%Doyg@}Hz-Cj|Szb*2OEiKep6U=~3fY>ZuRmti6tIXf}PKI78k3!Oq zP@%=U47P`>MN=O|iv5oGfc9fmrT8dC%Rj!T6LDN6fdzv#diR$w9Swv?FU&Z1oyYd? zh#XUt6M1hj4Kq~&q*>T_aZv6+WKHBX&PAFuCV7fC9#Nrmc z7On+TG}_-&3j&%!>R5=<-ogQQLrF)1TD7>bC3~qN!pO!E?}^ioEi3bfaiC_yCwtsb z=cXOrW{$j5o=^S(opzrv^wY_cGL{34z$iDWuvS)bUj+p_MORO`h83<|QqLc5)+B(E zlxybyHk`*MaomsXhVxOuMcpq*jrW)=ulLsG?2<6}3J%kqm&9XSc`-@u>ez3QDG8T6pwX z-lcl*`NMnJ~vdiDtYDEPbqGw>h`zx zDyqv7aD&^kEhOOY>|>Q=o$;vi95cPqGmconiDGm!L4CNt-;<(Yg{vbC(D{rp!OgDh zCR>YthmM4JKhHVJTbFL;3&l8@5!<{fr=}D*&QXtmRIky5Iz^n&<)skNPPJlgxGB#|Im*V zyGENmj^+Wrw>OEy86J3T9~u(YGCr^~%$?2{$q}ZL%3(3G;KmhacxI{lJU?M!K2IpzIAAAe&72E#k}b?4 z+_Z1E98#SS6=DUF8oJicd5g_ht8&M$Kr8rFZQS6*q0GKiKE^J~f{Zi=h=QWHx*bVI z+Au}-iM^tahbb${42am#dR-zZyf~ENZpRm#!2lNuZ>`Q{zwh2T!9rc|=h#?zcJorf z&t(FMhw%cZLBD>F+p&fEK@2q)%=*;&=rCC|502Yq!wthqo#0`C2eIzkXwTYhHeI-Ys(U_ zZIi;8&}E=2A+2qgIM%A1L430QUAG+H)2w1cd;5h9#~USfNlC2B{DW+=^Qq=bZ1%8e z!@`ZQYXmJR)qK2!`-NnZz7DFwaKTDRZq$eM#Ug(jhkO$6DXaMc16 zs&yEdRHXUOWXI=^2sy`Wa2irXd02T?5aak)`RW z=(rw(7c;Azz87u!4Cy8KPMuy*Z(DJd59w*HlLq_SprL-v0Dj|;XV9}N=z^aAXa`Lo zqU|;iQr%W;+9j#obDlD`JdwiXeN@a9&8J6!3C@$%?G&Hy1=@gEgAzSsdYR&V_aQ@` z9z(TNEOY&}^E3mA=+>i%;G>LmY#&`A+~1Yaeri((+wA$ZDicBzJWD=4fFcy}0J7fS zStqAoIP~(3)mS- z7oYC?FK)`EVK0uZf-n+w1TQe_@^{_t1g-KN`JbzcAnP>`r_`G9?Dg-`4Q9*&`t4bu$RT0}nZA36NBcK=?K)BjBw%HXJqD~StS&EF14NKk~Atq|;3hx9tT z^+V7>Us?Le?WonW@|{gh%89S$jyK3%i<(%KEw8qcFmx` - + + flir_camera_description - 0.2.5 - URDF descriptions for Flir cameras - + 1.0.0 + FLIR camera Description package Luis Camero - BSD - catkin - robot_state_publisher - urdf - xacro + ament_cmake + + urdf + robot_state_publisher + xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/flir_camera_description/urdf/demo.urdf.xacro b/flir_camera_description/urdf/demo.urdf.xacro new file mode 100644 index 00000000..6a963bf0 --- /dev/null +++ b/flir_camera_description/urdf/demo.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/flir_camera_description/urdf/flir_blackflys.urdf.xacro b/flir_camera_description/urdf/flir_blackfly_s.urdf.xacro similarity index 97% rename from flir_camera_description/urdf/flir_blackflys.urdf.xacro rename to flir_camera_description/urdf/flir_blackfly_s.urdf.xacro index 02b6b736..ab779f82 100644 --- a/flir_camera_description/urdf/flir_blackflys.urdf.xacro +++ b/flir_camera_description/urdf/flir_blackfly_s.urdf.xacro @@ -1,11 +1,11 @@ - + - @@ -18,7 +18,7 @@ - + From 2602b01423c199792c812e46fd249e27f75d48df Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:17:45 -0400 Subject: [PATCH 04/11] added flir_camera_msgs package --- flir_camera_msgs/CMakeLists.txt | 48 ++++++ flir_camera_msgs/CONTRIBUTING.md | 18 +++ flir_camera_msgs/LICENSE | 202 +++++++++++++++++++++++++ flir_camera_msgs/README.md | 2 + flir_camera_msgs/msg/CameraControl.msg | 10 ++ flir_camera_msgs/msg/ImageMetaData.msg | 19 +++ flir_camera_msgs/package.xml | 29 ++++ 7 files changed, 328 insertions(+) create mode 100644 flir_camera_msgs/CMakeLists.txt create mode 100644 flir_camera_msgs/CONTRIBUTING.md create mode 100644 flir_camera_msgs/LICENSE create mode 100644 flir_camera_msgs/README.md create mode 100644 flir_camera_msgs/msg/CameraControl.msg create mode 100644 flir_camera_msgs/msg/ImageMetaData.msg create mode 100644 flir_camera_msgs/package.xml diff --git a/flir_camera_msgs/CMakeLists.txt b/flir_camera_msgs/CMakeLists.txt new file mode 100644 index 00000000..7470ce73 --- /dev/null +++ b/flir_camera_msgs/CMakeLists.txt @@ -0,0 +1,48 @@ +# -*- cmake -*- +# +# Copyright 2023 Bernd Pfrommer +# +# 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. + +cmake_minimum_required(VERSION 3.5) +project(flir_camera_msgs) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + msg/ImageMetaData.msg + msg/CameraControl.msg + DEPENDENCIES builtin_interfaces std_msgs + ADD_LINTER_TESTS) + +if(BUILD_TESTING) + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_copyright REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_pep257 REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + + ament_copyright() + ament_xmllint() + ament_lint_cmake() +endif() + + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/flir_camera_msgs/CONTRIBUTING.md b/flir_camera_msgs/CONTRIBUTING.md new file mode 100644 index 00000000..cfba094d --- /dev/null +++ b/flir_camera_msgs/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/flir_camera_msgs/LICENSE b/flir_camera_msgs/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/flir_camera_msgs/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/flir_camera_msgs/README.md b/flir_camera_msgs/README.md new file mode 100644 index 00000000..193370b3 --- /dev/null +++ b/flir_camera_msgs/README.md @@ -0,0 +1,2 @@ +# message related to FLIR camera drivers + diff --git a/flir_camera_msgs/msg/CameraControl.msg b/flir_camera_msgs/msg/CameraControl.msg new file mode 100644 index 00000000..d1d83a09 --- /dev/null +++ b/flir_camera_msgs/msg/CameraControl.msg @@ -0,0 +1,10 @@ +# +# camera control message +# +std_msgs/Header header + +# set exposure time in microseconds +uint32 exposure_time + +# set gain in db +float32 gain diff --git a/flir_camera_msgs/msg/ImageMetaData.msg b/flir_camera_msgs/msg/ImageMetaData.msg new file mode 100644 index 00000000..dff6f4e6 --- /dev/null +++ b/flir_camera_msgs/msg/ImageMetaData.msg @@ -0,0 +1,19 @@ +# +# meta data messages for camera images +# +std_msgs/Header header + +# time stamp produced by camera, interpretation depends on camera model +uint64 camera_time + +# computed brightness, either 0...255 (valid) or -1 (invalid) +int16 brightness + +# exposure time in microseconds +uint32 exposure_time + +# max allowed exposure time in microseconds +uint32 max_exposure_time + +# gain in db +float32 gain diff --git a/flir_camera_msgs/package.xml b/flir_camera_msgs/package.xml new file mode 100644 index 00000000..7f80d259 --- /dev/null +++ b/flir_camera_msgs/package.xml @@ -0,0 +1,29 @@ + + + + flir_camera_msgs + 1.0.0 + messages related to flir camera driver + Bernd Pfrommer + Apache-2 + + ament_cmake + rosidl_default_generators + + builtin_interfaces + rosidl_default_generators + std_msgs + + builtin_interfaces + rosidl_default_runtime + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + From 90a16c9b7956d33c6b7059b56372264d3e5745d6 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:21:23 -0400 Subject: [PATCH 05/11] added spinnaker_camera_driver package --- spinnaker_camera_driver/.clang-format | 20 + spinnaker_camera_driver/CMakeLists.txt | 166 +++++ spinnaker_camera_driver/CONTRIBUTING.md | 18 + spinnaker_camera_driver/LICENSE | 202 ++++++ spinnaker_camera_driver/README.md | 255 +++++++ .../cmake/DownloadSpinnaker.cmake | 36 + .../cmake/FindSPINNAKER.cmake | 74 ++ .../cmake/TargetArch.cmake | 155 ++++ .../cmake/download_spinnaker | 155 ++++ spinnaker_camera_driver/cmake/install_files | 1 + spinnaker_camera_driver/config/blackfly_s.cfg | 117 +++ .../config/blackfly_s_gige.cfg | 100 +++ spinnaker_camera_driver/config/chameleon.cfg | 111 +++ .../config/grasshopper.cfg | 97 +++ .../spinnaker_camera_driver/camera.hpp | 38 + .../spinnaker_camera_driver/camera_driver.hpp | 121 ++++ .../include/spinnaker_camera_driver/image.hpp | 55 ++ .../spinnaker_camera_driver/pixel_format.hpp | 61 ++ .../spinnaker_wrapper.hpp | 68 ++ .../launch/blackfly_s.launch.py | 74 ++ .../launch/blackfly_s_gige.launch.py | 65 ++ .../launch/chameleon.launch.py | 69 ++ .../launch/grasshopper.launch.py | 62 ++ .../grasshopper_with_exp_control.launch.py | 69 ++ .../launch/stereo_synced.launch.py | 114 +++ spinnaker_camera_driver/package.xml | 27 + spinnaker_camera_driver/src/camera_driver.cpp | 683 ++++++++++++++++++ .../src/camera_driver_node.cpp | 30 + spinnaker_camera_driver/src/genicam_utils.cpp | 97 +++ spinnaker_camera_driver/src/genicam_utils.hpp | 35 + spinnaker_camera_driver/src/image.cpp | 41 ++ spinnaker_camera_driver/src/pixel_format.cpp | 97 +++ .../src/spinnaker_wrapper.cpp | 101 +++ .../src/spinnaker_wrapper_impl.cpp | 444 ++++++++++++ .../src/spinnaker_wrapper_impl.hpp | 89 +++ 35 files changed, 3947 insertions(+) create mode 100644 spinnaker_camera_driver/.clang-format create mode 100644 spinnaker_camera_driver/CMakeLists.txt create mode 100644 spinnaker_camera_driver/CONTRIBUTING.md create mode 100644 spinnaker_camera_driver/LICENSE create mode 100644 spinnaker_camera_driver/README.md create mode 100644 spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake create mode 100644 spinnaker_camera_driver/cmake/FindSPINNAKER.cmake create mode 100644 spinnaker_camera_driver/cmake/TargetArch.cmake create mode 100755 spinnaker_camera_driver/cmake/download_spinnaker create mode 100644 spinnaker_camera_driver/cmake/install_files create mode 100644 spinnaker_camera_driver/config/blackfly_s.cfg create mode 100644 spinnaker_camera_driver/config/blackfly_s_gige.cfg create mode 100644 spinnaker_camera_driver/config/chameleon.cfg create mode 100644 spinnaker_camera_driver/config/grasshopper.cfg create mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp create mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp create mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp create mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/pixel_format.hpp create mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp create mode 100755 spinnaker_camera_driver/launch/blackfly_s.launch.py create mode 100755 spinnaker_camera_driver/launch/blackfly_s_gige.launch.py create mode 100755 spinnaker_camera_driver/launch/chameleon.launch.py create mode 100755 spinnaker_camera_driver/launch/grasshopper.launch.py create mode 100755 spinnaker_camera_driver/launch/grasshopper_with_exp_control.launch.py create mode 100755 spinnaker_camera_driver/launch/stereo_synced.launch.py create mode 100644 spinnaker_camera_driver/package.xml create mode 100644 spinnaker_camera_driver/src/camera_driver.cpp create mode 100644 spinnaker_camera_driver/src/camera_driver_node.cpp create mode 100644 spinnaker_camera_driver/src/genicam_utils.cpp create mode 100644 spinnaker_camera_driver/src/genicam_utils.hpp create mode 100644 spinnaker_camera_driver/src/image.cpp create mode 100644 spinnaker_camera_driver/src/pixel_format.cpp create mode 100644 spinnaker_camera_driver/src/spinnaker_wrapper.cpp create mode 100644 spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp create mode 100644 spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp diff --git a/spinnaker_camera_driver/.clang-format b/spinnaker_camera_driver/.clang-format new file mode 100644 index 00000000..1aa199dc --- /dev/null +++ b/spinnaker_camera_driver/.clang-format @@ -0,0 +1,20 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +... + diff --git a/spinnaker_camera_driver/CMakeLists.txt b/spinnaker_camera_driver/CMakeLists.txt new file mode 100644 index 00000000..2d1411c8 --- /dev/null +++ b/spinnaker_camera_driver/CMakeLists.txt @@ -0,0 +1,166 @@ +# -*- cmake -*- +# +# Copyright 2023 Bernd Pfrommer +# +# 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. + +cmake_minimum_required(VERSION 3.5) +project(spinnaker_camera_driver) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + +# the spinnaker SDK does not provide a cmake file +list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") + +# If Spinnacker is already present, use the found version. If not, download it. +# We can't resolve this dependency using the usual rosdep means because +# the Point Grey EULA prohibits redistributing the headers or the packages which +# contains them. We work around this by downloading the archive directly from +# their website during this step in the build process. +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules") +find_package(SPINNAKER QUIET) +message(STATUS + "Found variable: ${SPINNAKER_FOUND} at ${SPINNAKER_LIBRARIES} and ${SPINNAKER_INCLUDE_DIRS}") + +if(NOT SPINNAKER_FOUND) + message(STATUS "spinnaker not found") + message(STATUS "libSpinnaker not found in system library path") + include(cmake/DownloadSpinnaker.cmake) + download_spinnaker(SPINNAKER_LIBRARIES SPINNAKER_INCLUDE_DIRS) +endif() + +message(STATUS "libSpinnaker library location: ${SPINNAKER_LIBRARIES}") +message(STATUS "libSpinnaker include location: ${SPINNAKER_INCLUDE_DIRS}") + +find_package(SPINNAKER REQUIRED) + +include_directories(SYSTEM + ${SPINNAKER_INCLUDE_DIRS}) + +# list of all packages that use ament for export (may work for others too) + +set(ROS_DEPENDENCIES + "rclcpp" + "rclcpp_components" + "sensor_msgs" + "std_msgs" + "camera_info_manager" + "image_transport" + "flir_camera_msgs") + + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +foreach(pkg ${ROS_DEPENDENCIES}) + find_package(${pkg} REQUIRED) +endforeach() + + +# +# shared library for composable node +# + +add_library(camera_driver SHARED + src/camera_driver.cpp + src/spinnaker_wrapper.cpp + src/spinnaker_wrapper_impl.cpp + src/image.cpp + src/pixel_format.cpp + src/genicam_utils.cpp +) + +ament_target_dependencies(camera_driver PUBLIC ${ROS_DEPENDENCIES}) +target_link_libraries(camera_driver PUBLIC Spinnaker::Spinnaker) + +target_include_directories(camera_driver + PUBLIC $ + $ +) +rclcpp_components_register_nodes(camera_driver "spinnaker_camera_driver::CameraDriver") + +# +# the driver node uses the shared library +# +add_executable(camera_driver_node + src/camera_driver_node.cpp) + +target_link_libraries(camera_driver_node camera_driver) + +target_include_directories(camera_driver_node + PUBLIC $ + $ +) + +target_include_directories(camera_driver PRIVATE include) + + +# the node must go into the project specific lib directory or else +# the launch file will not find it + +install(TARGETS + camera_driver_node + DESTINATION lib/${PROJECT_NAME}/) + +# the shared library goes into the global lib dir so it can +# be used as a composable node by other projects + +install(TARGETS + camera_driver + DESTINATION lib +) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_copyright REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_flake8 REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_pep257 REQUIRED) + find_package(ament_cmake_clang_format REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + + ament_copyright(EXCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/cmake/TargetArch.cmake) + ament_cppcheck(LANGUAGE c++) + ament_cpplint(FILTERS "-build/include,-runtime/indentation_namespace") + ament_flake8() + ament_lint_cmake() + ament_pep257() + ament_clang_format(CONFIG_FILE .clang-format) + ament_xmllint() +endif() + +ament_package() diff --git a/spinnaker_camera_driver/CONTRIBUTING.md b/spinnaker_camera_driver/CONTRIBUTING.md new file mode 100644 index 00000000..cfba094d --- /dev/null +++ b/spinnaker_camera_driver/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/spinnaker_camera_driver/LICENSE b/spinnaker_camera_driver/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/spinnaker_camera_driver/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/spinnaker_camera_driver/README.md b/spinnaker_camera_driver/README.md new file mode 100644 index 00000000..5e94d837 --- /dev/null +++ b/spinnaker_camera_driver/README.md @@ -0,0 +1,255 @@ +# spinnaker_camera_driver: ROS driver for FLIR cameras based on the Spinnaker SDK + +ROS driver for the FLIR cameras using the +[Spinnaker SDK](http://softwareservices.flir.com/Spinnaker/latest/index.htmlspinnaker). + +NOTE: This driver is not written or supported by FLIR. + +## Tested cameras: + +The following cameras have been used with this driver: + +- Blackfly S (USB3, GigE) +- Blackfly (GigE) +- Grashopper (USB3) +- Chameleon (USB3, tested on firmware v1.13.3.00) + +Note: if you get other cameras to work, *please report back*, ideally +submit a pull request with the camera config file you have created. + +## Tested platforms + +Software: + +- ROS2 Galactic under Ubuntu 20.04 LTS +- ROS2 Humble under Ubuntu 22.04 LTS +- Spinnaker 3.1.0.79 (other versions may work as well but this is + what the continuous integration builds are using) + + +## Features + +Basic features are supported like setting exposure, gain, and external +triggering. It is straight forward to support new camera types and features by +editing the camera definition (.cfg) files. Unless you need new pixel +formats you may not have to modify any source code. The code is meant +to be a thin wrapper for setting the features available in FLIR's +SpinView program. The driver has the following parameters, +*in addition to the parameters defined in the .cfg files*: + +- ``serial_number``: must have the serial number of the camera. If you + don't know it, put in anything you like and + the driver will croak with an error message, telling you what + cameras serial numbers are available +- ``frame_id``: the ROS frame id to put in the header of the published + image messages. +- ``camerainfo_url``: where to find the camera calibration yaml file. +- ``parameter_file``: location of the .cfg file defining the camera + (blackfly_s.cfg etc) +- ``compute_brightness``: if true, compute image brightness and + publish it in meta data message. This is useful for external + exposure control but incurs extra CPU load. Default: false. +- ``buffer_queue_size``: max number of images to queue internally + before shoving them into the ROS output queue. Decouples the + Spinnaker SDK thread from the ROS publishing thread. Default: 4. +- ``image_queue_size``: ROS output queue size (number of frames). Default: 4 +- ``dump_node_map``: set this to true to get a dump of the node map. This + feature is helpful when developing a new config file. Default: false. + + +## How to build + +1) Install the FLIR spinnaker driver. If you skip this part, the +driver will attempt to download the Spinnaker SDK automatically to +obtain the header files and libraries +2) Prepare the ROS2 driver build: +Make sure you have your ROS2 environment sourced: +``` +source /opt/ros//setup.bash +``` + +Create a workspace (``~/ws``), clone this repo: + +``` +mkdir -p ~/ws/src +cd ~/ws/src +git clone https://github.com/ros-drivers/flir_camera_driver +cd .. +``` + +To automatically install all packages that the ``flir_camera_driver`` packages +depends upon, run this at the top of your workspace: +``` +rosdep install --from-paths src --ignore-src +``` + +3) Build the driver and source the workspace: +``` +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +. install/setup.bash +``` + +## Example usage + +How to launch the example file: +``` +ros2 launch spinnaker_camera_driver blackfly_s.launch.py camera_name:=blackfly_0 serial:="'20435008'" +``` + +## Setting up GigE cameras + +The Spinnaker SDK abstracts away the transport layer so a GigE camera +should work the same way as USB3: you point it to the serial +number and you're set. + +There are a few GigE-specific settings in the Transport Layer Control +group that are important, in particular enabling jumbo frames from the +camera per FLIR's recommendations. The following line in your +camera-specific config file will create a ROS2 parameter +``gev_scps_packet_size``: +``` +gev_scps_packet_size int "TransportLayerControl/GigEVision/GevSCPSPacketSize" +``` +that you can then set in your ROS2 launch file: +``` + "gev_scps_packet_size": 9000 +``` +As far as setting up the camera's IP address: you can set up DHCP on +your network or configure a static persistent IP using SpinView +in "Transport Layer Control">"GigE Vision". Check the box for "Current +IP Configuration Persistent IP" first to enable it, then set your +desired addresses under "Persistent IP Address", "Persistent Subnet +Mask" and "Persistent Gateway". NOTE: these look like regular IPs, but +to set them you have to enter the 32-bit integer representation of the +IP address/mask. By hand/calculator: convert the IP octets from +decimal to hex, then combine them and convert to a 32-bit integer, ex: +192.168.0.1 -> 0xC0A80001 -> 3232235521. + +The "Transport Layer Control">"GigE Vision" section of SpinView is +also where you'll find that "SCPS Packet Size" setting, which you can +change when not capturing frames, and verify it works in SpinView and +without needing to spin up a custom launch file to get started, though +it helps, and you'll probably want one anyway to specify your camera's +serial number. + +For more tips on GigE setup look at FLIR's support pages +[here](https://www.flir.com/support-center/iis/machine-vision/knowledge-base/lost-ethernet-data-packets-on-linux-systems/) +and +[here](https://www.flir.com/support-center/iis/machine-vision/application-note/troubleshooting-image-consistency-errors/). + +## Camera synchronization + +In the ``launch`` folder you can find a working example for launching +drivers for two hardware synchronized Blackfly S cameras. The launch +file requires two more packages to be installed, +[cam_sync_ros2](https://github.com/berndpfrommer/cam_sync_ros2)(for +time stamp syncing) and +[exposure_control_ros2](https://github.com/berndpfrommer/exposure_control_ros2) +(for external exposure control). See below for more details on those packages. + +### Time stamps + +By default the driver will set the ROS header time stamp to be the +time when the image was delivered by the SDK. Such time stamps are not +very precise and may lag depending on host CPU load. However the +driver has a feature to use the much more accurate sensor-provided +camera time stamps. These are then converted to ROS time stamps by +estimating the offset between ROS and sensor time stamps via a simple +moving average. For the adjustment to work +*the camera must be configured to send time stamps*, and the +``adjust_timestamp`` flag must be set to true, and the relevant field +in the "chunk" must be populated by the camera. For the Blackfly S +the parameters look like this: + +``` + 'adjust_timestamp': True, + 'chunk_mode_active': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, +``` + +When running hardware synchronized cameras in a stereo configuration +two drivers will need to be run, one for each camera. This will mean +however that their published ROS header time stamps are *not* +identical which in turn may prevent down-stream ROS nodes from recognizing the +images as being hardware synchronized. You can use the +[cam_sync_ros2 node](https://github.com/berndpfrommer/cam_sync_ros2) +to force the time stamps to be aligned. In this scenario it is +mandatory to configure the driver to adjust the ROS time stamps as +described above. + + +### Automatic exposure + +In most situations it is recommended to enable the built-in auto +exposure of the camera. However, in a +synchronized setting it is sometimes desirable to disable the built-in +auto-exposure and provide it externally. For instance in a stereo setup, +matching left and right image patches can be difficult when each +camera runs its own auto exposure independently. The +[exposure_control_ros2](https://github.com/berndpfrommer/exposure_control_ros2) +package can provide external automatic exposure control. To this end +the driver publishes +[meta data messages](https://github.com/ros-drivers/flir_camera_driver/flir_camera_msgs) and +subscribes to +[camera control +messages](https://github.com/ros-drivers/flir_camera_driver/flir_camera_msgs). See +the launch file directory for examples. + + +## How to add new features + +For lack of a more systematic way to discover the camera configuration node +names the following procedure is recommended for adding new features: + +- fire up FLIR's ``spinview`` application and find the parameter you want to add + to the config. + +- start with an existing config file in the ``config`` directory, make a copy + and give the feature a name that follows the established convention of + other parameters (all lower case, separate by underscores), for example + ``` + device_link_throughput_limit int "DeviceControl/DeviceLinkThroughputLimit" + ``` + The parameter name is followed by the parameter type (in this example ``int``), + which you have to somewhat guess. If ``spinview`` shows a multiple choice drop-down box, + the parameter is of type ``enum``, a check box translates to ``bool``, + otherwise it's ``float`` or ``int`` (check what input ``spinview``) accepts. + +- the hard part is the node name which is the last parameter of the line, in this + example ``"DeviceControl/DeviceLinkThroughputLimit"``. It usually follows by + removing spaces from the ``spinview`` names. If that doesn't work, + launch the driver with the ``dump_node_map`` parameter set to "True" + and look at the output for inspiration. + +Once you have modified the config file, now just set the newly created +parameter in the launch file, done. + +## Known issues + +1) If you run multiple drivers in separate nodes that all access USB based +devices, starting a new driver will stop the image acquisition of +currently running drivers. There is an ugly workaround for this +currently implemented: if image delivery stops for more than +``acquisition_timeout`` seconds, the acquisition is restarted. This +operation may not be thread safe so the driver already running could +possibly crash. This issue can be avoided by running all drivers in +the same address space with a composable node. + +## How to contribute +Please provide feedback if you cannot get your camera working or if +the code does not compile for you. Feedback is crucial for the +software development process. + +Bug fixes and config files for new cameras are greatly +appreciated. Before submitting a pull request, run this to see if your +commit passes some basic lint tests: +``` +colcon test --packages-select spinnaker_camera_driver && colcon test-result --verbose +``` + +## License + +This software is issued under the Apache License Version 2.0. +The file [TargetArch.cmake](cmake/TargetArch.cmake) is released under +a custom license (see file) diff --git a/spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake b/spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake new file mode 100644 index 00000000..9d610bd4 --- /dev/null +++ b/spinnaker_camera_driver/cmake/DownloadSpinnaker.cmake @@ -0,0 +1,36 @@ +# Copyright 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# 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. + +function(download_spinnaker FLIR_LIB_VAR FLIR_INCLUDE_DIR_VAR) + if(NOT UNIX) + message(FATAL_ERROR "Downloading libSpinnaker for non-linux systems not supported") + endif() + + execute_process( + COMMAND lsb_release -cs + OUTPUT_VARIABLE OS_CODE_NAME + OUTPUT_STRIP_TRAILING_WHITESPACE) + + include(cmake/TargetArch.cmake) + target_architecture(FLIR_ARCH) + set(FLIR_DIR ${CMAKE_CURRENT_BINARY_DIR}/usr/lib) + set(DOWNLOAD_SCRIPT "${PROJECT_SOURCE_DIR}/cmake/download_spinnaker") + + message(STATUS "Running download_spinnaker script with arguments: ${FLIR_ARCH} ${FLIR_DIR} ${OS_CODE_NAME}") + execute_process( + COMMAND ${DOWNLOAD_SCRIPT} ${FLIR_ARCH} "${FLIR_DIR}" ${OS_CODE_NAME} OUTPUT_VARIABLE RET) + message(STATUS "PROCESS RESULT: ${RET}") + set(${FLIR_LIB_VAR} "${CMAKE_BINARY_DIR}/usr/lib/libSpinnaker.so" PARENT_SCOPE) + set(${FLIR_INCLUDE_DIR_VAR} "${CMAKE_BINARY_DIR}/opt/spinnaker/include" PARENT_SCOPE) +endfunction() diff --git a/spinnaker_camera_driver/cmake/FindSPINNAKER.cmake b/spinnaker_camera_driver/cmake/FindSPINNAKER.cmake new file mode 100644 index 00000000..08ac5e55 --- /dev/null +++ b/spinnaker_camera_driver/cmake/FindSPINNAKER.cmake @@ -0,0 +1,74 @@ +# Copyright 2023 Bernd Pfrommer +# +# 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. + +# +# sets the following variables: +# +# SPINNAKER_INCLUDE_DIRS +# SPINNAKER_LIBRARIES +# SPINNAKER_FOUND +# +# or for the more modern cmake usage, sets the target Spinnaker::Spinnaker +# +# +# searches first in "SPINNAKER_ROOT_DIR" for location of spinnaker SDK + +# +include(FindPackageHandleStandardArgs) + +if( EXISTS "$ENV{SPINNAKER_ROOT_DIR}" ) + file( TO_CMAKE_PATH "$ENV{SPINNAKER_ROOT_DIR}" SPINNAKER_ROOT_DIR ) + set( SPINNAKER_ROOT_DIR "${SPINNAKER_ROOT_DIR}" CACHE PATH "Prefix for Spinnaker installation." ) +endif() + +cmake_path(SET PATH2 "${CMAKE_CURRENT_BINARY_DIR}") +cmake_path(GET PATH2 PARENT_PATH PARENTDIR) + +find_path(SPINNAKER_INCLUDE_DIRS + NAMES Spinnaker.h + HINTS + ${SPINNAKER_ROOT_DIR}/include + /opt/spinnaker/include + /usr/include/spinnaker + /usr/local/include/spinnaker + /__w/flir_ros2_camera_driver/flir_ros2_camera_driver/ros_ws/build/flir_spinnaker_common/opt/spinnaker/include +) + +find_library(SPINNAKER_LIBRARIES + NAMES SPINNAKER + HINTS + ${SPINNAKER_ROOT_DIR}/lib + /opt/spinnaker/lib + /usr/lib/ + /usr/local/lib + /__w/flir_ros2_camera_driver/flir_ros2_camera_driver/ros_ws/build/flir_spinnaker_common/usr/lib + PATH_SUFFIXES Release Debug +) + +set(SPINNAKER_INCLUDE_DIRS ${SPINNAKER_INCLUDE_DIRS}) +set(SPINNAKER_LIBRARIES ${SPINNAKER_LIBRARIES}) +message(STATUS "Found inc dir: ${SPINNAKER_INCLUDE_DIRS}") + +find_package_handle_standard_args(SPINNAKER + FOUND_VAR SPINNAKER_FOUND + REQUIRED_VARS SPINNAKER_INCLUDE_DIRS SPINNAKER_LIBRARIES) + + +if(SPINNAKER_FOUND AND NOT TARGET Spinnaker::Spinnaker) + add_library(Spinnaker::Spinnaker UNKNOWN IMPORTED) + set_target_properties(Spinnaker::Spinnaker PROPERTIES + IMPORTED_LOCATION "${SPINNAKER_LIBRARIES}" + INTERFACE_INCLUDE_DIRECTORIES "${SPINNAKER_INCLUDE_DIRS}" + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX") +endif() diff --git a/spinnaker_camera_driver/cmake/TargetArch.cmake b/spinnaker_camera_driver/cmake/TargetArch.cmake new file mode 100644 index 00000000..b419094a --- /dev/null +++ b/spinnaker_camera_driver/cmake/TargetArch.cmake @@ -0,0 +1,155 @@ +# Source: https://raw.github.com/petroules/solar-cmake/master/TargetArch.cmake +# +# Copyright 2012, Petroules Corporation, 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. 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. + +# Based on the Qt 5 processor detection code, so should be very accurate +# https://qt.gitorious.org/qt/qtbase/blobs/master/src/corelib/global/qprocessordetection.h +# Currently handles arm (v5, v6, v7), x86 (32/64), ia64, and ppc (32/64) + +# Regarding POWER/PowerPC, just as is noted in the Qt source, +# "There are many more known variants/revisions that we do not handle/detect." + +set(archdetect_c_code " +#if defined(__aarch64__) + #error cmake_ARCH armv8 +#elif defined(__arm__) || defined(__TARGET_ARCH_ARM) + #if defined(__ARM_ARCH_7__) \\ + || defined(__ARM_ARCH_7A__) \\ + || defined(__ARM_ARCH_7R__) \\ + || defined(__ARM_ARCH_7M__) \\ + || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7) + #error cmake_ARCH armv7 + #elif defined(__ARM_ARCH_6__) \\ + || defined(__ARM_ARCH_6J__) \\ + || defined(__ARM_ARCH_6T2__) \\ + || defined(__ARM_ARCH_6Z__) \\ + || defined(__ARM_ARCH_6K__) \\ + || defined(__ARM_ARCH_6ZK__) \\ + || defined(__ARM_ARCH_6M__) \\ + || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6) + #error cmake_ARCH armv6 + #elif defined(__ARM_ARCH_5TEJ__) \\ + || (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5) + #error cmake_ARCH armv5 + #else + #error cmake_ARCH arm + #endif +#elif defined(__i386) || defined(__i386__) || defined(_M_IX86) + #error cmake_ARCH i386 +#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64) + #error cmake_ARCH x86_64 +#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64) + #error cmake_ARCH ia64 +#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\ + || defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\ + || defined(_M_MPPC) || defined(_M_PPC) + #if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__) + #error cmake_ARCH ppc64 + #else + #error cmake_ARCH ppc + #endif +#endif + +#error cmake_ARCH unknown +") + +# Set ppc_support to TRUE before including this file or ppc and ppc64 +# will be treated as invalid architectures since they are no longer supported by Apple + +function(target_architecture output_var) + if(APPLE AND CMAKE_OSX_ARCHITECTURES) + # On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set + # First let's normalize the order of the values + + # Note that it's not possible to compile PowerPC applications if you are using + # the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we + # disable it by default + # See this page for more information: + # http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4 + + # Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime. + # On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise. + + foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES}) + if("${osx_arch}" STREQUAL "ppc" AND ppc_support) + set(osx_arch_ppc TRUE) + elseif("${osx_arch}" STREQUAL "i386") + set(osx_arch_i386 TRUE) + elseif("${osx_arch}" STREQUAL "x86_64") + set(osx_arch_x86_64 TRUE) + elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support) + set(osx_arch_ppc64 TRUE) + else() + message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}") + endif() + endforeach() + + # Now add all the architectures in our normalized order + if(osx_arch_ppc) + list(APPEND ARCH ppc) + endif() + + if(osx_arch_i386) + list(APPEND ARCH i386) + endif() + + if(osx_arch_x86_64) + list(APPEND ARCH x86_64) + endif() + + if(osx_arch_ppc64) + list(APPEND ARCH ppc64) + endif() + else() + file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}") + + enable_language(C) + + # Detect the architecture in a rather creative way... + # This compiles a small C program which is a series of ifdefs that selects a + # particular #error preprocessor directive whose message string contains the + # target architecture. The program will always fail to compile (both because + # file is not a valid C program, and obviously because of the presence of the + # #error preprocessor directives... but by exploiting the preprocessor in this + # way, we can detect the correct target architecture even when cross-compiling, + # since the program itself never needs to be run (only the compiler/preprocessor) + try_run( + run_result_unused + compile_result_unused + "${CMAKE_BINARY_DIR}" + "${CMAKE_BINARY_DIR}/arch.c" + COMPILE_OUTPUT_VARIABLE ARCH + CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES} + ) + + # Parse the architecture name from the compiler output + string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}") + + # Get rid of the value marker leaving just the architecture name + string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}") + + # If we are compiling with an unknown architecture this variable should + # already be set to "unknown" but in the case that it's empty (i.e. due + # to a typo in the code), then set it to unknown + if(NOT ARCH) + set(ARCH unknown) + endif() + endif() + + set(${output_var} "${ARCH}" PARENT_SCOPE) +endfunction() diff --git a/spinnaker_camera_driver/cmake/download_spinnaker b/spinnaker_camera_driver/cmake/download_spinnaker new file mode 100755 index 00000000..82392b6f --- /dev/null +++ b/spinnaker_camera_driver/cmake/download_spinnaker @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 +# +# Software License Agreement (BSD) +# +# @author Mike Purvis +# @author Chris Iverach-Brereton +# @copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. +# @usage download_spinnaker {arch} {dir} {os-code-name} +# e.g. download_spinnaker x86_64 /path/to/somewhere xenial +# +# 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 Clearpath Robotics 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 HOLDER 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 http.cookiejar +import io +import logging +import shutil +import subprocess +import sys +import tarfile +import urllib +import os +import glob + +logging.basicConfig(level=logging.INFO) + +URL_TEMPLATE = URL_TEMPLATES = { + 'deb': 'https://packages.clearpathrobotics.com/stable/flir/Spinnaker/Ubuntu{version}/spinnaker-3.1.0.79-{arch}-pkg.tar.gz', + 'src': None # if we ever have non-deb archives that require manual extraction, those package URLS will go here. +} + +ARCHS = { + 'x86_64': { + 'linux_arch': 'amd64', + 'current': '22.04', + 'type': 'deb', + 'folder_name': 'spinnaker-3.1.0.79-amd64', + 'shared_library': 'opt/spinnaker/lib/', + 'debs':[ + 'libgentl_3.1.0.79_amd64.deb', + 'libspinnaker_3.1.0.79_amd64.deb', + 'libspinnaker-c_3.1.0.79_amd64.deb', + 'libspinnaker-c-dev_3.1.0.79_amd64.deb', + 'libspinnaker-dev_3.1.0.79_amd64.deb', + 'libspinvideo_3.1.0.79_amd64.deb', + 'libspinvideo-c_3.1.0.79_amd64.deb', + 'libspinvideo-c-dev_3.1.0.79_amd64.deb', + 'libspinvideo-dev_3.1.0.79_amd64.deb', + 'spinnaker_3.1.0.79_amd64.deb', + 'spinnaker-doc_3.1.0.79_amd64.deb', + 'spinupdate_3.1.0.79_amd64.deb', + 'spinupdate-dev_3.1.0.79_amd64.deb', + 'spinview-qt_3.1.0.79_amd64.deb', + 'spinview-qt-dev_3.1.0.79_amd64.deb' + ] + }, + 'armv8': { + 'linux_arch': 'arm64', + 'current': '22.04', + 'type': 'deb', + 'shared_library': 'opt/spinnaker/lib/', + 'folder_name': 'spinnaker-3.1.0.79-arm64', + 'debs': [ + 'libgentl_3.1.0.79_arm64.deb', + 'libspinnaker_3.1.0.79_arm64.deb', + 'libspinnaker-c_3.1.0.79_arm64.deb', + 'libspinnaker-c-dev_3.1.0.79_arm64.deb', + 'libspinnaker-dev_3.1.0.79_arm64.deb', + 'libspinvideo_3.1.0.79_arm64.deb', + 'libspinvideo-c_3.1.0.79_arm64.deb', + 'libspinvideo-c-dev_3.1.0.79_arm64.deb', + 'libspinvideo-dev_3.1.0.79_arm64.deb', + 'spinnaker_3.1.0.79_arm64.deb', + 'spinnaker-doc_3.1.0.79_arm64.deb', + 'spinupdate_3.1.0.79_arm64.deb', + 'spinupdate-dev_3.1.0.79_arm64.deb', + 'spinview-qt_3.1.0.79_arm64.deb', + 'spinview-qt-dev_3.1.0.79_arm64.deb' + ] + } +} + + +OS_LIBRARY_VERSION = { + 'jammy': 'current' +} + +arch = sys.argv[1] +destination_folder = sys.argv[2] +os_code_name = sys.argv[3] + +os_version = OS_LIBRARY_VERSION[os_code_name] +archive_url = URL_TEMPLATES[ARCHS[arch]['type']].format( + arch=ARCHS[arch]['linux_arch'], + version=ARCHS[arch][os_version]) +folder_name = ARCHS[arch]['folder_name'] +shared_library = ARCHS[arch]['shared_library'] + + +print("CPU architecture is %s", arch) +print("OS code name is %s", os_code_name) +print("Destination folder is %s", destination_folder) + +if not os.path.exists(os.path.join(os.getcwd(), folder_name)): + cj = http.cookiejar.CookieJar() + opener = urllib.request.build_opener(urllib.request.HTTPCookieProcessor(cj)) + opener.addheaders = [ + ('User-agent', 'Mozilla/5.0'), + ('Referer', 'https://www.ptgrey.com')] + + print("Downloading SDK archive from {0}...".format(archive_url)) + resp = opener.open(archive_url) + + print("Unpacking tarball.") + with tarfile.open(mode="r:gz", fileobj=io.BytesIO(resp.read())) as tar: + tar.extractall() + + print("Unpacking debs.") + debs = glob.glob(os.path.join(os.getcwd(), folder_name, "*.deb")) + unpack_dir = os.path.join(os.getcwd()) + if not os.path.exists(unpack_dir): + os.makedirs(unpack_dir) + for deb in debs: + print('Extracting {0}'.format(deb)) + subprocess.call(['dpkg', '--extract', deb, unpack_dir]) + + # Copy the shared libraries into the output folder + print('Copying libraries to {0}'.format(destination_folder)) + if not os.path.exists(destination_folder): + os.makedirs(destination_folder) + libs = glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libSpinnaker*.so*")) + libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libG*.so*")) + libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "lib*Parser*.so*")) + libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libLog*.so*")) + libs += glob.glob(os.path.join(os.getcwd(), 'opt/spinnaker/lib', "libNodeMapData*.so*")) + for lib in libs: + print('Copying: {}'.format(os.path.basename(lib))) + shutil.copyfile(lib, os.path.join(destination_folder, os.path.basename(lib))) +else: + print("Previous installation found, skipping...") diff --git a/spinnaker_camera_driver/cmake/install_files b/spinnaker_camera_driver/cmake/install_files new file mode 100644 index 00000000..94fdfa4f --- /dev/null +++ b/spinnaker_camera_driver/cmake/install_files @@ -0,0 +1 @@ +libGCBase_gcc540_v3_0.so;libGenApi_gcc540_v3_0.so;liblog4cpp_gcc540_v3_0.so;libLog_gcc540_v3_0.so;libMathParser_gcc540_v3_0.so;libNodeMapData_gcc540_v3_0.so;libSpinnaker.so;libSpinnaker.so.1;libSpinnaker.so.1.13.0.31;libXmlParser_gcc540_v3_0.so;SFNC_GenTLDataStream_GigE_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLDataStream_Usb3_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLDevice_GigE_Version_1_0_0_Schema_1_1.xml;;SFNC_GenTLDevice_Reference_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLDevice_Usb3_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLInterface_GigE_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLInterface_Reference_Version_1_0_0_Schema_1_1.xml;SFNC_GenTLInterface_Usb3_Version_1_0_0_Schema_1_1.xml; \ No newline at end of file diff --git a/spinnaker_camera_driver/config/blackfly_s.cfg b/spinnaker_camera_driver/config/blackfly_s.cfg new file mode 100644 index 00000000..90e3515a --- /dev/null +++ b/spinnaker_camera_driver/config/blackfly_s.cfg @@ -0,0 +1,117 @@ +# +# config file for blackfly S cameras (tested for USB3) +# +# This file maps the ros parameters to the corresponding "nodes" in the camera. +# The question remains what the valid values are for the enum types. For that +# use the spinnaker GUI (spinview), or dump the nodemap by setting the +# "dump_node_map" parameter in the driver. Also setting the "debug" param helps. +# +# NOTE: !!!! THE ORDER OF PARAMETER DEFINITION MATTERS !!!! +# On node startup, the parameters will be declared and initialized +# in the order listed here. For instance you must list +# the enum "exposure_auto" before the float "exposure_time" because on +# startup, "exposure_auto" must first be set to "Off" before +# "exposure_time" can be set, or else the camera refuses to set +# the exposure time. +# + +# +# -------- pixel format +# +# Check available values with SpinView. Not all are supported by ROS! +# Some formats are e.g. "Mono8", "BayerRG8", "BGR8", "BayerRG16" +# default is "BayerRG8" +pixel_format enum "ImageFormatControl/PixelFormat" + +# +# ---------- (reduce) sensor size and change offset +# +image_width int "ImageFormatControl/Width" +image_height int "ImageFormatControl/Height" + +#(offset must come after image width reduction!) +offset_x int "ImageFormatControl/OffsetX" +offset_y int "ImageFormatControl/OffsetY" + + +# +# -------- analog control +# + +gain_auto enum "AnalogControl/GainAuto" +gain float "AnalogControl/Gain" + +# +# -------- device link throughput limiting +# + +device_link_throughput_limit int "DeviceControl/DeviceLinkThroughputLimit" + +# +# -------- digital IO control +# + +# black wire: opto-isolated input +line0_selector enum "DigitalIOControl/LineSelector" + +# white wire: opto-isolated output +line1_selector enum "DigitalIOControl/LineSelector" +# valid values: "Input", "Output" +line1_linemode enum "DigitalIOControl/LineMode" + +# red wire: non-isolated input/output and power output +line2_selector enum "DigitalIOControl/LineSelector" +line2_v33enable bool "DigitalIOControl/V3_3Enable" + +# green wire: aux voltage input and non-isolated input +line3_selector enum "DigitalIOControl/LineSelector" +# valid values: "Input", "Output" +line3_linemode enum "DigitalIOControl/LineMode" + +# +# -------- acquisition control +# + +exposure_auto enum "AcquisitionControl/ExposureAuto" +exposure_time float "AcquisitionControl/ExposureTime" + +frame_rate_enable bool "AcquisitionControl/AcquisitionFrameRateEnable" +frame_rate float "AcquisitionControl/AcquisitionFrameRate" + +# valid values are e.g. "FrameStart", "AcquisitionStart", "FrameBurstStart" +trigger_selector enum "AcquisitionControl/TriggerSelector" + +# valid values are "On" and "Off" +trigger_mode enum "AcquisitionControl/TriggerMode" + +# valid values are "Line<0,1,2>", "UserOutput<0,1,2>", "Counter<0,1>", +# "LogicBlock<0,1> +trigger_source enum "AcquisitionControl/TriggerSource" + +# value >= 9 +trigger_delay float "AcquisitionControl/TriggerDelay" + +# valid values: "Off" and "ReadOut" +trigger_overlap enum "AcquisitionControl/TriggerOverlap" + +# +# --------- chunk control +# + +chunk_mode_active bool "ChunkDataControl/ChunkModeActive" + +# valid values: "FrameID" +chunk_selector_frame_id enum "ChunkDataControl/ChunkSelector" +chunk_enable_frame_id bool "ChunkDataControl/ChunkEnable" + +# valid values: "ExposureTime" +chunk_selector_exposure_time enum "ChunkDataControl/ChunkSelector" +chunk_enable_exposure_time bool "ChunkDataControl/ChunkEnable" + +# valid values: "Gain" +chunk_selector_gain enum "ChunkDataControl/ChunkSelector" +chunk_enable_gain bool "ChunkDataControl/ChunkEnable" + +# valid values: "Timestamp" +chunk_selector_timestamp enum "ChunkDataControl/ChunkSelector" +chunk_enable_timestamp bool "ChunkDataControl/ChunkEnable" diff --git a/spinnaker_camera_driver/config/blackfly_s_gige.cfg b/spinnaker_camera_driver/config/blackfly_s_gige.cfg new file mode 100644 index 00000000..1fac294c --- /dev/null +++ b/spinnaker_camera_driver/config/blackfly_s_gige.cfg @@ -0,0 +1,100 @@ +# +# config file for Blackfly S cameras with Gigabit Ethernet +# +# This file maps the ros parameters to the corresponding "nodes" in the camera. +# The question remains what the valid values are for the enum types. For that +# use the spinnaker GUI (spinview), or dump the nodemap by setting the +# "dump_node_map" parameter in the driver. Also setting the "debug" param helps. +# +# NOTE: !!!! THE ORDER OF PARAMETER DEFINITION MATTERS !!!! +# On node startup, the parameters will be declared and initialized +# in the order listed here. For instance you must list +# the enum "exposure_auto" before the float "exposure_time" because on +# startup, "exposure_auto" must first be set to "Off" before +# "exposure_time" can be set, or else the camera refuses to set +# the exposure time. +# + +# +# -------- analog control +# + +gain_auto enum "AnalogControl/GainAuto" +gain float "AnalogControl/Gain" + +# +# -------- digital IO control +# + +# black wire: opto-isolated input +line0_selector enum "DigitalIOControl/LineSelector" + +# white wire: opto-isolated output +line1_selector enum "DigitalIOControl/LineSelector" +# valid values: "Input", "Output" +line1_linemode enum "DigitalIOControl/LineMode" + +# red wire: non-isolated input/output and power output +line2_selector enum "DigitalIOControl/LineSelector" +line2_v33enable bool "DigitalIOControl/V3_3Enable" + +# green wire: aux voltage input and non-isolated input +line3_selector enum "DigitalIOControl/LineSelector" +# valid values: "Input", "Output" +line3_linemode enum "DigitalIOControl/LineMode" + +# +# -------- acquisition control +# + +exposure_auto enum "AcquisitionControl/ExposureAuto" +exposure_time float "AcquisitionControl/ExposureTime" + +frame_rate_enable bool "AcquisitionControl/AcquisitionFrameRateEnable" +frame_rate float "AcquisitionControl/AcquisitionFrameRate" + +# valid values are e.g. "FrameStart", "AcquisitionStart", "FrameBurstStart" +trigger_selector enum "AcquisitionControl/TriggerSelector" + +# valid values are "On" and "Off" +trigger_mode enum "AcquisitionControl/TriggerMode" + +# valid values are "Line<0,1,2>", "UserOutput<0,1,2>", "Counter<0,1>", +# "LogicBlock<0,1> +trigger_source enum "AcquisitionControl/TriggerSource" + +# value >= 9 +trigger_delay float "AcquisitionControl/TriggerDelay" + +# valid values: "Off" and "ReadOut" +trigger_overlap enum "AcquisitionControl/TriggerOverlap" + +# +# --------- chunk control +# + +chunk_mode_active bool "ChunkDataControl/ChunkModeActive" + +# valid values: "FrameID" +chunk_selector_frame_id enum "ChunkDataControl/ChunkSelector" +chunk_enable_frame_id bool "ChunkDataControl/ChunkEnable" + +# valid values: "ExposureTime" +chunk_selector_exposure_time enum "ChunkDataControl/ChunkSelector" +chunk_enable_exposure_time bool "ChunkDataControl/ChunkEnable" + +# valid values: "Gain" +chunk_selector_gain enum "ChunkDataControl/ChunkSelector" +chunk_enable_gain bool "ChunkDataControl/ChunkEnable" + +# valid values: "Timestamp" +chunk_selector_timestamp enum "ChunkDataControl/ChunkSelector" +chunk_enable_timestamp bool "ChunkDataControl/ChunkEnable" + +# +# --------- transport layer control (GigE-specific) +# + +# default: 1400 +# set to 9000 to enable jumbo frames, ensure NIC MTU set >= 9000 +gev_scps_packet_size int "TransportLayerControl/GigEVision/GevSCPSPacketSize" diff --git a/spinnaker_camera_driver/config/chameleon.cfg b/spinnaker_camera_driver/config/chameleon.cfg new file mode 100644 index 00000000..e8779279 --- /dev/null +++ b/spinnaker_camera_driver/config/chameleon.cfg @@ -0,0 +1,111 @@ +# +# config file for Chameleon cameras (tested for USB3) +# +# This file maps the ros parameters to the corresponding "nodes" in the camera. +# The question remains what the valid values are for the enum types. For that +# use the spinnaker GUI (spinview), or dump the nodemap by setting the +# "dump_node_map" parameter in the driver. Also setting the "debug" param helps. +# +# NOTE: !!!! THE ORDER OF PARAMETER DEFINITION MATTERS !!!! +# On node startup, the parameters will be declared and initialized +# in the order listed here. For instance you must list +# the enum "exposure_auto" before the float "exposure_time" because on +# startup, "exposure_auto" must first be set to "Off" before +# "exposure_time" can be set, or else the camera refuses to set +# the exposure time. + +# +# -------- analog control +# + +# enum values: Continuous and Off +gain_auto enum "AnalogControl/GainAuto" +gain float "AnalogControl/Gain" + +# +# -------- digital IO control +# + +# NOT TESTED: probably yellow wire: opto-isolated input +line0_selector enum "DigitalIOControl/LineSelector" + +# NOT TESTED: probably orange wire: opto-isolated output +line1_selector enum "DigitalIOControl/LineSelector" + +# NOT TESTED: probably purple wire: non-isolated input/output +line2_selector enum "DigitalIOControl/LineSelector" +line2_linemode bool "DigitalIOControl/LineMode" + +# NOT TESTED probably green wire: non-isolated input/output +line3_selector enum "DigitalIOControl/LineSelector" +# valid values: "Input", "Output" +line3_linemode enum "DigitalIOControl/LineMode" + +# +# --------- image format control +# + +offset_x int "ImageFormatControl/OffsetX" +offset_y int "ImageFormatControl/OffsetY" + +image_width int "ImageFormatControl/Width" +image_height int "ImageFormatControl/Height" + +pixel_format enum "ImageFormatControl/PixelFormat" + +# +# -------- acquisition control +# + +# enum values: Continuous and Off +exposure_auto enum "AcquisitionControl/ExposureAuto" +exposure_time float "AcquisitionControl/ExposureTime" + +# valid values are "On" and "Off" +trigger_mode enum "AcquisitionControl/TriggerMode" + + +# valid values are "Continuous", "SingleFrame", "MultiFrame" +acquisition_mode enum "AcquisitionControl/AcquisitionMode" + +frame_rate_continuous bool "AcquisitionControl/AcquisitionFrameRateContinuous" + +frame_rate float "AcquisitionControl/AcquisitionFrameRate" + + +# !!!! NOTE: trigger functionality has not been tested !!!!! +# this may all not work + +# valid values are e.g. "FrameStart", "ExposureActive" +trigger_selector enum "AcquisitionControl/TriggerSelector" + +# valid values are "Software", "Line<0,2,3>" +trigger_source enum "AcquisitionControl/TriggerSource" + +# not sure what valid values are +trigger_delay float "AcquisitionControl/TriggerDelay" + +# valid values: "Off" and "ReadOut" +trigger_overlap enum "AcquisitionControl/TriggerOverlap" + +# +# --------- chunk control +# + +chunk_mode_active bool "ChunkDataControl/ChunkModeActive" + +# valid values: "FrameCounter" +chunk_selector_frame_counter enum "ChunkDataControl/ChunkSelector" +chunk_enable_frame_counter bool "ChunkDataControl/ChunkEnable" + +# valid values: "ExposureTime" +chunk_selector_exposure_time enum "ChunkDataControl/ChunkSelector" +chunk_enable_exposure_time bool "ChunkDataControl/ChunkEnable" + +# valid values: "Gain" +chunk_selector_gain enum "ChunkDataControl/ChunkSelector" +chunk_enable_gain bool "ChunkDataControl/ChunkEnable" + +# valid values: "Timestamp" +chunk_selector_timestamp enum "ChunkDataControl/ChunkSelector" +chunk_enable_timestamp bool "ChunkDataControl/ChunkEnable" diff --git a/spinnaker_camera_driver/config/grasshopper.cfg b/spinnaker_camera_driver/config/grasshopper.cfg new file mode 100644 index 00000000..2ec8d077 --- /dev/null +++ b/spinnaker_camera_driver/config/grasshopper.cfg @@ -0,0 +1,97 @@ +# +# config file for Grasshopper cameras (tested for USB3) +# +# This file maps the ros parameters to the corresponding "nodes" in the camera. +# The question remains what the valid values are for the enum types. For that +# use the spinnaker GUI (spinview), or dump the nodemap by setting the +# "dump_node_map" parameter in the driver. Also setting the "debug" param helps. +# +# NOTE: !!!! THE ORDER OF PARAMETER DEFINITION MATTERS !!!! +# On node startup, the parameters will be declared and initialized +# in the order listed here. For instance you must list +# the enum "exposure_auto" before the float "exposure_time" because on +# startup, "exposure_auto" must first be set to "Off" before +# "exposure_time" can be set, or else the camera refuses to set +# the exposure time. + +# +# -------- analog control +# + +# enum values: Continuous and Off +gain_auto enum "AnalogControl/GainAuto" +gain float "AnalogControl/Gain" + +# +# -------- digital IO control +# + +# NOT TESTED: probably black wire: opto-isolated input +line0_selector enum "DigitalIOControl/LineSelector" + +# NOT TESTED: probably white wire: opto-isolated output +line1_selector enum "DigitalIOControl/LineSelector" + +# NOT TESTED: probably red wire: non-isolated input/output +line2_selector enum "DigitalIOControl/LineSelector" +line2_linemode bool "DigitalIOControl/LineMode" + +# NOT TESTED probably green wire: non-isolated input/output +line3_selector enum "DigitalIOControl/LineSelector" +# valid values: "Input", "Output" +line3_linemode enum "DigitalIOControl/LineMode" + +# +# -------- acquisition control +# + +# enum values: Continuous and Off +exposure_auto enum "AcquisitionControl/ExposureAuto" +exposure_time float "AcquisitionControl/ExposureTime" + +# enum values: Continuous and Off +frame_rate_auto enum "AcquisitionControl/AcquisitionFrameRateAuto" +frame_rate float "AcquisitionControl/AcquisitionFrameRate" + +# NOTE: ..Enabled vs ..Enable on the blackfly_s +frame_rate_enable bool "AcquisitionControl/AcquisitionFrameRateEnabled" + + +# !!!! NOTE: trigger functionality has not been tested !!!!! + +# valid values are e.g. "FrameStart", "ExposureActive" +trigger_selector enum "AcquisitionControl/TriggerSelector" + +# valid values are "On" and "Off" +trigger_mode enum "AcquisitionControl/TriggerMode" + +# valid values are "Software", "Line<0,2,3>" +trigger_source enum "AcquisitionControl/TriggerSource" + +# not sure what valid values are +trigger_delay float "AcquisitionControl/TriggerDelay" + +# valid values: "Off" and "ReadOut" +trigger_overlap enum "AcquisitionControl/TriggerOverlap" + +# +# --------- chunk control +# + +chunk_mode_active bool "ChunkDataControl/ChunkModeActive" + +# valid values: "FrameCounter" +chunk_selector_frame_counter enum "ChunkDataControl/ChunkSelector" +chunk_enable_frame_counter bool "ChunkDataControl/ChunkEnable" + +# valid values: "ExposureTime" +chunk_selector_exposure_time enum "ChunkDataControl/ChunkSelector" +chunk_enable_exposure_time bool "ChunkDataControl/ChunkEnable" + +# valid values: "Gain" +chunk_selector_gain enum "ChunkDataControl/ChunkSelector" +chunk_enable_gain bool "ChunkDataControl/ChunkEnable" + +# valid values: "Timestamp" +chunk_selector_timestamp enum "ChunkDataControl/ChunkSelector" +chunk_enable_timestamp bool "ChunkDataControl/ChunkEnable" diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp new file mode 100644 index 00000000..7ae79bac --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp @@ -0,0 +1,38 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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 SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_ +#define SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_ + +#include + +namespace spinnaker_camera_driver +{ +class Camera +{ +public: + explicit Camera(const std::string & serial); + +private: + std::string void readParameters(); + // ----- variables -- + std::shared_ptr node_; + image_transport::CameraPublisher pub_; + std::shared_ptr wrapper_; + std::shared_ptr infoManager_; +}; +} // namespace spinnaker_camera_driver + +#endif // SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp new file mode 100644 index 00000000..ec046b7f --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp @@ -0,0 +1,121 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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 SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_ +#define SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace spinnaker_camera_driver +{ +class CameraDriver : public rclcpp::Node +{ +public: + typedef spinnaker_camera_driver::ImageConstPtr ImageConstPtr; + explicit CameraDriver(const rclcpp::NodeOptions & options); + ~CameraDriver(); + + bool start(); + bool stop(); + +private: + struct NodeInfo + { + enum NodeType { INVALID, ENUM, FLOAT, INT, BOOL }; + explicit NodeInfo(const std::string & n, const std::string & nodeType); + std::string name; + NodeType type{INVALID}; + rcl_interfaces::msg::ParameterDescriptor descriptor; + }; + void publishImage(const ImageConstPtr & image); + void readParameters(); + void printCameraInfo(); + void startCamera(); + bool stopCamera(); + void createCameraParameters(); + void setParameter(const NodeInfo & ni, const rclcpp::Parameter & p); + bool setEnum(const std::string & nodeName, const std::string & v = ""); + bool setDouble(const std::string & nodeName, double v); + bool setInt(const std::string & nodeName, int v); + bool setBool(const std::string & nodeName, bool v); + bool readParameterFile(); + rclcpp::Time getAdjustedTimeStamp(uint64_t t, int64_t sensorTime); + + void run(); // thread + + rcl_interfaces::msg::SetParametersResult parameterChanged( + const std::vector & params); + void controlCallback(const flir_camera_msgs::msg::CameraControl::UniquePtr msg); + void printStatus(); + void doPublish(const ImageConstPtr & im); + // ----- variables -- + std::shared_ptr node_; + image_transport::CameraPublisher pub_; + rclcpp::Publisher::SharedPtr metaPub_; + std::string serial_; + std::string cameraInfoURL_; + std::string frameId_; + std::string parameterFile_; + double frameRate_; + double exposureTime_; // in microseconds + bool autoExposure_; // if auto exposure is on/off + bool dumpNodeMap_{false}; + bool debug_{false}; + bool computeBrightness_{false}; + double acquisitionTimeout_{3.0}; + bool adjustTimeStamp_{false}; + uint32_t currentExposureTime_{0}; + double averageTimeDifference_{std::numeric_limits::quiet_NaN()}; + int64_t baseTimeOffset_{0}; + float currentGain_{std::numeric_limits::lowest()}; + std::shared_ptr wrapper_; + std::shared_ptr infoManager_; + sensor_msgs::msg::Image imageMsg_; + sensor_msgs::msg::CameraInfo cameraInfoMsg_; + flir_camera_msgs::msg::ImageMetaData metaMsg_; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callbackHandle_; // keep alive callbacks + rclcpp::TimerBase::SharedPtr statusTimer_; + bool cameraRunning_{false}; + std::mutex mutex_; + std::condition_variable cv_; + std::deque bufferQueue_; + size_t maxBufferQueueSize_{4}; + std::shared_ptr thread_; + bool keepRunning_{true}; + std::map parameterMap_; + std::vector parameterList_; // remember original ordering + rclcpp::Subscription::SharedPtr controlSub_; + uint32_t publishedCount_{0}; + uint32_t droppedCount_{0}; + uint32_t queuedCount_{0}; + rclcpp::Time lastStatusTime_; + int qosDepth_{4}; +}; +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp new file mode 100644 index 00000000..21e5fc45 --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp @@ -0,0 +1,55 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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 SPINNAKER_CAMERA_DRIVER__IMAGE_HPP_ +#define SPINNAKER_CAMERA_DRIVER__IMAGE_HPP_ + +#include +#include + +namespace spinnaker_camera_driver +{ +class Image +{ +public: + Image( + uint64_t t, int16_t brightness, uint32_t et, uint32_t maxEt, float gain, int64_t imgT, + size_t imageSize, int status, const void * data, size_t w, size_t h, size_t stride, + size_t bitsPerPixel, size_t numChan, uint64_t frameId, pixel_format::PixelFormat pixFmt); + + // ----- variables -- + uint64_t time_; + int16_t brightness_; + uint32_t exposureTime_; + uint32_t maxExposureTime_; + float gain_; + int64_t imageTime_; + size_t imageSize_; + int imageStatus_; + const void * data_; + size_t width_; + size_t height_; + size_t stride_; // in bytes + size_t bitsPerPixel_; + size_t numChan_; + uint64_t frameId_; + pixel_format::PixelFormat pixelFormat_; + +private: +}; +typedef std::shared_ptr ImagePtr; +typedef std::shared_ptr ImageConstPtr; +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__IMAGE_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/pixel_format.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/pixel_format.hpp new file mode 100644 index 00000000..ebaac53f --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/pixel_format.hpp @@ -0,0 +1,61 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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 SPINNAKER_CAMERA_DRIVER__PIXEL_FORMAT_HPP_ +#define SPINNAKER_CAMERA_DRIVER__PIXEL_FORMAT_HPP_ + +#include +#include + +namespace spinnaker_camera_driver +{ +namespace pixel_format +{ +enum PixelFormat { + INVALID = 0, + Mono8, + Mono10p, + Mono10Packed, + Mono12p, + Mono12Packed, + Mono16, + RGB8, + RGB8Packed, + BayerRG8, + BayerRG10p, + BayerRG10Packed, + BayerRG12p, + BayerRG12Packed, + BayerRG16, + BayerGR8, + BayerGR16, + BayerGB8, + BayerGB16, + BayerBG8, + BayerBG16, + YUV411Packed, + YUV422Packed, + YUV444Packed, + YCbCr8, + YCbCr422_8, + YCbCr411_8, + BGR8, + BGRa8 +}; +std::string to_string(PixelFormat f); +PixelFormat from_nodemap_string(const std::string pixFmt); +} // namespace pixel_format +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__PIXEL_FORMAT_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp new file mode 100644 index 00000000..a4821ddd --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp @@ -0,0 +1,68 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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 SPINNAKER_CAMERA_DRIVER__SPINNAKER_WRAPPER_HPP_ +#define SPINNAKER_CAMERA_DRIVER__SPINNAKER_WRAPPER_HPP_ + +#include +#include +#include +#include +#include + +namespace spinnaker_camera_driver +{ +class SpinnakerWrapperImpl; +class SpinnakerWrapper +{ +public: + struct Exception : public std::exception + { + explicit Exception(const std::string & what) : what_(what) {} + const char * what() const throw() { return what_.c_str(); } + + private: + const std::string what_; + }; + using Callback = std::function; + + SpinnakerWrapper(); + + std::string getLibraryVersion() const; + void refreshCameraList(); + std::vector getSerialNumbers() const; + + bool initCamera(const std::string & serialNumber); + bool deInitCamera(); + bool startCamera(const SpinnakerWrapper::Callback & cb); + bool stopCamera(); + void setDebug(bool b); + void setComputeBrightness(bool b); + void setAcquisitionTimeout(double sec); + + std::string getPixelFormat() const; + double getReceiveFrameRate() const; + std::string getNodeMapAsString(); + std::string setEnum(const std::string & nodeName, const std::string & val, std::string * retVal); + std::string setDouble(const std::string & nodeName, double val, double * retVal); + std::string setBool(const std::string & nodeName, bool val, bool * retVal); + std::string setInt(const std::string & nodeName, int val, int * retVal); + +private: + // ----- variables -- + std::shared_ptr wrapperImpl_; +}; +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__SPINNAKER_WRAPPER_HPP_ diff --git a/spinnaker_camera_driver/launch/blackfly_s.launch.py b/spinnaker_camera_driver/launch/blackfly_s.launch.py new file mode 100755 index 00000000..96ba8739 --- /dev/null +++ b/spinnaker_camera_driver/launch/blackfly_s.launch.py @@ -0,0 +1,74 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Bernd Pfrommer +# +# 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. +# +# + +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration as LaunchConfig +from launch.actions import DeclareLaunchArgument as LaunchArg +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory + +camera_params = { + 'debug': False, + 'compute_brightness': False, + 'adjust_timestamp': True, + 'dump_node_map': False, + # set parameters defined in blackfly_s.cfg + 'gain_auto': 'Continuous', + # 'pixel_format': 'BayerRG8', + 'exposure_auto': 'Continuous', + # 'device_link_throughput_limit': 380000000, + # ---- to reduce the sensor width and shift the crop + # 'image_width': 1408, + # 'image_height': 1080, + # 'offset_x': 16, + # 'offset_y': 0, + 'frame_rate_auto': 'Off', + 'frame_rate': 20.0, + 'frame_rate_enable': True, + 'buffer_queue_size': 1, + 'trigger_mode': 'Off', + 'chunk_mode_active': True, + 'chunk_selector_frame_id': 'FrameID', + 'chunk_enable_frame_id': True, + 'chunk_selector_exposure_time': 'ExposureTime', + 'chunk_enable_exposure_time': True, + 'chunk_selector_gain': 'Gain', + 'chunk_enable_gain': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, + } + + +def generate_launch_description(): + """Launch camera node.""" + flir_dir = get_package_share_directory('spinnaker_camera_driver') + config_dir = flir_dir + '/config/' + name_arg = LaunchArg('camera_name', default_value='blackfly_s', + description='camera name') + serial_arg = LaunchArg('serial', default_value="'20435008'", + description='serial number') + + node = Node(package='spinnaker_camera_driver', + executable='camera_driver_node', + output='screen', + name=[LaunchConfig('camera_name')], + parameters=[camera_params, + {'parameter_file': config_dir + 'blackfly_s.cfg', + 'serial_number': [LaunchConfig('serial')]}], + remappings=[('~/control', '/exposure_control/control'), ]) + + return LaunchDescription([name_arg, serial_arg, node]) diff --git a/spinnaker_camera_driver/launch/blackfly_s_gige.launch.py b/spinnaker_camera_driver/launch/blackfly_s_gige.launch.py new file mode 100755 index 00000000..683db39d --- /dev/null +++ b/spinnaker_camera_driver/launch/blackfly_s_gige.launch.py @@ -0,0 +1,65 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Kevin Janesch +# +# 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. +# +# +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration as LaunchConfig +from launch.actions import DeclareLaunchArgument as LaunchArg +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory + +camera_params = { + 'debug': False, + 'compute_brightness': False, + 'dump_node_map': False, + # set parameters defined in blackfly_s_gige.cfg + 'gain_auto': 'Continuous', + 'exposure_auto': 'Continuous', + 'frame_rate_auto': 'Off', + 'frame_rate': 20.0, + 'frame_rate_enable': True, + 'trigger_mode': 'Off', + 'chunk_mode_active': True, + 'chunk_selector_frame_id': 'FrameID', + 'chunk_enable_frame_id': True, + 'chunk_selector_exposure_time': 'ExposureTime', + 'chunk_enable_exposure_time': True, + 'chunk_selector_gain': 'Gain', + 'chunk_enable_gain': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, + 'gev_scps_packet_size': 9000, + } + + +def generate_launch_description(): + """Launch blackfly_s camera node.""" + flir_dir = get_package_share_directory('spinnaker_camera_driver') + config_dir = flir_dir + '/config/' + name_arg = LaunchArg('camera_name', default_value='blackfly_s', + description='camera name') + serial_arg = LaunchArg('serial', default_value="'20435008'", + description='serial number') + print([LaunchConfig('serial'), '_']) + node = Node(package='spinnaker_camera_driver', + executable='camera_driver_node', + output='screen', + name=[LaunchConfig('camera_name')], + parameters=[ + camera_params, + {'parameter_file': config_dir + 'blackfly_s_gige.cfg', + 'serial_number': [LaunchConfig('serial')], }], + remappings=[('~/control', '/exposure_control/control')]) + return LaunchDescription([name_arg, serial_arg, node]) diff --git a/spinnaker_camera_driver/launch/chameleon.launch.py b/spinnaker_camera_driver/launch/chameleon.launch.py new file mode 100755 index 00000000..5bfe7bcd --- /dev/null +++ b/spinnaker_camera_driver/launch/chameleon.launch.py @@ -0,0 +1,69 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Bernd Pfrommer +# +# 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. +# +# + +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration as LaunchConfig +from launch.actions import DeclareLaunchArgument as LaunchArg +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory + +camera_params = { + 'debug': False, + 'compute_brightness': False, + 'dump_node_map': False, + # set parameters defined in chameleon.cfg + 'gain_auto': 'Continuous', + 'exposure_auto': 'Continuous', + 'offset_x': 0, + 'offset_y': 0, + 'image_width': 2048, + 'image_height': 1536, + 'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8' + 'frame_rate_continous': True, + 'frame_rate': 100.0, + 'trigger_mode': 'Off', + 'chunk_mode_active': True, + 'chunk_selector_frame_id': 'FrameID', + 'chunk_enable_frame_id': True, + 'chunk_selector_exposure_time': 'ExposureTime', + 'chunk_enable_exposure_time': True, + 'chunk_selector_gain': 'Gain', + 'chunk_enable_gain': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, + } + + +def generate_launch_description(): + """Launch camera node.""" + flir_dir = get_package_share_directory('spinnaker_camera_driver') + config_dir = flir_dir + '/config/' + name_arg = LaunchArg('camera_name', default_value='chameleon_s', + description='camera name') + serial_arg = LaunchArg('serial', default_value="'16387017'", + description='serial number') + + node = Node(package='spinnaker_camera_driver', + executable='camera_driver_node', + output='screen', + name=[LaunchConfig('camera_name')], + parameters=[camera_params, + {'parameter_file': config_dir + 'chameleon.cfg', + 'serial_number': [LaunchConfig('serial')], }], + remappings=[('~/control', '/exposure_control/control'), ]) + + return LaunchDescription([name_arg, serial_arg, node]) diff --git a/spinnaker_camera_driver/launch/grasshopper.launch.py b/spinnaker_camera_driver/launch/grasshopper.launch.py new file mode 100755 index 00000000..81ec9187 --- /dev/null +++ b/spinnaker_camera_driver/launch/grasshopper.launch.py @@ -0,0 +1,62 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Bernd Pfrommer +# +# 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. +# +# + +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration as LaunchConfig +from launch.actions import DeclareLaunchArgument as LaunchArg +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory + +camera_params = { + 'serial_number': '17274483', + 'debug': False, + 'compute_brightness': False, + 'dump_node_map': False, + # set parameters defined in grasshopper.cfg + 'gain_auto': 'Continuous', + 'exposure_auto': 'Continuous', + 'frame_rate_auto': 'Off', + 'frame_rate': 100.0, + 'trigger_mode': 'Off', + 'chunk_mode_active': True, + 'chunk_selector_frame_id': 'FrameID', + 'chunk_enable_frame_id': True, + 'chunk_selector_exposure_time': 'ExposureTime', + 'chunk_enable_exposure_time': True, + 'chunk_selector_gain': 'Gain', + 'chunk_enable_gain': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, + } + + +def generate_launch_description(): + """Launch camera node.""" + flir_dir = get_package_share_directory('spinnaker_camera_driver') + config_dir = flir_dir + '/config/' + launch_arg = LaunchArg('camera_name', default_value='grasshopper', + description='camera name') + node = Node(package='spinnaker_camera_driver', + executable='camera_driver_node', + output='screen', + name=LaunchConfig('camera_name'), + parameters=[camera_params, + {'parameter_file': config_dir + + 'grasshopper.cfg'}], + remappings=[('~/control', '/exposure_control/control'), ]) + + return LaunchDescription([launch_arg, node]) diff --git a/spinnaker_camera_driver/launch/grasshopper_with_exp_control.launch.py b/spinnaker_camera_driver/launch/grasshopper_with_exp_control.launch.py new file mode 100755 index 00000000..fcb82a95 --- /dev/null +++ b/spinnaker_camera_driver/launch/grasshopper_with_exp_control.launch.py @@ -0,0 +1,69 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Bernd Pfrommer +# +# 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. +# +# + +# +# example file for launching a free running grasshopper with external +# exposure control +# + +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration as LaunchConfig +from launch.actions import DeclareLaunchArgument as LaunchArg +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory + +camera_params = { + 'serial_number': '17274483', + 'debug': False, + 'compute_brightness': True, + 'dump_node_map': False, + # set parameters defined in grasshopper.cfg + 'gain_auto': 'Off', + 'gain': 5.0, + 'exposure_auto': 'Off', + 'exposure_time': 10000, + 'frame_rate_auto': 'Off', + 'frame_rate': 100.0, + 'trigger_mode': 'Off', + 'chunk_mode_active': True, + 'chunk_selector_frame_id': 'FrameID', + 'chunk_enable_frame_id': True, + 'chunk_selector_exposure_time': 'ExposureTime', + 'chunk_enable_exposure_time': True, + 'chunk_selector_gain': 'Gain', + 'chunk_enable_gain': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, + } + + +def generate_launch_description(): + """Launch camera node.""" + flir_dir = get_package_share_directory('spinnaker_camera_driver') + config_dir = flir_dir + '/config/' + launch_arg = LaunchArg('camera_name', default_value='grasshopper', + description='camera name') + node = Node(package='spinnaker_camera_driver', + executable='camera_driver_node', + output='screen', + name=LaunchConfig('camera_name'), + parameters=[camera_params, + {'parameter_file': config_dir + + 'grasshopper.cfg'}], + remappings=[('~/control', '/exposure_control/control'), ]) + + return LaunchDescription([launch_arg, node]) diff --git a/spinnaker_camera_driver/launch/stereo_synced.launch.py b/spinnaker_camera_driver/launch/stereo_synced.launch.py new file mode 100755 index 00000000..e0e2393a --- /dev/null +++ b/spinnaker_camera_driver/launch/stereo_synced.launch.py @@ -0,0 +1,114 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Bernd Pfrommer +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.substitutions import LaunchConfiguration as LaunchConfig +from launch.actions import DeclareLaunchArgument as LaunchArg +from ament_index_python.packages import get_package_share_directory + +camera_params = { + 'debug': False, + 'compute_brightness': True, + 'dump_node_map': False, + 'adjust_timestamp': True, + 'gain_auto': 'Off', + 'gain': 0, + 'exposure_auto': 'Off', + 'exposure_time': 9000, + 'line2_selector': 'Line2', + 'line2_v33enable': False, + 'line3_selector': 'Line3', + 'line3_linemode': 'Input', + 'trigger_selector': 'FrameStart', + 'trigger_mode': 'On', + 'trigger_source': 'Line3', + 'trigger_delay': 9, + 'trigger_overlap': 'ReadOut', + 'chunk_mode_active': True, + 'chunk_selector_frame_id': 'FrameID', + 'chunk_enable_frame_id': True, + 'chunk_selector_exposure_time': 'ExposureTime', + 'chunk_enable_exposure_time': True, + 'chunk_selector_gain': 'Gain', + 'chunk_enable_gain': True, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, + } + + +def generate_launch_description(): + """Create synchronized stereo camera.""" + flir_dir = get_package_share_directory('spinnaker_camera_driver') + config_dir = flir_dir + '/config/' + container = ComposableNodeContainer( + name='stereo_camera_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='spinnaker_camera_driver', + plugin='spinnaker_camera_driver::CameraDriver', + name=LaunchConfig('cam_0_name'), + parameters=[camera_params, + {'parameter_file': config_dir + 'blackfly_s.cfg', + 'serial_number': '20435008'}], + remappings=[('~/control', '/exposure_control/control'), ], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( + package='spinnaker_camera_driver', + plugin='spinnaker_camera_driver::CameraDriver', + name=LaunchConfig('cam_1_name'), + parameters=[camera_params, + {'parameter_file': + config_dir + 'blackfly_s.cfg', + 'serial_number': '20415937'}], + remappings=[('~/control', '/exposure_control/control'), ], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( + package='cam_sync_ros2', + plugin='cam_sync_ros2::CamSync', + name='sync', + parameters=[], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ComposableNode( + package='exposure_control_ros2', + plugin='exposure_control_ros2::ExposureControl', + name='exposure_control', + parameters=[{'cam_name': LaunchConfig('cam_0_name'), + 'max_gain': 20.0, + 'gain_priority': False, + 'brightness_target': 100, + 'max_exposure_time': 9500.0, + 'min_exposure_time': 1000.0}], + remappings=[('~/meta', ['/', LaunchConfig('cam_0_name'), + '/meta']), ], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + output='screen', + ) + name_0_arg = LaunchArg('cam_0_name', default_value=['cam_0'], + description='name of camera 0') + name_1_arg = LaunchArg('cam_1_name', default_value=['cam_1'], + description='name of camera 1') + return launch.LaunchDescription([name_0_arg, name_1_arg, container]) diff --git a/spinnaker_camera_driver/package.xml b/spinnaker_camera_driver/package.xml new file mode 100644 index 00000000..a7faa3a8 --- /dev/null +++ b/spinnaker_camera_driver/package.xml @@ -0,0 +1,27 @@ + + + + spinnaker_camera_driver + 1.0.0 + ROS2 driver for flir spinnaker sdk + Bernd Pfrommer + Apache-2 + + ament_cmake + + camera_info_manager + flir_camera_msgs + image_transport + rclcpp + rclcpp_components + sensor_msgs + std_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + + + ament_cmake + + diff --git a/spinnaker_camera_driver/src/camera_driver.cpp b/spinnaker_camera_driver/src/camera_driver.cpp new file mode 100644 index 00000000..3cc2b5c9 --- /dev/null +++ b/spinnaker_camera_driver/src/camera_driver.cpp @@ -0,0 +1,683 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace spinnaker_camera_driver +{ +namespace chrono = std::chrono; +// +// this complicated code is to detect an interface change +// between foxy and galactic +// See https://stackoverflow.com/questions/1005476/ +// how-to-detect-whether-there-is-a-specific-member-variable-in-class + +template +struct DescSetter +{ + // don't set by default (foxy) + static void set_dynamic_typing(T *) {} +}; + +template +struct DescSetter +{ + // set if dynamic_typing is present + static void set_dynamic_typing(T * desc) { desc->dynamic_typing = true; } +}; + +static rcl_interfaces::msg::ParameterDescriptor make_desc(const std::string name, int type) +{ + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = name; + desc.type = type; + desc.description = name; + DescSetter::set_dynamic_typing(&desc); + return (desc); +} + +static std::pair get_double_int_param(const rclcpp::Parameter & p) +{ + std::pair bd(false, 0); + if (p.get_type() == rclcpp::PARAMETER_DOUBLE) { + bd.second = p.as_double(); + bd.first = true; + } + if (p.get_type() == rclcpp::PARAMETER_INTEGER) { + bd.second = static_cast(p.as_int()); + bd.first = true; + } + return (bd); +} + +static std::pair get_bool_int_param(const rclcpp::Parameter & p) +{ + std::pair bb(false, false); + if (p.get_type() == rclcpp::PARAMETER_BOOL) { + bb.second = p.as_bool(); + bb.first = true; + } + if (p.get_type() == rclcpp::PARAMETER_INTEGER) { + bb.second = static_cast(p.as_int()); + bb.first = true; + } + return (bb); +} + +CameraDriver::NodeInfo::NodeInfo(const std::string & n, const std::string & nodeType) : name(n) +{ + if (nodeType == "float") { + type = FLOAT; + descriptor = make_desc(n, rclcpp::ParameterType::PARAMETER_DOUBLE); + } else if (nodeType == "int") { + type = INT; + descriptor = make_desc(n, rclcpp::ParameterType::PARAMETER_INTEGER); + } else if (nodeType == "bool") { + type = BOOL; + descriptor = make_desc(n, rclcpp::ParameterType::PARAMETER_BOOL); + } else if (nodeType == "enum") { + type = ENUM; + descriptor = make_desc(n, rclcpp::ParameterType::PARAMETER_STRING); + } +} + +CameraDriver::CameraDriver(const rclcpp::NodeOptions & options) : Node("cam_sync", options) +{ + lastStatusTime_ = now(); + statusTimer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Duration(5, 0), std::bind(&CameraDriver::printStatus, this)); + bool status = start(); + if (!status) { + RCLCPP_ERROR(get_logger(), "startup failed!"); + throw std::runtime_error("startup of CameraDriver node failed!"); + } +} + +CameraDriver::~CameraDriver() +{ + stop(); + wrapper_.reset(); // invoke destructor +} + +bool CameraDriver::stop() +{ + stopCamera(); + if (wrapper_) { + wrapper_->deInitCamera(); + } + if (!statusTimer_->is_canceled()) { + statusTimer_->cancel(); + } + keepRunning_ = false; + if (thread_) { + thread_->join(); + thread_ = 0; + } + return (true); +} + +bool CameraDriver::stopCamera() +{ + if (cameraRunning_ && wrapper_) { + cameraRunning_ = false; + return wrapper_->stopCamera(); + } + return false; +} + +void CameraDriver::printStatus() +{ + if (wrapper_) { + const double dropRate = + (queuedCount_ > 0) ? (static_cast(droppedCount_) / static_cast(queuedCount_)) + : 0; + const rclcpp::Time t = now(); + const rclcpp::Duration dt = t - lastStatusTime_; + double dtns = std::max(dt.nanoseconds(), (int64_t)1); + double outRate = publishedCount_ * 1e9 / dtns; + RCLCPP_INFO( + this->get_logger(), "rate [Hz] in %6.2f out %6.2f drop %3.0f%%", + wrapper_->getReceiveFrameRate(), outRate, dropRate * 100); + lastStatusTime_ = t; + droppedCount_ = 0; + publishedCount_ = 0; + queuedCount_ = 0; + } else { + RCLCPP_WARN_STREAM(get_logger(), "camera " << serial_ << " is not online!"); + } +} + +void CameraDriver::readParameters() +{ + serial_ = this->declare_parameter("serial_number", "missing_serial_number"); + try { + debug_ = this->declare_parameter( + "debug", false, make_desc("debug", rclcpp::ParameterType::PARAMETER_BOOL)); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_WARN_STREAM(get_logger(), "bad debug param type: " << e.what()); + debug_ = false; + } + // Flag to adjust ROS time stamps + adjustTimeStamp_ = this->declare_parameter("adjust_timestamp", false); + RCLCPP_INFO_STREAM(get_logger(), (adjustTimeStamp_ ? "" : "not ") << "adjusting time stamps!"); + + cameraInfoURL_ = this->declare_parameter("camerainfo_url", ""); + frameId_ = this->declare_parameter("frame_id", get_name()); + dumpNodeMap_ = this->declare_parameter("dump_node_map", false); + qosDepth_ = this->declare_parameter("image_queue_size", 4); + maxBufferQueueSize_ = static_cast(this->declare_parameter("buffer_queue_size", 4)); + computeBrightness_ = this->declare_parameter("compute_brightness", false); + acquisitionTimeout_ = this->declare_parameter("acquisition_timeout", 3.0); + parameterFile_ = this->declare_parameter("parameter_file", "parameters.cfg"); + RCLCPP_INFO_STREAM(get_logger(), "looking for serial number: " << serial_); + callbackHandle_ = this->add_on_set_parameters_callback( + std::bind(&CameraDriver::parameterChanged, this, std::placeholders::_1)); +} + +bool CameraDriver::readParameterFile() +{ + std::ifstream f(parameterFile_); + if (!f.is_open()) { + RCLCPP_ERROR_STREAM(get_logger(), "cannot read parameter definition file: " << parameterFile_); + return (false); + } + std::string l; + while (getline(f, l)) { + std::istringstream iss(l); + std::string s; + std::vector tokens; + while (iss >> std::quoted(s)) { + tokens.push_back(s); + } + if (!tokens.empty()) { + if (!tokens[0].empty() && tokens[0][0] == '#') { + continue; + } + if (tokens.size() != 3) { + RCLCPP_WARN_STREAM(get_logger(), "skipping bad camera param line: " << l); + continue; + } + parameterMap_.insert({tokens[0], NodeInfo(tokens[2], tokens[1])}); + parameterList_.push_back(tokens[0]); + } + } + f.close(); + return (true); +} + +void CameraDriver::createCameraParameters() +{ + for (const auto & name : parameterList_) { + const auto it = parameterMap_.find(name); + if (it != parameterMap_.end()) { + const auto & ni = it->second; // should always succeed + try { + this->declare_parameter(name, rclcpp::ParameterValue(), ni.descriptor, false); + } catch (rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_WARN_STREAM( + get_logger(), "overwriting bad param with default: " + std::string(e.what())); + this->declare_parameter(name, rclcpp::ParameterValue(), ni.descriptor, true); + } + } + } +} + +bool CameraDriver::setEnum(const std::string & nodeName, const std::string & v) +{ + RCLCPP_INFO_STREAM(get_logger(), "setting " << nodeName << " to: " << v); + std::string retV; // what actually was set + std::string msg = wrapper_->setEnum(nodeName, v, &retV); + bool status(true); + if (msg != "OK") { + RCLCPP_WARN_STREAM(get_logger(), "setting " << nodeName << " failed: " << msg); + status = false; + } + if (v != retV) { + RCLCPP_WARN_STREAM(get_logger(), nodeName << " set to: " << retV << " instead of: " << v); + status = false; + } + return (status); +} + +bool CameraDriver::setDouble(const std::string & nodeName, double v) +{ + RCLCPP_INFO_STREAM(get_logger(), "setting " << nodeName << " to: " << v); + double retV; // what actually was set + std::string msg = wrapper_->setDouble(nodeName, v, &retV); + bool status(true); + if (msg != "OK") { + RCLCPP_WARN_STREAM(get_logger(), "setting " << nodeName << " failed: " << msg); + status = false; + } + if (std::abs(v - retV) > 0.025 * std::abs(v + retV)) { + RCLCPP_WARN_STREAM(get_logger(), nodeName << " set to: " << retV << " instead of: " << v); + status = false; + } + return (status); +} + +bool CameraDriver::setInt(const std::string & nodeName, int v) +{ + RCLCPP_INFO_STREAM(get_logger(), "setting " << nodeName << " to: " << v); + int retV; // what actually was set + std::string msg = wrapper_->setInt(nodeName, v, &retV); + bool status(true); + if (msg != "OK") { + RCLCPP_WARN_STREAM(get_logger(), "setting " << nodeName << " failed: " << msg); + status = false; + } + if (v != retV) { + RCLCPP_WARN_STREAM(get_logger(), nodeName << " set to: " << retV << " instead of: " << v); + status = false; + } + return (status); +} + +bool CameraDriver::setBool(const std::string & nodeName, bool v) +{ + RCLCPP_INFO_STREAM(get_logger(), "setting " << nodeName << " to: " << v); + bool retV; // what actually was set + std::string msg = wrapper_->setBool(nodeName, v, &retV); + bool status(true); + if (msg != "OK") { + RCLCPP_WARN_STREAM(get_logger(), "setting " << nodeName << " failed: " << msg); + status = false; + } + if (v != retV) { + RCLCPP_WARN_STREAM(get_logger(), nodeName << " set to: " << retV << " instead of: " << v); + status = false; + } + return (status); +} + +void CameraDriver::setParameter(const NodeInfo & ni, const rclcpp::Parameter & p) +{ + switch (ni.type) { + case NodeInfo::ENUM: { + std::string s = p.value_to_string(); + // remove quotes + s.erase(remove(s.begin(), s.end(), '\"'), s.end()); + setEnum(ni.name, s); + break; + } + case NodeInfo::FLOAT: { + auto bd = get_double_int_param(p); + if (bd.first) { + setDouble(ni.name, bd.second); + } else { + RCLCPP_WARN_STREAM( + get_logger(), "bad non-float " << p.get_name() << " type: " << p.get_type()); + } + break; + } + case NodeInfo::INT: { + auto bd = get_double_int_param(p); + if (bd.first) { + setInt(ni.name, bd.second); + } else { + RCLCPP_WARN_STREAM( + get_logger(), "bad non-int " << p.get_name() << " type: " << p.get_type()); + } + break; + } + case NodeInfo::BOOL: { + auto bb = get_bool_int_param(p); + if (bb.first) { + setBool(ni.name, bb.second); + } else { + RCLCPP_WARN_STREAM( + get_logger(), "bad non-bool " << p.get_name() << " type: " << p.get_type()); + } + break; + } + default: + RCLCPP_WARN_STREAM(get_logger(), "invalid node type in map: " << ni.type); + } +} + +rcl_interfaces::msg::SetParametersResult CameraDriver::parameterChanged( + const std::vector & params) +{ + for (const auto & p : params) { + const auto it = parameterMap_.find(p.get_name()); + if (it == parameterMap_.end()) { + continue; // ignore unknown param + } + if (!wrapper_) { + RCLCPP_WARN_STREAM(get_logger(), "got parameter update while driver is not ready!"); + continue; + } + const NodeInfo & ni = it->second; + if (p.get_type() == rclcpp::PARAMETER_NOT_SET) { + continue; + } + try { + setParameter(ni, p); + } catch (const spinnaker_camera_driver::SpinnakerWrapper::Exception & e) { + RCLCPP_WARN_STREAM(get_logger(), "param " << p.get_name() << " " << e.what()); + } + } + rcl_interfaces::msg::SetParametersResult res; + res.successful = true; + res.reason = "all good!"; + return (res); +} + +void CameraDriver::controlCallback(const flir_camera_msgs::msg::CameraControl::UniquePtr msg) +{ + /* + RCLCPP_INFO_STREAM(get_logger(), + "control msg: time: " << currentExposureTime_ << " -> " + << msg->exposure_time << " gain: " << currentGain_ + << " -> " << msg->gain); */ + const uint32_t et = msg->exposure_time; + const float gain = msg->gain; + bool logTime(false); + bool logGain(false); + try { + if (et > 0 && et != currentExposureTime_) { + const auto it = parameterMap_.find("exposure_time"); + if (it != parameterMap_.end()) { + const auto & ni = it->second; + setDouble(ni.name, et); + currentExposureTime_ = et; + logTime = true; + } else { + RCLCPP_WARN_STREAM( + get_logger(), "no node name defined for exposure_time, check .cfg file!"); + } + } + if (gain > std::numeric_limits::lowest() && gain != currentGain_) { + const auto it = parameterMap_.find("gain"); + if (it != parameterMap_.end()) { + const auto & ni = it->second; + setDouble(ni.name, gain); + currentGain_ = gain; + logGain = true; + } else { + RCLCPP_WARN_STREAM( + get_logger(), "no node name defined for exposure_time, check .cfg file!"); + } + } + } catch (const spinnaker_camera_driver::SpinnakerWrapper::Exception & e) { + RCLCPP_WARN_STREAM(get_logger(), "failed to control: " << e.what()); + } + + if (logTime) { + RCLCPP_INFO_STREAM(get_logger(), "changed exposure time to " << et << "us"); + } + if (logGain) { + RCLCPP_INFO_STREAM(get_logger(), "changed gain to " << gain << "db"); + } +} + +void CameraDriver::publishImage(const ImageConstPtr & im) +{ + { + std::unique_lock lock(mutex_); + queuedCount_++; + if (bufferQueue_.size() < maxBufferQueueSize_) { + bufferQueue_.push_back(im); + cv_.notify_all(); + } else { + droppedCount_++; + } + } +} + +void CameraDriver::run() +{ + while (keepRunning_ && rclcpp::ok()) { + { + ImageConstPtr img; + { // ------- locked section --- + std::unique_lock lock(mutex_); + // one second timeout + const std::chrono::microseconds timeout((int64_t)(1000000LL)); + while (bufferQueue_.empty() && keepRunning_ && rclcpp::ok()) { + cv_.wait_for(lock, timeout); + } + if (!bufferQueue_.empty()) { + img = bufferQueue_.back(); + bufferQueue_.pop_back(); + } + } // -------- end of locked section + if (img && keepRunning_ && rclcpp::ok()) { + doPublish(img); + } + } + } +} + +using flir_fmt = spinnaker_camera_driver::pixel_format::PixelFormat; +namespace ros_fmt = sensor_msgs::image_encodings; + +static const std::unordered_map flir_2_ros{ + {{flir_fmt::INVALID, "INV"}, + {flir_fmt::Mono8, ros_fmt::MONO8}, + {flir_fmt::Mono10p, "INV"}, + {flir_fmt::Mono10Packed, "INV"}, + {flir_fmt::Mono12p, "INV"}, + {flir_fmt::Mono12Packed, "INV"}, + {flir_fmt::Mono16, ros_fmt::MONO16}, + {flir_fmt::BayerRG8, ros_fmt::BAYER_RGGB8}, + {flir_fmt::BayerRG10p, "INV"}, + {flir_fmt::BayerRG10Packed, "INV"}, + {flir_fmt::BayerRG12p, "INV"}, + {flir_fmt::BayerRG12Packed, "INV"}, + {flir_fmt::BayerRG16, ros_fmt::BAYER_RGGB16}, + {flir_fmt::BayerGR8, ros_fmt::BAYER_GRBG8}, + {flir_fmt::BayerGR16, ros_fmt::BAYER_GRBG16}, + {flir_fmt::BayerGB8, ros_fmt::BAYER_GBRG8}, + {flir_fmt::BayerGB16, ros_fmt::BAYER_GBRG16}, + {flir_fmt::BayerBG8, ros_fmt::BAYER_BGGR8}, + {flir_fmt::BayerBG16, ros_fmt::BAYER_BGGR16}, + {flir_fmt::YUV411Packed, "INV"}, + {flir_fmt::YUV422Packed, "INV"}, + {flir_fmt::YUV444Packed, "INV"}, + {flir_fmt::YCbCr8, "INV"}, + {flir_fmt::YCbCr422_8, "INV"}, + {flir_fmt::YCbCr411_8, "INV"}, + {flir_fmt::RGB8, ros_fmt::RGB8}, + {flir_fmt::RGB8Packed, ros_fmt::RGB8}, + {flir_fmt::BGR8, ros_fmt::BGR8}, + {flir_fmt::BGRa8, ros_fmt::BGRA8}}}; + +static std::string flir_to_ros_encoding(const flir_fmt & pf, bool * canEncode) +{ + auto it = flir_2_ros.find(pf); + *canEncode = (it != flir_2_ros.end() && it->second != "INV") && (pf != flir_fmt::INVALID); + return (*canEncode ? it->second : "INV"); +} + +// adjust ROS header stamp using camera provided meta data +rclcpp::Time CameraDriver::getAdjustedTimeStamp(uint64_t t, int64_t sensorTime) +{ + if (std::isnan(averageTimeDifference_)) { + // capture the coarse offset between sensor and ROS time + // at the very first time stamp. + baseTimeOffset_ = static_cast(t) - sensorTime; + averageTimeDifference_ = 0; + } + const double dt = (static_cast(t) - baseTimeOffset_ - sensorTime) * 1e-9; + + // compute exponential moving average + constexpr double alpha = 0.01; // average over rougly 100 samples + averageTimeDifference_ = averageTimeDifference_ * (1.0 - alpha) + alpha * dt; + + // adjust sensor time by average difference to ROS time + const rclcpp::Time adjustedTime = rclcpp::Time(sensorTime + baseTimeOffset_, RCL_SYSTEM_TIME) + + rclcpp::Duration::from_seconds(averageTimeDifference_); + return (adjustedTime); +} + +void CameraDriver::doPublish(const ImageConstPtr & im) +{ + const auto t = + adjustTimeStamp_ ? getAdjustedTimeStamp(im->time_, im->imageTime_) : rclcpp::Time(im->time_); + imageMsg_.header.stamp = t; + cameraInfoMsg_.header.stamp = t; + + bool canEncode{false}; + const std::string encoding = flir_to_ros_encoding(im->pixelFormat_, &canEncode); + if (!canEncode) { + RCLCPP_WARN_STREAM( + get_logger(), "no ROS encoding for pixel format " + << spinnaker_camera_driver::pixel_format::to_string(im->pixelFormat_)); + return; + } + + if (pub_.getNumSubscribers() > 0) { + sensor_msgs::msg::CameraInfo::UniquePtr cinfo(new sensor_msgs::msg::CameraInfo(cameraInfoMsg_)); + // will make deep copy. Do we need to? Probably... + sensor_msgs::msg::Image::UniquePtr img(new sensor_msgs::msg::Image(imageMsg_)); + bool ret = + sensor_msgs::fillImage(*img, encoding, im->height_, im->width_, im->stride_, im->data_); + if (!ret) { + RCLCPP_ERROR_STREAM(get_logger(), "fill image failed!"); + } else { + // const auto t0 = this->now(); + pub_.publish(std::move(img), std::move(cinfo)); + // const auto t1 = this->now(); + // std::cout << "dt: " << (t1 - t0).nanoseconds() * 1e-9 << std::endl; + publishedCount_++; + } + } + if (metaPub_->get_subscription_count() != 0) { + metaMsg_.header.stamp = t; + metaMsg_.brightness = im->brightness_; + metaMsg_.exposure_time = im->exposureTime_; + metaMsg_.max_exposure_time = im->maxExposureTime_; + metaMsg_.gain = im->gain_; + metaMsg_.camera_time = im->imageTime_; + metaPub_->publish(metaMsg_); + } +} + +void CameraDriver::printCameraInfo() +{ + if (cameraRunning_) { + RCLCPP_INFO_STREAM(get_logger(), "camera has pixel format: " << wrapper_->getPixelFormat()); + } +} + +void CameraDriver::startCamera() +{ + if (!cameraRunning_) { + spinnaker_camera_driver::SpinnakerWrapper::Callback cb = + std::bind(&CameraDriver::publishImage, this, std::placeholders::_1); + cameraRunning_ = wrapper_->startCamera(cb); + if (!cameraRunning_) { + RCLCPP_ERROR_STREAM(get_logger(), "failed to start camera!"); + } else { + printCameraInfo(); + } + } +} + +bool CameraDriver::start() +{ + readParameters(); + if (!readParameterFile()) { + return (false); + } + infoManager_ = + std::make_shared(this, get_name(), cameraInfoURL_); + controlSub_ = this->create_subscription( + "~/control", 10, std::bind(&CameraDriver::controlCallback, this, std::placeholders::_1)); + metaPub_ = create_publisher("~/meta", 1); + + cameraInfoMsg_ = infoManager_->getCameraInfo(); + imageMsg_.header.frame_id = frameId_; + cameraInfoMsg_.header.frame_id = frameId_; + metaMsg_.header.frame_id = frameId_; + + rmw_qos_profile_t qosProf = rmw_qos_profile_default; + qosProf.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qosProf.depth = qosDepth_; // keep at most this number of images + + qosProf.reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; + qosProf.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; // sender does not have to store + qosProf.deadline.sec = 5; // max expect time between msgs pub + qosProf.deadline.nsec = 0; + + qosProf.lifespan.sec = 1; // how long until msg are considered expired + qosProf.lifespan.nsec = 0; + + qosProf.liveliness_lease_duration.sec = 10; // time to declare client dead + qosProf.liveliness_lease_duration.nsec = 0; + + pub_ = image_transport::create_camera_publisher(this, "~/image_raw", qosProf); + wrapper_ = std::make_shared(); + wrapper_->setDebug(debug_); + wrapper_->setComputeBrightness(computeBrightness_); + wrapper_->setAcquisitionTimeout(acquisitionTimeout_); + + RCLCPP_INFO_STREAM(get_logger(), "using spinnaker lib version: " + wrapper_->getLibraryVersion()); + bool foundCamera = false; + for (int retry = 1; retry < 6; retry++) { + wrapper_->refreshCameraList(); + const auto camList = wrapper_->getSerialNumbers(); + if (std::find(camList.begin(), camList.end(), serial_) == camList.end()) { + RCLCPP_WARN_STREAM( + get_logger(), "no camera found with serial: " << serial_ << " on try # " << retry); + for (const auto & cam : camList) { + RCLCPP_WARN_STREAM(get_logger(), " found cameras: " << cam); + } + std::this_thread::sleep_for(chrono::seconds(1)); + } else { + RCLCPP_INFO_STREAM(get_logger(), "found camera with serial number: " << serial_); + foundCamera = true; + break; + } + } + if (!foundCamera) { + RCLCPP_ERROR_STREAM(get_logger(), "giving up, camera " << serial_ << " not found!"); + return (false); + } + keepRunning_ = true; + thread_ = std::make_shared(&CameraDriver::run, this); + + if (wrapper_->initCamera(serial_)) { + if (dumpNodeMap_) { + RCLCPP_INFO_STREAM(get_logger(), "dumping node map!"); + std::string nm = wrapper_->getNodeMapAsString(); + std::cout << nm; + } + // Must first create the camera parameters before acquisition is started. + // Some parameters (like blackfly s chunk control) cannot be set once + // the camera is running. + createCameraParameters(); + // TODO(bernd): once ROS2 supports subscriber status callbacks, this can go! + startCamera(); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "init camera failed for cam: " << serial_); + } + return (true); +} +} // namespace spinnaker_camera_driver + +RCLCPP_COMPONENTS_REGISTER_NODE(spinnaker_camera_driver::CameraDriver) diff --git a/spinnaker_camera_driver/src/camera_driver_node.cpp b/spinnaker_camera_driver/src/camera_driver_node.cpp new file mode 100644 index 00000000..ef93e6a8 --- /dev/null +++ b/spinnaker_camera_driver/src/camera_driver_node.cpp @@ -0,0 +1,30 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +#include +#include +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + + RCLCPP_INFO(node->get_logger(), "camera_driver_node started up!"); + + rclcpp::spin(node); // should not return + rclcpp::shutdown(); + return 0; +} diff --git a/spinnaker_camera_driver/src/genicam_utils.cpp b/spinnaker_camera_driver/src/genicam_utils.cpp new file mode 100644 index 00000000..7a21f04d --- /dev/null +++ b/spinnaker_camera_driver/src/genicam_utils.cpp @@ -0,0 +1,97 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +// NOTE: much of this code is adapted from the Spinnaker examples, +// in particular NodeMapInfo.cpp +// +// + +#include "genicam_utils.hpp" + +#include +#include + +using Spinnaker::GenApi::CCategoryPtr; +using Spinnaker::GenApi::CNodePtr; +using Spinnaker::GenApi::FeatureList_t; +using Spinnaker::GenApi::INodeMap; +using Spinnaker::GenApi::intfICategory; +using Spinnaker::GenICam::gcstring; + +namespace spinnaker_camera_driver +{ +namespace genicam_utils +{ +template +static bool is_readable(T ptr) +{ + return (Spinnaker::GenApi::IsAvailable(ptr) && Spinnaker::GenApi::IsReadable(ptr)); +} + +void get_nodemap_as_string(std::stringstream & ss, Spinnaker::CameraPtr cam) +{ + gcstring s = cam->GetGuiXml(); + ss << s; +} + +static CNodePtr find_node(const std::string & path, CNodePtr & node, bool debug) +{ + // split off first part + auto pos = path.find("/"); + const std::string token = path.substr(0, pos); // first part of it + if (node->GetPrincipalInterfaceType() != intfICategory) { + std::cerr << "no category node: " << node->GetName() << " vs " << path << std::endl; + return (NULL); + } + + CCategoryPtr catNode = static_cast(node); + gcstring displayName = catNode->GetDisplayName(); + gcstring name = catNode->GetName(); + FeatureList_t features; + catNode->GetFeatures(features); + if (debug) { + std::cout << "parsing: " << name << " with features: " << features.size() << std::endl; + } + for (auto it = features.begin(); it != features.end(); ++it) { + CNodePtr childNode = *it; + if (debug) { + std::cout << "checking child: " << childNode->GetName() << " vs " << token << std::endl; + } + if (std::string(childNode->GetName().c_str()) == token) { + if (is_readable(childNode)) { + if (pos == std::string::npos) { // no slash in name, found leaf node + return (childNode); + } else { + const std::string rest = path.substr(pos + 1); + return (find_node(rest, childNode, debug)); + } + } + } + } + if (debug) { + std::cerr << "driver: node not found: " << path << std::endl; + } + return (CNodePtr(NULL)); +} + +CNodePtr find_node(const std::string & path, Spinnaker::CameraPtr cam, bool debug) +{ + INodeMap & appLayerNodeMap = cam->GetNodeMap(); + CNodePtr rootNode = appLayerNodeMap.GetNode("Root"); + CNodePtr retNode = find_node(path, rootNode, debug); + return (retNode); +} +} // namespace genicam_utils +} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/genicam_utils.hpp b/spinnaker_camera_driver/src/genicam_utils.hpp new file mode 100644 index 00000000..c43844e1 --- /dev/null +++ b/spinnaker_camera_driver/src/genicam_utils.hpp @@ -0,0 +1,35 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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 GENICAM_UTILS_HPP_ +#define GENICAM_UTILS_HPP_ + +#include +#include + +#include +#include + +namespace spinnaker_camera_driver +{ +namespace genicam_utils +{ +void get_nodemap_as_string(std::stringstream & ss, Spinnaker::CameraPtr cam); +Spinnaker::GenApi::CNodePtr find_node( + const std::string & path, Spinnaker::CameraPtr cam, bool debug); +} // namespace genicam_utils +} // namespace spinnaker_camera_driver + +#endif // GENICAM_UTILS_HPP_ diff --git a/spinnaker_camera_driver/src/image.cpp b/spinnaker_camera_driver/src/image.cpp new file mode 100644 index 00000000..62318f4d --- /dev/null +++ b/spinnaker_camera_driver/src/image.cpp @@ -0,0 +1,41 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +#include +namespace spinnaker_camera_driver +{ +Image::Image( + uint64_t t, int16_t brightness, uint32_t et, uint32_t maxEt, float gain, int64_t imgT, + size_t imageSize, int status, const void * data, size_t w, size_t h, size_t stride, + size_t bitsPerPixel, size_t numChan, uint64_t frameId, pixel_format::PixelFormat pixFmt) +: time_(t), + brightness_(brightness), + exposureTime_(et), + maxExposureTime_(maxEt), + gain_(gain), + imageTime_(imgT), + imageSize_(imageSize), + imageStatus_(status), + data_(data), + width_(w), + height_(h), + stride_(stride), + bitsPerPixel_(bitsPerPixel), + numChan_(numChan), + frameId_(frameId), + pixelFormat_(pixFmt) +{ +} +} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/pixel_format.cpp b/spinnaker_camera_driver/src/pixel_format.cpp new file mode 100644 index 00000000..f259dc94 --- /dev/null +++ b/spinnaker_camera_driver/src/pixel_format.cpp @@ -0,0 +1,97 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +#include +#include +#include + +namespace spinnaker_camera_driver +{ +namespace pixel_format +{ +static const std::unordered_map fmt_2_string{ + {{PixelFormat::Mono8, "Mono8"}, + {PixelFormat::Mono10p, "Mono10p"}, + {PixelFormat::Mono10Packed, "Mono10Packed"}, + {PixelFormat::Mono12p, "Mono12p"}, + {PixelFormat::Mono12Packed, "Mono12Packed"}, + {PixelFormat::Mono16, "Mono16"}, + {PixelFormat::RGB8, "RGB8"}, + {PixelFormat::RGB8Packed, "RGB8Packed"}, + {PixelFormat::BayerRG8, "BayerRG8"}, + {PixelFormat::BayerRG10p, "BayerRG10p"}, + {PixelFormat::BayerRG10Packed, "BayerRG10Packed"}, + {PixelFormat::BayerRG12p, "BayerRG12p"}, + {PixelFormat::BayerRG12Packed, "BayerRG12Packed"}, + {PixelFormat::BayerRG16, "BayerRG16"}, + {PixelFormat::BayerGR8, "BayerGR8"}, + {PixelFormat::BayerGR16, "BayerGR16"}, + {PixelFormat::BayerGB8, "BayerGB8"}, + {PixelFormat::BayerGB16, "BayerGB16"}, + {PixelFormat::BayerBG8, "BayerBG8"}, + {PixelFormat::BayerBG16, "BayerBG16"}, + {PixelFormat::YUV411Packed, "YUV411Packed"}, + {PixelFormat::YUV422Packed, "YUV422Packed"}, + {PixelFormat::YUV444Packed, "YUV444Packed"}, + {PixelFormat::YCbCr8, "YCbCr8"}, + {PixelFormat::YCbCr422_8, "YCbCr422_8"}, + {PixelFormat::YCbCr411_8, "YCbCr411_8"}, + {PixelFormat::BGR8, "BGR8"}, + {PixelFormat::BGRa8, "BGRa8"}}}; + +static const std::unordered_map string_2_fmt{ + {{"Mono8", Mono8}, + {"Mono10p", Mono10p}, + {"Mono10Packed", Mono10Packed}, + {"Mono12p", Mono12p}, + {"Mono12Packed", Mono12Packed}, + {"Mono16", Mono16}, + {"RGB8", RGB8}, + {"RGB8Packed", RGB8Packed}, + {"BayerRG8", BayerRG8}, + {"BayerRG10p", BayerRG10p}, + {"BayerRG10Packed", BayerRG10Packed}, + {"BayerRG12p", BayerRG12p}, + {"BayerRG12Packed", BayerRG12Packed}, + {"BayerRG16", BayerRG16}, + {"BayerGR8", BayerGR8}, + {"BayerGR16", BayerGR16}, + {"BayerGB8", BayerGB8}, + {"BayerGB16", BayerGB16}, + {"BayerBG8", BayerBG8}, + {"BayerBG16", BayerBG16}, + {"YUV411Packed", YUV411Packed}, + {"YUV422Packed", YUV422Packed}, + {"YUV444Packed", YUV444Packed}, + {"YCbCr8", YCbCr8}, + {"YCbCr422_8", YCbCr422_8}, + {"YCbCr411_8", YCbCr411_8}, + {"BGR8", BGR8}, + {"BGRa8", BGRa8}}}; + +PixelFormat from_nodemap_string(const std::string pixFmt) +{ + auto it = string_2_fmt.find(pixFmt); + return (it == string_2_fmt.end() ? INVALID : it->second); +} + +std::string to_string(PixelFormat f) +{ + auto it = fmt_2_string.find(f); + return (it == fmt_2_string.end() ? "INVALID" : it->second); +} + +} // namespace pixel_format +} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp new file mode 100644 index 00000000..1728d92d --- /dev/null +++ b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp @@ -0,0 +1,101 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +#include + +#include +#include + +#include "./spinnaker_wrapper_impl.hpp" + +namespace spinnaker_camera_driver +{ +SpinnakerWrapper::SpinnakerWrapper() { wrapperImpl_.reset(new SpinnakerWrapperImpl()); } + +std::string SpinnakerWrapper::getLibraryVersion() const +{ + return wrapperImpl_->getLibraryVersion(); +} + +void SpinnakerWrapper::refreshCameraList() { wrapperImpl_->refreshCameraList(); } + +std::vector SpinnakerWrapper::getSerialNumbers() const +{ + return wrapperImpl_->getSerialNumbers(); +} + +bool SpinnakerWrapper::initCamera(const std::string & serialNumber) +{ + return wrapperImpl_->initCamera(serialNumber); +} + +bool SpinnakerWrapper::deInitCamera() { return wrapperImpl_->deInitCamera(); } + +bool SpinnakerWrapper::startCamera(const Callback & cb) { return wrapperImpl_->startCamera(cb); } + +bool SpinnakerWrapper::stopCamera() { return wrapperImpl_->stopCamera(); } + +std::string SpinnakerWrapper::getPixelFormat() const { return wrapperImpl_->getPixelFormat(); } +double SpinnakerWrapper::getReceiveFrameRate() const +{ + return (wrapperImpl_->getReceiveFrameRate()); +} + +std::string SpinnakerWrapper::getNodeMapAsString() { return (wrapperImpl_->getNodeMapAsString()); } + +std::string SpinnakerWrapper::setEnum( + const std::string & nodeName, const std::string & val, std::string * retVal) +{ + try { + return (wrapperImpl_->setEnum(nodeName, val, retVal)); + } catch (const Spinnaker::Exception & e) { + throw SpinnakerWrapper::Exception(e.what()); + } +} + +std::string SpinnakerWrapper::setDouble(const std::string & nodeName, double val, double * retVal) +{ + try { + return (wrapperImpl_->setDouble(nodeName, val, retVal)); + } catch (const Spinnaker::Exception & e) { + throw SpinnakerWrapper::Exception(e.what()); + } +} + +std::string SpinnakerWrapper::setBool(const std::string & nodeName, bool val, bool * retVal) +{ + try { + return (wrapperImpl_->setBool(nodeName, val, retVal)); + } catch (const Spinnaker::Exception & e) { + throw SpinnakerWrapper::Exception(e.what()); + } +} + +std::string SpinnakerWrapper::setInt(const std::string & nodeName, int val, int * retVal) +{ + try { + return (wrapperImpl_->setInt(nodeName, val, retVal)); + } catch (const Spinnaker::Exception & e) { + throw SpinnakerWrapper::Exception(e.what()); + } +} + +void SpinnakerWrapper::setComputeBrightness(bool b) { wrapperImpl_->setComputeBrightness(b); } + +void SpinnakerWrapper::setAcquisitionTimeout(double t) { wrapperImpl_->setAcquisitionTimeout(t); } + +void SpinnakerWrapper::setDebug(bool b) { wrapperImpl_->setDebug(b); } + +} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp new file mode 100644 index 00000000..34460471 --- /dev/null +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp @@ -0,0 +1,444 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 Bernd Pfrommer +// +// 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. + +#include "spinnaker_wrapper_impl.hpp" + +#include +#include +#include +#include +#include + +#include "genicam_utils.hpp" + +namespace spinnaker_camera_driver +{ +namespace chrono = std::chrono; +namespace GenApi = Spinnaker::GenApi; +namespace GenICam = Spinnaker::GenICam; + +template +static bool is_available(T ptr) +{ + return (ptr.IsValid() && GenApi::IsAvailable(ptr)); +} + +template +static bool is_writable(T ptr) +{ + return (ptr.IsValid() && GenApi::IsAvailable(ptr) && GenApi::IsWritable(ptr)); +} + +template +static bool is_readable(T ptr) +{ + return (ptr.IsValid() && GenApi::IsAvailable(ptr) && GenApi::IsReadable(ptr)); +} + +static bool common_checks( + const GenApi::CNodePtr & np, const std::string & nodeName, std::string * msg) +{ + if (!np.IsValid()) { + *msg = "node " + nodeName + " does not exist!"; + return (false); + } + if (!is_available(np)) { + *msg = "node " + nodeName + " not available!"; + return (false); + } + if (!is_writable(np)) { + *msg = "node " + nodeName + " not available!"; + return (false); + } + return (true); +} + +static std::string get_serial(Spinnaker::CameraPtr cam) +{ + const auto & nodeMap = cam->GetTLDeviceNodeMap(); + const Spinnaker::GenApi::CStringPtr psn = nodeMap.GetNode("DeviceSerialNumber"); + return is_readable(psn) ? std::string(psn->GetValue()) : ""; +} + +static bool set_acquisition_mode_continuous(GenApi::INodeMap & nodeMap) +{ + Spinnaker::GenApi::CEnumerationPtr ptrAcquisitionMode = nodeMap.GetNode("AcquisitionMode"); + if (GenApi::IsAvailable(ptrAcquisitionMode) && GenApi::IsWritable(ptrAcquisitionMode)) { + GenApi::CEnumEntryPtr ptrAcquisitionModeContinuous = + ptrAcquisitionMode->GetEntryByName("Continuous"); + if ( + GenApi::IsAvailable(ptrAcquisitionModeContinuous) && + GenApi::IsReadable(ptrAcquisitionModeContinuous)) { + // Retrieve integer value from entry node + const int64_t acquisitionModeContinuous = ptrAcquisitionModeContinuous->GetValue(); + // Set integer value from entry node as new value of enumeration node + ptrAcquisitionMode->SetIntValue(acquisitionModeContinuous); + return true; + } + } + return false; +} + +SpinnakerWrapperImpl::SpinnakerWrapperImpl() +{ + system_ = Spinnaker::System::GetInstance(); + if (!system_) { + std::cerr << "cannot instantiate spinnaker driver!" << std::endl; + throw std::runtime_error("failed to get spinnaker driver!"); + } + refreshCameraList(); +} + +void SpinnakerWrapperImpl::refreshCameraList() +{ + cameraList_ = system_->GetCameras(); + for (size_t cam_idx = 0; cam_idx < cameraList_.GetSize(); cam_idx++) { + const auto cam = cameraList_[cam_idx]; + } +} + +SpinnakerWrapperImpl::~SpinnakerWrapperImpl() +{ + keepRunning_ = false; + stopCamera(); + deInitCamera(); + camera_ = 0; // call destructor, may not be needed + cameraList_.Clear(); + if (system_) { + system_->ReleaseInstance(); + } +} + +std::string SpinnakerWrapperImpl::getLibraryVersion() const +{ + const Spinnaker::LibraryVersion lv = system_->GetLibraryVersion(); + char buf[256]; + snprintf(buf, sizeof(buf), "%d.%d.%d.%d", lv.major, lv.minor, lv.type, lv.build); + return std::string(buf); +} + +std::vector SpinnakerWrapperImpl::getSerialNumbers() const +{ + std::vector sn; + for (size_t cam_idx = 0; cam_idx < cameraList_.GetSize(); cam_idx++) { + const auto cam = cameraList_.GetByIndex(cam_idx); + sn.push_back(get_serial(cam)); + } + return sn; +} + +std::string SpinnakerWrapperImpl::setEnum( + const std::string & nodeName, const std::string & val, std::string * retVal) +{ + *retVal = "UNKNOWN"; + GenApi::CNodePtr np = genicam_utils::find_node(nodeName, camera_, debug_); + std::string msg; + if (!common_checks(np, nodeName, &msg)) { + return (msg); + } + GenApi::CEnumerationPtr p = static_cast(np); + if (!is_writable(p)) { + return ("node " + nodeName + " not enum???"); + } + // find integer corresponding to the enum string + GenApi::CEnumEntryPtr setVal = p->GetEntryByName(val.c_str()); + if (!is_readable(setVal)) { + // bad enum value, try to read current value nevertheless + if (is_readable(p)) { + auto ce = p->GetCurrentEntry(); + if (ce) { + *retVal = ce->GetSymbolic().c_str(); + } + } + if (debug_) { + std::cout << "node " << nodeName << " invalid enum: " << val << std::endl; + std::cout << "allowed enum values: " << std::endl; + GenApi::StringList_t validValues; + p->GetSymbolics(validValues); + for (const auto & ve : validValues) { + std::cout << " " << ve << std::endl; + } + } + return ("node " + nodeName + " invalid enum: " + val); + } + // set the new enum value + p->SetIntValue(setVal->GetValue()); + // read it back + if (is_readable(p)) { + auto ce = p->GetCurrentEntry(); + if (ce) { + *retVal = ce->GetSymbolic().c_str(); + } else { + return ("node " + nodeName + " current entry not readable!"); + } + } else { + return ("node " + nodeName + " is not readable!"); + } + return ("OK"); +} + +template +T set_invalid() +{ + return (std::nan("")); +} + +template <> +int set_invalid() +{ + return (-1); +} + +template +static std::string set_parameter( + const std::string & nodeName, T2 val, T2 * retVal, const Spinnaker::CameraPtr & cam, bool debug) +{ + *retVal = set_invalid(); + GenApi::CNodePtr np = genicam_utils::find_node(nodeName, cam, debug); + std::string msg; + if (!common_checks(np, nodeName, &msg)) { + return (msg); + } + T1 p = static_cast(np); + p->SetValue(val); + if (!is_readable(np)) { + return ("node " + nodeName + " current entry not readable!"); + } + *retVal = p->GetValue(); + return ("OK"); +} + +std::string SpinnakerWrapperImpl::setDouble(const std::string & nn, double val, double * retVal) +{ + *retVal = std::nan(""); + return (set_parameter(nn, val, retVal, camera_, debug_)); +} + +std::string SpinnakerWrapperImpl::setBool(const std::string & nn, bool val, bool * retVal) +{ + *retVal = !val; + return (set_parameter(nn, val, retVal, camera_, debug_)); +} + +std::string SpinnakerWrapperImpl::setInt(const std::string & nn, int val, int * retVal) +{ + *retVal = -1; + return (set_parameter(nn, val, retVal, camera_, debug_)); +} + +double SpinnakerWrapperImpl::getReceiveFrameRate() const +{ + return (avgTimeInterval_ > 0 ? (1.0 / avgTimeInterval_) : 0); +} + +static int int_ceil(size_t x, int y) +{ + // compute the integer ceil(x / y) + return (static_cast((x + static_cast(y) - 1) / y)); +} + +static int16_t compute_brightness( + pixel_format::PixelFormat pf, const uint8_t * data, size_t w, size_t h, size_t stride, int skip) +{ + if (pf != pixel_format::BayerRG8) { + return (0); + } + const uint64_t cnt = int_ceil(w, skip) * int_ceil(h, skip); + uint64_t tot = 0; + const uint8_t * p = data; + for (size_t row = 0; row < h; row += skip) { + for (size_t col = 0; col < w; col += skip) { + tot += p[col]; + } + p += stride * skip; + } + return (tot / cnt); +} + +void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr) +{ + // update frame rate + auto now = chrono::high_resolution_clock::now(); + uint64_t t = chrono::duration_cast(now.time_since_epoch()).count(); + if (avgTimeInterval_ == 0) { + if (lastTime_ != 0) { + avgTimeInterval_ = (t - lastTime_) * 1e-9; + } + } else { + const double dt = (t - lastTime_) * 1e-9; + const double alpha = 0.01; + avgTimeInterval_ = avgTimeInterval_ * (1.0 - alpha) + dt * alpha; + } + { + std::unique_lock lock(mutex_); + lastTime_ = t; + } + + if (imgPtr->IsIncomplete()) { + // Retrieve and print the image status description + std::cout << "Image incomplete: " + << Spinnaker::Image::GetImageStatusDescription(imgPtr->GetImageStatus()) << std::endl; + } else { + const Spinnaker::ChunkData & chunk = imgPtr->GetChunkData(); + const float expTime = chunk.GetExposureTime(); + const float gain = chunk.GetGain(); + const int64_t stamp = chunk.GetTimestamp(); + const uint32_t maxExpTime = + static_cast(is_readable(exposureTimeNode_) ? exposureTimeNode_->GetMax() : 0); + +#if 0 + std::cout << "got image: " << imgPtr->GetWidth() << "x" + << imgPtr->GetHeight() << " stride: " << imgPtr->GetStride() + << " ts: " << stamp << " exp time: " << expTime + << " gain: " << gain << " bpp: " << imgPtr->GetBitsPerPixel() + << " chan: " << imgPtr->GetNumChannels() + << " tl payload type: " << imgPtr->GetTLPayloadType() + << " tl pix fmt: " << imgPtr->GetTLPixelFormat() + << " payload type: " << imgPtr->GetPayloadType() + << " pixfmt enum: " << imgPtr->GetPixelFormat() + << " fmt: " << imgPtr->GetPixelFormatName() + << " int type: " << imgPtr->GetPixelFormatIntType() + << " frame id: " << imgPtr->GetFrameID() + << " img id: " << imgPtr->GetID() << std::endl; +#endif + // Note: GetPixelFormat() did not work for the grasshopper, so ignoring + // pixel format in image, using the one from the configuration + const int16_t brightness = + computeBrightness_ + ? compute_brightness( + pixelFormat_, static_cast(imgPtr->GetData()), imgPtr->GetWidth(), + imgPtr->GetHeight(), imgPtr->GetStride(), brightnessSkipPixels_) + : -1; + ImagePtr img(new Image( + t, brightness, expTime, maxExpTime, gain, stamp, imgPtr->GetImageSize(), + imgPtr->GetImageStatus(), imgPtr->GetData(), imgPtr->GetWidth(), imgPtr->GetHeight(), + imgPtr->GetStride(), imgPtr->GetBitsPerPixel(), imgPtr->GetNumChannels(), + imgPtr->GetFrameID(), pixelFormat_)); + callback_(img); + } +} + +bool SpinnakerWrapperImpl::initCamera(const std::string & serialNumber) +{ + if (camera_) { + return false; + } + for (size_t cam_idx = 0; cam_idx < cameraList_.GetSize(); cam_idx++) { + auto cam = cameraList_.GetByIndex(cam_idx); + const std::string sn = get_serial(cam); + if (sn == serialNumber) { + camera_ = cam; + camera_->Init(); + break; + } + } + return (camera_ != 0); +} + +bool SpinnakerWrapperImpl::deInitCamera() +{ + if (!camera_) { + return (false); + } + camera_->DeInit(); + return (true); +} + +bool SpinnakerWrapperImpl::startCamera(const SpinnakerWrapper::Callback & cb) +{ + if (!camera_ || cameraRunning_) { + return false; + } + // switch on continuous acquisition + // and get pixel format + GenApi::INodeMap & nodeMap = camera_->GetNodeMap(); + if (set_acquisition_mode_continuous(nodeMap)) { + camera_->RegisterEventHandler(*this); + camera_->BeginAcquisition(); + thread_ = std::make_shared(&SpinnakerWrapperImpl::monitorStatus, this); + cameraRunning_ = true; + + GenApi::CEnumerationPtr ptrPixelFormat = nodeMap.GetNode("PixelFormat"); + if (GenApi::IsAvailable(ptrPixelFormat)) { + setPixelFormat(ptrPixelFormat->GetCurrentEntry()->GetSymbolic().c_str()); + } else { + setPixelFormat("BayerRG8"); + std::cerr << "WARNING: driver could not read pixel format!" << std::endl; + } + exposureTimeNode_ = nodeMap.GetNode("ExposureTime"); + } else { + std::cerr << "failed to switch on continuous acquisition!" << std::endl; + return (false); + } + callback_ = cb; + return (true); +} + +bool SpinnakerWrapperImpl::stopCamera() +{ + if (camera_ && cameraRunning_) { + if (thread_) { + keepRunning_ = false; + thread_->join(); + thread_ = 0; + } + camera_->EndAcquisition(); // before unregistering the event handler! + camera_->UnregisterEventHandler(*this); + + cameraRunning_ = false; + return true; + } + return (false); +} + +void SpinnakerWrapperImpl::setPixelFormat(const std::string & pixFmt) +{ + pixelFormat_ = pixel_format::from_nodemap_string(pixFmt); +} + +std::string SpinnakerWrapperImpl::getPixelFormat() const +{ + return (pixel_format::to_string(pixelFormat_)); +} + +std::string SpinnakerWrapperImpl::getNodeMapAsString() +{ + std::stringstream ss; + genicam_utils::get_nodemap_as_string(ss, camera_); + return (ss.str()); +} + +void SpinnakerWrapperImpl::monitorStatus() +{ + while (keepRunning_) { + std::this_thread::sleep_for(chrono::seconds(1)); + uint64_t lastTime; + { + std::unique_lock lock(mutex_); + lastTime = lastTime_; + } + auto now = chrono::high_resolution_clock::now(); + uint64_t t = chrono::duration_cast(now.time_since_epoch()).count(); + if (t - lastTime > acquisitionTimeout_ && camera_) { + std::cout << "WARNING: acquisition timeout, restarting!" << std::endl; + // Mucking with the camera in this thread without proper + // locking does not feel good. Expect some rare crashes. + camera_->EndAcquisition(); + camera_->BeginAcquisition(); + } + } +} + +} // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp new file mode 100644 index 00000000..27fbd8ee --- /dev/null +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp @@ -0,0 +1,89 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2020 Bernd Pfrommer +// +// 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 SPINNAKER_WRAPPER_IMPL_HPP_ +#define SPINNAKER_WRAPPER_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace spinnaker_camera_driver +{ +class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler +{ +public: + SpinnakerWrapperImpl(); + ~SpinnakerWrapperImpl(); + // ------- inherited methods + // from ImageEventHandler + void OnImageEvent(Spinnaker::ImagePtr image) override; + + // ------- own methods + std::string getLibraryVersion() const; + void refreshCameraList(); + std::vector getSerialNumbers() const; + std::string getPixelFormat() const; + + bool initCamera(const std::string & serialNumber); + bool deInitCamera(); + + bool startCamera(const SpinnakerWrapper::Callback & cb); + bool stopCamera(); + + double getReceiveFrameRate() const; + std::string getNodeMapAsString(); + // methods for setting camera params + std::string setEnum(const std::string & nodeName, const std::string & val, std::string * retVal); + std::string setDouble(const std::string & nodeName, double val, double * retVal); + std::string setInt(const std::string & nodeName, int val, int * retVal); + std::string setBool(const std::string & nodeName, bool val, bool * retVal); + void setDebug(bool b) { debug_ = b; } + void setComputeBrightness(bool b) { computeBrightness_ = b; } + void setAcquisitionTimeout(double t) { acquisitionTimeout_ = static_cast(t * 1e9); } + +private: + void setPixelFormat(const std::string & pixFmt); + bool setInINodeMap(double f, const std::string & field, double * fret); + void monitorStatus(); + + // ----- variables -- + Spinnaker::SystemPtr system_; + Spinnaker::CameraList cameraList_; + Spinnaker::CameraPtr camera_; + SpinnakerWrapper::Callback callback_; + double avgTimeInterval_{0}; + uint64_t lastTime_{0}; + bool cameraRunning_{false}; + bool debug_{false}; + bool computeBrightness_{false}; + int brightnessSkipPixels_{32}; + pixel_format::PixelFormat pixelFormat_{pixel_format::INVALID}; + Spinnaker::GenApi::CFloatPtr exposureTimeNode_; + bool keepRunning_{true}; + std::shared_ptr thread_; + std::mutex mutex_; + uint64_t acquisitionTimeout_{10000000000ULL}; +}; +} // namespace spinnaker_camera_driver + +#endif // SPINNAKER_WRAPPER_IMPL_HPP_ From a825693707668c2ebdeeeabe84bc165165ea01b8 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:23:21 -0400 Subject: [PATCH 06/11] update licensing comment --- flir_camera_description/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flir_camera_description/README.md b/flir_camera_description/README.md index ec69f18e..d456a0f5 100644 --- a/flir_camera_description/README.md +++ b/flir_camera_description/README.md @@ -14,4 +14,5 @@ In rviz, add a "RobotModel" and set the topic to "robot_description". ## License: Licensed under BSD License. - +Copyright (c) 2012, Carnegie Mellon University. All rights reserved. +Copyright (c) 2018, Clearpath Robotics, Inc., All rights reserved. From 0cb94c454fcc13a785e21c364d475bbfcdfd88c4 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:23:48 -0400 Subject: [PATCH 07/11] updated README --- README.md | 41 ++++++++++++++++------------------------- 1 file changed, 16 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 021a8056..4b19e82c 100644 --- a/README.md +++ b/README.md @@ -4,28 +4,19 @@ This repository contains packages for FlirImaging's line of cameras. This reposi ## Packages -### Spinnaker Camera Driver -The camera driver supports USB3 and GIGE cameras are planned. Note thats support for FireWire cameras is dropped in this SDK. The driver has been tested with a Blackfly S and Chameleon 3 camera. Differences between cameras requires that each camera model needs a customized interface class. If your camera type is not included, consider contributing the interface by referring to the section bellow. - -##### Contributing -Due to differences in parameter naming the configuration is separated from the main library. `camera.cpp` contains the base class `Camera` which can be extended to accommodate different cameras. The base class is based on BlackFly S and `cm3.cpp` extends it adding support for Chameleon3. To add a camera create a new derived class of `Camera` and add the model name to the check in `SpinnakerCamera::connect`. - -When contributing make sure the travis job suceeds and please use [roscpp_code_format](https://github.com/davetcoleman/roscpp_code_format) to format your code. - -## Licence -ROS-compatible Camera drivers originally provided by NREC, part of Carnegie Mellon University's robotics institute. -These drives are included along with modifications of the standard ros image messages that enable HDR and physics based vision. - -This code was originally developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie Mellon University. Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012. Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution Unlimited). - -This software is released under a BSD license: - -Copyright (c) 2012, Carnegie Mellon University. All rights reserved. -Copyright (c) 2018, Clearpath Robotics, 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 Carnegie Mellon University 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 HOLDER 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. +### spinnaker_camera_driver +The camera driver supports USB3 and GIGE cameras. The driver has been +successfully used for Blackfly, Blackfly S, Chameleon, and Grasshopper +cameras, but should support any FLIR camera that is based on the +Spinnaker SDK. See the +[spinnaker_camera_driver](spinnaker_camera_driver/README.md) for more. +This software is issued under the Apache License Version 2.0 and BSD + +### flir_camera_msgs +Meta messages with image exposure and control messages. These are used +by the [spinnaker_camera_driver](spinnaker_camera_driver/README.md). +This software is issued under the Apache License Version 2.0. + +### flir_camera_description +Package with meshesMeta messages with image exposure and control messages. These are used +by the [spinnaker_camera_driver](spinnaker_camera_driver/README.md). This software is released under a BSD license. From 1395154f0ff07fa92e8c26b8bc1b5a8783f66f6b Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:24:08 -0400 Subject: [PATCH 08/11] adjusted CI workflow for ROS2 --- .github/workflows/industrial_ci_action.yml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index c5cd03a7..e3636245 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -1,26 +1,24 @@ -# This is a basic workflow to help you get started with Actions name: CI # Controls when the workflow will run on: - # Triggers the workflow on push or pull request events but only for the noetic-devel branch push: - branches: [ noetic-devel ] + branches: [ humble-devel ] pull_request: - branches: [ noetic-devel ] + branches: [ humble-devel ] # Allows you to run this workflow manually from the Actions tab workflow_dispatch: -# A workflow run is made up of one or more jobs that can run sequentially or in parallel + jobs: industrial_ci: name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) runs-on: ubuntu-latest strategy: matrix: - ROS_DISTRO: [noetic] + ROS_DISTRO: [humble] ROS_REP: [testing, main] env: CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci) From b7cae8ce45c523786468bed700702cdab5c1d42f Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:37:24 -0400 Subject: [PATCH 09/11] resize image and move to top --- flir_camera_description/README.md | 3 ++- .../images/blackfly_s_description.png | Bin 39099 -> 61981 bytes 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/flir_camera_description/README.md b/flir_camera_description/README.md index d456a0f5..730fbd1c 100644 --- a/flir_camera_description/README.md +++ b/flir_camera_description/README.md @@ -1,8 +1,9 @@ # flir_camera_description -This package contains urdf files and meshes for FLIR cameras. ![banner image](images/blackfly_s_description.png) +This package contains urdf files and meshes for FLIR cameras. + ## How to use Install this package like any other ROS package, then launch the demo: ``` diff --git a/flir_camera_description/images/blackfly_s_description.png b/flir_camera_description/images/blackfly_s_description.png index 82528751ba17f2297b3085e778613151fc111176..86a43ad19fe587995c266517a28872c38ecf75a9 100644 GIT binary patch literal 61981 zcmce-c|6qL7e78ql8}n5NwSPBOH(FIS+YdNo{Tjy82i{VWJ#8iCELi@x3QD4kEKP{ z$vT#pBK%DMrM12_W9}G_)YN&$#9RJB`E=T~LoN<4E@C1R*y+8g(w-}h# z3_LvTr3qI%O+9^qMF?t{%(xAV2+~wlHu4=`KJ18vAc*s??{-wD(E6th5lOF56hX5JJ;){V+3T*GP&yp|DpSHGUN{HgSP^lLLY~NKR$Vdc-yM3`(~+xATDu3 zkz+g`g zHOIdpC894#{2Sev=l|ycxkx`iz$ART80oHV5r)E^AThh!S8Y_t^q_VvxCGZl(h)sV z+s41?U+T0SWDSA$!h5%2@@vnV$~`TpAV_qooh=up?#`_qud&Sg*(M@?Ezx%3w!%^# z>Z0#Cu3(V4z)7&>=V0P_NQ<}a;W?r$%H*^ zvi!E|?GvJ;A3kAE!}R`c6}By1X z&C)Bq`)|>n9tp2kj9!$fWdGv+yOU1`hTw*-vU+h(h4nwHWf%V2eMz2!`QYxA*>iqa zmG!}YA9zVEgId@AU=?pGhH)8AC^3coKhDDY|8kbL z=>^N4?RT3s*GHRoPT9;66vI?1ZU4GsWZNg#Luz^+Fm#;Mfgzav;Xm&)fz0z6CRYeQ z3YTCy^}l!KPPo4hco(7%w!HjrRXIP+lTyL#y1{=xxe^?yB8+B_6jl1S5x2he+^pSp z&8jE%cw4mDu`~T!_=|Tp%1E0-MGuDMP&GypZ~w2s*qn|H)7$TtFI6hr|NUH|j&J(O9;xuZnY`21l1*M>Q);F~oCFzUy?1%^q(tw3{$q15 z-(&Ba)0(Yzn92WUF}>ISNWV;F9rVAY$qv)!O$7Z%08ZfFUB2|nvr1tg$z2~j|Ie*V zzzRPnsOxHvo}@mOXOA3>`L|@fZ+s#W+pYro92Be)3b9D2v&uHP_is*1Im8$2O#hoh zi0AcHa$~6;*!6$T#i;U%yYgw_2r_J%?%#X*7)rcB6GKc!BP`R9GLxNK66_?6f1b!U z(vvfbZ@$OR;5yT z;onFPvwsYlOTU+@pfMYOEpPr?Y=}&f6<|-H|1I}3&gKVT%=&+x_!OAV=6icN5ib94 zh8&Tc|2(zQVKpD>J7-~LVqCuT`2Sj}FYK0;JX?jM^DyK6u;~j)x{tu@kzso5!qn~h z&cr^ZM*A;w$Q+b{%@b|1H}Mk$?gYWj3uDV^QI(Pp-{icS`L=sVm_>?p$9wiBH}6H- z|Iglho+4;VlICcwi^V8Qs|>T@=uJ3TiX`|V2gUO-6igy~-GudRLSqtyx(JD$KOAec z$pn{!pFe|Tjj>35b`rHG@SLjVDQ7M zswqf>K05*{qIrU`mn2C7>9eaLEN_s}aw+{RaI!OH1Og{}ks;=zG%tlbpYHe$bQ-l` zO*dY3Z?PHt&mm7)KUNbzSIEk))PS=*U}vPuqAA$$%(?P))JT%he62EG6Ee){wx_8u z#*iHF223}e^fRK+_hVx+lDh-#ViR8a8PcOuU_C5Z}3u9lJkkD+D;H25UkIW zk24sF$iqUZX`4Aey$I4I1IAfQ`)y8@(w_)L65-Pmfj=k;YnLRY_D*_vIgMCOZk>kq z5}aZDXZ7#>K4d@pFDUumGFfS_YS(sz%kzBXvLTk=%P0Vv5D3|Bc=$hkjK0o!3Hl%{j28e6bzm zUb85YTD3SoEExKhMT3)*N-U?;!dWzM_&QUmRv8GKGbd^W+;?`^ee3JLLsjy!aOgEi ztCcN0J3)fyBVR|YON8u*14J{f6Q7#%)0)e@w@UJ1{4dA8=O!xF|eVE$eTxDrFUZ`&fRuwYARe2cyMkm83+Z`K^hl zj$Bw@=Pj2p-rwSH^$VdfG^bT!1{D)-?d-C$u#arj$KmsMx^~r2$)d{t;uIT0*2$jJ zii*sKusRYU?`O9=^a0sRj=SVZS0r0Uob#X$pT?-PM3^MW@s=45zep6JFAUSdN=fkl zOCjsF4!CkulrveIe4;uPZoIn6z{AX-LLY^~wKKPVA0Kyw@EjbR227lhnQ7%7h-%s) zoYf2M4-IBBbs3zvOwwmpftS-r+xST|9w?ar~(`Czs}E(IRUo5Xu&kEF78h zYh&&PsJ+^_1c{s9*}0W|eiJx&p^m6l8IzsQFcu{03py9P1si?ynXWoVhP2^9$9)l> za-x6xovbzPd#xvYCOpnzTx+40`26?py^9n5-o?W=d86$L$ionZ;7vLB#S5~<4*7WFMGhNAW=@ejVg%=U0g~Z zf~>j1T`1w!=LNbqRa zYVEA8RWBQ9BPf2_cJI~5jr(1I!eNmpoGZ?*YD^CPa>!nvU7wvpO*SR!algBp*k6CN zyy-FPcGFtDO7e~R9L5p|sqOEq7ciE`UI-^cNWW#6i(E8b(NbMQ+0?A>xu=2zMz<@= zC)cxbz_tw8`;4Y_6mN@XHZ}>eLS1vu`EVB6;Z-EdjYct&P-HK;`s}-43)h^QlcD@c&9i?`~*~R6ozKCWH%Jjpebfn6w z_|5piAkvTD_G-tjxo-L-3V~($Si4fZy?bGdUs6?5bekX<(HfXRg=oyTtO<53^Lm`gPW0WAbrRi&o8v_3oEfXB(N)>_KYq9#%(0gPY(VG3$!l%)KQfpQ=LaB2rdYU{K zgu*pNgAw(n5;_qacrikSz(-<2wY#A*aWtmY3L2}gFznR}go4l3NIRlN%>D)b}BGPjJ z+-+_lc<4B@+m~w9Soxiu-PPj?wb>;`rt1v zaCxUN`N`G=`r5Rt00efnTY*2;HX@Pcx8mdP1L9hItyw8gk7vEp-Fhr_?;fc(X)d)Z47A zmgdLHn;L~eVB^jDY~?OZ%kkD2G}{~p?MTsZVvxQQ5e4DFqfSJXzvQ-=ZPbFx9|nE^ z^MCWIGX9#7_VeMna}Q>{KdkB6j?K5Xhof-7Xhc^%VziEj?l()JQ1HVlkE5hFJ`Z~$ z%@shOA(`6Lo|F!;$m!YHSvpJ_wO}WGe;bM@8Lo995ta`RPqxZ9?3R{u8|O?t_#0Ee zxE@mXh*{$oTt}qa>+=+)o zkR-|VO5f3fN*6EGvE|SUg#Oy!K5K(>F>C$-9L;PtLFU#jTkUI(R$rB^1Veo>SBhB! z7QOss7DzjSBtp<3ZPLA$49S;ECP;QoJ?njZNoDr-N$23 z6L6ba7c(rJ0Z(-OR;zYt1KHIoBcvD)W-6jGB%gQF+n?vo68qGmXsu0#jL=-X8Yh~Y z4}~-f%MBSTeKOcUpfZJz-6B~SCTA-5p$py)lOGO0wAPNM>;qfz2@?r^7>?!{)yx z^^)O@ruo&?dmxbVoHbrhHOcWVa-tUiPFnAtwa_C?s8j+a&*JRzUC(ttt_ z>91c=2jtJ=ym`%I;H?l`=XN@_ZaOdmr=B?lIMuH?B>iBRD**@_1 z$0F4}(N!*M%AX4@y?6b3FA#lQ8V4L)-Q0M?(R9=&_?fSlS_1wZu_OCxVu` zkC}?6T@E;gU}myjN&%3DNQ6QeJ+c>^4CaVOig6T%TR(uy8wSWrGZU(rLViw9n*%_N zaO>8smc9AZ&i8RYj%)$)DzAQZozgEJDmGyN#A+)}hR~CC%NX1dEgqtLAiG z_3n2UBuZHGGv*Y<6d$-C(shz~VzkL%rVJr;1~s9}1enS+0;tFfYUx}1Ar_-INd(QJ z{^jQ4MG@&p7-=x@gYJAh>BoP<_q$4AnT{gkr<7Xl9=}g1l%&F(EX{o1(V4_?%g5%8 z#OBh7%3WGxvDboD2v-U&u`Z8ThEKSgm1l!O%ReZ&rE4b5SUF zjzi*`&je?ZoVSG=1V=14e0>HaZq>7y)&%}6BM?M`1nlEele?vQR5dT0iy*m)h(FY4 zuX~krM?<#4#=BQIV^jw2S$rjeK}<+99!Pxk^LyKeP&K+IGfM%b*wghsr5Ae0*NtrF zgyV!NOaDSRU&h2nAa*U8aFZQ&GHAqb>jk2+>{mjQGf|UV4c-7pR_HWfq;46$xqs^- zP|my?p=~Rqudc2J#(!y9J2(nu^21i52-P7I|BX5!8pf$kw+`E3IxkOzVlp^SRqUff z_T2(ALetp_BjYGFjoUPS2`A%)KXW74&6TpsF8 z$MCff92L{1rtjXH3(k4i?x${C>ELkUhRlTa(4rSevMA+&52g3^hqoqb>yBJ_>xIXg zP?0&=Ft8IQZANtc(M(fmlv94*4W}Uc(YAcKEc)*CUqFVeA8^O@0BMy0Bxj7Llegw7 z$6&~`W2`;R^oSfyBVNBAU?J@v45T2Y416-$d8r0LJGS{skLt|2gXZLD`jK5p2=%5U zX{#yRnhVmU%lMX0m;9?2)_YBiDR8)A2b-VjN8x z%vxW~MV+`BnwE?RFzE4~&?%rSdNI$yarhx&-thC>7mBKvy8%XM`jiSQ3WrBv;iqlk zjeHouajoB_rlx}8g%)?EH+~Zq!@ISx%`1^%G8AoF%FgQQ^ad9C3q&5i_+*r9*96tc6y=o$f+>J#+1PvfRgk80YrNLK|kA=NCIsvteG2?aeUGUV@&0YY5YhqRqn zuq$n7p-pJXBsl)gAggk z-;$*E8MOYa5&Xllnx8jN-I{PXQy7#+T|N~ z02Y~S33ynsd~gx@d8hatO2h-mf7eo9hkkr1^l7`Vh>CW|D?{PJt>aC^9$w#ss=N|H zNO?9L7+_BTW8XpsSdyF#JjNa_t*UPoji1`P9zaNHRVrX2sob%}|Wh$^I$B z%-s5IeEc31`h9Hd_Pu++w)C=*EKS=y4V^@Y{SY6E8~TE|ayJ};18xdE>Dy~jDnhwi z$L&Oo>%gq^2m6<(|Q53a%K1F%+=#B-OW<+E7mfDk*gKbb#e#3 zO-$q?=vowC=e%l>e8jx#TX;eLHAh=(L(yXRb28XKFtl?s?I_64&j2e3PagJ^<*Y91 z=+LmYF950;fB%R`cd@(G%Vma=r+d%#UQK;{OTLbeO0@%Ou0k%)`G_9Z!vb4l+84D? zu`$oQdV5uFrrW79PY4Y2rlE zN_KMml~^v;x}@OqX~Y>QL)aybNX&be)FrdUWLt#DN3Pr|e|@Yl71mcJ!`xLFtp1{u zaJ}Jj@18r9k2Rvq*x$y+2AJz8<<*RX3|FbQa`5zeXK+yN00f>(ndskMv=banpRBLF zZft3*Lgjbw4SVMidc&K63G-Iz}Pt!R+?JKQ8G&D_qM3zJoVyFlTJ77 z1rmYvZZIYWrfkuhu>ESCk|7xY8x5fKO4I%D6g%OX&Ps)p4wBo5|XZLff8ONjiGBp0oHu15Z7FyI3zB@+%S8j|y zsddR7@j4F7T(fLyVInH82(ynw!^qY9}jd`{PxXC zK~JTs826_)MJ0yE-!Q!V1OnMOJ2{bLzBV@pz;o}ra?=N5=TNwo_d9~^5Rx2AYqjJ< zGNc1t*ki~Du(bN@RH{7!JBhN({nGs~Ty`GALaAj2-b9gMw)oIwa`pC!U_Yk9m{9Y3 zR}>?bkvI5QUu3&b0FUtZx3_r-lp|6Vk;$sL1N2WQwNwo8dcwS5pJy*7{tF7nc$Fa8 z=u@#C!18PU(^~Jl{_Rn)@dk`@8hcgCJ5KcedA^@fpU@-Mt_z1k6%`98F*62+KxvV# za}HF)Ilu>slr6mm)HmK1qu!T0vSmevLq}h2lFm5oMt+f~B$N+T-BN8{WOx#e9`iC} z$LO=8`Ghs&smUMCaa;NeS9XYs0`%bBE79Rml`AVNR)6;8(R}9^0v@T?UGKL%sWj>x z%pE@9009B?KzGKb3ZowPrIhu@n!5LN+pET_Y_HE)x`?kkGMveHL68i~x|m>SrqJLw zdfnzsVpJHmCQ#!>B$!@V;Er%nbLVE$#6olWN z;KT)B53gV!?b3yM6?}@=P!H?y&Kv76+P5T)HD$2}Fnji@rggQ!kdiveovrFPYMEpEO z(o!NV(i?F+L|txwD|7~-*n!@fP5z`+TFO_uVuGE6)}l-aiwq!vjV1v_U&riH9`}tJ zS0aYn7U2_k{Pkne3*ZB|J<>8WiwJ*&J#NP4@-ig3^rm)TAf@8$&FMvS@)hYqnxQhF z&G6yWbaSa2R~&{9PUb$@vvN@dk_wbbotIKz17flF2kAj! zD7t;!j**`1rcMJ9?c#Y3G1D%Z{+Q)jduHRmpz9s zS{;D}aNB3QKZ9z?MrkUHaB^f0+tUIZlh8}>vyoCpP*grPNwys;b)PeaUC_hte6UZW z4P^x2K`b~7#hfyz$SyZs&Et4RM2=yR1~T!>;@$C7j4T)PAjBeUGUI}TZZP#G7f|(M zEKdeozZ<S3`Z<-{_@ZPSfiwo$O;!)^cmiyGSvvdeU z?v9r*>0*4U!Oe*#k8VN~kd|}SeZQ~x-0eHqt_Zr3qP-Bgm-I)b`0$}Ua}=r;;PIsZ z%ov&T7Lz9VkYoUQdUFH%?7#Y+vtd6Wx@egChoEefX#v^gmYJCuMNL1{T83ZOHC?;f zbcm-S@JKY3d$#p9^?O6{gKFL!y`1?P-Gbq(x5=H!`JFv|x+W`Ez8ZeIeeOi2D{pu< ziikv%89P0D2Ka>=b8FTs8+NssbpW(X?ve}=g{^At<%%Vm>~ZsacI65$=e4c0M~uud z3g0&cM@o`H>m`*m$+Mf(D$e<-ToX1-MnXdf5^?8FlrQKFzd)5*7xF;W zc$;%;zx@U@Fg~v&y`v#jGPQem;%Tdbe|UY4B3QvcFc63a;iN9r-c)(FuB0ar9)qtz z4VlS_bV$zy{++Ds5S9L_JoTbC9EVHn_c7N_KS7Y&?i%t6Y{Gp9K8N9}2CmytVywBa z$dmsCY?SqNvJP#+iVb9#6T*=mmLi%>1c^jLxO5NBp7>Fega>D`97?Uy^mD<3UJYg`J(oN@Rfl)}nWwNGS- zPCPVHr^00z-~sWRm<)Rg(5ESuaj#$4$vOSsiMDP4?NsFnLihBPfF}S|T5m)3sxN4RlD+ANz~b;pNX<6kp=iM?u>8mT+JMZ!dWg@s++vWH z+5PQMr+n1zjqz`J%{x4YleOutbq~YMR1(uR!IAri_o!`Yjqjq`2kJBflmOE=|G+y- zYmvPmJooCG7tSEuDpl-)W(6x~3>HNFP7*~zGzA>6vKhs_XX5x()ft%P#~*Pb!Ncn82}L*=YPkmM z&0RcEKh@;F%iTPYQRh@Di9vWw_61Cy{t0z08CHNj&!sU$R{$Kj<~-q5-jg%s;a4C) z=gg4a^isOytMRU3fN@!#zqPi{vI;#H^>z5v2opZCkO6C+bh%^LJ6gS=4gDE|_V7^U zN5^8$im7h!T~RzzeO61kOsNG*-~uw;TOkyC*pTJcuV>FeG$CW2XojAt5}YeDJU4zL z{<=V+STJrtBR9%;llv(NfY$(F;qOlcjxj*qC3zQ3TQf(z?6O_PWVldHpFVBN;(=km!#16}2c?kGEQ6TNuA0(CiL1DOwT0C2m7t;}G~UQ%LqsZ{g+U zc|G$=oU=4MadU-JtKVCI*P9{v&=Inmp&cF?jG+cD$QSXEhB+0YQ+ z<1>A56cRXfjx^J$d4**8KDpaQw^`*CG^VqzOJ;vad&se-wysWBM+ZD&5cJ!qu9xsL z&GS0wvO^cV@sq7u`MH3%g9Ltlge;s~>mm-#=dE=y{PeM{?bJ$u(au#}h1fsuw=H7% ze!%*2v%jQqqHyZ^s!|>7RpukN!&tSv%tYue1I^t#K(7J_T&*%+3C?oaI}qj8s*LA1 zumZ-~x`bPON$hG2b1QlHlFg99zZ0rK*Lt6NA>2l z@L?53eXM%YVbL>C*XB@ZfRy;~1-YHKO8{2E_28)(fK_q)fyy|e#zd>yU6@XhRVQWx z-DD#V2b{Wh0t9V*to8v4vdVM%>?{n2GR(<`kG{XsUXE8#zS-O<6IE6wWg#C5P7um| zC-KM%mgJ5oH38~VhT(p^czjxajVTVVl9*@3**JUlTFgxB6-6I~6@Znmsv;+kPGsjY4QdEE3(@*Vot8Ox|pgVb=Vwh63oNN~S~w%G?lG zb_sTLgYd{V*Jr?qvEbFa_UM@I(x(q|v2*eM?HPdaIXXN&xf>fBYx%?82zEoCy$$^w zjV`CPPfbnr_Uf?F0|j|nvRADkChgHRxTZ^ILdo5TKCP>6wEe9vU#%GT-Z|xWIIpF1 z*tYi)T*?in2-Rgh!M5^+mxeAy8#Pv-&YhP)BHGcCp)$<5U?yCa&eWfmFJA(UP?|RD z=e&nds@D|!y7`tM;GUOXe+AfCAKzc$X*y6Unh%&tV4%AHm2F2U|K?1TxVkdEfX2iNamQ+S{nzlA?#TQpvIWoENjIVvr|3fNQIO+_ju#sH%XOq_ zi?k?WFpBUtPnA!dIpCJ(dxMK!#EBn@M;(8JNOD^52KqMjXO z&m8;gC7}8%bK5h$(M-TUmdkRVShjwurV_14Eay~1qtV$k00IzNBD5Zb0?>zw;SbN2 zv~QRzi4*O9ys)qMFoSAzv+dnHk@fNzP=&ZqHmf!J3_g$eYsamj=Og!SB{s=kqvw0M{j!lQMwDF+Pji+mdF29`UgF(^iTJA|LtmbaT* zP|%;BTU$mJM=c(#8?CV7>zJTWXxo1F_Vy*Wg3T%gYym|h%&6>Own&%L zeLFCXAbDkGTUFnO(L+$Tv7WgzCmRKFy!T2v zX!VM^b&^%!ofPfolYN`dZx*mN%MhHGzN7le9?Jxxs&J&cRr)?b^Qm2HCv8}dpd%xC! ziPU@uf^wOc5|G&{F-z`zoXHrzjYkrp057Zit^+|22)lrWQu$0hFE4M8ofDKVEj?X4 zsYx&A0cgYm5yh(+KLqh?mFZkdT^)k-)VdJ11THFyna^JAVrr(W09=@9f#k!<-IoF; zhXe89ace8-yMBV2*W+ygar@Z;$w>8#JJ$8WZILtd_(dnvC8#2I`?Io<$+4ZYecr`J zaTg>Ww(g2rb2+=YeemBsT3+^&;cBYAGP$HDU;ZTOc8n9k7-)^c4PTJ}nG6qx_uhSe z`}1GWd>!kya;RIp&6aD)AAXPQ9(LI_?TfzmZg0xg+j6}zmh+SSqPP zJ`TRyVWZkpiXAmewzog2IV^ZhBF`cZomNiEpvLh6_Xvj6Rl_gEW1cIEU(-`RIkB)} z>wU|m{Xzstg3&;f-L1rVPKr`{6HeOvy|tA;heFS1mUGH)mkzw}07f`m4!-o}%fAUOmJ@XC_ z1w}=x9jdQfq6tE38hQjAeVqb-cXzI1eA@LezWto~C`(5rx6KVq8UYO5o?%qJeGqDw zdEGx@Gzrk2{TE?e;(`Nob?OzPZ*trrJc=Go2PGPr9Z&zrzP;#>K@~EVB;_QeJr6!^ z_y8Q5)&+f0eXejJRlvU<5I|W8-+f`1RJQ!?GUd4UHcnhmqQ&1JAQ3A*sSI4h5(23FAjwI=ii{u3Z5_P)n!vv|*=! z=o}i>Cp`LDJT}+U^2gf_u`suIu~}C1Mpyd zpurLH#LaC@`e<5OS2q?ww^rYz`v3_f9>o)0L^I7D$V+*i*g;1>*A~%A*8{cK*=bRS zN{b|P*~+0UDkv*0L4TCV)qq?};J-)9OgE@6$2L#@{4N9eMI9|fC1&28acO*R>l?y6 z_n;ooh@PwNSMR<7ETzo&%?T+{QPJv21Xc~5*kudI+$`pH4+1PkQ+w=k2eQubcF5k9 zCCM>?3g352X(N5WYwA2|N0}r4jeqSm`Z;T=FX-6xUPVyY*cew;RRw{YmF)lmn-7y# zG_(p9X@=Fr@By_sdAbRpvmo%ukmuzWi1uTYl3yv%m}w|L@((z8H8umn95wy;FBN6p z4_F)VmhXyqW{D;ZeYP*1_(vSqPp54mFWHGppA_ixL|qe-3KPQN_2-xxrZ1@}f4X2X z3Xmp}PHt|o;zA%H%?rRlLdt9axM2^&qX2em=_bb)BfE&ih#9C{)wLV<AK-#{*8<=#fj?O=J!!$KvMuj z{XNr>X+AnJK3?l0%J}<{1A9($%m%djgMNK|_ly7l&N?ud{*;4XzZRyan?`8>#yRmS z2h7|m@LzJX_c@LI^CZ_DNz&@@2RKWs+`B)k6GhhA1j(cI4l7-ewQ2t-8*!(3i8}q* zEg?;n@etgzrJ~TW<&zd8k7acdbaD(uYydJn6%GO%%V!F>GeHSqOxjYeF|Mv92)$a( zZh5L;ltXY$Fi>Y=H;1p9r;dHnrk~p>%gOvi7@Zf>e~NT@HS=qF`gzcwtWjFP(eK5* zZKf69M~X3;8Y(FrQ)wFdK-E|g0qg7Q?G3I1G*qt7a6NVvqqJRVym_bbr3->`+4p(E zT>JKzytJ8 zB6^Hv-f6=HVUnE1XjfFGPD$1NePXTN?X%uu3Gu0Zc}N7nxNe1b5uzk4zrC*F7r44k zg|bfECq2@oy#=Qns@pw$I!Cig9cdW7WYB^dqirR=pZs&QOZvgkcDLWVd17^Wb#=cb z$ZKmGRV6Avhh6}i_sxV_XKg^4&Rix~j82S=^=t~pbPy^4T!Dg877CeFnAduzVu z!{MBCgb^lWsZ=C1%hC9@TZ)di;e?=J%H_RNT@U{ded1!XWkC7j+T>DE&O+n5mOPyhJntB`PuEH9uAufZyFyI(mu~J_HIu^7@JPHu9^w@bq`Is4AVls}E z@unHnE&y9bNm7v}CUa{yY&X#v;qB1g=Igp*B+bChM9sKQ zzp3VR0^bzx+!)-d9{BZoGf_xw!ZAP`5>moax&6~2fJF0gG2DCQ_cTPHOGP}I{t9Ap zP`IZiGFm)k5Rz|nK7Ev~|E|*%`_0s`>?cD98Tx#6Cdk$#K$>Q)el=QN)m8tiNJkd}p0UHxz*E4U9NgET<4x84@N&vu+UCD6F zjJpgM4Dao*$Y*wyTj}a&w_|{qe464D6VoEs?lqCccHKsR zM4!~5FL{{^sjGW7zT$J-wtck7u}7q1j)tcR=TU- zh0NV+_VWSrpzcL+n{>HC1U!YgU;kQ(|J>GKgR=Ft$qGux-zuSIkmb4F?^uoz*i=u31Em;89fYN z8`D#qaqblGOsC*ZN>qi*<(yoYMO=bHgPTr-toqf9V;MJ`BnX!hktukw*H!y_gp7c< z+?lCQRYpfNht^VDJjz9r9DD)(7K@BSm~(|2PRVj(>orddYGgTnhsI<|ebh@XerD&h zWL+$#1{H>9)VkO!>m_lx0Ez7P%#}a&t4R`o{VoAwIfq+jn^d$j1#_@-nc4;r(+>%T7Df5v3<-5}2#i{$G_6patl*i{LXMvY+RX;80?B zO?T9|Eyt1F>dnSIQTv+ywdV(d;8okuJ5AE0)1KKv~HU3 z*qe1Z4~1Cekw*{u4vL<18Kw3aacdpw?&eq$2(VnL)w#?^;{gwj%JtC zIu||x8bXnaw|yDqZGAe2EsZ~nIKh{u(#F2_4#m|5Hm>$pt#*9fuL~#Lh%5TiP0Po2 z4A60fFG8b;ufdK+^E#*$R0}EmBpGJ~#MWF!`zAi@( z_|xqc{>}TAHLpsTJL!qP|KqWUjVhz z1VTtdkz->q8UA^@*nX@SxMK>ijVKMToO~v(eR54ox}R0$^Tz#Xr-!;D3O*ipwwXB( zGxL5B>S+4lN)JUxOd9EB>DQT`ge^(d5JC9Gt8b}e9#t~HG+U>h0F2n-!Rq?@z*&cy z$C6=1^HW6Hcyr*C7e0GXF@>C+&_ht1X1ZE%wVS@%*;A{IvK?S$ghc~Kb-wA(_~X=o z>s^ea-YC`0mAx!PH$iAvQ1nla@#Whwz%48;8!v}4jnO4Gxv)YxPj=yZjthP@S z>0b8Z23N}U47G@FLV=4DxLz*c$OwxZ23i5kzhW%XYG+_M)MWS5nTQ&wLLy_rTUa!LEpG5J2h{T zieND)Tx~!~N73#KEQp_aW(v@(XHwe$%96|Gielhb!uA1sICyMUW>Su>*TYsl{JEUJ3QGH1(cBMWyK5Uq+IQMZAwoiT{w*~&0p=(v83 zj_zq~ZC&>TZkk$BT}$6yy@(i*df2qKtw0D4UgBHqem60P+h>Y+DZANFB_Svw4&iX8IH-?4Wdo*uCqEd@SvzWvl`Vj^2$qCOKt3KtuQQGN=RRmV0+* z?wnV7dtcGp>ET_eKc;mnn9smH!=?w#gq^2qw~FH$1?SM6e7MaMPY~E+@;^kNli+oE z6RJ!4E6O|W(fH}@?beL7<1}952;s=oXI5?PsFn_TP z+(%N2a9GGwSuFmo@e!6#E;eV>`XI>pb_pJk@; zhzg3^u=>Bd2l^6tMK_{ZY=~B?`E7`a0DZ@1ut?kxmhbD+&nTLtWi`FnS{kK#^K78h zkG>>SyTR|KZ-EPA5!z5EVo;EdvdA@GYS|qyd=$tG(rzG`TUfXQ<)D!f(uu##@NpNB z(+^nGzWZ)vEUE(ERLqW&#O7Ju-oDlei1B(ZuI=y!8npHdMV#XH3uP`#^>@}O>)5pY zhP6-`;Okj;(ocTI7R_!D%Nuhf?X63KQ4^}3mZ;ar95#8oTJKYpYt?AH8NogF4M%jW zRv%Jpx7DZuzFcS$kXU`Q$`^rVVjo*xU7hyd-TmC(&n!1Yec68LRdSI0Qk;Ra_{ia0 zd2`9_(U>RMZZQ_6W>>kNYP>O-MzXaan)E*Vs}!9`VKj1RL5~>$%hqd^LER&ML*>E} zAFbp-P{v^5BZ`z4B{LnMi||$(&jGQ{rMiV>_o&(b4aqLvNqbNuyPs z;@zU_~tfJ@9F%(*Mn`AK+m+ql7hY@vZycGdpLT~V5F0>Mpv`{S7I_PnE*0dadGjY zCJN@v7?wrWKK*r14vg{nH>o}Pz`i(J%i@?DF=hXf1C#t_O8GCzNEzY}?EqKA)xkz< z*4m)nP{uIiX&*+7PMO$HNaw%KDEcX{4YqY-_lCk$m+9Hl_nA_qe^#B?;nY9ubf@qj z;L;?ko}g?K_UV3^bn)tL6-4OsoxvsT3B4ZkZhNhbdU#`=5LH-5?Uj6y)1t;TVKLv^ z1tKVD;Fw8VSE{UgJyp~Hxe#82L&Iw9iB$cMhFMFjf)4N*Uain^tV}+n{vDpeDG{19 z7JI-f1$tLrErJdUDvT>XjVgCv=typxx|#`unDw=&(9_G9%~n$6^D;ThXDV&>@Fc(- zW;${NVuWcg1N4T**_6aHWaw_!w@87m9~4b0X0reOfloS?#V)q)(me=`I=RA4T6kXy zh9x%63E*}jC>~-LFQB(@O6exY#~8mI6i(Kv3_E2|SRNa5{jcyyVAT0f4U<8zB4^PsK)S1_>~7S>U!OV$4?D0G(fh3ZO_Ql3mqrjd~pTrs}Q!m-nOdh1e++ z5jpH{!eT*PZkUzbyP7!sJ9~e8{0gWYfk$)V)e;{c&-E^HVC>b1S^OXh2i1k=HpU!0n{78**U(_!?Mf#4;OtQ=-WmM)&p_DwN0msH zhf0NfgHPH2<_H4=3Yyy5{GFX|L?YkIr^c9mEEVz$I6~q7skl<(h0$jtuPTO)ofHEgPFS%mLb{i`v(s1kGm-`;^Y|pY7Z#bz$dO z>z%t|#h*v|YB}(cR6IWL0Iq@kGiI~Su3+B&V2_&XcP{hpIq;&D=i_AMluibk)^A3X)YDdrNeeaz59x_FLNI}lMz^Hf-bsj#Pgp`}EO-~m93IASzvv(3 z!d(#>MA&X=7vx)|0Eg82F|{qR&C9r7lD8+Ob+g{rXP{^4#Z8z-Zc5JyQKJe9Q-`zX zQ4+Q0OZGO4x@{`qfw-$&q<;Zlp|KGwt+^~|-zt{0yN>eE)Yt`uS)i8;A;#IyGlW?G z!!I8#%3N`Ktwz*k37jThmGJgY=h*vIR3CogL)CWO@Y0r+7C3t~dvq=r?q>_F7Z2tR z@mY7BGOeWVFrbK2pZh#wN!IOD2Li2muxYXN*9?)xC8zFs#h;JWYeNMLD+&{n@A^L2 z(OCK#WP(NXlui3fHna_F4mZ&WO}tbXOHeAIMbx|B-SKA5 zX|SmiTVi}o=$6|O%0ej6hukbrSnKu@lg_&#Yfl;0E_a1HGOje`cA(o+q?3HmMJUJ2 z5|d8f+tnppY@Q2BWV+jQ*qt$rVKfMm@iSOzlAZ3Jdouf1rud`j>Cl5~jYnE0vPT3! zeUZz&?cM}L=#;ouC58$TI0MDLe_b3NRkjHjRD%!URcxrm1xJ2r@M^g_5gIYQXXccx zKpt}=qESz{LmaobPIow5m7mb9Nxze;kpHUn!9OD$HESy%KE$hB!t5M2t=2T8JoNVA zYJmhVmw;ILY_AFI5WbhkEV!NgjgV&^@!j%~+9ld%d4!QpxAJQ~|snBpy zo?_*MPmF1N$egmSd4!M8va5*3kv9SwP+!H&X>T6zFPV_kvY9Q{#kPA!+|LiiZ}K)@ zo$u16O$I6a#_<+HnljDq^1jfa>+$~A>z=U%))cAKWb|y>a(N@^s(J&@d$FZy)Zcj8 z0JCpYbxVs4G#~7?cW4_xa^39&d`tyn+{(OBd46rtA=qjUPMqH##1zL?mzD;8{`~hr z48PN80-vvZhohs!F7hOd&p5AAEHRV$zR^vF1Z$+uvp{U2zqt77wV;LC4=1q*Z-9Hrrc}1jn>iO`uB9yIX^rtU7W%f=XQde+ zKtom9p=?Lwlxrydu^J;A!i95j4+GS?fhXKxh!2=pSAa(jXcHI8C7g75)t|^Z5nOn* z)_@nu1RPIZUOW_db+xtUXJP4X#`ye^ zfbZ;rCghwlHMB9+-SG|YV}N*D-eT$JiCO_YX&`popK7}s06Y5n*0qCIpv0+$x>jUj6utGrU&*aZCNmHyULU{(3-2N-thM%W_5vC2ObjQ% z8p^wcH=2cng%!t66zE4X24tM50A1z$LL1olnET*2$7?Rt(=&5(wso_&cU^Fbpu(y( z&xIZGlQ5;t;f**IroMGE<=B*r#uL4ui@Dvs?&eg2PjZxTuT=9!>dykCP{>y$wdpvT zNPaopT#!0Z)lKcF^MXv~K@QteFT@#Uk|Uh&#S4`CwRUbI*L+bg!k(on0TPJGN5{eL z74@X=v=Tlmjr#?vw8=c3h?H6TEUgMXn~q@C{NwbjH%EazM(hBeGaH6IHP4G=Mez^+c zDDd*8|99Dc;6~5=?>+Y1uhSg-xG5qZ2GQeq|T^DJQBaH;e0++j;%TmQ--}KeV5UKj}tocn+9{b0=*~ z1CM~x?~;{ms4`<%(VM)fSpb1MQc|;r&+h}lgQUg89$W<%v6Be4eJj%Io7vJ2(%%rA zSi_p~`%8Cs)#qMoo-}e-pFgnyrEnNAE0*CtYZ^F|(Dq%BR@x7}_+-a&E&eTI@!3JY|(<-G(e+|%isx>Mkpl5R8V;KI2 zE=7%>+%Zx~c^R~}ZR!my#u(mmYPwT(^Ob3p$Tno=_5kmW$Q4EKPn~c{u;$;ipEChL z4^8Xz+}st|V7Z#RKOA{iJ==&juik3hFV;6^qmdGS!uast)2bd*Cc$dQ$tk;?^b-ny zUt_2d$u`aBT$TnKgGw!6pj`tY65zVv4itbD7D})T7mvMD2uVwOB;uLre|bFZHV=|F zA$!1Cd6E0I+@Q3My+M?9qEyIttHy#$E%mbtf@Y=YX{J`uV}M0o|G4 z@Z3XR$sxq;?Pt)2ygXhTi=WD^_)_FU7R)DMI!&2dpVke-JQ zLSp9$+WdMDYd-|uFo&)Od-xIaANY-L+hVFx`corm$84N*$m3xO+#7qreb+zk*GXzX zJOsARPLLe`{Ao<}dShcFT1*-gGgI?d>iM$8hLRT}x`D7kEi`c=@Dl@1!}Dv8q!#_~ z+a8z3GO>T6llQUoevr-VF^8kzPhqAiX%rJP6y8L=;k{&at0P=YI<0P6m=iG3et6ns zb+veZ(?A*w(FwqdKoVK~5zq!v3E-9D#3n&y%95}O2{Ets?63NB&a~(&I6Xgq8{T%& z4|0SNOCj$c&v>6)wZW$Oj03ro9&i_*U;2yD^pPhz>_-625L4U!J zCUy8)5G5%L=YS|SieY3u zBh0`6^!j#*Y@~vA{9)=yl~GrC!}RW{<-0^!$^)wH--82Dd5kFc4+8lAzot@ReD)(i zeI(yko^9$&B%1`Qncagq^t_RhQ(O%|c2oB=3?%3uWlLB#2IMW+tUDRtpG3GZM6FD5 z$ylvLt%prmi1xeNs9aJQO`c#wP&Sc82#~3U1MJ$7gWJjehOdCbHHQ+z|5Q8Q>aCQ} z5GdyR_{YJZsf@>6`cu9@b_!HQpxv^sd8EP;0OOqCM}1i-lE`Nhl3uN$y3ZU5kLrmd z0t5{G2KIiH$f=u2e}hb9Rw`B;>L~W0HU^-??1drOA}d4M$v!-n z@j5xZe(JbDMUmvz?*qyX{ulp-%CvS44+kEKd>hnEKj6Iwxy-rD)(bi|rG?L#kQG%0 z66ZXnlZf>mO9MqR7q|+bP^c;c>ZBk(=#ZO6_UrEM&s;wQ1m@&ck^})sqV)hINo}5p-{(vo zDz*k|VgQffL{&o099Q&1Wq!io=xxpoCz`f=qKddwXrkv*dRy=(V#s!!tka`SBA)5L z>160Zs@P8fpuGZgLK*=UyS=9vzs*Lg!(W6X`>-&g|c>+27F z{cUW3n|b9dewsC$m`;XBwqljQg>cnFddygF%I$%6c+9KG$UO8%S_5ktU2TgPa`K(i zo1BO(GOU!@{udW6Kmefqa%J3T9inzJQX+&umEwJP@usIyzn)t*Evh3`suFT3`N?5O^{8l_?3s_*_Ng z-8CQU+Jy-qx}F?SQG*?hpqS#mQV+IupUb(OQy}Gf(N7coTI00A#_1XEr-1Z*u=C&; zTQ8~1C)rJ><-aF-q((*ri-py?Rrn^Nr-tM*+ozVEbf}rEX})5LGp`2A@xg-knK2vv zBWdqsn)SApSzo))>s=lEs*IcF?y_5PvR)<%l8xV zJ8sAx(;GiZ!FpCD&7kL2mHUa!15l?J6wwq*n`yo|k+8)5N42vPji7sAS8;G~z}Yi+ zbH9FFrvA<@fggfXTp{kF>EO=T#RaTg<+jD>T+2+koC!LRswiw%t50HTnOZYvo9xwBf4nUE z_UhKH&^yK2R^YmCgL2 zV9c`4K@#oknlzmh82ZSfUmLdhz38uD(OXOJ2EF97#`Kk)s46%AT9`w!i8{M<-gvCt zb`or4VjM16Wg@HI7D0-ppZa4q12A465~X(?z>wM16-2;L@gdGsZQCcG?htb}&5P0XA}A(EF8_>a=&_xH5*0gFg^ zOr_u&KxM8#u2CjzH4&QIt+!+d5G z28BpeU#uC}3ma3zbrj{X6X>KanJ6zv$dI}rD0fck>3(-qBtZ+-&|2SqERb&rMfJyl zeaW1MyrN%1FKG=Z^r$RkmkCe*;F~Jpqv@Twvt-hEI%XrscH<0VP}vD>fQQzk%Fo4C z<;*1g#b%bykRJ5y30I~6HD(%Qoe+4>`#gN^Saq8!m7Nx1h2suh<5`=Q% z0yaINr;3#lfWBI@8G>hv?8pBGQ;L-9qpB3@Zo*pERK@G^SrM_5)PdQ$wcTA?Au0ii zNdCrskSQy~t)XiJWf|U_=g~NuuKF12BaS!z--=^;Wg4&IDvExE|yeY!VU@5xGe1AC;vrP-BqSG)YOYtCYS6 zHfnwe+CK?!2*6pQ`it5ubSCMKkB?8WCtz>K)eESE8r@Dw&5Y-Cn8>rWS<^i#sneE3fzzUXr8z-OT9sCeCDE$groh{r25{h)6oH;TuOY2Q_dUZ5+Eo=c!6peke0s| zEty%TlhOIgyX;2~=(vC+WEoldZMeJ}oMe7J41>1>ecfkLxh<@q>_?fylL1cl{LpfDig4S=@rmFzXUAi#tvi{`ahx94hz35EJ9j2 z!%b zWb$sjkhVMbKRE3eAK9255MuTtC{lkv5MhA80eG;3f5HV_khsu0<-J-Yx4e!n5n7p9 z$XLT=GoK1erb|*&pldjSHm+=uE1Q|V?GSVbieK;L#j2Se(dMb`lrPdmw)7WJ$EoN` z^m2Z^?vZpWOoX#x%#tZ=cv(d8X?>Av>zf)WTYp8TF2?VbBe>wdOK^g)IO$KpZM&U- z&X`&i^QUJD%!;ZTlp>z!xFx+FvAzZJxNnP#=f0SaQD&u3AnhL^j^+{e(-R59rA~7k z!ziuDC`KrioE&~s(PJ$wfKQElqL>70J@9Cn*QMbVb$rd>VSuI4o1uwAyAZe8fCy#X zLO?S6IBfL6#bddK5RLQYE5kA(>PbM&2bKy*is8)8!qAh6pYIp!v%Cz(5B$BIrBMR- zq_loBYAT^=TVTzbP+_zq<0DZEVTdiwiXG6>UdxUwO#i%!&dK4KW!Z6xV;nsO(wfQJ zPF8#4$1Itmi$?}+h0`y5{$9u3r@^ILIV(0AlhsUM1`%1@4|UWrC%sOtQNAPJ`=hF(_vf9CXz@QP00Q>9uV?frIao*$D%zh4cgA7i!@#;uRtK;jM0W!fL1+!i09R1#bJJLs5uNas8k`$ycr zMJBAm?MM~JTdyac{j!#;D~wn{ep>7rfzgcCiYzgVB=N1t-<9ZK_wU{rT=FoFIjjR- zdU_nHE*9MVEGrL#2A_O7_7FyUC9nM zEmogn0$1_*xtlp#MkXd&x#y6LiKld01J-B7S=)&<^!C~W;Mr^=Uz2LvPZ>QTZv)B z+i8^mVER69WZbROjT$f`K#}kA&P<3Z(71kgzz533NVTzhSXqdU6$F;vLkx)Ajyjjh zeZRZ=iYIC;K;Z?0%*)gYDePV1AYrm^djdKyN-ZShF#$JW#)gLWtx{DzNoJSbUb^(Q z!Km5U@3BEOAMsyKrK3`uguj27BSt?Cz}ey9%5mS=Js-5nKpv+J!>p$&ekqNSHzJpG z`VskWq2Cvx*GH@^pZdOL8SXu?h73kHrQ@A^bTKONy19}rl7opJAs2S5OfDz*6eKlP zjOg9|-Ww{LDrNU3*UHfTTX)nE9h^B;X;(6B3zT&5a?mkRi2IpsnDhw&qQ|MTL{8c< z+_N)3%bL-=yiOq+6tqf>E_LjusOhY)0$G23DyIDM^f?KVQgJuP>)He2Vg9Y%uQ+~# zJ)T;F82-KvQ7%R%@u9jP^MWfhKdRT`Ex=+^;0BUD*TU_L!MA-lz~^mi`6AUr6$mbX z2bw2^-J6$~I92P1PS!#jr-Z)*;CbN8pfvY=l|(JIKBFssLPFgGA`E-TUdeB#kaqM# zg)c8(JgIh9W3=?}&^|>flh6pFw(-sNeG++mK{i^_XD=4vhBYouXTuJB3jj{6#G*82 zf-5ccB~E%1n91LkiLgaErmqM(#8p@8FCGwgS=q%91mGI+oBHWlxQ6!$MXlohL#Dm* zt!?le*HTHe>bj|~vm?BQ^B!?bU@yj3Q%?hkN>v|wYdd?$mz zhb=wxQUBW*lv$f=>?sNgdZLRvLBm#CCkY6)5suWrSHnT;-*|F$jFm_q_<;uo6Ts*f z>*{J-eJ*0Qu0*tE);%);O&~SepQURhMTD@T6symryfglUyNG8tBzEJ{rle9&y-K=T zMQLo5FY;UHN_}vS3(zn&*Vh9uddu&}Y~>-`WO6e*z7;#*RE-ql49MnLw@Bn}6E*!) zq0b-1YaY3>{NVtxp+$6ixL0?#l8?d>1pLy>0Fh?1=Tx_^G!atn7Pe6|`A>nqD`7b0 ziU0N|gnJ-C#ef-F$#X3}<%gb|QnwSoG5~dR;}OXItY#m8+j4D|7%Y?G;ys}_y~b!= zBw0o_0t*#_rH1^lU0v->Mq5|^c8c`7r(E)}v8imyDA!GR7PXSg{LXRh5=xp}YoQy$ zq=yQfe4eb0SLX$(sO&KAHTgbo?``ne%N?|fA6$X#{lISpP4diak-oi9G-h|G8xrT- zI`?;Teip!+t=&+()R;g(K5wn9Aq_4z;I~K_NdY|@o2~28!DGPx1X4?jrOSfDQXW-G znUu)nKdp?)&Bcod>*wzgh#8%ahhio{+<;qcmD;HCj}R1Rw&!}`d||oaKx$N+7xOu= zb?nD^K>_ukOFM=;gc2M@n2r_h{2GePC0oQ+RD$IZx_pBkemB7WHq_KqwQ%f^ zd_GaEfEd+8(S@R^<%BguoS5PV{IaIqmpo0BI|iXiz+TMZ&1QhF;fcqZs8%2sYIt$q zHSPcSxH5-dZ)Jfsg(O}pQLld;3SiDJ`lY|?^uga*1kOH|1`>dlTBTtPIW1d&uK+zc zby25>(FEO2d=!y=amldDnS2>mnlI%{l$5Ur%$u+|KCK$0Jt0 zl#X0nlo;x}HkHtt01YPi0TE0o;SPV01u^FClK0Ma+sOUzshj>L^mdl^Bkz@SyS$cz z{kjMeg{0}t@z0Cem_I>FI1MmV>>!?*A;yKRHM%{Bn+G>`JxKiQ*CR_~j&y*y@ux_H zRm4lom-x087EI8jqQ4o-p+)Fedr@+;Pyuwv2?R$75PL7^aGxf*ydIk>`Lbem=p!4i^qeT?17TCZRFW7h5oGvHMPvjl>?8k%%P&<>z}Z z0pEl94^jds(~t{;mc7X!t4f#hO;d*d`Nm`x=Uy&OJI0f2h*CQ$)zi+c)svgen~;kT z!|67!jb?8tVH~>W+;E*Q#hT!QGao=8L{yv5A$?mhOr;y1z@!Nzl=%c;133DSkPjtd zi6-odE5v&;gV*`w0@-#V;!;EE9S1vD@;vf(kUeW5Ud?$5XzlH3Niscc+vRTuz6(Vf z4lA;HyV<^QWY#Xkw}jH0=G)8YS$}pY7DOalyUask3B@zf&Q0Auk<@~ev@_ga^ser{ z=i>E?k{XLW;YOGK9aYUAJ;%D2kl$a+cQj&&VcJei71bvi;Oo1$s;H}~saX>$UlAf= z2hM$SCeJ_jq{ID6zXuUxr3Cf@*Z|Im^xX$~gC0EnGjP^2uHJsjm zOpD6HXOX$zjG-wth~U9@J}||um9JxLST=h*_5?cOf7^fWW%=l1iU=p=A%vPh^Gh5% z(#(>8a6DQCF0-s}h9`f@d9FZ*YCwF{pz|1K;pt{kUrv1FeB z6DGqAB(O?-J2VIvRLH5P`PcaG=g;U!^y@3Pa0DFzif&B;0fX}nc}7}qdVV> z-*0U$Z_+ONUsxrCQ*BN@6m?AqZ>ktrcou@OI1kx}l71=kBt&^7 ziuYLTIc5Z$NJ?*Qa!V~41x9RG6!QK=M*?#KVo|LX)*pB%mp8!0uFPA-UZu#A1Jtv7 zXW^u58c-D2C%9!RaPahqA{Vy={mChq)U6N;ZwB16i440#Wg6m<^MB`ApNAAOSH%Ps zQA`%L&A~X2+57{YyXM5suDw-HKPRUn>}1Q2`Ht9K(nh5}N^@3ZU|Z%XSl{?}eBaIY z>y-7TXxTe;LpY-H*$ADacXVj}D#O9pF8&$L6){{|FN2q0Oe z3o^!^SrXaWm#Zj(*+XSzWuQ?M&kb`cM(soJ+DEk6_;kBT*xj=;^xOU(o4QK$=M^I><^kk4;Ar z`8(D8(>p&4G-0c(o245ybB3@!?8ZCZvazB`J^*t~`%po`@78bOz_B(DP9>pl)zxG0xxY`S` zl0Vtp7roa-W(<8ZPu2Fe|GZ#<{z!2q(Sp6DZbyI5UlYE+i-iB%#78SV$V!MA1D}e} zyoI-iPr0~Ud2HNG->NK zR$5N{tMfy9?bTHnp$j2{PEth9vE$6{ecvHg9@8nC-1fFmu*!YLT=Hkp?QZ{<@Fg_$ zuy{tV$)3ZqB^}1Pouj$nG@h8O8SU|rq|aQ~d?kE$nNJumEW1+#ofk~yvGk+c5PK*D zn^7z6^}|EAB(bvEsLJbS8}74D-wpmi8T8knIpfL znP|bRX`=|h;X_j2dq=cti!$4jZX1Jpmv9&AV(=x`m``b*4`@kD7tg`?iilDm*T<1n z2aGFiOE@xkYn-URvv;aC@LcD6ctQ}(*o8d_1fh?727YV^VgDn zL}>7#?Vh}k?_uZnI>ShdmchF)clN?&3YOtfH1?DJJ1!cPK83)-vYKi@ptJS%2K-A%Iw0aEwHTEE4oK&}LpKh9 zCkP`AEKiOMK4@fDNR^F6ahhnAI!x1$Hu=4&OR?b%cG(i83P=$4>waXtXf!#S2~hhUs?cSr77lZH>Re4H+3%~oX5DNmHYag@+-S9 z(E#(9BC1GgOwU+f38> zx-Lnk7wXE$e&iy>fN@joyO_?Rl%pcd9Z%LVaf_jS9sEtuJ}+#}Qq#Q`RYwXVNb82{ z@}I4xKs$ew6lr(*$^amWsG;;5R+;H!e+H#n{PS)tXOx16uH)Wd6Z%x$Jinw&A|I=HB! zyZaYn*1$ll<+YBl3tNFL1|n3A>3Lx;l)U-_GZO^i>bIJ?`oFbzxpxV)ux;KpsxKS; z%Ke>R*>6$e30Re5TV&CjdF42d8Pp~*ucE+o7Epm5dx~k#FQvv!@`$A}8yq zW58(np5F?}7z{6dr*+pMOAL$6zs8;7!`qsg_+&NFI$kd4GGcFzjF*kKe-snMXWL^O zHDC+T(8w_41*rJehb|`m?l}mg;dL)_-V@c@HAQLQhAbQ%0dF|R8`k65!l?E_R-N4) zo>Me}&Tu2Vr5nr@WZi+RXK{rSbT#CVg(0S$p(>?QTPf)o4Zl*J;^p>Ng+B`W6E?sjwVYf?J>&pJq>aN5bo(6x%Tg zK77H*fm_@Ap^>cHoMGj^>gg&XkUiJ%`B?G3eW(HhbB&;pVuE4P z5prH++bdNpzZi#+y|nw^t$xqa(qB|J&T(-wN7M`>{kjRWK zo=y40(Jxt_08%sko6zhz>4(vilLwEbF~1;IZ=y$NJH=Kh+MaWp=_TJ9Cz>?~*vUu- z8ZtrE+@7e@y1h-@9PQj3H02ufbGW~Iwem{Cf#T2mEBde?9x`0NtE~dC)sCvg5_i4H zV^=5ANrgWh?gNqvQ?dL2{vg#eVBW5Kbz28%3}-7gdqG;f$Y~KoLR3+rGnm0 zqCR2v%h<)d?D!}u zTvmDBnaLjU5FmS&GlVAP&wMC#T6ka-9iV7wTACOzsg)LoPnU~T+B-xikT`j{eQ7aK z*>8+*yW6=x<@GW6{rCF(fO?|_wSDR_Yus9TGEBHld-iQMcZjTdHVIN{pVD_RYO~ML zKfBv8jgS{6ei0aJC8I=Ap3%1`))}->YsMpLN;I`-nto1P^BN(i=caimoBQqfUGMy^ zFf_Qddr(V)IiUz2K8jXp8=;4IHiUrrjpN{PIxHoLezGg5TUa^IVLbiW_6so`VzivU zz3ZAGgR=-xFFSaSU%v+5i~EiH8%YPgy8&MIj=MX*f-%olS?#&7ACM#p7}|4YK>&T% z92yt&cE4uf=99;eypWWU`{fpeK8)mE?E5AcDNSlcWY+Ipt79E~u$r)vVHzcC0C&v0 za@S+%t0mM3-jd3@v338(on3*`^()5U?flX4pLc8h$$SGM82}<85%AM!={lZXf#z0g z=OYN8YCgJX5wZ8br?RoKhNY0A>eCfJer`GV?#fEjEJ!Dj#ocFvNJ@*^E0T3u;(n?9 zc)kf_xQ2JMna2D;DGwA0ng1e*>ZCG$!=Mrkrn4^6)jr&Y_OaVds-NQhrfIjHf0ZiG zLeZL{1?i@a;I5RM=3wrkW0l&Z^Q-D_n%XYTn#=U;*GB%1uHBy6p0yTH34V|&I?8i}_fQepkv;@< zJ4$Nxoh_u8S-x(YyVjlzM-&a}MlnI0<)^J!7*#0!_Zg;u@Z#|4&)npoPiJA@pCTB? zUsmt?33z5MHr%>~a?an@rI)!cWD>>aEirSfycwE))sE7AT%fHtF z2GMC)YMLQ*dZklzSuMz?SWQ!t#T6}9s>9>MY4d3D1}+LB{k@i`*}YQ^q_7PQ0G%-j zrVs}@3Hm81tbk+>nRTDpd*CAt4(?xGPCM2gYW-r!ea03>{OMR!_Ge$*_aYkE!4I|; zDRhAtyUU|lc-jcj(#dBE$sZQ}n3@}FHiT++sq8CZ-8QF8+<%w5OaW&)=l$`$88nNu zMJsco#rxcTHIIEdWp}%SC*tg1K8K1o5Rir@1t94g zwP9N-x*Dyl|01vsozzVY?(5UCKdWHjLWa-m%QvmFaG|nIYc-r|546qHcu@NWv9_4m z#jyX(ADM1F(6Cp*%&|%(SU&Rs1}$J=0HZ{Q5C}vyy@G*3Hkc06b`D>~%}k~k0D!jRIk|y}ll$50lC2Y#aMri6nMnmzgiP!o(QT2$vt6>l zA2Nks_xEh^-A6W76g`8q82qPbZc&iA9;h z7mOq3>z01IiBGu+3JS*4V|4-;M)J7Gw^6AuFbLL`hkG<3C`XIPEAzl<0krx$1Go{_ zW>P4Ykoj$-E1x#wg0l~MBBEc?3ey4?nbo!vz&|?6c;qeifHSmDMNEw`uLr5ny_>jL ze*Lz;YL=(pCH@t4hG=h?&O1iLjy&?q<4Zrdxb}Cy6A(5JPz`HVm6B=T-(x2aP@*-Vf5gy>c%%eA7cG|w}AJBdS-aI1FQ_0-+a=M%_~36NJxqFk1F`Z^dR zV7s4h|J4mF+NY-F*Gi3Wx}1;?t3x185Cxf+@5JGQMa*H$VZbD%#0Vw|n`SmU_3{na z1Ht=gST0xsVl-uLJ27DILwlPf|D=rhJ&_eOpFu%1Y$gC__`c%QqPln|TWf7XKou_6 zmbzTEZ%D&t?UZntB`=QEwT8#kaes7ww&!twx92Z>@NTvH@b1=o%D0N-Js4v6HMZQw z&=GZw5hND4=qEpSs^S~h?C0ITXWqs>iw>3)iW;-80}}W0opi)gyjF9=YuQE8JG)T2 z66~)F^w3h>SB0>$bQp=jT5!I~*t^H+FCU#O{%!NzY2Mq+D4-U2H*~5d=&epfBm;y@ z${KMjL+xj0Diu>~{iEjqFx$bU*S*-Fh{BRdwl$PQZ_6_hp4;X5e8{83bW|A~Cvsb) zzi9c{6%|ESsWcBymrDgjg7>ONxdb=zv!^I;mhwkb{^%u)uzu!`(lfc4DgydbS1@Rc0#L-(ZQc@xca=Vx452o)c#rG< z^``xepRWmX$#2MMz~rAZ^~TpIe-vw!ax~)~!?M3e8R(NUNMQ#~v>Gr(eE4&}zk47N zC0ToVCh2g8$e;H4hGFlEr;x8E1vs+3-%TUL)7~OE`uPb^;Nf-J3Q#&MG!GzrxHc>F zcVfWDOCt--Wrsqa4MF2Joow+!?4haB^8NxPTAWO`(?1m9KR%X~j3GIwQbj>v!Pt?QsySc&NfyB;96uuJ zjKg@$ST58?DV)EVR`*pO7nGPDmrJy_{wtP2mkZy?Wk1oL`3w`ru_pWyl_N`meM5MV zR`rUg`sNEU#(>2s2rG+pc(z&DQZ^mh7_)DRC_{~UANo$Y+_D<^C?mzCzA zZJDC`+~7DSjZNRbJFR@Lhy&u6EUY0#Kl+Y|1SRH~kJ>I!?h5XOXR?r*?;RHXFC-;H z-XDn_45SHap1{9SwpO08WTF!q(efRp|2 zg5S&=PcI^x(~E;Ju zO9SDE?d%{z4D)8<(GQTk)nkksiKT41TvsDime|YItwY?Hc-)-_Ff*IJI!iT7@$#7G|6ZlWImyQE8o_!C?Dj~j?ZQha$d=1Im@)@(NQ6QgD$nh9r@j4+ z?}O-Y;qo~PKmZ5J==+@Jl}R||24=9I3Mh@?f{7z-@24l}KpZ;teB;0@1)$gun$*oS z?`%b-9$Aj}ovhu%?59i2QFRb&9zPUQ>ph~DQf5YHBE4%4v(0B>v|!9P>jmM8t-~+v zk=?`2vFoC@Q_uUvk-1ZpT_DXo>UQav~Yg{zXnt z`4xlVYF|$z?H18cYgM=}rZ)E!+E;udQ1pzO=cy>N3gq}+v~V|lQ&oaMiqf);*{>?a zjzm2+MEI90>BLpL-qhBq0HBi*pmzQX2y0Ag(bQI(G7DpKj1sH3uC5vV0VU$7R1Z=$ zSC;wF@41(pAv(ai;v#zbSZro4|KitY?}*T55uWc8{&jjjI8Tw5`x@y=%NqjxAFudd z-*;9>c3eS=^!(r!!td1|^_LK{jaEgz6;sTclLoWLsQ_Xozeta*xUf)=rvJ{41q!u9OScA@mjURp0`kCE^f*@XiGQx@F#58VcC|g74edk8*)i| z&I@Qdi6RZXS!K|)Hq*g#7n4MVu}-%BS2jfd9F46 z)w`20Kpl}FkB)Z|+_MkTO?|(@14h|ZkMy`wfr(f?=jDXJB_C9xHj?|RuVe+?U*OG` zy<)M+vKqYpjD^BwhR9+_SBhwxM#UE}-LTR0jg4S=N{0&@EU|`8e_5NKx$fHsW~e>{ z#RwG@#CI2$74Ba?bKi9rT+rU3xI4RAOXs+YcPn%JbxD_)^7wOb(<6e_xan^fWp^%1 zl3@nqd2}$(e41B}S$I6!*!doJu)N7Y@boJViTd0xxamW!n(DFbS$qXV(`xP8A{SfT z7VcPX#uR*>P+{1P_zuzxr?I@x=|n+O`u?h8+&Eu?N_~Ivv`s`aF~fDA6R9NZ zoC;zgMJt4EYgAE;?;h|%CZ3r>w%~5-=V5jVpIZ*)nog#1FZJxXuQm0|^fYi(iwX

~EPzcvW}n_J_#Y2Bx{u^YlL&t;)jh8g-m%ufNqtK_m(85@y&^DJTJ>VILE z-F)Kky>ZcKP-HxPbhm!vz;PVD#9YtFO>Y6I31D0rK(&x1 z4y`KxKbp=utjYF&*rP*`Zls0LAtekXq)TGJ=5%RY0cjEG z-|hE3UJm~kw(Gv`Po8IkU6EMnhp8rtFXxLx$b%`HhBP#KU{ais8toPO5tp%znWob= z%_HRRucsy)zD6p@yyiK*dJwT(l9C$WT@ua#bT2m4FKBt0C+cfrE3SUf5H-z=4MJ-ST=<` zfcWdZ&-~L(;D74*`;6$rtGiFsi^%j5JPY)Q(6^>mxHC%BoMq{7C8S;H zsxu}Lg8?(Q>7o!*%_jKEHO0?K&e|bsrBHJN1ta6n%36Lo!zC=>7FH>nD(dGNQ$hQ!vkt%86Bz+) zG$`A0yZq(8FTbc*bktXG)8?f%U}ecI!#3n*ssJcZR7UI!iqze$X!>Nm_-@kgv*8w) z+r4sk#5neND)RRrq1$uMJ3Z&kJ8UytY}`{KjgqCt7B<*e zU-$6ybB8bxMq|Jyd2dz#B5jb{sr`%4Kc){Z!iLq_FIv0FJ`erM5=p-ZJso^saSB?8 zmIdnrpCRAD1y{FJ>&UKpg+F^-*{Ge(QjzN(9UZ`|&9Ked{Vrmhr;pBAyTS=GGxDYkxpFrwI<{wv&b8ng;F9#86+GU(Z(jibdwzb2E;|nEKQ>nlV$yvS zZW_usx0W|L(rw#AjVUshx5pKIXZ512#$B83^t5S8+OS*auCKr9`h&>R6#(7X=}7A4 z%c?Yuz?rgo5FhoEmS*;R4wKBLrCwnlQ!$_W@73|VE-l_$1!QwG_Bj%t>IzNuS~Cpn06dxjZ?^8B38WzO)fFzxKghOnd6Y?{3Eo5jdK=7I{F*+G8UuBsISD zPf;qBfC3iH`F!gR?}W{+Ze+<#6I%q zFE7@-w3Pc9NWvfg5ETGV)BS;x^i3#Q2bu&l8q}wdfQIcII_BeH`-shBabo9e>ZJA)Jry~~x-~f@nRpHvyeI2^)|~6uK=nts=(Bs_^M7(jNVYn) zH6VECz8-a-5`GpRH68aS!oBp{(1Fk)kl$*IxnJ3{CjY3cwrGF5RN#<~F@ynxHlDl? zFLeu@oj`cI4g^-F=}^7CeqbVmSl2S06Vq7ptXD*}@Sr>izMFue!h+0cCo47#P{Qw? z`xa9CGRyFN=EawIAl5}zwdbJep2DW}cT*16EQ2ZRq0qq4!O9`H;nVQ9iQ0!53*o=F zvE7ff!G!*yjA4p1!g{qm&(LINgoPzlvky$;at^1`i3kc8zXrHAw>@>4GfJ&dsLd(3 zRzC-KjzD9|sc*Li9Z2((>@0n!YBB>1jepWr8$~b1D>h8+M_J0~?5eCz58kz?NKsrZpO5w z7^JqfC$yF21OD`9aZ4VDCO(sv-w-=km8AnPVWC*XE7YAC6roLP=d#-ABU1nQm$cd$ zSYNeznl3;u!=9m%`Z)ort>D*GS0w$_1zWAY7QwS;&suA4|9+dm_;fGYMi;Vjb0{nD zcmDBj^aCS^2^|}VadjpCQuGkxhGI{&*90dP^C-^r`5$p=*FLSZ6{-2D*{75lG)a(| zqGX#+H-z-cNTB``C5~dtYMh+a{9=7Ba@o_#Hh%H7kf`v7L-W!Rz*7bO0UOl<1dP{1 zz-|cHx&7ANWPz)=HwT$6F4;U~F_t&zL?T4A^CodQb}{{aFi-SkvN*n&f6<3Oce{xp|fYL4I6BTj}C*6H3C)n)##cjU9_qG}zGU0WHenBr6 zgU2-NEsTopAy!@0D8K^oeAE+t8!@WK%cUIT6NdTLwGNF^_e z^QkVL=TQS|JMAZ3O8{2h-RfhM=qo!qK*i+Z>f3pIJ9gWBeIsVS$@8QhGT@2X@mcYD zVt2a-mwGLodeUhWqalp)*-x~U>!%spW!z4gUV*%)8W`lULvC&ugKR3j+g2WRJMJQr ziZ2`w%EmGb+a4V%eE|M#>D&^ay2SvvJ3tlrgK;#lwB}ZGVSo6VB$5+&I2sV^stvT- zPNQd0m)Kjk3F}|C&Lu1PfVrL^q0ftP?#!S0q|hyt6*!vWi^#P$+vKOzYs&qPNnQh^ z!Vi{xQ#3nm%)`^F2HUF_3X&N*SimjG5b6TVq{T@D=5J_J@xCLeRd*e0cD)LeLKw}j zu9l-kzm+!4sa{sooUy0Y{vk;AYc98Y^`|=lT-N+I21(khG$Iv(bf7ncl(3m4LVfqa z*)^W1b>=VTRt@xXOv%MCEhMaC9*f>9`C9qXQ6jrx3>Af-;2jQrEMDt&Xjw8gmIN?+ zxacAORCwNN7@5GTPQ-6}Is)%8n0Dh!_w8$6k?Rxo5K;a3LI*Qf>aNrq+4Dbox0lYwj|=N>G3^LqLSe~zM=*G=_or2YNXAf}_-6vn+y&bAm2c6)sf z?83G@4E&cLXQQvESs1+Qvx`8yzm}R`YB<+DJUF&-4*igo$w%- z^Wne*^=v<@{{mg;5T9@hNvBV zi9>B>N*@O86&{kjX-j^^FTvp2qqlO%)+n9XT^sh%otZv*gAJ1R=~&ZuWyI%&-}}5< z?N(mEPuM-|p&1Dk$}?*_@JIy8ZOi!TL_+X)pP6 zEj{8X%y6!^W1$*_82P6qv8>K60m*X-OU_a*AHlNX8w5u>X#v96|U#<&{4AkdgAFe{lZLaj-yhIi1ot6#xnA>l z(zhSWPg?qvDi%pB*tdg2(3CH3A>#%W;r20oV>O41&3WaVdBIPKi0sQ$I@_ANtk~Wt zKTjnFs5AiH%=i>}%}c18Xt=!*PvCLabkcv*cBV(56S5z-*g?&aKXM z#|Ng{F2H>*XKV=a%z59x2;+-P4{JQAc0*^V2M+1I~CVyLhU;yJ#Qq+G9alxrcgEOwsEH4XVN9I$!J=Xr-b1wq+sNK zfkI{V-d}+|%BCj(&T1?16@ZoTZ3lYMBZ~kPk(-?=H8?`3JcSJ}egc`9DP@7*__ULE zk++dFgC0u>PYeEED7JqO^8_MN{il@^t_#rimUX(qN0vexSRG09eU#`7?nPAUsb_X_ zD?2mM-n{xr&4kKhpO9%a#kM3ii_l#EU7Vlk?{-`hq@b3=?g)(P1Mjr8SHN2b%NKAe zyzI7G06r}fj|B1Ai%&q;-d2%Xw4qvi=DXY4`Z#t^^^TwNi}BWVBi?Rnhy#!o{^(`J zaomPJj^#b*dGiwMk15?Re$)y4iusX-PL#IFwl7N1p~a_Pkn)*yE0%=xSV#=E>aS&; z@e}Zu82|e`e5ASb z+r*zg0Kx|NsX6qOcm_w;sp45VeP3XM|Jju~YFkR4aT^WYHt#ARE({w!t@&tJ<05wr)N_!nsDjYm-xSj!aqU=N5>^gLz8MbD&y)ver6sNrCXbFEs$0)m8gPqEc zP|=q)$tN7r|4*|@m;X&YwX3QS6EVRGr zTA6UubZS1C1v`fjQpOYw+5kRcKn_J9PmBr6_(#-P^I2h231?!8fN;)TumO-R*BlGc zhTU)3;s}fvARHMSaZu&Bz)q`kKJ^W<;w}MwonT!_WUEwjCG_~aVie9=z)Q7ela6tB z)|a8irnX2+`E``Pc(52Mit=<8=tA%y0bAI>vzRy4Cb}6(wxvAGDh}k@?4D*I~9E?Tqw%CZy z!l=xja`9t`7>PonPHx?>o56&zEb)50E`+<+eF@2rr%=R;oX5vz>B5ipz&HLM(4FN|^Z@NMFUu9+0*?=4pj1Y-gb5PiA2e`CX9xOzIl zHKCKqV=~l;V2Xd={~zm1FKrPO+m68a0YK(l| zf$Xo9YAPDI;rzHYY?0yX!nEocV%eLp%g+E}Zp3u5!UPn~& z+PFUvRxL&k?ojG_Jj5~QJY$Pj+aK}+b-@bZ0whs2hsMG1l_+As+{vb*lA_K|CQIzU zdQg?m&2Ns4R?N9ryHo3Hu{6@Wa351N!(WHj^N6=@P-Ex`?dH%o%a4t{w-O>Ttx?#1 ztapmCGHai|;EuyD)Z@G&F03Ac#r68S2D!z)t)po6eLp`DUJU-X5OvE6Pp^*#ueAw`P;h%{}?0;OKv*-eqp_a1fn%`()0OrP**I#B<)MO!V;V_A5BIt zk8wOF-~wkpM0FFIyiozgv~c%c&B8cQ-wJ@vb5BiJY>oYfkF|Pfo&~ zI3jgyGB<|VeJ3*){ca6JBJ-pNeVtd*68fV8>`8Hgufhlp$EIo(2n>1 z!@Y%zC4=T5v#1_j!ptZ-9V6kza?g zvAabeF4SwK{cIv1o*6QHWi`O4*B=M|bo_;iIn_C+pD6##OE4?f+H5q1DlhLy-ZXeTrpFAbhSYx4$(X*BJ zj)k^$I=JpsO!$3}X}zg%j@V}=(y0Hf`i*pIDC+ zXW-d!{CurCJq`d=DcsO#Wu92LM`nV*w*8mzu%iuu*KQ)eY2h20?4`#cenylHBnhN_Sd(4YkiXRAw?DQW1j3VkzKcAPnP>r>50 zW-GE(PKjV2>@A4TMKv@0wkJcg0kwJ==o{o4D>(c9J)gRe^gxE}Rb`Gfa(Tuh9~rvL zccC--1jL{b_tE2xv#bJ62Gpr7jSbAgr5&Ngpg1M0pv|U|VH+tpG-_B|vs-g`ej?4K zZ#WqIAaal=b}5#{R~FW5Hy^Q^`)o^e*})Ip%ytCDVN&7}sPKyFdywDtJkznN3oHYH zaR0<|oy9{c?^K0Ic6!px;h%pJMq~lH6H6?;84U}#d&Mx^J%|sEau_MjM$vQl_TV_6 zdn3`{wmbV)P|v%YwG6rm-U9}zH6!Yw84kkqCSH&C^KUSFYN81#j;w^~zS8R%xJHfU zF^K-I84KfKg(<#!TrA)nZWNy#_Syhi0IyJ5wFs>dBiTVNF>9mY6P_Ete4`KNq+#g8 zd9M5T>CGK>wkDD6C}U2B{F^#LYnI_`1Z7T!kRoQ7&P_xWA~%xQiaM&C22j+irm{61 zwVnj2q_7X_)NOO=H&Q1_ldY%UXI)Dks${^aH7VxCLmS*tPQ0PgH=>M7Fcz>1ke{E-Pfl029#A~X8-O>HkFlv`1CUEj3AIJ$jt29!_`O&1uVe#?1D)4+S^3s;&Cj_<7@ zQJ>O-0+ekq{2ed}c~6(gdez79m-5e#b+{pXE;A~Scj!LhmL{k@hhpUE@?ftM4}tu# zK=bo6j$@{N$w10@fpW*=lN0`%z*C|&7`1g{Y0!UjgA#5VX7`84-?~PIVpx1?*#VXe z59863A#DM-R8{koOBxP=`F_t$b{z9tVF55Rj;Ozv?A1f=KB0=EA~n^MiXbN5@sA%= zt54qqH@iH1TiI3*E!B55q!T3<|Jp-CFOH+b;uynDDM{wAAXujZEtue8)qY*9On@$> z5;xRTVPNawhSET5LcHxaabpk4qDoi~+o>FU;q>O5t2I?G9$(?9aeE^32`z<_oFb%CC^IMdSb2a>mT z=k(rQw>>AV@e`~;>(p_|eO$>#+j(T8<|x;UTxRBJUU}7Cz@2VCHQ>JX>o1g85Up`O z3l)hiKU>Lqbbou>-Mq4Os6xwQee=fszMJ=O&B&$aI0y^7x=18$`D3a-!PbAcs1V#D zGz-1H=EgQ^NvPY;?`0p&FUt_9pEqSloJ}t`hcn*RwjTlM&B<2 zDU$bvsGrsR#8Vy;rw12H<=!NA;P7bFoCo_c#Om~E`d^Hts;W?Kko~D9f&BN&<1udd zd1B6~{)W{Kd7?o)880Z@IKCOu_tuWOj)y7=t^Sh$cFH&00M_=ICMI0DiiQdVoK3~^ zQ>mj+uEv)5rr3vyE~6B-L_fo{nV{D9j!c-Kq55sz<1c7~`ss9UU(K?qK*oVaEWo58 zWmJue$)oIVuo|9v7;wzKV_D_3~&rzTm!iH?GtA>W_jN@OH4xOA<5se~ngZduw`fwS5 zb?bNe3UrY7K=@^{NE%v5irTT}%#Zv^?3dD%+4RFli`W{i49&VCBb^KX1%X+e5-yrt z^YIH0x>f5D>QuJuOnMyvi`VG7-4|-)cI@Ej4H!QRUm{I+i7Fv><&V-nD*kk)x0#Lw2k!#jS@?X>Fi|(lcQWAnQ{cuyE2k4r=#as!**fc70HysDP8= z%kd0bqrhx22*m6C=DR+FFdcT4;_`iXaV0_@7zSbYynAc(jTV4m=Nc|wVTwRQ>?Z4j zq^*WmhP4vDF(bcLEPJxfG2!Nm^`E7$f5 zk)eqv{B~4J5ElnjOHFYoo28}l-diJbHVu{htI0+@3;mV81F6h+bqH1+WbnmIqJrVE zeaHbWgZ~*h14tQ-KD3&YvJXF!2R75YrDr-MhQ2b#$uJe?lQ-bSG~7eI(CYn#2^U`6 zY{aXejmAWPaokQ1XbL;@pGI*!rEFh~2A@!I(`|lut?y49DLHhSkk1=aA|q1HkzhUN zGhDAnu0R~2kL)9pYWG2BasPlh8RkDRlH<1hw8T>1P5@&flT3d3BHp-MECc!c0x>67 zjZc3tvE!8?@oxT&jx%metz0wD*iDH(vM3(71AXj8t6_!?teGoB zwAD$VyNJPXwBWNg;bsvsCN5GTxAE@g6xd<1O;_HQLb-`deDOb3qC_!Ug<1j=SQ)wy z8kyd#@hlC;?u8L=(jcV}IfSl>{pR0?anU-P4nCT1D- zYG0vLAR&_);3AgO(}t>+nKpA=4C>`mdL}RK`%uUeh3dCFk#?-qJv7_lY2*LW$_W!q zvxa(p>jkIDBikMHucP>)2x*d{9BIqVLfWGUJNwdwhm})Kd*>rx8JBRqhoVVb{cSko z#PzaetzJ7&3;`J-&6CYU!*TMTzbO=D7x;{QXp>mxKZID|sG$ug-hpks@tKf>lMZ-+ zabHaFkSnCIH3NYKh#2!ny*f+pp!UQ_w+x{7Gpahf{rP8M zja!vhV_P;Eq2{jRRACv+p?V^OE0sMvSA_aL)GMbwWldl;?b{4PTTV(;l(LA+0FMhm zBsBgMd-UJRN4nf+qQ8w=T5j(bzB!#2cxzz@U^y7f)_Gl;)%efYH>FK9D4q5U8~OY9 ze5k;r;)M`Z_`{u2*ss(RkksnW9zKWJ_T}g+>r^9{;AwTw)XqSmJlp_Xq8N-9POM;= zQ+{#HLx2Jnuiv5ia8y#jLo3hEtjtsn>e=;&jMiXtZ(cyK%q3&@roaoZhtL1esp05U~FZ7tNy0#R+%oMz7pGn3d&%t7YgktgT8|p zLbYi=^Met%_@7Dfe3YRXu;ufvN+LQh5DQ300WT*>bn2^vMNl(M?r16Col@lTm}Av< z``e(wu0yV3%b5X0N3=b7q55k5%MNW;>B;f>F9&O+RiamDzhY>SfLo0H^AAFG^a~Oc zgn1EW2OEro2wBM6P*4vnL&>DtChj`ig3N2iWw$+R6Ah%w$()w!l~Z1ZaDq|F62}3( zI`y+^+;Q=N^Jk{ZjfF}28?(0$9hU?6+-ok7iQ~n(3w3;c#SOlirP&|4j8nQ9WlXr` zjzIG$l4PysQ_O&-3~n-}3fFTHgQW?E09FoP9sWpWnc8~N5+Jd{ltIgxaaidSDXcse z@U%VDPP|H19U}J&SVyF*i18yvPN+p~qU=~*fa=~q^hUjU?)cXWWnu>vD?QYI#xtRt zlG;uee%lC8VA(5M==)v~_>IXenLq4_mUhz-kC8uvh>u~ZKYb{54@TBX;M3a4r_dQ) z@=TqzdQ}ZF8Zz(-O@V9z6JQGk=ng7EYk}pCo401&A(w#)_0C}iHOpNXBuuJBuZd-0 zoRgm|sZKH>!>Q$t#F+1NewQ2kCR-gE`|R$iPE=0E6D1Mb*{Zsp`w%3ToxW05BYy&o@^q(a8Gt@Wdl=<{MNZ8yS_iHOk&U`@=_Ysuw z_!&qMUjWv+k3I`SSDQ2{7o2%w3a5hdwv+^ zM{VjKgn3>ev|!4uHvuKEC`mo8uXF+7OzR?y#-y)dz`U{RQb!ihWTT(@;6hI^O}4={ zBEQrSJxHt*N+Z)yPr9{_$;Y|v6rujit*?(OWa?%#Tkn-7(n9^-yBavYNKb|3){ubt zr2PHBQWSRsc3rQbDyLseXWp76)UKiqa&z+|=(IOgt48s(huz2@j+m6F6do$!a}A*I z^h9$BRk=h`T{SAe`za!rdpcIec-3s;ewn~=K-AX7U<87FxZcqSf_PN`m>ag6qI1pG zQ#Yi4hI`DJhiTcQoREI{w5Ua1?cZ=FV|*J1v(Y(+?BR!Myx6yT zW=z#8O&{o+C0LM6;_|mq6ohem0z&|l1AVS)C_>nT{QSPt5nRdTRJIe$V)&=0lNsh8 zAEmv5Ar)FI>f&&W<0eC295K|@{fIzC&9ds_4k80)I{wSr>l)!$ESNw%M6Zarc`3Nr z_~vVW=q*#!x5a9h(Sc}-+>3)wo!+z3Kd|J3zZd@;s&AMAE?WT2JxBOGs~8OV2E_CPO_FLjGBt76wV-K)ojyY0-SE)7TH8!}y+E3E zlk{FB4qt80D4JeqrKn5g6Wa!!5+Q1wwPp7V`=;=W?2aMe3G84gGpQB`-9l())KtGv z=9PXvTYd^jr_$WuL}>cfPG)eI`|pDILXDqCxtSyf2-sC45_pu_@;O!c;E~{C8{oSm zKnZK;P6?|5(yW@pvAq{=v#n4O9R5-swcsnG2QK9p?`XqT&*k6tqejgI6soc%iRsdY zX+vIXK>5hxR8T5m>y!!pZ?l3X3IHs!2PWmoBvVu+c~77qpRje8T4d3)&>BI1oG(6E zgV1EUL&S^UP`FJqHzlcexlH|6*)xMUAGQjZpIAP-$9t8zBfzK!_0*J$DAiz>>GDe^MjF*B%oIbo zonw@6$9Zip%g73S4M-YI7!l0Kpi~EZW%?A9>uKp&S#Lz9XKx*-E@|a_){K|ws8CT~ zB-I)rE|t1px(?%|NJ+UH|J+GEH`PL3(AIcHq?OYw`Vt6K-8r~*)j>6*p>-S^F@HGz zOQ6iJuNP2pGKD18`wO`X?2-i>@aRDD!BO?UoQ%N3tq6FWwyK|FnxGOezeFygk!l;M zqsR$tST!GAzx(r4=U;)=-RF@2{WZKy^4I!-Ba{ElUZ7Z2(6CoM&r-fB`Ph5wK!$w7 z7^Ze~G&$TKYWD{F1{`ng_D3u=&w|hveY($lo>o@6nZyt60{Ejbu9tGi+qXwlplO%e zSX=p*`ZVo!=^r>v{7BQ|>wgcmiWP|Mt<*bXWt?4M6PlB^nv6%OPqBl{(Ryu9Jo)Co z9S{b6$7T2eWw`Ai*LveD5In7zL8d>$aiLnvHmtRdzpD;4g<+eHlRqcx(;=rRLuYO` zh$=p8E3RyFQ%=d+1EA1=Im&a`bdP z7;g~x%My;4wO{B%Hzp9Y?#Zc!O!}{Y7`}Ce>JjrlqKWW2;pP|Zw|*;Q-3Pd@^7&r^ z_sr{L{v@LL1}9ekbu3;;UqdfP>}+l&DfkC|RdL776Mv9d`exN$*59qH$)w4wI>t5z zFun)@J3K-pK__LTM(lv?=egwl6}6IxA;w_s1NE{78BO>i0xx(gQ{*}3oZ zbS|lJW(GDTbs@Sc=UB|TX?5iaDc`(Jtz0-rdF}T>b0(dutPZDX z?mXqyxKMR6!g0`7>(SGvSIxG?X~OdvgY1S8K5+5|2yvNnss~$_$CjRor@D}8%6T9y z$6`Uz2KPt|VRu5MZFYuI)!)lWG>!UW_5YDNqX{1)OrEbZUhnVeJ>1&Qde&%E3)5Cy zT(5z?fUZsXgRJqj9+nSMi_g0@!dcB_e{QEIY^c@~CFuuq#b%Bo`0`&`%ja)83cr>G z$%Q7FI&Ey6BAb~$w$)r%0b}d$)+D9wzv@C58whr2rkBF01?q@P&cC+(66@CP>Y#AY z=A{D$$Q2;3>!9eiKi+-==JTgeNZYz999cIMwRMk#LWP2n*}kbnreg}FCYk}LAEXHN z&qN;qMvut~n;@DW*TCmalSzgRN6}o|uaSF(puC_?B~|^5&5K$tXEN@32yw(K+l3Cq z%PzB3FThnKHc)!rY@SwB?bYLHYqn*;=PDM?_wIKgt4bKtU-)Ln2l7O=K{&7-$~7!x zUClGhrm}a@>Kd|!2pWI-bp;iSAO7>J@WeUsM??7An*)Zf@h*9OqlEvL1^s1)#q9%I zfODh2PEK@(Y+!*is?RV3#E6;V{!?5Kcx_ENO1~*yhhYZHAtBDwiV;0cT&$0DVU))! zEHmZU2Hl`Sq1%wAUx$m|@U0`+Y+>x6I8gP0(ZerJ$eTXRNhi#0coRPyz(N+O)Rn6x;aQrRIMNl@?RTAfnSoTx)eOCNp^rvd%ym zBT_Z1!IfUTXY?dMfy+H`@i+XqP`W%?ix^1LGj=|FkXPNGnb|3)`p6`S&05XvL+6On zJW_$+{j2g~Suo1rtc&%~TVSt4OYWv^O?C8U$TsV9Hd3E5l9LOUkAys6Tf15*kYi}j z;e5sGAqv&fii_em;z>a~I`#>MbyPN2vSXd^V~I6);>l<_4rFQU%gkX6lTt*&S%~F) z1^eW2NidUjxQ2DwSCZMXbtDSLU4o9=%wUvDv(doM>_{+Q_+0{zCZ1*mRDx zSK&}}0?5Z+XeMy~dfO~Xvn?NI7_R|?4z@I9(*kk6?VXjcnIj0IKe+jwxIgR`JjMP1 zSO{H}!kCN`!0^2KDK6u#z47$c+%{NNdT9i94|6DX4_CK1V<@^e=(p|L#tv1zZRp#i zj$el%xuoCfz;|8yv$mi@VeJ{tG9`tt?+*^R9_)|Dz?c&n zsmi%+02e&DyswQ>l$IT1i5B3FQt!%}Qe0{);7>4JXGIFhtEb`4#X6bF@%ocVK$ zHERjxlWmXYI$M6)z<_Rb`hH_WWFz&|Rsch7ZnEuln4pv`EI|<^o&fAO@~8pPQrBbk zS{xH2`KqvuV@Cd=Cq1yS0gm1fj@WN{iEjvML>$qhD?{Csje?oYx;a4J?(YvNM}t$z z$7{KaE7V#L*u`1j1>LcOWyn12MO%h`RA+5&yv%zk10r57aj^=O4y{GxJASV>8S>&A zjuUJy`By4X_)*VeDh3$C;Q%F=3D;yUeoI`VE_p8=5QAvw$`WVw>pO`X->s`)huqF0 zcbdWf_RD*0hkT;50G5Bxl9U$DjC01eKT%O&S_X-n9Qa`Tr56| z)Rr`n=N%$RRkbgjnPV)hJV5u~8prIZy-6Kjd|RUn3HMO>SaBF;E2uz%ASYVrBPS}; z``0(Og*gi6EaTe>Ws8qt5$(F~#qX_hRyul?6unXPmFQ!|EiQKi){rG&p+fND z;o(@O9^xzgHyO6S`oFQwl{KmG5_)%^j$O9=#6ExjN@EHP|MK;Yvi{$>nqww5og+l~ zZ)D{cgTt8srEU94Ckj=v;qhP72`JpvB$XQ}fdO;5Kd#))xeqje*OH1hgnnun69@B0M@ai#ZgD z2u7yA$QbmhL7+2BegbL{3fgTe`8>nnJ?~yAPXUo77{7sM{OvNk8x>&a3BbN{%i_!Q zYF(d*F^l;Xcu26aoHhcBwXcwpR$L>e_1{%F+w z{tyefLKIE=>FDvfGRt6kz+e?T$D_8;VNzY%CvuE{h_7%enHmGg14u$w4uRiZ4uBI! z5Nk{UX=aODwq`}M)Dx7z_!GB~rhQwMtKu?RZrIan`LNza-Y>*v2J)wiX-C&1r>}|J z4>p7%6ZaikmTINS?WGGiR2C197@s}UU{C!VXiRL=!W2=Nf=E+Ex&+khIV8}vXBT|M zwIH(SKDZxryA)juuKFbWsG)oCDgzuCClX5`2iLTR&@+HaK|@}Dq>8%)LT=u6uQA3)Hm-z(zqM$c3FzA21F-YvK_54kW zReIqxHuDUrsS!5L%u@A?HF>yF24IEf#YMMHyD}WxYXHLq`x;P?9PB_Oy0Kec{esws zL@h7+Xfb6Zg6kijJFf1g=b*(#;3rO)J1id1NqsHsUbw=MT}Au<^|+U#OPusLi7-gQODo)j_{C*JIA-|(EMEpz5w!*O>B-#t7r%-Td}b1FLmv5#1M}f zF(wPH4Clu8GiUR8XL;PRv{MNfEQ2my@n7nf&|dWYc}i9Nv!V}+{?+G>K@g$hlv)9qXf3c{Ofrx=8M2^cm2fkjur=rJj}4i=vQ}( z4#HYmA?pulva^xJ-~zaoEQ9!C`-?k!dGWjJ&&S;-$M>~C=ZPZVbi0O`qhmqEIevXm zkFZi(FulT~Ch$e8@~RO>=n#X;FlAPM2p-1?s?wA-F1U!&YN!GT{$?r0c*S?VOy&O4 z=)x5?fpa*2yIx6u*w%{qP2{ZUd`_^%{fyNwwOz10n%F95H(pSxwlVY)sOfser=+uKLq%kX|2@eY9_wwI`R-dXrrS{Fm&^e^ zsWk?1%UQO{T-S1iAwO!3{0C=f11^Ri+C0!Sh){#h?-UfrP=|o z6QEj2*m47Uj7_dM6=5s`*v!CAv(u(;{E~! z&<##d0$w_R8>IXFZ4`%OxC`dLDY+tq5sWf*@b5-oeIi&NTz2GZ4E0e-IR`|a(>^QW z2>@Aw)Wz*JLkZ?bXVes9FdwJ&+fhb@{dVXVMU|9@#QlHm6sWYgE-|KA+-1=~{jbf< zWr2~-PAQG4@+mw;Et2ig37_}wwMv|qkduQ0AvlBI4JB>-ZgJ>NXD{`Zb-ZL=LUCoB z;P^#&4Q>VJJ#9ctZJol_Nmt9JT2<~CWqHsh ziwl&Xt~wW7X!|{>v2)Fyop*uJbU<~1CLMto%c!!Z4xJvA(wbS2-g;3R4cXIW~Nq9qJx@;3_!wOH~+&aAF@ny2Xm^UnTD zt2Cxk+0GzWzkVrcrvdpYR(Wx<4~xR2R>^$_DYsZ{bI4G2M(j!5O~8}|DmZ+l73 z!VlYSZ*fuw*@k^N&4+>63i%9Mji~?|NtKlAcYrDtb$X@)WR#+%Oct;Hu<1kVhzIrq zx4qs|PaOTL&19E5Qh%pR01o6igz+g3XyWLY6F$Vqid!#{rlT&kY2bNl$^o>()at~a2GB)(nUzfK^)WcVcPyL) zU+9n7@lO&zF-|K6)Aw(5_Dc$v+ecGB#c5dh4a5VP<_`k&PwVPvWc6P|n?8KNfMsln zGbHxQuGiKLYOX{5KkG{p@Qc1L6=RO@2;qVOLV``%H1Px=!wWFWr?PFu_T#|}AZ24s zLSqA+pRex(DOZ1c={Yh$o-eWzj-%Hr@JX{Ge{UyXtp9hT(dRt6sRr&+$3Q?Y+%mVc zw#JxPb%JB)6U*c$hr6LacjODi5I-D#-Ue$|hcJ8uff^Qm&6$UEb}UoY)mO>_qOrNl z$oQG-YZ}Ty7P@E<$rC|d-O!zcoi?+oNNvc>?NZ2DOQ(6z?c-W-!y0)w^!byM`wi3n zge`&ZHZV~F>xzEh_yX`&?DtUUtB{A3y}pHoL~%tjc1)PuRHI@EfaECf01(O#fm;rk zpmq5VQ0R(FQ#2TO0$Y}zX{X_a{nvZ-zVVx4GY_Hnc)HMYL@?2- zq)6n|AP};|CT4&u{_kQs^;~YpR|~+j_WS-$8 zhn&mrLE7YmsM0hp;3dZ;lg;MX9g@v9*j^VTyZYk!e-Wk}q4fpk9W5@A)9AchrE)in zX^$QP5`X2o2kJ+1{f6q*|IO?7dm?nL^R^qNE;UvS%m9!|3J(&n zJ(YdC=-Br@|E|INY6@sUR3dHy0khY%o1YeNoyetpbIie(6<|oL=RsA`54bUZ>-9me ztf^rXXtWjU-t6zn3<}7PEye}CGt&afrd^98%r<0k{Uw0Zx;vzYI!{yZdBi(hKyr5Z z#}5?l@|vn~`1*d_Gy~-+@YM4~bns zl4(TRYq<0C8r$^WoX&|Shr0S8YEUFPzS+yM_p87wrPDoET6`NA*Q5qE6&HU0{R^z1 z?X7zL3&A?@(~(PnDmH)5)CBOEHGXaXqg6+WY1wW%RDVtiJkZ5VA9Kn%;k{W*aC9O! zb2BpwI8K0=i61|P%+n+&d-D}kXnIO!`?yZlFr{8H?VZ)d!^6X>S)$8vm z4xCPISt6MT>LK#|X?g7MV)wO&x&;?tZ_?bw*!WpQkO7tKqk)OuVfT zKYto(PcOJcF;m(EC9zfga1hPql_f5p8U%_Y&5$%N4B0tCC^SHlh(8X9j5B7$rS8Kn zk8r&>8Vh-B=3W-ws*)cRzcv}eM+<$|P4%RH*aqh7+K5P$*C1r|oieYI& z4lRTnlcuI9#}IPPIb_b3C~^!HMUg4wFfVGzTTVlamROR6gd9SaQ+>Dh`|JDu>Yp~x zp6B=6hwHxX>uN)GQNtP|nfN8`CUCLtrquh|hq8Nm0)9X{ORU(bUwiNAZb=b6vx%U+ zW_uT{eG*Zg@%6LUOoii%i*<9lKXi25nxVl3KDA1-x`7V#_kZeXoAG4m+w_XT++KN7 zp0U|`4}5Kl`#$|zUh~}=5duntY=zz(Vd zu83L|RBvyu{#8SQj~h5p)$(r}k1a+0o$k7;l$Q{-iVED5b3Ugxe=K$`%Dg+^lC1=P zOHlpcThuQ2$K3=nOy>pu0*-uNVMS%AxQec9*n z1sjBXDW)Uwy$9c0coThs!#Y<7Ilfb57f;?~jqUS_OVJ|9CkJtH4aolzTsh1Q23J5G zv|(OzE`cV{Hdcl0$aRn+d&`7T7@YvOm`_h~R26U{fs$b#%~3zABkF3i_G+Ke=p^Hk zsj2Bva9Z>2P(62Ryh6qXQmS;{afBAAQdqN|{DoP)@ zibi7wx^)7Gh2y6g)-Xg-yKzJr&&UfKzQb6+!)ks;HtjE4QtH-_+^^ z|7=K|LRVMUX`M|s=1kami9|3O7Xke(>+%`ju!ktB{i$asd~ytTbsRrh>%ywZelI~s zaAclywxS!$Ccb%%0mYT*-_4Tt121W`mQ1vHg33v`BgDetxp=40u&^8~!E9V|iT3m6 z^=}b6f7gDM;u0l^EmHL6-Uk^E5DInjBj*%<;nPB94zv&@xu~9<)CUZABeS6T;p^9% zp=?0DrtG`nEzyQ-RtC2?g|58G{QUfreRCq?6?ctWOmbwMgjd4WK_M;I;Skwdz(6*Q zx?83cw>tFh#0~F^Thd)a`kPlN)ZuW!2)azqJfKJ>jeb-NhcXxD`;G*=^Mb98TSa2y zXC0}Q+DU81@?%U~Ny$6!TG8Z_=s3`i-+W1HOoMisIon3@JhqF*lBOShg29IOewdB* zTcm{uwel|Bp(&pA}HM0fFgZ(mA**bahi zINkwZ|6H4cm(y~$yqTc7Xq3`nm0UjozJM9KqBfARAaFzx5?x5j&4EJ0IWwQ4UOXqE$ zvTCCqk{q)~cM4221cX>r@uJH`zrM8c0x39B$F6V%rD9!ixtH0~>O-?x1 zV7Mxybgh)(sUWj541Kps%&Q-}toF%A*p7acckWlmav?I!MdC*({0Nqpc>Ce5cht8I zeibA0mm^$_oo?6Ig5>@-{eOJc7Z;@y7d}OcmbMkz4;<6fEXXYI5g_}YB79kKV8_xE z5osUZ=^elx%a*~UexaqzhY$O*8PR`cz@hx1={f2T^lg4dmjT2(S^{R>V(i)JrSC#Wy>N9~}0z z5^S9cB3sm5+;y2;K9gARotEb+LZdZ4IB0jkxKO{Y{$Qd;8>c=3(Q*PQ(fNEtW4g6g z-L#_hnT(z8;jZ0=S@gugm5t@)gp!g=uQm#1G`O#Zd#YW$)bs};EspTc1Fx;CYgn`x znAe+~Mv>dXjhy|r=_6vkExW&eQfA^Id2{}7j~X|I)3HaCB<-1#m-pwKV9mfxw_CxC z=HTO(2NonGjm!Xe-@0Ud!5jB}+Q^)pc?@|p1N#`x7Xp!)b<1ZvnhRG`adp%ndvf1r zkH1ogubXmM@pK))bB;%eWeRAo19eGye2uk^a#_>|WAVZyooYbja%blyH%9rRADn5cW(gW9 zR3w&oK^Yfsw&hq0b{pZUUdRBFH-#VN@AO*9)X7dB=9mz~GP9rum z^%lKS)=`U^f<`L2-Ovi}$yHKP@^@aMeJS?^T4^SAHA3zF?W%Tn8EyB1j}bcSYrpoj z2~$p`^Aoo#xT_a+B-2eAx$nA1C+2%TGB~t=HTYnZ2M$T@&n@eRhVQtx2HSfJhHc-1 z=6!SAvRxs;_sY9gYa5rvIelN(FYNm_RA%wDfXvU2Eg?dNW)4=l8)vv&ZQ?DT&_(4E zomEA6V*mb{Q35~pNNo#&$3wRK>Y0s z8OgooyHx}$_UN%6^6;lx%AQqCKbe+=YF?-t`E$qVUcOEwhyhv$*Gq53LH0^m<~x@6 z$N5V|p0?T6y7fZZ=gY7J1G#kUc8H+qQZ`4sD$5qNHeNH_-%Bua@B#>k(ni&|t_(Ew ztKW$<3oz$X%l$@z4S?bjuci^k)dtIvo!5flrz`Ure0Na|1oSA$z<2RX)$aE6`Sl#2 zXjD!a>ZNt3b1vY*Ins3Z$~x8TF>Q>p6Eq?~gsLEKrITM<=|p#EMSKstu5Sw%JJ@BC z(Q>Dwg?(H>X2>*8&ho!R*4};)+!mP$F&Ht!n{irUSxM_eZ@(+MFUZlam^HXYL*Ebtx8I7l>dFWvg@aGm8wMQpWp4e5Iq*1oDS^S>ICQ< zdj}fBL{W$Qgm&3P{TqI-s!ou!=N*0`xf=k?kFz~2)|By!i$~fZV^*`kOGfiyUVgs2 zO`=xZ!!aZ08;lc`W!#BE<72xmrd|0%Tn0Nj6E9jGYTYBw zu2yK)_n?ab_qHjz->U|iM(?4eYnFBTC>78&G7S zHVCE4-vMs`pl)du@{&cxMMB0=q7ObN^BurVwC~d^pF^Knb09|8S`X)fG+E147`p6D z5Oc6RQuX}Of@Dx)QlUn>;32A6eDiB>wb@@`@NRu;^T>7R+tVA|*gyTWM6|sxLT5=k zC^$U7%t?9sKxKCS;MUi0W46EaU9l;;aO0B+A~^ru0>CzgIh4&PT|x%nZOzJZ!z=eK z;?PEBx{+%Up`j04rvQ^mN&-HJ*|P?#4`>^?u6IkHHaEH2tJEVkP>Ynx+f{MHDy_;* zlb9w2=dr^uVT6 zYv$qc@`D&UWjmm<&2H7fp`lM_K2A(bxc%ZS%2avGoVrKx99n_i5{?^{J+<@tr}nTG zUW7(UWKfW5J|hEpU7pTE=4|w}EgvX*|C~Kz`{d6cCjzlEkZcp;G6Pl9(&~@#ab{m% z)tE-!wdI7>Pyw1=HcSluVP~C1__--r#W*cZ^fVj_Ex1L(Z#}*TDz`kv> zg^(UE$Pt*93s1InI~~&u%*Q_~j=g|Dq&>zCneU0gsN)iymA^CGm=1nTeI8o?U!Qy; zH>7y48ouTr`{I!%11t5o>YhLUtVVqBz^g}|*m}Zbfn0>bj?DFOp69OQP&36?jn8d) zJCLJ-Mgsp!JH|pdr1>_q`8l0|JPe0XM_U^i1Axj;a9-v!yl@ER?vpNrIF$j!BJnr3 zBXDsSFb6mgr>stM5fIP^BxBBkl+@DF5eo|d6WpqgNu+)_Ks`#VQ#fq_j@rHbJ71yeuJj8fmoU-6p*!kz#r_+S+mqCe#OOk2f_myj>~|7T7_4R8r!46t2hS;nooP=9smGg&-V=*rMBl zbt`vWj~@K4_O4qA8HIRGl;kFBZ)>kQd$uW;5*!o+c&4Sei##E)nAwMau5fy&wr}fe zekB3KJUA7E)YoH5j9=F?;gpJ@*;$2;I0W<+ok1udc)kdCQX>c__UnI^Xb%8)zM+}O zKO4cyFLF$a`L%7(ExvUyY|X&J5wXdBd`3U7Wv$f&joca(S%5HpWDsg`uk|eHe_{?6VT+QOajt%@>x6 zEqZ^r^wCNXtWFXOeAjx><2wmne*@=cUSvu{Dk512{+kn!F@Ti>_YEqb4C?CHa9$f7 zQqh3{0iWkDJvP_R>t2(QM|{!f>FetY)qX>j8<){D37ma#sY!64wew*vC#NF<$T{9_ zCqr`a#ESC6KH*Uj5q5$%#x%0~Zx*E0SG(B6vpL6aG$dMY=hP*<^SuT_=DuqPf$@Gb z=WV|GJdS_aacxY6E9jL$$bJLdWlpK(FOG1&|NL3^gHjKwfY1CD(lDr9`qX$F9#=w4 z(GN}h;(I(gTEQTP0`D972WE>n$@0KMc0bbwuCP>vU`=~MhH;@3Z-9$pL$)0LizO|H|mvne;&N*(;bawN6bfNrxMVn7hgF=(u&`B3U z+Gu5(TUg|FgC-Lx4`ABixy>N^f#6}lM4DD}qd&}Fa@sDlXy988du%SwN zc^&eJSy*ry6)=Juq?J(gNonGPdgmcZVE%}h--4A|Bm%W4+#vLF*mshoBbsbCVAICI`xc@DNYl7O}$w@1-b$MYbwjHrsk6S|w#Zp7+d#GKE zcS4cB)8<-RZ3K;!NUw3c#)W;aUq5!Wv9h`=9AKQ+GLx94&Davcf*oX{q#U_HRz!~z zGcxw#?A+=lZj1x}_BTKE=-GO5K57?dLy8rWUONJ-Hb?df%l|(R#LMObOeI3|u8~Nw z>((wt4pwCdSR)9Ih-mQeG)%j8YCFDo;HZ2O?V~ekLG{`37ZXp-1&tiOkr$>XWyqg+ zws8Ib06Yq0Sx@mMTxQ@wNM~+LZAN(G=h4x=AkAdyGBsm~$$q62C*e~W-ZsfB@3loM z!!*S>M7s2LiNoqH?D|R!{p4u5?`%kdir3JJzedEYfnj@<{QzrZZik=A)`zqWEux5Gwjw_Y?^vJ8ffZB^NP5&On@4}jG_JqF%wBz5uKT+sfjYx5*qjNRfuog*<=ixD?DqJ=aT6By=FY`)HZaq+x|4}D!k z=%x#M_qA)7QWXrV$PMmPFXeM)_Xo4(`5fF32Hn>r^&KSb)oq{GVk(vSikSp-oKaN= z`Aq?x!C;6cyA?D0@sX+`x`kSzBsN@?T$eP#nRrB;C!CzdzV&6h?tXSsCt!Kc zbH=ubk}Tka)O-Y5ib7I{3-mOH%6IW^m%B;-oe3pElt2>vyyx({odZGV9}e!!HLjD z9Q#LeTb1=T8+G8((V%p|!PW%aqgTECvp)XfxoB!rG#dH3zyE;cu=Inv`jhca{t+8n zP19kh(0Qd4<*VMj^jo)8zi}CXG;tEUMx&<~pa3%hq`z%2NE7H~ThkaZ@9>_+4krX8%0- z=Me%y>fU%Bp|BH)5RE}#xW4D=aJ#s};f;6l%9$qp5qm*v+zGXtTCRkI1k45jOdnwb z?(1^7;hFWV0uMjGOBpRV12`e_9{TqgU)mdFgRokM{OJIVFJ6c~Ml=EiN6K z&6abmX1Qof$PyOwrnSKuqoLuPMGbQb&G~A6{+!MZ8yUYtmJq)pFPXi6_;kr2t^e^c zA?HyTbxZPJXe$2@IqK`Vy8$Uv+H5}z1KV!y%ge)Y=5sJW9SCOPj~`pL%Z7=q93@T% z17EbLeNDWcIR)hfQBs54uxoXXJA#lkl=$0m@(B5Ya6DL$3mJhhy-q#|a|-O9F%#VU z{S)q@#QyyWPRgRmpU)$xgL9F>X~tFPB0TV) zmaAmwA00(5OV#g6#%A4`{8%p{#L0NVFNY&((^8R7L#4j?qi2IjVqJhcoGR+W|FoxIy=Y z>`L*#Q&%{$!fLDYgq^djpPi}}yjqA#Km+`m)13{}0@m{Mv@{{4qkncM=;Q&>HYGeX zbkO%2w5*j?tC7_rcjx;&o|YVvLY~zfAJ&V_xQ$S7kCxbsslFm`esAbu42F>5X$w6c z`rep^ySuLcn+dKbo`F$OArTRQp`jZ0fhygz#J+T8LxpwQIzas5K(LY78|6>SS}~ZJ z>(LLY!|^7^cVaKp%4_dzIieqGwe}Qk@ZXkN#yFk`-I$iIk%N+v8F;&wl&q~T?dzhh zZI)`Js5jzVHoZ@lwFEq%z>|aa@Y?lzx1L}S2ph%q>cMa66Zo{sjV8f2*cRd_Ph33> z!eU9uSK6FoZ|WFo5BV(Uy8`o0omdUdIO~yRKSU9)JPcKeJ+DT8@J0o*;E^N8Pyml%DZsf=CDP4&-$&R%m>y5UO6CI9iem8 zp2tvb=1ss4ECHN$J30;y1Ve{Iv5G58NWPV&&;>I(7zslwrM@YPq1$%M&vQ%jXMJ+B zX)9EqhZ|*QIaCzmN+w?d#vdR)HWxj6!c0TB1-fGBCS13Bv@Ghtb#cVt5`h#Y=PLPH z9zuH>IgOC@k@W5$O>;*;_~v1$I676j3!W*p3|_qIY9>)>`XPY$uz}#%t9xwqy;lv- zbsaUedCgowc_(3H?1%^6?a1x-Qd<0{S;3+xuQ`A9@%94KA_vD(E<_CSj7v;|l8en; z5N(Sj1k?`zmQ>3&BV=BZa`m&WBm9z&NzhEtb83) zPdwymXxgFD4BT-=>~(MXICPtJUA=owwhx2RV_Jv`k5JRgp)`-4GkbsM4zOn!Jw4|X z=5S(rmIpLF#&Vty*UD;?WG1sfk@ z!5bYei?c(zUt4{~fTyv!&TejQhK!orK89JSy5KQ6xX`MSvS);iC-RdWnSR^I4T4wQ z9CS8x+1-gPs{B^T1T)|X65x*7jMTBzqe7(I4(jrc9{@y0 z!7Q|P5}uUs_m7Hd3IKvt`mBR)x~jo^?fn{d>h3!|13BS+C)rB&v-9{I=D|OQT$YsE_?kV`h7t>q`U|r|^TH;q zs5j8fR}KvQooV`0+&qe4chqwNO>1yjG)CpM@)^AUiO(GPf^U8ll_QUKm}85)Y&$0y zM19^ga(`V}bS3St)wTJ*gXdq#VGk;;Cew!sXTm^HKZurr)#}-po_<#gcwLR0*7f;y zGeO`;fp;W>i6^aIk*?rf1(!eo(02YSy#8xI7a;kY(x2{P=3{qv7M#yCzkiRgI3kO& z7&)c)`x;q0<38i&>2s7H;-9vrF7Zyl9noeqn|BX`zy(wtbZTJO+Aw>QsKi6M`5a)8 zg-PANHDiNNujV?S$QU{iM$oQB{a$j3s=3{=cYbcYY&(t2Qc};tjw|09fh?7+=qmqC z4_1S3P&~_L9M{nLX|wv-6OD}ya14bTqC#RmJI~)%=z^&!mIYWzOl6ICl6A+@TfCTO zj^lkrGG+*IdCTswqV6o}_Q}H4h;O9BwW|%MfiSCm*E}p@1LU#>S~)wPU%uD^#QBnv z-3D*a3CchbQvv5Nd}F^C#4ao$kVMkS1-R|?PBzR%q({K_M3oPiLd&$_L$yEvMHH2> zyjd#|%pLmZaT#fpL218^rlvWphcaI~Iu2dsD~iBT6|RaK!&b=Hn6^^y^PuoS=_;6c zSF=o*f~Oy#k)+coso4<7mG{uVmnml6f+0xahSPRV5HCDg=#*nA=p@u-S!o1GuuGf4 z&(9CF8D#+O;(CILn@uvje=zGPz(K?5Rt;+p!o<}tnXdOKz%K>ha%ebdOt`LB%zbHB zZSYqO=QC&{fi7VMol1awfF5-Q0oK3vd7y+@r9jg3d4>m>vueZ`yE8 zpR6(h$b-IQ2TQM+WQ3A9id-fMf~~`I#b@)k>3>O)%bOgXL%mGJ625A9)oCT4+%xoA zjUIAK2mv5Xbo5B@{xf@|QPjh@qLPxidCBFlbx7=Xc92IkQu+p=R8Z)G>&}~O`m-jF zxK)7CRr6zn6vZOI1W%#M$gD+Nii}Z$mD^_J;NeG^FUp7J*;NOGNZcux^Yil5M3Z4V zqqJ1MtSRbp?O|%h~!Qt66VrCtpk$Ii$ieJY#3` zqk$PhhIl-OB^>VckKQkpyH@q|Qfij97c^EH%!d z;vnkl#6&9$jCQd;(@pZ+DW05YZ+=6dkeA@5fByUlN98E7iAEC#6#+pf{5IC^uNrx# z#U&-1YcrRGj0EB}sz?vLE1=ymH@jeDCeCw2AGP>cg|B*O)+QUML>_oy$ofS6+B;he zD`v30*?TPE+?k4`St0ocZq-Ng8ETEZNPRWEiRl&CHXjI_{?UICh{XmR@5b`0o6q;kKiJ&d>~LwAvVe=3Mv)zJzQO2-Dhdjz_V&9HxZZBc zXqn_t;F0ftXq`Q=u2V95w{jMSXVoF~Gb9PqWP>y|v6#8KJaxL7wWtk-9ghK@+6AP( zrB76aUHqw?yL+=YS>Ap#ss|ZM@Q3Q^t_1|7-10(_qMw$Ol-QQ`n`ZnTs*65hr9S>p z0G!*~;gE=ncT%;*cPY0l^3@)v0+Rb=Xc#J+zyG2ZeII4Qvks}RuB)@k2I?;WcBFO% z(cGa~wIV*nV|ksm#+LB9fko>HvlhjD@ zIlIhl{?7+|{9HWUNdNx>D)3+b_XGErby4sE1i{GkM7e<@ G`F{YBm8Q)A literal 39099 zcmdSAWmr^i)IN%+fHX*`q$4E_Dh#bSgn)DjLr4q4P|}?P$RI5(C?Jf0h%hwLUD6Um zcXxXp{JsBk-t*~vK3u@Q81{Z*JuB{YueHNemE}kX?-AnQ;E=qKhpFS>T>pcEgPTQw z3qHwldPD#&x9sI#IpN@teZTs-w&IgogM-72^8)rv(=BysCD zTH~Mn{4EOw1`E!-JzWY9Sp|~x?tW=AUO$6Z&-TA=3cJm&oOURu5T?g0N`~SbZyg2g)iw}BUgt-q%>q$ob)XO$6GUP1% zIS}07&g;55U3`szZx)J!6CvU0QlLF_3u17f$MI{n zF8wAi$MSGj3~~@rQnakhb&`a3$+deiAC5$7%J{h0e|OnR-;*O^h9T#0aCC)f@{pRm zwbl0>4|Z=BcEJ}87WkOLQ*AJ1HIr$zP`{6_A6`j%n(k`)viJ*09Gpx!dVHK;m1{ms zIDTKR1U{XfiN(iZUcMTTyKn6vwW@fpnf4kvdy@kWPHM~!IHDjjus9r}L76X_tH<7V zaJ*!}7&tgy&##6ew77?BfAi`FoNbb;%eX=WFaP!|2Tsu4t3P{xo`Y%q|J_^3Q%8pL zbdbFD8qRie($%!&|4qB1n@XB)c$h$R^N)^S`GfxtJ^0@D8vd6w-IW0UUysE7yxNf0)lzhI z4uIHT|HSb>Qt9PiCja&L>Kup$t)J_UJgU$(fGK37YUZwyv;MEu{LiA+A6{uymLHf4 z>@4Kplr}WHu?`m;I53L;CGCj9BA0`N&HXWJ;hW5&SCf~U8DDNlXPWz5!*RIU{{I}* z|2Z$EqLDoyCD^~i@l?3l1_$&1Bk*bDeiqT=+}fUn9s^M-ocq#X1mnyf1Hx|W`y{vo zTvt2zABCc^<#9jB z`(97>e#Rl^zY^!$kCS*D8;xo>1XAf@I5_+R)&i2^eL}(**cZKmaQO;2MYKsV2V}#g zYV0jizQ#~WMC9<`&b3EZ&ra|I#{`ivx1-3@Msqqv@VZ1G6{dRAC8#N=YlVd|nzGo* z8i%t(1_}{TkL<$Zd{{lAy|BJZPJ9wNq#NK#3Vmu(NN)k+NAkz z*`){eMb%2g*+z0NdvGJh{ema1cZTyyDvSi)$E0p~<0Rk%lB*Bg9w=nn39Cwm(p^3L zjd$+o8aI0g_=uZ3Bf_o{a*bi{$^gG-RxjgTA{b$L{!Zy8o7jUoml_o|rqxK*CMd%zZV>vtoWHqU_DK*X|rw)3|-g6Z7V-B#m{? z-n1xDD;f#zVt5qSYx>_Q`J7>Vb4S*O)+{$%Qb=!q{N%;xekJIxf-q#;J4?jA)J5*! zuM3>i`GapLsk>e!70N#)qrIBuvd=L-xuO_@;d?}KYgOMbY|O8aQL(|f`NNgMY!9#Z z{Zf&1x1I4QDauuhk{e`46im7Qy_9n-jwgQAQE7;Hp0?~foKvgCQJwTAwlmGcLy7S> zqoR@y@l7y1vr7WKvHIi6$QZPo{EXMDnyHqZop%&@Lxd5$RZk8M4qb9Jt;6Iq?HSwO z_w4rhT`4i7GxNg1PSnSEe0(ySS7l~mV(itzv~lm@kWFcW3=&Hjt&c&CjMTkb?!ygV z$n?K5pdfvMqgMCgj`#wqo;ljg+S@O}s@Y?xUKG0NoIMUsKG?0;E`Co0 zZO(OUShUo(QQrF9zR>6wZLXChIYl&?Od)cRuSW^UEip&u{0Rj`UKpGrFT8rw zIlYw=)tjs%$(P7=wB;MuyCjy@<0dXCB7z#Y-o*pN{nRh$`=D(!?HH<|0*U+VhD4rUHoF$VYwhPCO%ZKe6bCMw?=t%wFZiFExYTh{xL=&U%iPY9%eYJ!y3!BH z?d(AY;hl%67`=jsoo$Vro~|yqF~1Y)tF4AFBCubgM zL3LK(Xqz95ny~#0V_2<*dGow2awWX%YD9FnM_v^-q-0=+Mu58tFSz!R+f-IfU*Raa z(2Izxpp+ze%(WTKj^O%OXmu4Rjc;Y!X_Od{TL1^D*!ntfCQG8!L`7O1S~O%$yOrGa zvN=5)V@+5d+&+$#Qpw@!xr#n;$^&)G)@lUbEZT~?r@+kl<;Q!Pxarjey1J5y5w3xh zIiaZxKupL73X^C7_>()&_BxWT@unIXEjEOZ$Q+mpzj4&R5rdIg9H3^cTyO&V1G}5O z`HMhf;TXC;;G})q8iSvnKEX`j`U`x%)I=lNgKV4txN_eIrqXIp?pS|v=AtX_8GQ;~Y z%p>#9SVd)t)4}qxR;gaLW5qXR_NO+yf!zGTd*=AlU7}9Y9J~+Qt1MTaQUBf*0b&Yh zR?xvTkD09oD=5(<{87<=?iy@>}vnxc;8V%c)rHLrw#t@7SN*CRMq5E`PmNU92Ju{I-3*?qPfC(et zGj`{2pns|)Q$DysrLagq+SpJ*_`L48na}#kM#|6$X696vx+!R`^Lf}DPS*Q^=``tf z`ua?==P4=r;4W>)LxLM@s`O%M+sB;O3#s7Jpm@@<} zh$y8e8@_+}ro6t!kt41pQ9&S|gA1;}kn#G$(r+gwP~y@LZI(B!~ONK=;B z)@-xkCQcLWSmEWo&&fH1*H5(@Q?WBh%Gb=|j?v@MZy!G~A`f;N1*o;530MRnm*7&c zwwMzF@%Qo~Doe{5X4d@4td^3iT^~YGODmA%W6J0@_4q4vRL)EnYAL}1qN4w=V#8JK z+>M$hNuP6hB4N(Ny#~MOy;?Iv^s9%TdA$Ul*vJ_YwtFfRXgk@N+fHVVCkd6_{H(CF z;23=HNfkNghXCfnr=z|afuic9;Em=>Qej2(OGq_6a_4Io|4bL9^)O!4j=qD+TQt|6 z6aX5#y_~@s(eMR=q7SY~5g)44940rZ^=a9AQbj(Pt3pFT49|s`S}`{}2Bq1sI*qIK z{@NH%vKcviF~!SeaQWM-@3pE@wv!8U_^ny!_n_}_l{&f9om$Z$CL+?U6)_bvgQ%GP zq|UHkw}ZDluGFclFyDx>&w0@b{)-eQ0Kz^o>@!%wfx;)l>l_IWssjei4QZp7dV9xh zrE)>2O;~Z|xtT7VS$ZrjSu7|Yy|iTC?-7ffL-LgcVB~A?RJrADxQ&|Iy}Rym*#v%;D;MoFfDq?sfCqh2#DPg}MU9V=8X zb2w7*V?A)kCTs4?D#G^N1EDD7r@@W8-pH}B3O!z(D#`Uk(C!Dc3COFVJE9=~RFBX6Y;jH?VZbrFK7><|Qx zxo-U-vaFiaj+$Q^!M?%hi;3wJOqxM+Tws%xoBNvY#B1xAxq_Y2D>sS_&kd1K6m!-f z1LV}?WMkab#T5Z|YyXi2A>chT$oeq_#iaFNgs3GjK27^`JBZ(+r43K=v7TtEI$8*- zQ6;YOb5LZ^G~ampIIL*I>6Cxwi#FOk^Cg@nJ59Ko6CjHrvWGB^)RYuXp9s5!Y-rjG z!0*<%;Q1 z1WLQZQM$$IsNlMe9PATDv^wXz31}RDGG{6}HJ@;aGXCIGh$((^?kuwdeP`xiWVR}I z=sW)xQyN`?X_?1mY@Xk=(PO7KSKfG^d)W&lCiKYYK*=VVW$<+jMNv>v)40kR?(l^^ zBI{k3TSUSV_LKWnJ89o$q27;a4Fcu*@E%rH?*4XY+R^!z!n#dII5Bf@Sn3dV~QE3Ntu>;Y>g$6E^Q!Asu!NH=Fm5 ze73GnMf?%EL;PcG-I)JQR4arD>ekt8!WAu#Y%>#d*tVE!{BF;?x&D-@aP~;LX=nL6 zfa63#tU*zluo$0c^5)&XByW*8T^0nsxYzO1GcPH&M_g|Q8eD}^w2T%VMJU=WN)lUD z?Yb+jH7ooPQD&j!M6pxe;)%Z9ZCfHJ-+y|$cBZ4VO?-&F^?8W8aF(1AH}+ptjyG}d z14f$72g!e*)zZF$a_01kK+@C>B__R(=iLtejCjwEo_8eK_}Q@_{n!;X!z>fqQ1N2# z#eTXia_(Ao^CnQc7i_VsKWEh0XSB%G1ga)tV=|BAr+8B?``-+N?kmoks0)tB8NM^& z^h#NC7FAwP^6+t(ZLU^SV#p<2fbD5@(e!jKboBHLVNVPGS_s}f@~H8u-JWfBx@Ei9 z{sTGZ^Fw`gYIGDTAraZ$Nb#YwgaV-ck}A*7V{e-(43`;Wj-=C2u7O`47zt*OU=1DI zzJ(9>L125-T{Ly$8;AY<=p8pWug;&1;hr9?ijUiDYQDS>l;`W_On=TedWM?tLsN>6 z(!~?cSP*h!>%rf9zMC-?!V7=r4||yuw3|_{RSqoHeag$*M&!?li5buxDLlrswR9-P zqV|?+XvkX`<_vW(9HR;e&_eClJsFY z4H#=N2dRjn!rIf9+2E0YEbi>q3v1r<( zP49{hwKuGLCdJF%yu!DzjGm|aEB*Z!5e6bJeV*Nn4%qC>EHr|{E|p=hRWUJNkGU3q zy4Jg~R)rGW3yzmAe7xwy>j$CahYcb_4yN@JOmardEJ)_`eV~foUUh`lQ<4>n$2@k3 z@i*apdmzLMTB;f_^T%(k&PG(fo>0)_ttwS!j?r>zEDoHt|7lNtDPJ_+dDYW;bq)>Z!fdw-pyX`B}-^9BTh54W1LW_vomIy zVU=Cq1|$C!@}0Ib&%L#G;*AH_4>ds}hn`PcC=`nKN4dz~wWBZ?B(~kG>hwm-bFz5H-&D znD#FojU~m!4sLE=r*QKaWWC=nDi>4}Ghc{l^?Reteq4_}YM^cXtg~A7QlddqZF{B> zzyC7h<~P-cFp2R~U2b;wMpsU!I*)_h{rys(Qgf=3<8Lu5uA#D)q9;eXyoq}!)tvsc z5zTO3;RFA`Qy!{_=F-yAy*;OjV|YV}731OT5$FDjI5c=~x@AWJ_oQFC#X44K|azDJuiy^?25TaDKO zr0XJ@R+}ATgt;G0S;;<(*G`@mpsNiOU2RhQ4+Sdlj@ZwuR!-UpCUt=+l`Q!0d>L4= z1f+)VCEVN9+q(wN>smFbp7;1(q+`Xz@Nfc>CANL5_N;Ima3qtOobl~u`oq^e`^_t9eNiM(N?z&5i3iixdeX|uuuWyo~#CCPcW{>Zg3np2Yp6;|VVJqL+@ZWha``}&D z5!Uzb$==fH!|WkzEzWqb7-e)~l*OB0Mm-4L+aHWbP+dh4UYsf?*fBSJV?iYS-KRc`3?i2`b0B+ls+QQvRZv7y zn-^sgP`0KTH_DZoJJfb$Q9?1Ard&)&EeO_N#m~*8t1`7{A=Y^kdkq&o_Vy2F5J1Oa zbkrSTi*_aIfJYGThF7-@NaL-EQ9ugW5Vxs1XM*y&T;KG~458!I-!MapjOq@zKV)@J zNskXVhdpW~WeGwqri0W0TkE}axFP0cNJIPn8O-=3zPekDJ5-S~-XTwuf}%7=w{3xR z&yLu9pS)WFP!{Z{RsvJwJB_4xNsYbB0=fUT&J6RQp z$hn<{J1{pk=l1bbZEnEAoS!rqwr^4|3CWn1jJ*{?FYMZ3&?K6EnRLQ-ah4?6W!lklbLocpBxND0Apmo&6-#%ju* znkO-Wpn!XEc*My2?^#h#4`=scdw>68sv=LDh?p4rg5$usVBdq}s{0OGQxbJi*J{0y`5qkyN}HkElQW-JEU0 zKr*m(e=Aj&0Rg7^A)>&yD`|e$OVTJ$vkZe7vJ-4KQ@8y+wRMys>HTNLIl0?HiEyK4 z3gIj&4BJG?mz)$L=@|@YE3)dkdU|MAVHM{FncIrdOBEBjQ%u$@6x}+{-iZ(22_(49 z)~@e7BgWP~-nZ`9Ywl|DoMm((q-J2{t1s=MV?nc*P$F4q<69vcU7Owo#|pR|l%A1+ zA-=QUX!>KPcXFpU=dK3DDE?8M z?!De)xfz`sgMm-A#LRCn15D!klB%vloWW z!aN zvi!Nm&T;!cc_=zgD#FDIc!?&j3J9xjwP76#j=+QD4uAlg*qP^?orZ8TIB5F4;UWWk z^t-Cb%bUO5=U=sBOGfSy;~Dv!WV_1UrPOQ{h&)KUCf6+nAC)H%<+{+yr*SIgk7+hr z$&Kk;P{@u!nhD;eZXE!{fV3VE^mZ%-@A9C)FK_nu2_+!BN+O1=OWz8K-66&c^uL!T znWUuokfGn0&ljzjA8NxHPZ_a-xEh{T4F3#*xBii~oP-D2YGLXw>RIB-bN+VAbJ)|CN-+g)t zXeUD>qphQ}Kidfvrd>wnZj zDIMr7bg?UUV_YY!eBli#Q#g;p=WHMtKBEo{w6Hk6v)O88sp!0;2-#L@GxF1^_Iy{- zmhKu*&pFxL0k!(dk+qtmDbH1974W63oZKxuz6`JPG2qGue5y7&ULm_>ZU^}Bki5r_$tT&!C%bkXE3%Sx zk8cy*>x`titc$V!Oj+>WEXm;XWWN12h5PP&`{m`OnczbzLQ6}_VCiQ=4Ty^5%(5f8bzL)3c=WwHQpPgA5$qbcj^+`LO$bNuS@^4Re|MeVQdU;8M$G|)LH8m+Uc`J-MhU$wRi-cCoGMM~qQblbfB1e4PusV`E9Dz_9{VdjPHhS;vYotc?9Ms8@cxq2Kl~q&P-6`UgV&8POX@^i|hR%vCH$5Jpkzdg|yU@ zR!~p?aBGqhP(>FTUT%oAc>ynflG0D+ z9>;)JO%A%*-eo>Vn-VR|%C@F2c0(R;Bvip_&Vd7~H)e*cVuQI2yJ+?H0f=4Qw}%d@ zs|hBhDzSa;mv1(I=WiyM=NSLut@@0Tjrod8)zZH&5cX-K#auVP6%AzauqlTC}_$UicWnG@t2_ocEXsTAN!! zqE(%m*ki-)!M~E}>nUYemNmo2f0>(_K1l2YsK{000I_#Z9R35Wpr3%Mbq|C5s$#k< zQ|#Z)`Tb#xDS3CSaLLPQqKmgp9yL7=>=T@3&UoTs5zQu&8B9@J-HX0wE4kL6I@ZnN zxf1Kvky(BZ_S;CvMF1_9vaD~nc7G~&1*PQb}INkNzv8P%eL$uqmt*x!0p`nTfth>m$ z?|g%%@X?avUmP2r)F)5JABW8Eo^b=6tZdUzx&WS?HtuHNbA&4IiF%t7gPyO{~)4}i@wRpnVIv(;a8ORKF(@}TON6FpVD zYhR1`MwC);Obym$h`WR5D?4~#pr+0>HZf7@eWv{DHb@fGC^3;i>!sd&W~^Sl8t4lZ z$nHvR-8+X6eTVAt4G;`Qij2_x{->vy@ITg(Ir&cL0X7GPYys*&`|aHE3V9VA`6)70 z4qW|SzyrQI@$x|+RK~^FKv)x50E)yJ%fP_!>f}vRWJ2A-F3rcDMT7u@e^%|213wp6SXggSB>Mm`LGPxR&<$p>L+a zH|?{uh}u0x&H5?;B$8P2+X~(}q6MZLqnovC!nJXQxovZJk-_H@=1}_lyTS1a=HiSt zVpC}Uu*RA1BWrhCo^`MPR-x!eY>4!(th^CFxqD;y`E2zaX0M`INc7*Gsi`-mr|Kl+gUL!Tos;2vwOYS_ zSNfR!@04uG=(Q#0R-Tb*sngXtYdq2+}`eliVvC8*3{I1C^k6) zkUU>{OU?MhcB7pxtj5n!kE+QWbD8J#p^e&a;rHFIqX{%EEA#@FWshm9Y_XU5LmhlZ zWeVAHB{TYJtoF8A3z|1ZOfWR{-ET%-W~&2L9h*+?Tp$dV7ERN(Wy$x~9oh|T%uLR< z+F2^v;5A!lP?Nq)&&I}uCs2bvhMw_>i9eGy#rDV7+v)71j+z~Xb4bEN)7Yn=5-%U$ z!&)0zv)WgT_Q^5T^DH7Zb~&AFbfOpZwR5bhS^y)U)u~eta!<5E1@ddP-Nli~s0rg; z#(6}<1Jw!LfE5C7dKI=3lb!$^5KP%2f7^h z1}FnR)R|k*o-z+eBGhaZ3$)kfqNEPbKAp<}3GTO_b;NV&>yJmu&NW9YN$=u${ivwW zD$v$>m=NmSZH`kqzOlEr=YFDZv^~uJ4xmBF@fAhn!gtr^E0{rASHOBg;QQO zp6hT>WhHm&v}1`d(-bxj2%6Cp9CjR3gr}v1O%B0O(lhSiYAx^0v69>`_P>l)A8}C# z@Fi4LE~+<-9d1w#o!ze-<)KyNX|_8~r!BRt<4UYD|LLh_`{<9O$~z0uv|*tGDvBpI z+1M@Nly09osV(CKYXLF~UL7g=1F2cWWdd`l-?%nsd`{2F0-x_I^pKax$7yC%V+?iG z%Ya88>HT~vENcvX4_33w@7B(}Ixj06StG%_9?`DPG&XdLC+XE)a=SaV?6haD*4YlR z`l^BLTEZtFa046n&Q=vMj z9SgJVzg1$-3wSJ^R6|ZYUs+@F@*b_|P|Uh@xz{zs2#QRzV#bvTu%!1QWmk{Pqh1^j zdnm6j{2MITF&-xOS*;F>DAm&zy67Lh(+(jm{c(fESMh6tv8c3nrJ$$eO-Gl7_Yr>U zuTPG5K6}*ER(YBt5E(F1Xiv63yYx8bY|vRcBGZf0%jT9o(lDOBz?%Px8)+)8GA9-EO?fbC1Bzy#yo( z+DXg{{YB>ZaP%z%Ls^?v;M@CqOZSUx5esD)sNS*_BIPmT8_uGg!)>|H(kj(YyBJu_ z&E`t?A%|0~uc*)z0Q`^Bs!8&w3PZ#{v|Zy`77r`SYYxE2#|NHbe9Y3tFr2VSd0{K> z;#qEOYO?3ov?4>hmN|z^)wZR*#u-(f!IHGLAi2{595E2RYlh1wRo zPA@i#2s@7$xA1LP4HX$zWfazIeQwI>9wA?>ORU#Dj@QDB8$y7M&SjMmxHsItbp+7r z{?a$~zd|bGA&WBGubNE+K;ZFEEg7RH#^V}pJK!Gf98yt#Y4G?YFTCKKr`RZ9h3der zdpok*=}ymhi`@&vdQ7ZAnbxUAZW{AnQ9S-LO|n`?5AYUXiI0d%Ng z$ry9=15yI4gR!~#fpnzoa|v$!4idUlj&(0mQKxg#tFe0Gt8bZc_X;%aAt_kXi-Wo!h%YyO_R!S@PNPTM3Sp4dU+j?!no0 zgLj(7FS67skiO<>cfF@g{OrGN!8CHGGu?ljtL5=Mh0S9lUO_AbP+4J_gmV{2@+r#~b0b zdqLX_wswy*MkOh^c({ZrRf+j>dNeU*Z{sqQ*Z`*F$pg$IpzMI?(L)0R1KD<#uevQX zQvTehEnw3~yzhI~S$~tP|2e&|MND;bDam~15wKK{<$&A|TjRM^(kr=hW>2p1)4MfP zxSRd(4ugNDbTWjQ!}=(ZZ~+!Ugs1`s?O~Tje!|-;iNC^dm7C zA3DVt*I#)DX<#CN7GBKTUqLC5!V}JjsRj&qnMCQ)i}2k|Nihh1dg<7Ka^vqby@7I1a5>ON6Vao>(={u{qw* zC>nX>&ggTn8lQ-N`|16NIhL)D14Ub2hwBgPtgOU83dtXnCVvM@iB-;(`y9R6#NUNQ zvmhkKAHsXj*c_SP2hg>?nEEKk>e#~Ea(6~71TyAjq5|-L&*{E+x2MH$j`t(%X{0?~p;o!Ai0i5T2k}c~8Rb?L+)e|uLxpuHJt_py3m)|R}T4rNSiGD=Zv8nGBl%}RO8(*Z8! z+`JN)?8yf*H?e8Qiu2QhwaCcG*x1{0n>OCwlhNb;N6VUO^bvC!GSXe2KGMY(%9%>Su?k zDsg(}O`EJGpf=@Ly z7mgKx$22Mz-G=ef*4@HBd%hRO#}*s*I@LfRSjE6w$VJ6Koy$K3`xcNnOM4(ej#ntv zE3hbWcXa%5n%v`x^>+32)IW2C(*H)$a{#6RI1vrY!!vK{)_6d~Q-HetA3Kaz!~3C# z#?U`$jCBOUQU@1AacX z-7HUY_2|sT#-^}v1;(E@RCDSyxOOnt8U?yL^M|Y_6c)pzjstyVy6K{wL5T#=%b`-H zh`vLTU037CR@25>+nT&DfMWU<`X-i!if-^ZN18GjUf@-Jb?<~z7^=kp9C5yqpPw6` z1qLmd)0A5~v{iQ=Cb3_xNL_4wKK+ZgzOk{j4mbDCCHd(cxEQg+HN5pJA<0EFIqk7_nmA_h#+lFKOh0ifacT4Z=aQ zu5I0DL%yNXS@p|wL3&aEV%@}*v(>GaUzh3`u%Z$bSnvS0>A)cUvS3nX5VZk++b(`gk_}cAIUVx4+VdC6gQ(~{lg@__tPFt z4oy8k)m9Hq|CN<4pzOOtCFK?~a`X1ryV?1_B*^2fZaEmndemV@rhe;i#)I$^5Mk6B zhTQQh$M^D9%RS_aii-H? zTGI>|2wHEx4%O6AT>KNRw9)+~(z`xv4p}scdd5vfxbkz)_l(7{HM;I~uba5GH?!eX zUX2LPg^yH5FQA+Nv83(4CVW@sMru0ff+0wRf@VFIfVJYd+aLbf`Hxwwy6&=EUb#Qa zj;@>CUK<=msqE&UXup@FRFV>5aDy>FR7B*><9n^GisIBN8`mDwlLo4yf*1cKPvkTu zKQ40TDBw}6&}|FgqFb0N)0%_tod|OHG_tPF6}=T&TW^!Pu*e|2j06_uEGoemrR5J) zSc5t0{dZ!&3>`r}8bv-saUCu}>OJbDGBpPLsqn))FtQ)zAuv1SIZ5Kz^MYiFeF;v$ z|24zcR=>h{upq+2Akm!*n*P<==vUalxA~$yj8FZ6(bb-=a@9qdys_p0VOj^UdAx~Y zphH{sbNLa4=#ay70YTKWeZdH#Cyo@ECpUOOaW79(;X-UnFi+EZ47z#v)sZJ)trx`0 zQ(X5_JzLB;RojDK(e3e5AJY`SE+EM8O2}LN75{^9@B9e6YGOS;}@UwaQ=4R9invPoX1 zrj12aC>IF3Z=;sb?h$vR54DlP^rCv6CIFq;>g{HrF#WUNO5F3XtAiR8MK1u-^nOvI z@JZZ#m&Gbp>unUCa7EgX;-Y5jHPh(3fy>9poora1tn%KiiJaY|24L=17o3N96ndCh z9vd;|aG?6#3yz0nHym9KIJ4>_GdX$^y7ExhiM|RTuX;_f_$HyPJacf$9+L_sp6pi6nT6*a3m5z6=7^U>>}9i?c0y7I|m2vbWmi<_U}4g^FJuR&&3Kl zf4>K{ecRfPXbnqUyzSOLb^PQx>tio~`zlKzRf>2xt?mmj}9;D2+UU722do(kGFM%-JCO& zdjy5F0V7}nTqvFZl>^*@^56YIdl=yfj!M3vq5$J$;f21V{^@!u)YW|fLU;n)4saN# zaDXmPZCreD@@yKaI=8-4lWe1GP0kAMJNB>}Az9^|kBi2hZr?N`Hrb|Y&2_Fa7&fif zVtfl@B19jd-3I(>73v6avxclG>I*|*A;CZ6i}5-(nG0s<=WdZRSfYtKHfol^k3J~b~}gmLwP=M4=eYDLpREFqAXydhCY z5n@tLUoZ|WcrsaaRBeRKqk)AG-Eollbf#6UG81wH1?0u&Xqr~gzZ;(dDrm|~ z4AbC6gxO6aNza6DL;HxC2*?@aN%V6HiZRMDQK%b>pq^x$@8KA`UD1{%dT=Pm9HP!m zQ+IjFCA`^YOVw6P&T4;Dp4?!IE%#XZH1%~rl94yAB~hAQ&g;UX=!mYhm5x?sC(n|U znwX5Qc7j$!5_+sY|0gCB9{H>qG~Ymf)@n$xDx?hkc`G!?2|7`zTUN_enL{adF?9Pk z9dp&o@#jdLDgbyfGOCRwCt#a}nyij^*7)uYjdMC`vG6&vWiiE7^8Dc0UCweaSDYV# z)rvzuu->llA)W;ZXM({XVA8Vrd~8m}?Y)&URdrWet}iTfCJG~__Rlz`b5#*ACWz4Q zlDdLHjXZUI4KSUA&KakmIvIq@y7bA`*>;93%;zXwG$Ph0`<9kFK%NMhCBn z9dxr>$E#OO4u@ljK6@><lJSMbZooskzA*OCsH&gC%chcpz?L#;cdCJT6awQ z*R;lGc% z7bUgyq;R=&J~*N!-g11LPG5fv-m|?)9D$XSD?*_RZ4q08)^eoE@LQg5;35(dz>$Aj zAr{LaZ+NBsr(?u7D#L33Y`_c{Dw~xn%qbKcniu5U)3InJO{S{TnhOs!CI3v( z8k`wA1G^;-2e@bZObh7cGU=0)?%$G(gi0cCZJ1&8wFsZB^>18)`o^zn92QRM>o@;y zpKaB8I_1SDat4FgMa|yAEX#&^w9ik~hTZ;U^;kXXr$Y8`00uYaK-e02ex3eyG%ey+ zC93VIS(&g1PYl)FQCs*Jt^2`%)a9e-t5;vDTd1FdWEM0cl}<`BMz%PR+}y=6A-rEL zd@mBT_ryP2*+)I7jsfqfO$TPS<;lA)o_~>7ZsH%(mJzb(nV{)cY1a;HCj+4$K9X+N z53pHscMD)8K;E5LQ%ITgyF80<-iPTX3uxSNtWeMUSyraaeZ>{?CO(Le3ruhbR60N- z{|ktKnK(*aD1fb?`)Iie%vJ$xW_yn>~jJ!NfV&Y5r$HWdc?QN?-RJz-x~ zNZD=orolDvg5iA>03f1iyzvTpH$WVh(>p6a1+{dR*p_~C{*N2NL1cGC?XEk}@2FRw zKYkt4Ml${q=yQYMNo`wvU?6^mmkD3ozqiDA=sG?NJNe;O2wUvA7V9cytHbP%LE9F< zr=Tne%9iDg%M$(9fQe6tpO+eCSYO*hriOQw(X+`ADhd^PsT^fn!`|j`KA9WptKsM_%;y1*jzeD@B({b zb-9;?+B|+Dd!bced(q}5@4HdZ!vwyETbXGHZ|)HVQ9aO3ZIIn3VB^}tAGB7cP}3lN zl+8BFxZ0+^jSu}RR`!+%B6ubLLV zJmo>>B%TvfWNkY>cZPm6IN@dmK~em#tb04%tt%#44iAU)UcxNsbOwoGjV;u%-C`tk zI*0Efxo#TFb+il*s|U6(nEdSli8JUQxRcNhs`u_IPnv)Xm|ORbtR8om3C`_6K_#i= zRim8P(2k#RN2WxR!;Rz@JNn#!^d@t0l`#Wv^L!F21pQmfo0FcNo@W9|^Hgor3eyD4 zIwOJkbUJTRvQz6@&~fl&>|EclA-e<`({1r$9(iXLdOokdGoJOipx|~i z_UlkMgeQ79ReQIg3;V)xp(WlsdM+@KPevE}#z2X1{w*vDXV;i9>%_oniw5+qoK|tf zE7&nnPT!q{ia!x^QH3RU%hnjWih1=W&pS3wPO2zm8%aP%DWXLR<~w_y{+ue`wSgEF zmeuZo_bV7By{0dsdT4WVb1UGyG0(}8q>-O?EEp>eJk=VtU@V|O8Hvu}eY)lLlz*X* zaTW?UgSx0rypb%n{aJh2U7#;072TQD5})4R-D3kEqjCRY13xi{mUE%g(R>3h*bSz(bLLmV}9D<>6O#Yn)v>m&6dt4Jz#6&+q zLlK=U|8jm&iK}9U7*^_@d(c8%F;NcT5-$8Ic%nW+ShT%K2)mT+`>HF>7&Qb6%`;OE z2BP((jY&=9ut@m?Nco(ZZsc6t6_(*HbI<*5aTKG<7^bek<_u zf@Rdt>$tk+B5-K~;LN!(GYEWT(|*P+Z-dpg-!lZHyv}xA>RRO;3l3x4f)YL zl*$P6Y80llE?0m=%d^F9xadnQe<=Ycy03X+JA0g8^xPv2Ek4~J?!*=0nI>Q>pn#u`Cbj~8O1EbuCjd9P{<^5_EZ_;p zR}1FfL{FvuGt2iQu099Ukz6Hko(}^70l&i4ZzP^!sA1h8F;k`P6aH`=!dfVYvQ zk0K4n@)rMAurtNW(zS!XGUs;>dMBDD`&ljTPA!?5I7C3B$s!*fu&{s+_5sq)Kr={7j_h(n*3 z-pv)R3N?8J!tG55aiztA>Te@!K?qEq_AWw!wRlJ&{pV1*IylH##U~%n`|Inu@M70K zQndj+-@eHiAg0UUy5egs&d46y4*Ej$T>aXZQTVi`_}AOM$9wHLUX}@5S6h5sNG0QA z@6He6j6h)fJJ59mcpbfg7niae-z1~?KhnRED>e~|7!YH;LCjzT*74Lz@>cMe_HKe2 zS4BHj4r1Hykc5k`(WY73aj#np zoj?b&CDL?T1mdmjtGh~!ualHQxKW?NTyD_Hr8E>wBqBkN7YLN{EEXx&c-I@r7-k;k z&=&?kHyOgQYClZ4Dxm%4KUg8Okz8f??_s|{vrFy(=veY$BADe}r z1e6dU4z!3r@;<66c-ED^t1M;&X4msG#udh?{4=IAwSHbo3R>>5p_eLEQ!r%xALb;N zt5ssz|5&oYL4+{l43Imm+>;HV-VIlr_nZSY1HJZK+=POBHnPh|Iwc7Ac}rO zLGciIzM1+k9A|?3s{_a8t6~Q8F`GAu!gU%tp`Wk&+((VKjy~hyDvOCIXoxZk5y4`y;~;^ts0*iEda0UdV{#U%2G?_RerZ+5Kaw3X3RDy>vUsMPctk{MM! zK}~S2DXx(kR50iYtG^tR@ZBwo(a-Sz2o!ub>T(fb*ix$G(juwroXkWlg4?zO7Y@Z*c??I1->07W}qCh zkA@LZOnCl1+{Rv(9QYpEEf@&f!ENv{JL3T8?W6E-gX3P(rx&Id8+|LZdLTMOM`7`w zgp*t1PnLCVi9yP@F6Y-(=9N+j7tk>K8qk`r#MU}jEE)Gm`)2>N+SRihk29DPi1pk5 zFQ&c%DC+Nf8x#d;1SyeNP*Pgy5Gm=-1(Xz!Sh_(%5Co(_N?N3(gr!y_mhKSgSa7Ag z+4ti2_n�vBup`+w&$;#xAsddReWUIU$7LndYQOsN@g{?h;!wq1Q56=#VEMvb z_eJ~Dx4;YTIIvO0S-GB$X;3H-tJ~}4fSwW9XOn9xjpNK#(`tfGGxv&`3e>v=gqe{B}vj^>e*mBD4ND$j$N>v(JNvB z+s$ivrwE=+E3W+}>J7qM?<}EBij1s{nDQ8GAvd)sy+P9hm(c#d!W~XO*x#G-6xaj* z8{btv{BSd=du_bSIL;9|H9rqFpD|UD+A`X1d43bqCN zA%)F&w!;*tg-WQ)n=CgN!PJrB6^Kjyh0Y$BqNBFlGcIDa_vLAMjnU(!`jm);R|iqY zfv=n$50}i^iU`X>P#RY?yG4+_yq;KD%R0-GxU7dpm=S>U0kwVaehwAAXM#<|F#;Z> zc)?>+hIVjH4S2a^kMhO#K(x|#V*jb*`NFi3;TR0 zNaH?r!Mmo?D*|**;|?{-CA?PUoB>HBr56?1vGiIH#6ReRX;y@Fb$5BjmCxSFodjLI zl#>vt6MeyKthez2M}5tDF7H*B0X~FCh2_Hssf$gZqx)+F9pe9%#yiKa7_Ypj%8NVY zKH+VEAQz@*`=ASxwX?ThUu>%|YxX()I}Wf5v*?;jNfySfPJ4TsQ@^c^QWgm0W*m83 zewR)ST_nruSXTmsJ#IK-nkR}ywxN2B;ei!L(y00Hf+!Q%ZT}Hk2=mS zp74yApMm$uYE-_WJPTjZuxfWyDfk+`z~F<2Kxo7uG;s~$TT)l~r`;arzK#nCXKNJ39T2Vq|^9`~jOzH;o?hs|%Eda3~Ypbyuso&dbeRlr4cIRpNPzAp-f-(V* zUj0z!^3~Gf%o>5}-qP#^M@|z<6}MImLk~fi%)b=6+0;p#etr4GOZ-vHdC9CJ`M=kY z`QgLqt9@rFvb?WWPk)-TnOT+DtZLy#QTC+g z6-uVCa+AmHdI2KRSPB3ijG_T;$-f#1yk~`JbFNtz8K9`sae;_HF}w3#=C7lYKjA&Y zp!>dde=yr73vqoxs_;pl0I`ds>BTyZME&YLT#T==7kG%aPSNGfPH;p?=f< zE}iWuSwYBYht)f_{M~>{?NNdb#}hhY<1#$}YxtWL{H~1&He;rqCBA!33JDe_4($U0 zP@mJoY@wT)8&AtaYgFhr1MWS#Yi<~LFu*_)*fyoX9VK&`RA(=NxF2{tzEDCm#a7hw zTW+j$7qliG#Q-K5=pfZ5CJ1lwyo(V7u(!}y5FbtpkUlhu<{VzvX9{lnzUX`cJF*Dj zuB%I__gbPhdd+F(j}=uOB1eGghgD~Alz0x?2@IH4=vPcmPE15Bx3&(!a3A{-5Hd!{ zudmO~G#s8RER;t?cG?yFjZRbckd4n=VryF+AGyz3V%0 zjT%>WJNl5lIKRZCjsCs^WZBzp}-ANsu7 z(0@gTua`3+3)(E!)y48SU+Y{Udvd1)wr1jH)aAH=zMRQo2Bjn~E5A?dH|~1khJ>ga zXK%51R3*Ftn55(LYh~}qTtgz3rRxoy9tx$sr9SOU`gTx4+FF^r==^05 zEVLfG-x0rABEsuDEJ=e~vh1)latxKeC4?Ee4FCB}gPN*vNqF2akPP`imJU}&y*A!+ zaNsG=K&8-4zK8suT&Z4V*8`Hqjr+*=3kGqNtu_RU@3=c)9Vr^z%5RtsIClRs`tO)7 zHs`arrU}`P(NwNo?S%yDT5k$SCQp3@T!+gYut*&aKnnNo^p;;kt{=`=X&Xel$2Mj$TWd}Om z1ot*77L1Ld{2pIXr`psZ2Tpc*z5s9)-qaWc>sUY!5 z);`>ojGNW3I@+C1Pr;}k$$gjo3bAJhIB~OI@72BHnfZo5n6af!o^1@#wua>_7`*+I z3+5iotKfP9*rzB%CT3&B#%b_3Iayq$i2g51vQbTxk`YVwYaO`t+Ruco-X5~IUBrB{ zG#Cc1Ds{4Ob9(Y&a2q?o;XAi(TaZQwI2h=yW0b^Yg&^>l>6F zl&H8hiEzlKBAVGIiaU^yy**U#P^b&v=1beCn%A73z1MuBVJw06ii|`@XApwHM)Vu~|SM za<6BUvM`CW^pKEaslFPR6C9e(T{W8-J5{^vI@8}9GGuW?ZJ@FH`CeCTz`e(Tz`isr z9OtHj$P6Gxk9d~8_#f=A4)U0c6>D&}lpRut8t4H8;(rZyh}QK5i^nrE2MlDhq!`|J zE#~<6yO$DJ(ZJ7-G1;I11!z5u+bzmC;mkjQVbB1xkmO*`nFvra#_&e!k|p6N1}bd4 za8sU#fV-(qOma@OHWjn*9Z*SDWM%Uq@y)_yjFzxc$cJV&f;3AQ*;J3y<^F|K;GEwT zI!b+B{JA)#x42Z|q^uoeI_j)Lj0$69%c=jEo8US8x37?tP1)M_t%ID4wS5fN+TL<4 z18}rTPYu+yBkp}&mi}|-Q>?~1Zuii>cC5o2;1;=_1gH1BUy-e%BD*L{7c$%0#yD}# zR3!i;O7#7)wr?5Vu2&`&34lR6e&eGB^W-%XCCx;ro}Wr^9G9}9Aw5TJjwCR73y|F7 z=5g{1tti0{5U1HPEc<4Z}L(C3yuntepK zLjugs#Wy$Y)coIZ<6u>q_MQ|jO_iTcGGWOk6ZVEnI zoH{EAw&q#n6O*Ucz}n7v=7a!*pE+nqrEqZp$$n1pw$> z+|=Nzdn-(Q!sunfwgkmtFuM?YEDXz75Ruu9A)Ug-=+Rh1?r zBow=rQvEz!G`=j^exyJDb#lFmeZH`z*-dr(IlR7hLR)5BuRH*LR=5Mm1k{9fwZC5r zjs)by0KNyP5mWT8f&Ekad#D8j%)Ep^PH&Dv8)}b}QUIwp{k$aHrkl$WKW*t_0k9W` z-g@|pfNkP0a=-OZUz2J3u-Gl$ay#K(JA6SmejI4Sp->($m)F=zVcn8R4TmIpSZ$hH_$ z3WXY@ve8j(K8{dztF7Yprxn5FCT{>le?G23x>(1^*jSn6Zolk4Rlgaup@=cu<&LNE zsSZ9MGiIvU@KOUX)Kl7MDEHdrTTQNPFrP1(@*6h=tbq*570Y7qg?WSw7~qw_;%>}^ zQ@}|@;Ggz>(I8=Zt%F?QlHu21*u&3u91J+ed)9KJ7PM1zT<_wByp_xY*%-j<>;rF2s=3k6Vda*tB|JsgoSpP^@DeK&1_pDl9F2yNMR z>j#FsBz1sZ=F{|~sQfR%=P-;HT!Xt@RgOY`!Y8uM0aeW|o~Cl-(~MQP!+wU!I`p{& zGc~rE_h?~syp;L>z4b;}M~0Ic&g8{Z;788@2&DD>GNJw*VIR$=-2?P!v1{R{E~0Bv zpNV)8{-O(6KVVw?qwxaDt^pUwOaB#fLFGNU;|cO|dC<&XQsV1OoJJO9w^z%Aw)m{f z>%YeZkNZJ=3lvkL<(aQ1Clt#GkyCs%nES`PD~&|8!xkQ2>kJwedbdyBnaq&1^$BLJ zXm1!(t~Q71zWkX@rPtz#ONtD6Z%V-fc%XTC%y2sND$1!NpS1i+cMZ(gQ!1!xX~|M~ z1PH#6I6%<^P7owe(%AXyKhgD5tBIt7C{0_jjGBs4M+Kr)${& zb_!<|r#H3VyCd*RK8ZCNnCDyKCb0De^v3L{Y(cqY>hV~O#7 zOaT{+89Id2LuaGdi~lkP8XsE2!nk6p(*>m6G=Kgebk1wBb7g8eKbe`yfSU=V_a8lu zG$uzp*d;_&t?b3-f2a}Pu&u~{+x>n*Gv4}v6%1B6{JO8dABf9I!H(n$VMQ=JOV+s} z?Cx(!eWNem6MvjZI15@j@DlSU_R3^lj0PsY_$N0M+0oI_+WKrhB!=?mLlYB|=0FUn z5C_=W+FDt4K*`LYP#{sO?XaR}Gk<|6y3Z__1Krv_98-vQ4DURU*gO)!ugKq&*Ac#31nNb!h;I`TV3u=}m<(>v#fQm^;7-RCg3k2<9M zG*5)WXv#-b+hHIaYHQ1g1I$eTlPN1Y5;nmL#9a9ojlvrmKxp591t4=3v&i3c+_P3=jaH2FifD!Ns=T`axg+hVpQsT#$u%Q3`?YYQ< z)sO~wd&BGN0R|5Neisr-0RSj;jY!M4bmCd1MZH*h2|Gbru&`IP;W^OO?ZXV}9)=7F zgNDXLvB9a9q133+>xCGmY@v))kaIp2jB^eqBWqeW%SIzkvwZ@XemL5RI+eK=2{!(4L&Y~6tjJY(~a zj=sk?`zU`mRAWw;+o`Oyl$(b~!KX3<%yGM8NFXP>L!y4ZPJVeXe~4L`6TTWIVQ{$g zG}bnGcOJ1^&KMjXSsTOuS7g&;nwK!z6b}KI{q^;Lk6x_Pvk_)&QFodFhr{_4yc*V# zQdhfsd*KeT^8x1$zU7zu+ktcYEOqipoPxd*Pjd9s*y?)L2*4zchUFAg-FjO}cyWRwJzT?-qgra=^{AkC_=FKlPCx7WNev#|-<+;G;m zw1k1z9e1;4&maO3=QWPc9*XywIb3QlXCH;``P&A*VXIKQ=bJNgfy(OFaNNSJt)3L% zv_ftjaw*9vvjskEro}@g$C8*nV9R5TW{vMcHJDHd)%i;L9_i{(_8LA=FsL7siBk?Y z?+F3w@GGjA=FwfdPk2|S1y}10EcXZTuevGd%Fjig6CV2^yg$RCrdfv4XiS0>KYx0wYh zBBoL#u57@>Dl*yUutDUU;k*>kDwW>a(#aGC2Ft` z?(9U03^!*I5AEFOttmFGV4$ZD@NqwkH7e^wS()KGQAEwZ7?*h^ne1cN!T2b^(gIVPq-x2Lsud*_fmi9-Vv=K{t9R>*a+;hf)*24FBkFqQm8A#? zc2|v&puWA!jh~qjIM&ybO8)8YelI_!xnT_Kv5ARy=m8ps9+zbTt0Fm8o=)CFRU_LT0`@+B|<;XF6rk3mR{F^%xUha(DGg~j% zgwn|T0x?$brhD<5S*0;;PaNwsC|V$Zk=M%lbTOkWIChU8vp1gIOxq&LZM#Es)`Zzy zn?m>}U+MZRc)xwoRrOQ8Mi^+an#Jes~Wbqy^sYPXxwE zns={n&VXnzEMtlZr#DG-{^V~~sJcCmINpk#%hs5ah#wU+p%WsygTmj%N=Dvf75QMU zh^X@ueUun=yI0t{TKr>&OAm*0X8~m-^nt<`wv7%Q4KnehPl!#Q^??Wa>Xr1VpK{d> z968GLIIaKfdqNS|_uJ|-ZkmdJIge(?^i2_0r!5Z?afQD^^X_vURoCa&Bvv-nxvg{i zKXBV;ctTSQ8-3}W%L+*}-q=P6-sft_G~6;~i~TLe8cf;)w3{uMQ-%BvYQfFNRs~1l z?vUJ_$2>dp|K`3_RBw9Ho{DwhCik*~qvKU?IA+fiaXvdw ze^sW>i8Aq!@gD9@FB{LOVYEHC{N89VN7WN0#EFV0HhqxFiOOv?e5;5JYtx*-Z3$SSJj=?YUk-Gv2U)Ba zTQY7@-3nsc#WQNkIgx}8V5j3XaVGo7@7#NdQ}=1NpKH{kUaNMfQIchxejFcP^Li<9 z{qHaj%iSsC9n+M*lV3Q3R9UIIxcnrV$iu_a=&Cw0KCb&Oerc$$_rhFRo1M=#dV+V@ z-Gw!qW@l#yx?IV6Kj?cf;};v115L0)9T%F=ZVSLVMQZnG2py0A+0u4)0&-I1GEeDa zitWgSNEaMTxq*rme-!#4*i#N<-U7=0P^ZTXldMU)3#4S?eK%%OY|R!7n|cXR6VC;2 zrnR`ZyB}U%o&)X}AkC>R#)Bz4i;Fq0sDUj7aV(%d#Jj*V$`A5I9oQxbeI-n~m%u*3 zA3ZTJMKiievoToCZo!)`Yjl2>P7c}_f^9)+12?-@ipT8)%xX#=-xJbSq!Xh{c({L( zp$$C_^NwLS2x7VV2o^)E8N`=Fsh-30Fdo+Uz%+o|&>VkTfGN>_BPZ)oQI20H5!WVJ ztmkw1mn7exWgRX{UMiuDVYx%w9XcD#_2XP=JhQ~ZcO*4Ic$6Au2aLg$%F_%rp~Un& z{go*txFJ4>RX~%_bK7H{$Vtl!u(1lX^0d4d1QNGRH?!s>Ht(QJxZmy3q;w)NVjX=x zt|PSswR=nEe6Dd!iN=SIlWnREMP|0TKeyrCpOA^u!EXr5RRpwaBeqXv5qI+qJ0Dyv zE%F?&)*=3Eh^4wN9+AxdHNo7JI`7R^J4VJStc8GoPY2gr~ET>{koj9w*ax+TX|=Mek*dvqeW@VOJe;aw^El}NesV| zPE|eEK4~T~I|Ek5b5#{~9G}Lpd-n_W`@r*#xvW|wZ?PMY@j8)% znXPI744zi?6x6`L0Avid=C-LHjM%}{9P6YmH@AaV4}M+TESp@PrP%5)I`Mk0*MsuY zIo;SN3KZ}d_P_Y%nvlmFD(GS)LX^A<0%cCl68|U^-)TCX9Ft>?mky4nCr@F-g23Ka z$+`C{{vGIRA!BJTj2QtN+w5CcQ)5}^tUee`^6^Qcl-~(27!d?&o2P!JT2IrTV9L{s z1-9p!@4v*~b|vrOEW+K_4%=hsKUy~XZU6Jv9TeDeT!G-V{V*`V%pqA}$3dV+{B;gn zDUdTbmA358lxik(b&BDG^}^D4UZBW4^ymDl*7yjJhM`T3| zElb8}tYsT7%PCyJRbDb|j~N|E&3E49a-MBL%e5-DSy1zmC$aaGOlGXdfjOp;x&1Lk zY~;e;OP)gO`}pN)%w8Tpg*pa)8y%hf-u0_d@!c>#u?pShbkl?|Jw4}lH&%#~#gEN) zWOFJN{bt6&ab~H zn_*h2zX<|G8B-i8GV!OtUjc!&#;KSbw68U)_2II8_3G6`i&Ar)*j%7pR?UzvSD22k z4(Au%Q4%@+_BUpQj_Yg%}^e6j{=3 z&rdN-{^-vdbNx#UrW1sy{?U-{2^x}*T*YSB+3ir(keK8B!mwO6idH$C*ap8A#$uh{ zg3R^#@5wIq_FQ^SIclfPl(JsjC(pVc07^U0%x)}I^Jq60IKKe04wCzLS^RLC9x3|( zYp(l3IT!QFn4c5J#MZkcJdGd030|{OqLPWdojUF1?`vJiYxiu)kOe#6H7JVlwkCur zrX({>Wt~;;wVs~{n%$y)q;}3fZj3+afX^Cip#i3mniP0cc&Kk@(O+!7y|Uc9ahl3C z_r&r`Z@b1^Vlr!(OWl!=l2N?TSkL+F)a)$L8EtS}7YH%$hv$BVcyT1P2WojRdmYCK zIhiVdYKW?8X!!g3?S<79szl1y-POq?;kz%wFL2>L$h3B<5f^FI$_ECI67HOeFQ>jY zd?0uIl8HYTyU^L-0a9@DT8zXwC~pM^cD5WL>#i4olP zTdO(0P)c*dDHjt_GLBrDGsqx{n^W@8gJcU>2gZgqsr2OJ>SFrUQRJmCED}J4UR& z)Sc}pj@Y*d$B&L&2(3(5PbLP!Q&)bfV z3BahN1F!n|zf4SA<5}UEPOqRhC?AE%V-)FIT9aOe$EojbqpP@%T zE-c9&EM{^er6k;(L)4cK)hR1O)>e<4W zy$jPKsk=-E^)SN&yCq5CXZ}LKI9=!A5y+gbS2z_fkg@=;oH8W#l&VW$vodNi6NdeMI+hQE#Wu+yUuQBn9J#tAi3(z!w&&v;%=~+bjrw?s0CAy!tH_-&P5RTDLt~t z1wCTQu6dupCwS2(o4phT&OdVR_ei8gvfZ!U6G4l@YxgwTciCz*C^DNkKw3zF*SAjTWV+Mo`@|s_|D6v`jW>KFOFwSZ=~4BIfr?% z)kiFhzJG>O$4_WsYx}d<9)0vH>xIoiT-epB*`{XM=ivR4khLWv1v2mi^a6?VDsoN2 zL-9IEUmXcw?)p>|EpZvI6|8rPow;TkqojLU&tC8J7n7meh3!Ar1$h4Zpsqr5Il~6n?@QAuG3Q4`k z%~_wIoE1%9btpM<9KLx`Bh}Rr;=(5wmi`Hu_p)Trxs8rQU-CP0IN55cYj=I@%tJ2n zqGC$trbtlgEfH za?BKH(#@1#c#cmlpItv?~7`+Fcpbls(7W`Jk^&W@UjDwFKon z<^>}}S(73&Yj@ZvTko~N*j)bYxj#e{2+#LN^jULP$vb^1_Q9?|?q3_=7scE-vGrox z{`4@>Oo^cUor`LDQuwRNneeiV?E|KWvaI{rU1#@0vB-hG^B2ff1B3f#RN_K`Su&F$XCA$)4^SBGL&V7%_N1d z#B*)VV;lDiX0(E1lTJSK3D>DHv3k+uH$3{N_im$+$55GVuLfzyi!nuv-NuE>FH*!m zGbXD#_{HV?ropF?D&HLOo>~`@WiO6HU$pXzI&7bwlwFV$@gQ^hB(KGZhDd$~XR9kC zuIx6vm~1gbgt;zB;Ep>55GY93lurA*F=zfDxyrI6-$qeWt}I;+`w@nGOndjSoa1%y z$JDApF&PaEOAT{+3o~c=zb`Iz7_y}#G-o~1m+{uevqN2F^qi`RGBnC>xf94~rYf>Q zPTV3EvS{n1uRQwdh-OSy(J|`G$pnkQ1hf38*FMS_x~V;U<<;!(sX;)-fK zQxCXgIo8h#b#Yqu%Gq%!*h59|oRPQA_Rr%B1_$bM70TkIv_oYAr|dpXK1k7}ohE8n z&TDu1x40`bBKKg)nkX0diX!>3p2-4rpO^~qlf00nKx3|puOH8>>^9Wh`?AMjvq!p3 z+`-{~pc)*taIDQff7K)tnZx#$xvw6=_RJRDVboelSzV*Z!%UU4 zj*tggK+4iarWHe#NE~DxCU^o?UXK zgPP~s^#x6N@!!)Vwnu?{)@@La@;G>HyEOUiB>jc|y;zWykK;M+yZMl{Dli2rOn$t5 zijLSiS#ZKyrd(!?Lba&8*z{-|w|kPVsugntKlSzX1COeu1U{@@UG;vh zAeN?Ep>$~^V3tqnT?V)@C#Q_Jg9Lik{xf+nHvH!}gO-$|=v$C_Y9e}VU}})n-?vJ6kZ_QymulQ zU^7|_fy^fLXTKezpT{MPqCqwplEb!ZO)W1Jqdr7a-^vxb`*NT-JO=5@1pThOczzDX z-j>-6wup*~vLxPg?(NbvlHW@-G3OE}r!;ZnU{)H2$wW9}W#WO#5Cki&PmFF*b({Ls#4 z0^{G>7hWkQ$u@V+kJ`=Oxx7rVPWQ91N%6gf#RJSR^d|g5Attb4y#sbiiF|yxTX*wt z7Z^4fCg%5ej*wuBTu?#MEjv=NEmNoxb96+h{FJG;F3A%%f3&x`Sv2(~*~$gWwEhk% zrV!$xDmJmxU6HMFnrU$=^ygch1u2b!6{18Plc=!e&PF z(OFu*ZF=xbxdM3JSnQSu%hVn^bIjHiiGr-$U* z$(<-yI)>IHJV`-f7P9A>q^Z>})bZLbJ0UL0%Q+lj>PXSsFsE(rp_H49tB&$X^ACV= zGBxa2=i<(ur`Il?r;1YNOnQPYtNA1TKa0W5M^wnsSWHDVdOroFOo7B?Y}gH2LKlMQ+r0v5epG-^+`+}Y&rjw;N?Y3 zXl_^pvSML~XmkSvbLzFuwv{EJsq^RwelsglGQS)0A{F}nA&htaXv$wZb zK_MJFe{Q;^#}#ryE$b}XIi1!reNw!LY0Y?LKpSht?k75NnVGnSb;6#~yLlDcSj3}# zs1vRATx(JPR$y>F*OO(eyUySg3RZQ{#1Pb_{NnVAdD#CjZGgDeHu3&rqtuBVsJD*Fv>qtk_|(*~-g#f7z7e+5PN3Ji{Tq zAJUwd6R|AG6wLfy^qHA7DO=cJ43Hg2|Au5~g%jbC);n-;Py{?wG1$DsKohl^aIF*7 zR4jRGZ#zBO>dG>*288k{wzz?CM?mbKU35e_ImkUR|91ih}n-+q?L#!5-9LReT-;XyI0+@sjM}!M@ zKJun?dwO_GfekgNKG1si$;{L=>r!_KjL(rCQ>_$KV;9djI?Ad>kV|Xypvsbxbm0$f zt)=%r@=tmu>-(lwpZ(6@Co&Pd|%Rn>?=0> zk97X&!t7zl_wIzbo&@?>@onp-JsSJ*cSiu8^rop|W`U1n#jyvHht(4qolQIUpwHHE zDW8Jg$r@q|9tOmYH1uGAPSOKs3?!30rjh(pkONmOuGP}n1M{OeCJ=S z)Uu3eKc|#wzOu%o`zL+omXmeH(z6H95IA&zm#~c&Os*&z1+!B?MGj2D+N--+2Q-33 z3AAE=gEy9diGRdi`EVOImwVQk|B2s7v2+hn;4fZ6m>o%B2e|eTWEfl<6Fr5U?D~IIM;ftLWStoY<_30})4J8MXTX#}Mz#O+L{40n*s$6K zh9yEq_k)z6+Hn`h8k~Z{IP%l=b0O!u_FbBD4)6f+kK9+sJA1&nKO9NoJ}{OHxJpZP zbuU;WnTF;>pG&`gUS>Il3Fg=E?@{8*J$&zKG}i^XpudT2&B0`T$ol#Y0tJ?p86#M- zAPh_ZjiXJjkL0b`^%f26Ln=hpkJOB=NCoqxnPes=c_thn*tl99f8r6#ZnfHKRTU09(Mntjz z3-kB=vcKycp{Mj0;h1m8OT3_7N|wB1vW8_ma)CHtXLeLwz(e)}^J`Et9^Al>MHbJ; zpR^oFuk7-~fGYp_u_@8F{+W404hR0*WIH+|*B=H3;=vONt4{0{EXJw1T9M#a)NfF- zjHTydAkv$$+ERv^%_|!5Sh<)lKEuMImUtoiOy|pK1>&kv(=6Q)OC$(KNoYjCKX4v9 z^Sz;KI0;v9&!@Zuq;U9 z=nk}?HbP(~CtXkleE_v3Xp3pHhbI_g&8PLQh>10BY|BF3uT3k{vl%&J4}HEMx*yau z%-SO5e=1p&3H%aob?;onV_`NoC4d=T3O9``qHBkpabOi%7CmaQW^TW!`Qu8h{dO}& zVBb{W*HfF1gH+;;)L3N6j`9R~>P&S_Jh+eyp1AsMR>fJJ|m zH<~^OPnH&(1QqMzZEt0zi?d6b{wO92EK6R-oCosU?KXWH!=D&qlGU0fF#I1#61NEh zDdfYmP)}hE0hz)N%d79yJb7+m1>u+zLT1J{3&z&hfFTNLp;LKk_3t6kC0O5jsA<$2 zma`i%@zP&X!MPdk+;>F^<)4Y?7RD|g=wcDVxFnJ-i!(n|G17RL)y_H=X_@vv!TtF| z^mbevm2;@6(wHg(l17^NvwA}u{b?;9b7$j(&-07t=*_-K9N9>?An+!?#)y_c#EG-} z=aXcNvfa~ku#i{_kacR=NCb75sZvHC1rkWTF_3!V()B4K;$!T>_A50)Ii82f){+0V zPrB$hS-|U@=81U|;{zWTIqB52b1~%t)lNp411)ohLSgQwZa2OuX2Kh$t9D+M&GHfg zGx7GHeNpG~VXqVd{Cr?W0Bf5T4VTd9)j0p2&F#c0~Dl!TKn&NT_x@F|p2XvwTdbE*#L zZ4Ul><^S`JAw#go|27^Z!Hrl!iyLF@3a2x*C0I)!o@SDLM(0sab1eZ;Ja~-pb4HSm zDx!D33>rCgpccdR?dzsNfbi=WtNs;t!uG~S6+*z^Kx-j_z!lj@ElrFi4UQpiueLGc zOP>im^?PMw(gni%FA5>U&oM|1)MVoc6YV z4^m#29DBby69?yqZ#P{?*V|8ccSyk08d2&o+*<{vNu<$jdBfNb^jcMYa$kln#1peAo*(v8v6yP zuhx}xA&NY=UadM@yK?En%2}9`lMGfNSrK2vEMtJ18}%WEJA!j?{>~Iw2aQVIj4KvK zYuX!&q=$SF;PvihAz`jqB4F1Xp`uN@6poF{x6nfgoHuwQ|3!uOb1pN%$3$EuFFsgT zr}a0?GTkC|DXL>o5jS^j-BPA~*Gz`Of?5sGdmn9-xOj8^v8Q3p<^hMo0%0th90X3u z!)aT@kS=aP!C`cxzJMg&0$D70{JTH5 z_ltoMjc0UE{&c~z23pF+I(th=*nL024*p*prg+A!;VD;=y2B;dA<>dHSsR3ycv=rV@*-^043>>Oa`nejjpjO3{6b=sX`=2yAD+P~i1yk-ge z!I5f4J-NOHT<@+YmjwNhm(7S!jN0D2SKpGzBO)DnP~#=@f&2ZYPt#Ge4Ac}VbQR-v ztFN%JmN;e=15YeFnU>(wBoo%Y53mAgM0q+I_r=~S5`a03L3Glwr$CXg$m0GNb#;F6 zX8)IUkFJ7r>duXHM<7R7+OMxuKgYd&tdm~R`W;YQK{g?dH@89Y`EAF4S|y#)TWme)F_?iVKia|6fyA9tdUr#@}Ha}Kj$z6X#c0-j z{GJ*4{eJ%Rj@LWy_w#+epU>y{KHsPKpRAi=86s&T{l&=aQ@Z%lK3;FCa$b~O<_ zuHr=Sh=sviXeBWH!%{klMpE^7ESz zo&N?w`;XqA82QxF{o>FQS>*I9_XLT5{aUcNEWTt`S~j>cGT6tC{_T6crMxMqo}MjI z7QXT~P((ffWiIk;gN!HV^{>44svUGJ&Q0`1F=M_LfFoO?sn*-fl7X&LOA}LB|6_`UjDchGEY!0*yjkd*PL+CEY)>b-?2Z!qtLn*1=tLry&o zn+2jhtL>m58%Doyg@}Hz-Cj|Szb*2OEiKep6U=~3fY>ZuRmti6tIXf}PKI78k3!Oq zP@%=U47P`>MN=O|iv5oGfc9fmrT8dC%Rj!T6LDN6fdzv#diR$w9Swv?FU&Z1oyYd? zh#XUt6M1hj4Kq~&q*>T_aZv6+WKHBX&PAFuCV7fC9#Nrmc z7On+TG}_-&3j&%!>R5=<-ogQQLrF)1TD7>bC3~qN!pO!E?}^ioEi3bfaiC_yCwtsb z=cXOrW{$j5o=^S(opzrv^wY_cGL{34z$iDWuvS)bUj+p_MORO`h83<|QqLc5)+B(E zlxybyHk`*MaomsXhVxOuMcpq*jrW)=ulLsG?2<6}3J%kqm&9XSc`-@u>ez3QDG8T6pwX z-lcl*`NMnJ~vdiDtYDEPbqGw>h`zx zDyqv7aD&^kEhOOY>|>Q=o$;vi95cPqGmconiDGm!L4CNt-;<(Yg{vbC(D{rp!OgDh zCR>YthmM4JKhHVJTbFL;3&l8@5!<{fr=}D*&QXtmRIky5Iz^n&<)skNPPJlgxGB#|Im*V zyGENmj^+Wrw>OEy86J3T9~u(YGCr^~%$?2{$q}ZL%3(3G;KmhacxI{lJU?M!K2IpzIAAAe&72E#k}b?4 z+_Z1E98#SS6=DUF8oJicd5g_ht8&M$Kr8rFZQS6*q0GKiKE^J~f{Zi=h=QWHx*bVI z+Au}-iM^tahbb${42am#dR-zZyf~ENZpRm#!2lNuZ>`Q{zwh2T!9rc|=h#?zcJorf z&t(FMhw%cZLBD>F+p&fEK@2q)%=*;&=rCC|502Yq!wthqo#0`C2eIzkXwTYhHeI-Ys(U_ zZIi;8&}E=2A+2qgIM%A1L430QUAG+H)2w1cd;5h9#~USfNlC2B{DW+=^Qq=bZ1%8e z!@`ZQYXmJR)qK2!`-NnZz7DFwaKTDRZq$eM#Ug(jhkO$6DXaMc16 zs&yEdRHXUOWXI=^2sy`Wa2irXd02T?5aak)`RW z=(rw(7c;Azz87u!4Cy8KPMuy*Z(DJd59w*HlLq_SprL-v0Dj|;XV9}N=z^aAXa`Lo zqU|;iQr%W;+9j#obDlD`JdwiXeN@a9&8J6!3C@$%?G&Hy1=@gEgAzSsdYR&V_aQ@` z9z(TNEOY&}^E3mA=+>i%;G>LmY#&`A+~1Yaeri((+wA$ZDicBzJWD=4fFcy}0J7fS zStqAoIP~(3)mS- z7oYC?FK)`EVK0uZf-n+w1TQe_@^{_t1g-KN`JbzcAnP>`r_`G9?Dg-`4Q9*&`t4bu$RT0}nZA36NBcK=?K)BjBw%HXJqD~StS&EF14NKk~Atq|;3hx9tT z^+V7>Us?Le?WonW@|{gh%89S$jyK3%i<(%KEw8qcFmx` Date: Tue, 18 Jul 2023 09:45:34 -0400 Subject: [PATCH 10/11] polish README.md --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 4b19e82c..9dfc1365 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # flir_camera_driver -This repository contains packages for FlirImaging's line of cameras. This repositories intent is to make use of Flir's newly developed SDK: Spinnaker. The camera driver is an evolution of pointgrey_camera_driver. It has been updated to use the new methods provided by the SDK. +This repository contains ROS packages for cameras made by FLIR Imaging (formerly known as PointGrey). ## Packages @@ -13,10 +13,10 @@ Spinnaker SDK. See the This software is issued under the Apache License Version 2.0 and BSD ### flir_camera_msgs -Meta messages with image exposure and control messages. These are used -by the [spinnaker_camera_driver](spinnaker_camera_driver/README.md). +Package with with [image exposure and control messages](flir_camera_msgs/README.md). +These are used by the [spinnaker_camera_driver](spinnaker_camera_driver/README.md). This software is issued under the Apache License Version 2.0. ### flir_camera_description -Package with meshesMeta messages with image exposure and control messages. These are used -by the [spinnaker_camera_driver](spinnaker_camera_driver/README.md). This software is released under a BSD license. +Package with [meshes and urdf](flir_camera_description/README.md) files. +This software is released under a BSD license. From 0bd0320a654742583196e146a20f1a0512c0ad6e Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 18 Jul 2023 09:49:22 -0400 Subject: [PATCH 11/11] more elaborate README --- flir_camera_msgs/README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/flir_camera_msgs/README.md b/flir_camera_msgs/README.md index 193370b3..2a5906cc 100644 --- a/flir_camera_msgs/README.md +++ b/flir_camera_msgs/README.md @@ -1,2 +1,5 @@ -# message related to FLIR camera drivers +# messages related to FLIR camera drivers +This package defines messages with meta data as well as control +messages. They are used for external exposure control with the FLIR +spinnaker camera driver.