diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2c93ba6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,21 @@ +.project +.cproject +.pydevproject +CMakeFiles +msg_gen +srv_gen +CMakeCache.txt +cmake_install.cmake +build +*.pyc +bin/ +CPackConfig.cmake +CPackSourceConfig.cmake +doxygen.cfg +*.bag +moc_* +~* +*.cfgc +docs +lib +.settings diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bec293f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +set(ROS_BUILD_TYPE RelWithAsserts) + +rosbuild_init() + +set(ROS_COMPILE_FLAGS "${ROS_COMPILE_FLAGS} -Wall -Wno-deprecated -Wno-unused-result -std=c++0x") + +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +set(UEYE_INCLUDE_PATH /usr/include) +set(UEYE_LIBRARY_PATH /usr/lib) +set(UEYE_LIBRARY ueye_api) +set(UEYECAM_NODELET_NAME ueye_cam_nodelet) +set(UEYECAM_NODE_NAME ueye_cam) +set(UEYECAM_LIB_NAME ueye_wrapper) + +set(UEYECAM_NODE_SOURCES + ${CMAKE_SOURCE_DIR}/src/ueye_cam_node.cpp +) + +set(UEYECAM_LIB_SOURCES + ${CMAKE_SOURCE_DIR}/src/ueye_cam_driver.cpp +) + +set(UEYECAM_LIB_HEADERS + ${CMAKE_SOURCE_DIR}/include/logging_macros.hpp + ${CMAKE_SOURCE_DIR}/include/ueye_cam_driver.hpp +) + +set(UEYECAM_NODELET_SOURCES + ${CMAKE_SOURCE_DIR}/src/ueye_cam_nodelet.cpp +) + +set(UEYECAM_NODELET_HEADERS + ${CMAKE_SOURCE_DIR}/include/ueye_cam_nodelet.hpp +) + +include_directories(${UEYE_INCLUDE_PATH}) +link_directories(${UEYE_LIBRARY_PATH}) + +# Add dynamic reconfigure API +rosbuild_find_ros_package(dynamic_reconfigure) +include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) +gencfg() + +rosbuild_add_library(${UEYECAM_LIB_NAME} ${UEYECAM_LIB_SOURCES} ${UEYECAM_LIB_HEADERS}) +target_link_libraries(${UEYECAM_LIB_NAME} ${UEYE_LIBRARY}) + +rosbuild_add_library(${UEYECAM_NODELET_NAME} ${UEYECAM_NODELET_SOURCES} ${UEYECAM_NODELET_HEADERS}) +target_link_libraries(${UEYECAM_NODELET_NAME} ${UEYECAM_LIB_NAME}) + +rosbuild_add_executable(${UEYECAM_NODE_NAME} ${UEYECAM_NODE_SOURCES}) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..45ddd7f --- /dev/null +++ b/LICENSE @@ -0,0 +1,46 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 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/Makefile b/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..89cc7ab --- /dev/null +++ b/README.md @@ -0,0 +1,47 @@ +**DISCLAMER:** + +This project was created within an academic research setting, and thus should +be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +code, so please adjust expectations accordingly. With that said, we are +intrinsically motivated to ensure its correctness (and often its performance). +Please use the corresponding web repository tool (e.g. github, bitbucket, etc) +to file bugs, suggestions, pull requests; we will do our best to address them +in a timely manner. + + +**LAYOUT:** +- ueye_cam/ + - bin/: compiled executables + - build/: CMake-based build files + - cfg/: dynamic_reconfigure configuration files + - docs/: generated documents + - include/: header files + - launch/: roslaunch files + - lib/: compiled libraries + - src/: source files + - CMakeLists.txt: CMake project configuration file + - LICENSES: license agreement + - mainpage.dox: ROS main documentation page + - Makefile: ROS CMake package bootstrap makefile (DO NOT MODIFY!) + - manifest.xml: ROS manifest file + - nodelet_plugins.xml: ROS nodelet specification file + - README.md: this file +- ~/.ros/camera_info/: camera calibration yaml files + (see documentation for camera_calibration ROS package + for more details) +- ~/.ros/camera_conf/: UEye camera parameter configuration files + (generatable using ueyedemo executable: + File -> save parameter -> to file...) + + +**DOCUMENTATION:** + +www.ros.org/wiki/ueye_cam + + + +Copyright (c) 2013, Anqi Xu + +All rights reserved. + +BSD3 license: see LICENSE file diff --git a/cfg/UEyeCam.cfg b/cfg/UEyeCam.cfg new file mode 100755 index 0000000..493e026 --- /dev/null +++ b/cfg/UEyeCam.cfg @@ -0,0 +1,90 @@ +#!/usr/bin/env python +PACKAGE = "ueye_cam" +import roslib; roslib.load_manifest(PACKAGE) + +from dynamic_reconfigure.parameter_generator import * + +from driver_base.msg import SensorLevels + +gen = ParameterGenerator() + +gen.add("image_width", int_t, SensorLevels.RECONFIGURE_STOP, + "Image width (not withstanding subsampling, binning, or sensor scaling)", 640, 16, 2560) +gen.add("image_height", int_t, SensorLevels.RECONFIGURE_STOP, + "Image height (not withstanding subsampling, binning, or sensor scaling)", 480, 4, 1920) +gen.add("image_left", int_t, SensorLevels.RECONFIGURE_STOP, + "Left side index for image in sensor coordinates (-1: center)", -1, -1, 2560-16) +gen.add("image_top", int_t, SensorLevels.RECONFIGURE_STOP, + "Top side index for image in sensor coordinates (-1: center)", -1, -1, 1920-4) + +color_mode_enum = gen.enum([ + gen.const("MONO8", str_t, "mono8", "8-bit Monochrome"), + gen.const("RGB8", str_t, "rgb8", "24-bit Red/Green/Blue"), + gen.const("BAYER_RGGB8", str_t, "bayer_rggb8", "8-bit Raw Bayer (RGGB)")], + "Color mode values") +gen.add("color_mode", str_t, SensorLevels.RECONFIGURE_STOP, + "Output image color mode", "mono8", edit_method=color_mode_enum) + +subsampling_enum = gen.enum([ + gen.const("SUB_1X", int_t, 1, "1X Subsampling"), + gen.const("SUB_2X", int_t, 2, "2X Subsampling"), + gen.const("SUB_4X", int_t, 4, "4X Subsampling"), + gen.const("SUB_8X", int_t, 8, "8X Subsampling"), + gen.const("SUB_16X", int_t, 16, "16X Subsampling")], + "Subsampling Values") +gen.add("subsampling", int_t, SensorLevels.RECONFIGURE_STOP, + "Output image subsampling ratio", 1, edit_method=subsampling_enum) + +binning_enum = gen.enum([ + gen.const("BIN_1X", int_t, 1, "1X Binning"), + gen.const("BIN_2X", int_t, 2, "2X Binning"), + gen.const("BIN_4X", int_t, 4, "4X Binning"), + gen.const("BIN_8X", int_t, 8, "8X Binning"), + gen.const("BIN_16X", int_t, 16, "16X Binning")], + "Binning Values") +gen.add("binning", int_t, SensorLevels.RECONFIGURE_STOP, + "Output image binning ratio", 1, edit_method=binning_enum) + +sensor_scaling_enum = gen.enum([ + gen.const("SS_1X", double_t, 1.0, "1X Internal Image Scaling"), + gen.const("SS_2X", double_t, 2.0, "2X Internal Image Scaling"), + gen.const("SS_4X", double_t, 4.0, "4X Internal Image Scaling"), + gen.const("SS_8X", double_t, 8.0, "8X Internal Image Scaling"), + gen.const("SS_16X", double_t, 16.0, "16X Internal Image Scaling")], + "Internal Image Scaling Values") +gen.add("sensor_scaling", double_t, SensorLevels.RECONFIGURE_STOP, + "Output image scaling ratio (Internal Image Scaling)", 1, edit_method=sensor_scaling_enum) + +gen.add("auto_gain", bool_t, SensorLevels.RECONFIGURE_RUNNING, + "Auto gain (overruled by auto framerate)", False) +gen.add("master_gain", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Master gain percentage", 0, 0, 100) +gen.add("red_gain", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Gain percentage for red channel", 0, 0, 100) +gen.add("green_gain", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Gain percentage for red channel", 0, 0, 100) +gen.add("blue_gain", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Gain percentage for red channel", 0, 0, 100) +gen.add("gain_boost", bool_t, SensorLevels.RECONFIGURE_RUNNING, + "Analog gain boost", False) + +gen.add("auto_exposure", bool_t, SensorLevels.RECONFIGURE_RUNNING, + "Auto exposure (a.k.a. auto shutter)", False) +gen.add("exposure", double_t, SensorLevels.RECONFIGURE_RUNNING, + "Exposure value (ms)", 33.0, 0.0, 300.0) + +gen.add("auto_white_balance", bool_t, SensorLevels.RECONFIGURE_RUNNING, + "Auto white balance", False) +gen.add("white_balance_red_offset", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Red level offset for white balance", 0, -50, 50) +gen.add("white_balance_blue_offset", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Blue level offset for white balance", 0, -50, 50) + +gen.add("auto_frame_rate", bool_t, SensorLevels.RECONFIGURE_RUNNING, + "Auto frame rate (requires auto exposure, supercedes auto gain)", False) +gen.add("frame_rate", double_t, SensorLevels.RECONFIGURE_RUNNING, + "Frame rate (Hz)", 10.0, 0.01, 200.0) +gen.add("pixel_clock", int_t, SensorLevels.RECONFIGURE_RUNNING, + "Pixel clock (MHz)", 25, 1, 100) + +exit(gen.generate(PACKAGE, "ueye_cam", "UEyeCam")) diff --git a/include/logging_macros.hpp b/include/logging_macros.hpp new file mode 100644 index 0000000..27abaeb --- /dev/null +++ b/include/logging_macros.hpp @@ -0,0 +1,112 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 OWNER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#ifndef LOGGING_MACROS_HPP_ +#define LOGGING_MACROS_HPP_ + +/** + * This header allows potentially ROS-agnostic code to either use ROS logging + * mechanisms, ROS nodelet logging mechanisms, or standard output mechanisms. + */ + +#define STD_LOGGING_MACROS 0 +#define ROS_LOGGING_MACROS 1 +#define NODELET_LOGGING_MACROS 2 +#define LOGGING_MACROS_TYPE NODELET_LOGGING_MACROS + + +#if LOGGING_MACROS_TYPE == ROS_LOGGING_MACROS + + #include + + #define DEBUG(...) ROS_DEBUG(__VA_ARGS__) + #define INFO(...) ROS_INFO(__VA_ARGS__) + #define WARN(...) ROS_WARN(__VA_ARGS__) + #define ERROR(...) ROS_ERROR(__VA_ARGS__) + #define FATAL(...) ROS_FATAL(__VA_ARGS__) + #define DEBUG_STREAM(...) ROS_DEBUG_STREAM(__VA_ARGS__) + #define INFO_STREAM(...) ROS_INFO_STREAM(__VA_ARGS__) + #define WARN_STREAM(...) ROS_WARN_STREAM(__VA_ARGS__) + #define ERROR_STREAM(...) ROS_ERROR_STREAM(__VA_ARGS__) + #define FATAL_STREAM(...) ROS_FATAL_STREAM(__VA_ARGS__) + +#elif LOGGING_MACROS_TYPE == NODELET_LOGGING_MACROS + + #include + #include + + using namespace ros::this_node; + + #define DEBUG(...) NODELET_DEBUG(__VA_ARGS__) + #define INFO(...) NODELET_INFO(__VA_ARGS__) + #define WARN(...) NODELET_WARN(__VA_ARGS__) + #define ERROR(...) NODELET_ERROR(__VA_ARGS__) + #define FATAL(...) NODELET_FATAL(__VA_ARGS__) + #define DEBUG_STREAM(...) NODELET_DEBUG_STREAM(__VA_ARGS__) + #define INFO_STREAM(...) NODELET_INFO_STREAM(__VA_ARGS__) + #define WARN_STREAM(...) NODELET_WARN_STREAM(__VA_ARGS__) + #define ERROR_STREAM(...) NODELET_ERROR_STREAM(__VA_ARGS__) + #define FATAL_STREAM(...) NODELET_FATAL_STREAM(__VA_ARGS__) + +#else + #include + #include + + #define DEBUG(...) fprintf(stdout, "DEBUG> "); fprintf(stdout, __VA_ARGS__); fprintf(stdout, "\n") + #define INFO(...) fprintf(stdout, "INFO > "); fprintf(stdout, __VA_ARGS__); fprintf(stdout, "\n") + #define WARN(...) fprintf(stderr, "WARN > "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n") + #define ERROR(...) fprintf(stderr, "ERROR> "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n") + #define FATAL(...) fprintf(stderr, "FATAL> "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n") + #define DEBUG_STREAM(...) std::cout << "DEBUG> " << __VA_ARGS__ << std::endl + #define INFO_STREAM(...) std::cout << "INFO > " << __VA_ARGS__ << std::endl + #define WARN_STREAM(...) std::cerr << "WARN > " << __VA_ARGS__ << std::endl + #define ERROR_STREAM(...) std::cerr << "ERROR> " << __VA_ARGS__ << std::endl + #define FATAL_STREAM(...) std::cerr << "FATAL> " << __VA_ARGS__ << std::endl + +#endif + +#endif /* LOGGING_MACROS_HPP_ */ diff --git a/include/ueye_cam_driver.hpp b/include/ueye_cam_driver.hpp new file mode 100644 index 0000000..0a6a461 --- /dev/null +++ b/include/ueye_cam_driver.hpp @@ -0,0 +1,328 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 OWNER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#ifndef UEYE_CAM_DRIVER_HPP_ +#define UEYE_CAM_DRIVER_HPP_ + + +#include +#include +#include +#include "logging_macros.hpp" + + +namespace ueye_cam { + + +#define CAP(val, min, max) \ + if (val < min) { \ + val = min; \ + } else if (val > max) { \ + val = max; \ + } + +#define IS_SUBSAMPLING_2X (IS_SUBSAMPLING_2X_VERTICAL | IS_SUBSAMPLING_2X_HORIZONTAL) +#define IS_SUBSAMPLING_4X (IS_SUBSAMPLING_4X_VERTICAL | IS_SUBSAMPLING_4X_HORIZONTAL) +#define IS_SUBSAMPLING_8X (IS_SUBSAMPLING_8X_VERTICAL | IS_SUBSAMPLING_8X_HORIZONTAL) +#define IS_SUBSAMPLING_16X (IS_SUBSAMPLING_16X_VERTICAL | IS_SUBSAMPLING_16X_HORIZONTAL) + +#define IS_BINNING_2X (IS_BINNING_2X_VERTICAL | IS_BINNING_2X_HORIZONTAL) +#define IS_BINNING_4X (IS_BINNING_4X_VERTICAL | IS_BINNING_4X_HORIZONTAL) +#define IS_BINNING_8X (IS_BINNING_8X_VERTICAL | IS_BINNING_8X_HORIZONTAL) +#define IS_BINNING_16X (IS_BINNING_16X_VERTICAL | IS_BINNING_16X_HORIZONTAL) + + +/** + * Thin wrapper for UEye camera API from IDS Imaging Development Systems GMBH. + */ +class UEyeCamDriver { +public: + /** + * Initializes member variables. + */ + UEyeCamDriver(int cam_ID = ANY_CAMERA, std::string cam_name = "camera"); + + /** + * Terminates UEye camera interface. + */ + virtual ~UEyeCamDriver(); + + /** + * Initializes and loads the UEye camera handle. + * + * \param new_cam_ID If set to -1, then uses existing camera ID. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + virtual INT connectCam(int new_cam_ID = -1); + + /** + * Terminates and releases the UEye camera handle. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + virtual INT disconnectCam(); + + /** + * Loads UEye camera parameter configuration INI file into current camera's settings. + * + * \param filename Relative or absolute path to UEye camera configuration file. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT loadCamConfig(std::string filename); + + /** + * Updates current camera handle's color mode and re-initializes + * internal frame buffer. This function will stop live capture + * automatically if necessary. + * + * \param mode Color mode string. Valid values: {"rgb8", "mono8", "bayer_rggb8"}. + * Certain values may not be available for a given camera model. + * \param reallocate_buffer Whether to auto-reallocate buffer or not after + * changing parameter. If set to false, remember to reallocate_buffer + * via another function or via reallocateCamBuffer() manually! + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setColorMode(std::string mode, bool reallocate_buffer = true); + + /** + * Updates current camera handle's sensor resolution and area of interest. + * This function will stop live capture automatically if necessary. + * + * \param image_width Desired width for area of interest / image. Will be + * automatically bounded by valid range for current camera. + * \param image_height Desired height for area of interest / image. Will be + * automatically bounded by valid range for current camera. + * \param image_left Desired left pixel offset for area of interest / image. + * Set to -1 to auto-center. + * \param image_top Desired top pixel offset for area of interest / image. + * Set to -1 to auto-center. + * \param reallocate_buffer Whether to auto-reallocate buffer or not after + * changing parameter. If set to false, remember to reallocate_buffer + * via another function or via reallocateCamBuffer() manually! + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setResolution(INT& image_width, INT& image_height, INT& image_left, + INT& image_top, bool reallocate_buffer = true); + + /** + * Updates current camera handle's subsampling rate. + * + * \param rate Desired subsampling rate. + * \param reallocate_buffer Whether to auto-reallocate buffer or not after + * changing parameter. If set to false, remember to reallocate_buffer + * via another function or via reallocateCamBuffer() manually! + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setSubsampling(int& rate, bool reallocate_buffer = true); + + /** + * Updates current camera handle's binning rate. + * + * \param rate Desired binning rate. + * \param reallocate_buffer Whether to auto-reallocate buffer or not after + * changing parameter. If set to false, remember to reallocate_buffer + * via another function or via reallocateCamBuffer() manually! + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setBinning(int& rate, bool reallocate_buffer = true); + + /** + * Updates current camera handle's internal image scaling rate. + * + * \param rate Desired internal image scaling rate. + * \param reallocate_buffer Whether to auto-reallocate buffer or not after + * changing parameter. If set to false, remember to reallocate_buffer + * via another function or via reallocateCamBuffer() manually! + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setSensorScaling(double& rate, bool reallocate_buffer = true); + + /** + * Updates current camera handle's gain either to auto mode, or + * to specified manual parameters. + * + * Auto gain mode is disabled if auto frame rate mode is enabled. + * + * \param auto_gain Updates camera's hardware auto gain / auto gain mode. + * Will be deactivated if camera does not support mode. + * \param master_gain_prc Manual master gain percentage. + * \param red_gain_prc Manual red channel gain percentage. + * \param green_gain_prc Manual green channel gain percentage. + * \param blue_gain_prc Manual blue channel gain percentage. + * \param gain_boost Sets the gain boost. This parameter is independent of + * auto/manual gain mode. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setGain(bool& auto_gain, INT& master_gain_prc, INT& red_gain_prc, + INT& green_gain_prc, INT& blue_gain_prc, bool& gain_boost); + + /** + * Updates current camera handle's exposure / shutter either to auto mode, or + * to specified manual parameters. + * + * \param auto_exposure Updates camera's hardware auto shutter / auto shutter mode. + * Will be deactivated if camera does not support mode. + * \param exposure_ms Manual exposure setting, in ms. Valid value range depends on + * current camera pixel clock rate. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setExposure(bool& auto_exposure, double& exposure_ms); + + /** + * Enables or disables the current camera handle's auto white balance mode, and + * configures auto white balance channel offset parameters. + * + * \param auto_white_balance Updates camera's hardware auto white balance / + * auto white balance mode. Will be deactivated if camera does not support mode. + * \param red_offset Red channel offset in auto white balance mode. Range: [-50, 50] + * \param blue_offset Blue channel offset in auto white balance mode. Range: [-50, 50] + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setWhiteBalance(bool& auto_white_balance, INT& red_offset, INT& blue_offset); + + /** + * Updates current camera handle's frame rate either to auto mode, or + * to specified manual parameters. + * + * Enabling auto frame rate mode requires that auto shutter mode be enabled. + * Enabling auto frame rate mode will disable auto gain mode. + * + * \param auto_frame_rate Updates camera's hardware auto frame rate / auto frame + * mode mode. Will be deactivated if camera does not support mode. + * \param frame_rate_hz Desired frame rate, in Hz / frames per second. Valid value + * range depends on current camera pixel clock rate. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setFrameRate(bool& auto_frame_rate, double& frame_rate_hz); + + /** + * Updates current camera handle's pixel clock rate. + * + * \param clock_rate_mhz Desired pixel clock rate, in MHz. Valid value range + * depends on camera model. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT setPixelClockRate(INT& clock_rate_mhz); + + /** + * Sets current camera to start capturing frames to internal buffer repeatedly. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT startLiveCapture(); + + /** + * Sets current camera to standby mode. + * + * \return IS_SUCCESS if successful, error flag otherwise (see err2str). + */ + INT stopLiveCapture(); + + /** + * Waits for next frame to be available, then returns pointer to frame if successful. + * This function should only be called when the camera is in live capture mode. + * Since this function will block until the next frame is available, it can be used + * in a loop, without additional delays, to repeatedly query frames from the camera + * at the maximum rate possible given current camera settings. + * + * \param timeout_ms Timeout duration while waiting for next frame event. + * + * \return Pointer to raw image buffer if successful, NULL otherwise. + * WARNING: image buffer contents may change during capture, or may become + * invalid after calling other functions! + */ + const char* processNextFrame(INT timeout_ms); + + inline bool isConnected() { return (cam_handle_ != (HIDS) 0); }; + + inline bool isLiveCapturing() { + return ((cam_handle_ != (HIDS) 0) && (is_CaptureVideo(cam_handle_, IS_GET_LIVE) == TRUE)); + }; + + /** + * Stringifies UEye API error flag. + */ + const static char* err2str(INT error); + + const static int ANY_CAMERA = 0; + + +protected: + INT reallocateCamBuffer(); + + HIDS cam_handle_; + SENSORINFO cam_sensor_info_; + char* cam_buffer_; + int cam_buffer_id_; + INT cam_buffer_pitch_; + unsigned int cam_buffer_size_; + std::string cam_name_; + int cam_id_; + IS_RECT cam_aoi_; + unsigned int cam_subsampling_rate_; + unsigned int cam_binning_rate_; + double cam_sensor_scaling_rate_; + INT bits_per_pixel_; +}; + + +}; // namespace ueye_cam + + +#endif /* UEYE_CAM_DRIVER_HPP_ */ diff --git a/include/ueye_cam_nodelet.hpp b/include/ueye_cam_nodelet.hpp new file mode 100644 index 0000000..e455dff --- /dev/null +++ b/include/ueye_cam_nodelet.hpp @@ -0,0 +1,176 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 OWNER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#ifndef UEYE_CAM_NODELET_HPP_ +#define UEYE_CAM_NODELET_HPP_ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace ueye_cam { + + +typedef dynamic_reconfigure::Server ReconfigureServer; + + +/** + * ROS interface nodelet for UEye camera API from IDS Imaging Development Systems GMBH. + */ +class UEyeCamNodelet : public nodelet::Nodelet, public UEyeCamDriver { +public: + const static std::string DEFAULT_CAMERA_NAME; + const static std::string DEFAULT_CAMERA_TOPIC; + const static std::string DEFAULT_CAMERA_INTRINSICS_FILENAME; + const static int DEFAULT_IMAGE_WIDTH; + const static int DEFAULT_IMAGE_HEIGHT; + const static std::string DEFAULT_COLOR_MODE; + const static double DEFAULT_EXPOSURE; + const static double DEFAULT_FRAME_RATE; + const static int DEFAULT_PIXEL_CLOCK; + + UEyeCamNodelet(); + + virtual ~UEyeCamNodelet(); + + /** + * Initializes ROS environment, loads static ROS parameters, initializes UEye camera, + * and starts live capturing / frame grabbing thread. + */ + virtual void onInit(); + + + /** + * Handles callbacks from dynamic_reconfigure. + */ + void configCallback(ueye_cam::UEyeCamConfig& config, uint32_t level); + + +protected: + /** + * Reads parameter values from currently selected camera. + */ + INT queryCamParams(); + + + /** + * Loads, validates, and updates static ROS parameters. + */ + INT parseROSParams(ros::NodeHandle& local_nh); + + + /** + * Initializes the camera handle, loads UEye INI configuration, refreshes + * parameters from camera, loads and sets static ROS parameters, and starts + * the frame grabber thread. + */ + virtual INT connectCam(); + + /** + * Stops the frame grabber thread, closes the camera handle, + * and releases all local variables. + */ + virtual INT disconnectCam(); + + /** + * (ROS Service) Updates the camera's intrinsic parameters over the ROS topic, + * and saves the parameters to a flatfile. + */ + bool setCamInfo(sensor_msgs::SetCameraInfo::Request& req, + sensor_msgs::SetCameraInfo::Response& rsp); + + /** + * Loads the camera's intrinsic parameters from camIntrFilename. + */ + void loadIntrinsicsFile(); + + + /** + * Saves the camera's intrinsic parameters to camIntrFilename. + */ + bool saveIntrinsicsFile(); + + /** + * Main ROS interface "spin" loop. + */ + void frameGrabLoop(); + void startFrameGrabber(); + void stopFrameGrabber(); + + std::thread frame_grab_thread_; + bool frame_grab_alive_; + + ReconfigureServer* ros_cfg_; + boost::recursive_mutex ros_cfg_mutex_; + bool cfg_sync_requested_; + + image_transport::CameraPublisher ros_cam_pub_; + sensor_msgs::Image ros_image_; + sensor_msgs::CameraInfo ros_cam_info_; + unsigned int ros_frame_count_; + + ros::ServiceServer set_cam_info_srv_; + + std::string cam_topic_; + std::string cam_intr_filename_; + std::string cam_params_filename_; // should be valid UEye INI file + ueye_cam::UEyeCamConfig cam_params_; +}; + + +}; // namespace ueye_cam + + +#endif /* UEYE_CAM_NODELET_HPP_ */ diff --git a/launch/bayer_rggb_image_proc.launch b/launch/bayer_rggb_image_proc.launch new file mode 100644 index 0000000..37a6ad9 --- /dev/null +++ b/launch/bayer_rggb_image_proc.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/debug.launch b/launch/debug.launch new file mode 100644 index 0000000..edd1d02 --- /dev/null +++ b/launch/debug.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rgb8.launch b/launch/rgb8.launch new file mode 100644 index 0000000..828d066 --- /dev/null +++ b/launch/rgb8.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mainpage.dox b/mainpage.dox new file mode 100644 index 0000000..d7f053c --- /dev/null +++ b/mainpage.dox @@ -0,0 +1,23 @@ +/** +\mainpage +\htmlinclude manifest.html + +A ROS nodelet and node that wraps the driver API for UEye cameras by IDS Imaging Development Systems GMBH. + +Please refer to ueye_cam for general documentation and usage. + +Feel free to expose additional features from the underlying IDS camera API. +Documentation for the Linux IDS camera API is located at /usr/share/doc/ids/ueye_manual/index.html. + +The program flow of the ROS interface is roughly as follows: + +
    +
  1. setup ROS environment
  2. +
  3. initialize handle for specified UEye camera
  4. +
  5. configure camera parameters
  6. +
  7. launch frame grabber thread, which will dynamically set the camera to live video mode and query frames from it when there are subscribers to the ROS image topic, or set the camera to low-power standby mode otherwise
  8. +
  9. dynamic parameters are updated on-the-fly, with the exception of those that affect the internal image buffer (e.g. image dimensions, color mode, etc)
  10. +
+ + +*/ diff --git a/manifest.xml b/manifest.xml new file mode 100644 index 0000000..fe39b16 --- /dev/null +++ b/manifest.xml @@ -0,0 +1,20 @@ + + + A ROS nodelet and node that wraps the driver API for UEye cameras + by IDS Imaging Development Systems GMBH. + + Anqi Xu + BSD + http://ros.org/wiki/ueye_cam + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 0000000..4c5a2e5 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,5 @@ + + + Driver wrapper nodelet for UEye cameras by IDS Imaging Development Systems GMBH. + + diff --git a/src/ueye_cam_driver.cpp b/src/ueye_cam_driver.cpp new file mode 100644 index 0000000..a81e2bb --- /dev/null +++ b/src/ueye_cam_driver.cpp @@ -0,0 +1,895 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 OWNER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#include "ueye_cam_driver.hpp" + + +using namespace std; + + +namespace ueye_cam { + + +UEyeCamDriver::UEyeCamDriver(int cam_ID, string cam_name) : + cam_handle_((HIDS) 0), + cam_buffer_(NULL), + cam_buffer_id_(0), + cam_buffer_pitch_(0), + cam_buffer_size_(0), + cam_name_(cam_name), + cam_id_(cam_ID), + cam_subsampling_rate_(1), + cam_binning_rate_(1), + cam_sensor_scaling_rate_(1), + bits_per_pixel_(8) { + cam_aoi_.s32X = 0; + cam_aoi_.s32Y = 0; + cam_aoi_.s32Width = 640; + cam_aoi_.s32Height = 480; +}; + + +UEyeCamDriver::~UEyeCamDriver() { + disconnectCam(); +}; + + + +INT UEyeCamDriver::connectCam(int new_cam_ID) { + INT is_err = IS_SUCCESS; + int numCameras; + + // Terminate any existing opened cameras + stopLiveCapture(); + + // Updates camera ID if specified. + if (new_cam_ID >= 0) { + cam_id_ = new_cam_ID; + } + + // Query for number of connected cameras + if ((is_err = is_GetNumberOfCameras(&numCameras)) != IS_SUCCESS) { + ERROR_STREAM("Failed query for number of connected UEye cameras (" << + err2str(is_err) << ")"); + return is_err; + } else if (numCameras < 1) { + ERROR_STREAM("No UEye cameras are connected"); + return IS_NO_SUCCESS; + } else if (numCameras < (int) cam_id_) { + ERROR_STREAM("Cannot connect to camera ID " << cam_id_ << + " since only " << numCameras << " cameras are connected"); + return is_err; + } + + // Attempt to open camera handle, and handle case where camera requires a + // mandatory firmware upload + cam_handle_ = (HIDS) cam_id_; + if ((is_err = is_InitCamera(&cam_handle_, NULL)) == IS_STARTER_FW_UPLOAD_NEEDED) { + INT uploadTimeMSEC = 25000; + is_GetDuration (cam_handle_, IS_STARTER_FW_UPLOAD, &uploadTimeMSEC); + + INFO_STREAM("Uploading new firmware to UEye camera '" << cam_name_ + << "'; please wait for about " << uploadTimeMSEC/1000.0 << " seconds"); + + // Attempt to re-open camera handle while triggering automatic firmware upload + cam_handle_ = (HIDS) (((INT) cam_handle_) | IS_ALLOW_STARTER_FW_UPLOAD); + is_err = is_InitCamera(&cam_handle_, NULL); // Will block for N seconds + } + if (is_err != IS_SUCCESS) { + ERROR_STREAM("Could not open UEye camera ID " << cam_id_ << + " (" << err2str(is_err) << ")"); + return is_err; + } + + // Set display mode to Device Independent Bitmap (DIB) + is_err = is_SetDisplayMode(cam_handle_, IS_SET_DM_DIB); + + // Fetch sensor parameters + is_err = is_GetSensorInfo(cam_handle_, &cam_sensor_info_); + + // Initialize local camera frame buffer + reallocateCamBuffer(); + + return is_err; +}; + + +INT UEyeCamDriver::disconnectCam() { + INT is_err = IS_SUCCESS; + + if (isConnected()) { + stopLiveCapture(); + + // Release existing camera buffers + if (cam_buffer_ != NULL) { + is_err = is_FreeImageMem(cam_handle_, cam_buffer_, cam_buffer_id_); + } + cam_buffer_ = NULL; + + // Release camera handle + is_err = is_ExitCamera(cam_handle_); + cam_handle_ = (HIDS) 0; + + DEBUG_STREAM("Disconnected UEye camera '" + cam_name_ + "'"); + } + + return is_err; +}; + + +INT UEyeCamDriver::loadCamConfig(string filename) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Convert filename to unicode, as requested by UEye API + const wstring filenameU(filename.begin(), filename.end()); + if ((is_err = is_ParameterSet(cam_handle_, IS_PARAMETERSET_CMD_LOAD_FILE, + (void*) filenameU.c_str(), 0)) != IS_SUCCESS) { + WARN_STREAM("Could not load UEye camera '" << cam_name_ + << "' sensor parameters file " << filename << " (" << err2str(is_err) << ")"); + return is_err; + } else { + // Update the AOI and bits per pixel + if ((is_err = is_AOI(cam_handle_, IS_AOI_IMAGE_GET_AOI, + (void*) &cam_aoi_, sizeof(cam_aoi_))) != IS_SUCCESS) { + ERROR_STREAM("Could not retrieve Area Of Interest from UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + INT colorMode = is_SetColorMode(cam_handle_, IS_GET_COLOR_MODE); + if (colorMode == IS_CM_BGR8_PACKED || colorMode == IS_CM_RGB8_PACKED) { + bits_per_pixel_ = 24; + } else if (colorMode == IS_CM_MONO8 || colorMode == IS_CM_SENSOR_RAW8) { + bits_per_pixel_ = 8; + } else { + WARN_STREAM("Current camera color mode is not supported by this wrapper;" << + "supported modes: {MONO8 | RGB8 | BAYER_RGGB8}; switching to RGB8 (24-bit)"); + if ((is_err = setColorMode("rgb8", false)) != IS_SUCCESS) return is_err; + } + + reallocateCamBuffer(); + + DEBUG_STREAM("Successfully loaded UEye camera '" << cam_name_ + << "'s sensor parameter file: " << filename); + } + + return is_err; +}; + + +INT UEyeCamDriver::setColorMode(string mode, bool reallocate_buffer) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Automatically stop live capture mode, to prevent access to memory buffer + stopLiveCapture(); + + // Set to specified color mode + if (mode == "rgb8") { + if ((is_err = is_SetColorMode(cam_handle_, IS_CM_RGB8_PACKED)) != IS_SUCCESS) { + ERROR_STREAM("Could not set color mode to RGB8 (" << err2str(is_err) << ")"); + return is_err; + } + bits_per_pixel_ = 24; + } else if (mode == "bayer_rggb8") { + if ((is_err = is_SetColorMode(cam_handle_, IS_CM_SENSOR_RAW8)) != IS_SUCCESS) { + ERROR_STREAM("Could not set color mode to BAYER_RGGB8 (" << err2str(is_err) << ")"); + return is_err; + } + bits_per_pixel_ = 8; + } else { // Default to MONO8 + if ((is_err = is_SetColorMode(cam_handle_, IS_CM_MONO8)) != IS_SUCCESS) { + ERROR_STREAM("Could not set color mode to MONO8 (" << err2str(is_err) << ")"); + return is_err; + } + bits_per_pixel_ = 8; + } + + DEBUG_STREAM("Updated color mode to " << mode); + + return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS); +}; + + +INT UEyeCamDriver::setResolution(INT& image_width, INT& image_height, + INT& image_left, INT& image_top, bool reallocate_buffer) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Validate arguments + CAP(image_width, 8, (INT) cam_sensor_info_.nMaxWidth); + CAP(image_height, 4, (INT) cam_sensor_info_.nMaxHeight); + if (image_left >= 0 && (int) cam_sensor_info_.nMaxWidth - image_width - image_left < 0) { + WARN_STREAM("Cannot set image left index to " << + image_left << " with an image width of " << + image_width << " and sensor max width of " << + cam_sensor_info_.nMaxWidth); + image_left = -1; + } + if (image_top >= 0 && + (int) cam_sensor_info_.nMaxHeight - image_height - image_top < 0) { + WARN_STREAM("Cannot set image top index to " << + image_top << " with an image height of " << + image_height << " and sensor max height of " << + cam_sensor_info_.nMaxHeight); + image_top = -1; + } + cam_aoi_.s32X = (image_left < 0) ? + (cam_sensor_info_.nMaxWidth - image_width) / 2 : image_left; + cam_aoi_.s32Y = (image_top < 0) ? + (cam_sensor_info_.nMaxHeight - image_height) / 2 : image_top; + cam_aoi_.s32Width = image_width; + cam_aoi_.s32Height = image_height; + if ((is_err = is_AOI(cam_handle_, IS_AOI_IMAGE_SET_AOI, &cam_aoi_, sizeof(cam_aoi_))) != IS_SUCCESS) { + ERROR_STREAM("Failed to set UEye camera sensor's Area Of Interest to " << + image_width << " x " << image_height << + " with top-left corner at (" << cam_aoi_.s32X << ", " << cam_aoi_.s32Y << ")" ); + return is_err; + } + + DEBUG_STREAM("Updated resolution to " << image_width << " x " << image_height << + " @ (" << image_left << ", " << image_top << ")"); + + return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS); +}; + + +INT UEyeCamDriver::setSubsampling(int& rate, bool reallocate_buffer) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Automatically stop live capture mode, to prevent access to memory buffer + stopLiveCapture(); + + INT rate_flag; + INT supportedRates; + + supportedRates = is_SetSubSampling(cam_handle_, IS_GET_SUPPORTED_SUBSAMPLING); + switch (rate) { + case 1: + rate_flag = IS_SUBSAMPLING_DISABLE; + break; + case 2: + rate_flag = IS_SUBSAMPLING_2X; + break; + case 4: + rate_flag = IS_SUBSAMPLING_4X; + break; + case 8: + rate_flag = IS_SUBSAMPLING_8X; + break; + case 16: + rate_flag = IS_SUBSAMPLING_16X; + break; + default: + WARN_STREAM("Invalid or unsupported subsampling rate: " << rate << ", resetting to 1X"); + rate = 1; + rate_flag = IS_SUBSAMPLING_DISABLE; + break; + } + + if ((supportedRates & rate_flag) == rate_flag) { + if ((is_err = is_SetSubSampling(cam_handle_, rate_flag)) != IS_SUCCESS) { + ERROR_STREAM("Could not set subsampling rate to " << rate << "X (" << err2str(is_err) << ")"); + return is_err; + } + } else { + WARN_STREAM("Camera does not support requested sampling rate of " << rate); + + // Query current rate + INT currRate = is_SetSubSampling(cam_handle_, IS_GET_SUBSAMPLING); + if (currRate == IS_SUBSAMPLING_DISABLE) { rate = 1; } + else if (currRate == IS_SUBSAMPLING_2X) { rate = 2; } + else if (currRate == IS_SUBSAMPLING_4X) { rate = 4; } + else if (currRate == IS_SUBSAMPLING_8X) { rate = 8; } + else if (currRate == IS_SUBSAMPLING_16X) { rate = 16; } + else { + WARN_STREAM("Camera has unsupported sampling rate (" << currRate << "), resetting to 1X"); + if ((is_err = is_SetSubSampling(cam_handle_, IS_SUBSAMPLING_DISABLE)) != IS_SUCCESS) { + ERROR_STREAM("Could not set subsampling rate to 1X (" << err2str(is_err) << ")"); + return is_err; + } + } + return IS_SUCCESS; + } + + DEBUG_STREAM("Updated subsampling rate to " << rate << "X"); + + cam_subsampling_rate_ = rate; + + return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS); +}; + + +INT UEyeCamDriver::setBinning(int& rate, bool reallocate_buffer) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Automatically stop live capture mode, to prevent access to memory buffer + stopLiveCapture(); + + INT rate_flag; + INT supportedRates; + + supportedRates = is_SetBinning(cam_handle_, IS_GET_SUPPORTED_BINNING); + switch (rate) { + case 1: + rate_flag = IS_BINNING_DISABLE; + break; + case 2: + rate_flag = IS_BINNING_2X; + break; + case 4: + rate_flag = IS_BINNING_4X; + break; + case 8: + rate_flag = IS_BINNING_8X; + break; + case 16: + rate_flag = IS_BINNING_16X; + break; + default: + WARN_STREAM("Invalid or unsupported binning rate: " << rate << ", resetting to 1X"); + rate = 1; + rate_flag = IS_BINNING_DISABLE; + break; + } + + if ((supportedRates & rate_flag) == rate_flag) { + if ((is_err = is_SetBinning(cam_handle_, rate_flag)) != IS_SUCCESS) { + ERROR_STREAM("Could not set binning rate to " << rate << "X (" << err2str(is_err) << ")"); + return is_err; + } + } else { + WARN_STREAM("Camera does not support requested binning rate of " << rate); + + // Query current rate + INT currRate = is_SetBinning(cam_handle_, IS_GET_BINNING); + if (currRate == IS_BINNING_DISABLE) { rate = 1; } + else if (currRate == IS_BINNING_2X) { rate = 2; } + else if (currRate == IS_BINNING_4X) { rate = 4; } + else if (currRate == IS_BINNING_8X) { rate = 8; } + else if (currRate == IS_BINNING_16X) { rate = 16; } + else { + WARN_STREAM("Camera has unsupported binning rate (" << currRate << "), resetting to 1X"); + if ((is_err = is_SetBinning(cam_handle_, IS_BINNING_DISABLE)) != IS_SUCCESS) { + ERROR_STREAM("Could not set binning rate to 1X (" << err2str(is_err) << ")"); + return is_err; + } + } + return IS_SUCCESS; + } + + DEBUG_STREAM("Updated binning rate to " << rate << "X"); + + cam_binning_rate_ = rate; + + return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS); +}; + + +INT UEyeCamDriver::setSensorScaling(double& rate, bool reallocate_buffer) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Automatically stop live capture mode, to prevent access to memory buffer + stopLiveCapture(); + + SENSORSCALERINFO sensorScalerInfo; + is_err = is_GetSensorScalerInfo(cam_handle_, &sensorScalerInfo, sizeof(sensorScalerInfo)); + if (is_err == IS_NOT_SUPPORTED) { + WARN_STREAM("Internal image scaling is not supported by camera"); + rate = 1.0; + cam_sensor_scaling_rate_ = 1.0; + return IS_SUCCESS; + } else if (is_err != IS_SUCCESS) { + ERROR_STREAM("Could not obtain supported internal image scaling information (" << + err2str(is_err) << ")"); + rate = 1.0; + cam_sensor_scaling_rate_ = 1.0; + return is_err; + } else { + if (rate < sensorScalerInfo.dblMinFactor || rate > sensorScalerInfo.dblMaxFactor) { + WARN_STREAM("Requested internal image scaling rate of " << rate << + " is not within supported bounds of [" << sensorScalerInfo.dblMinFactor << + ", " << sensorScalerInfo.dblMaxFactor << "]; not updating current rate of " << + sensorScalerInfo.dblCurrFactor); + rate = sensorScalerInfo.dblCurrFactor; + return IS_SUCCESS; + } + } + + if ((is_err = is_SetSensorScaler(cam_handle_, IS_ENABLE_SENSOR_SCALER, rate)) != IS_SUCCESS) { + WARN_STREAM("Could not set internal image scaling rate to " << rate << "X (" << + err2str(is_err) << "); resetting to 1X"); + rate = 1.0; + if ((is_err = is_SetSensorScaler(cam_handle_, IS_ENABLE_SENSOR_SCALER, rate)) != IS_SUCCESS) { + ERROR_STREAM("Could not set internal image scaling rate to 1X (" << err2str(is_err) << ")"); + return is_err; + } + } + + DEBUG_STREAM("Updated internal image scaling rate to " << rate << "X"); + + cam_sensor_scaling_rate_ = rate; + + return (reallocate_buffer ? reallocateCamBuffer() : IS_SUCCESS); +}; + + +INT UEyeCamDriver::setGain(bool& auto_gain, INT& master_gain_prc, INT& red_gain_prc, + INT& green_gain_prc, INT& blue_gain_prc, bool& gain_boost) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + // Validate arguments + CAP(master_gain_prc, 0, 100); + CAP(red_gain_prc, 0, 100); + CAP(green_gain_prc, 0, 100); + CAP(blue_gain_prc, 0, 100); + + // Set auto gain + double pval1 = auto_gain, pval2 = 0; + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_SENSOR_GAIN, + &pval1, &pval2)) != IS_SUCCESS) { + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_GAIN, + &pval1, &pval2)) != IS_SUCCESS) { + WARN_STREAM("Auto gain mode is not supported for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + auto_gain = false; + } + } + + // Set gain boost + if (is_SetGainBoost(cam_handle_, IS_GET_SUPPORTED_GAINBOOST) != IS_SET_GAINBOOST_ON) { + gain_boost = false; + } else { + if ((is_err = is_SetGainBoost(cam_handle_, + (gain_boost) ? IS_SET_GAINBOOST_ON : IS_SET_GAINBOOST_OFF)) + != IS_SUCCESS) { + WARN_STREAM("Failed to " << ((gain_boost) ? "enable" : "disable") << + " gain boost for UEye camera '" + cam_name_ + "'"); + } + } + + // Set manual gain parameters + if ((is_err = is_SetHardwareGain(cam_handle_, master_gain_prc, + red_gain_prc, green_gain_prc, blue_gain_prc)) != IS_SUCCESS) { + WARN_STREAM("Failed to set manual gains (master: " << master_gain_prc << + "; red: " << red_gain_prc << "; green: " << green_gain_prc << + "; blue: " << blue_gain_prc << ") for UEye camera '" + cam_name_ + "'"); + } + + DEBUG_STREAM("Updated gain: " << ((auto_gain) ? "auto" : "manual") << + "\n - master gain: " << master_gain_prc << + "\n - red gain: " << red_gain_prc << + "\n - green gain: " << green_gain_prc << + "\n - blue gain: " << blue_gain_prc << + "\n - gain boost: " << gain_boost); + + return is_err; +}; + + +INT UEyeCamDriver::setExposure(bool& auto_exposure, double& exposure_ms) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + double minExposure, maxExposure; + + // Set auto exposure + double pval1 = auto_exposure, pval2 = 0; + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_SENSOR_SHUTTER, + &pval1, &pval2)) != IS_SUCCESS) { + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_SHUTTER, + &pval1, &pval2)) != IS_SUCCESS) { + WARN_STREAM("Auto exposure mode is not supported for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + auto_exposure = false; + } + } + + // Set manual exposure timing + if (!auto_exposure) { + // Make sure that user-requested exposure rate is achievable + if (((is_err = is_Exposure(cam_handle_, IS_EXPOSURE_CMD_GET_EXPOSURE_RANGE_MIN, + (void*) &minExposure, sizeof(minExposure))) != IS_SUCCESS) || + ((is_err = is_Exposure(cam_handle_, IS_EXPOSURE_CMD_GET_EXPOSURE_RANGE_MAX, + (void*) &maxExposure, sizeof(maxExposure))) != IS_SUCCESS)) { + ERROR_STREAM("Failed to query valid exposure range from UEye camera '" << cam_name_ << "'"); + return is_err; + } + CAP(exposure_ms, minExposure, maxExposure); + + // Update exposure + if ((is_err = is_Exposure(cam_handle_, IS_EXPOSURE_CMD_SET_EXPOSURE, + (void*) &(exposure_ms), sizeof(exposure_ms))) != IS_SUCCESS) { + ERROR_STREAM("Failed to set exposure to " << exposure_ms << + " ms for UEye camera '" << cam_name_ << "'"); + return is_err; + } + } + + DEBUG_STREAM("Updated exposure: " << ((auto_exposure) ? "auto" : to_string(exposure_ms)) << + " ms"); + + return is_err; +}; + + +INT UEyeCamDriver::setWhiteBalance(bool& auto_white_balance, INT& red_offset, + INT& blue_offset) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + CAP(red_offset, -50, 50); + CAP(blue_offset, -50, 50); + + // Set auto white balance mode and parameters + double pval1 = auto_white_balance, pval2 = 0; + // TODO: bug: enabling auto white balance does not seem to have an effect; in ueyedemo it seems to change R/G/B gains automatically + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_SENSOR_WHITEBALANCE, + &pval1, &pval2)) != IS_SUCCESS) { + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_AUTO_WB_ONCE, + &pval1, &pval2)) != IS_SUCCESS) { + WARN_STREAM("Auto white balance mode is not supported for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + auto_white_balance = false; + } + } + if (auto_white_balance) { + pval1 = red_offset; + pval2 = blue_offset; + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_AUTO_WB_OFFSET, + &pval1, &pval2)) != IS_SUCCESS) { + WARN_STREAM("Failed to set white balance red/blue offsets to " << + red_offset << " / " << blue_offset << + " for UEye camera '" << cam_name_ << "'"); + } + } + + DEBUG_STREAM("Updated white balance: " << ((auto_white_balance) ? "auto" : "manual") << + "\n - red offset: " << red_offset << + "\n - blue offset: " << blue_offset); + + return is_err; +}; + + +INT UEyeCamDriver::setFrameRate(bool& auto_frame_rate, double& frame_rate_hz) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + double pval1 = 0, pval2 = 0; + double minFrameTime, maxFrameTime, intervalFrameTime, newFrameRate; + + // Make sure that auto shutter is enabled before enabling auto frame rate + bool autoShutterOn = false; + is_SetAutoParameter(cam_handle_, IS_GET_ENABLE_AUTO_SENSOR_SHUTTER, &pval1, &pval2); + autoShutterOn |= (pval1 != 0); + is_SetAutoParameter(cam_handle_, IS_GET_ENABLE_AUTO_SHUTTER, &pval1, &pval2); + autoShutterOn |= (pval1 != 0); + if (!autoShutterOn) { + auto_frame_rate = false; + } + + // Set frame rate / auto + pval1 = auto_frame_rate; + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_SENSOR_FRAMERATE, + &pval1, &pval2)) != IS_SUCCESS) { + if ((is_err = is_SetAutoParameter(cam_handle_, IS_SET_ENABLE_AUTO_FRAMERATE, + &pval1, &pval2)) != IS_SUCCESS) { + WARN_STREAM("Auto frame rate mode is not supported for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + auto_frame_rate = false; + } + } + if (!auto_frame_rate) { + // Make sure that user-requested frame rate is achievable + if ((is_err = is_GetFrameTimeRange(cam_handle_, &minFrameTime, + &maxFrameTime, &intervalFrameTime)) != IS_SUCCESS) { + ERROR_STREAM("Failed to query valid frame rate range from UEye camera '" << cam_name_ << "'"); + return is_err; + } + CAP(frame_rate_hz, 1.0/maxFrameTime, 1.0/minFrameTime); + + // Update frame rate + if ((is_err = is_SetFrameRate(cam_handle_, frame_rate_hz, &newFrameRate)) != IS_SUCCESS) { + ERROR_STREAM("Failed to set frame rate to " << frame_rate_hz << + " MHz for UEye camera '" << cam_name_ << "'"); + return is_err; + } else if (frame_rate_hz != newFrameRate) { + frame_rate_hz = newFrameRate; + } + } + + DEBUG_STREAM("Updated frame rate: " << ((auto_frame_rate) ? "auto" : to_string(frame_rate_hz)) << " Hz"); + + return is_err; +}; + + +INT UEyeCamDriver::setPixelClockRate(INT& clock_rate_mhz) { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + UINT pixelClockRange[3]; + ZeroMemory(pixelClockRange, sizeof(pixelClockRange)); + + if ((is_err = is_PixelClock(cam_handle_, IS_PIXELCLOCK_CMD_GET_RANGE, + (void*) pixelClockRange, sizeof(pixelClockRange))) != IS_SUCCESS) { + ERROR_STREAM("Failed to query pixel clock range from UEye camera '" << + cam_name_ << "'"); + return is_err; + } + CAP(clock_rate_mhz, (int) pixelClockRange[0], (int) pixelClockRange[1]); + if ((is_err = is_PixelClock(cam_handle_, IS_PIXELCLOCK_CMD_SET, + (void*) &(clock_rate_mhz), sizeof(clock_rate_mhz))) != IS_SUCCESS) { + ERROR_STREAM("Failed to set pixel clock to " << clock_rate_mhz << + "MHz for UEye camera '" << cam_name_ << "'"); + return is_err; + } + + DEBUG_STREAM("Updated pixel clock: " << clock_rate_mhz << " MHz"); + + return IS_SUCCESS; +}; + + +INT UEyeCamDriver::startLiveCapture() { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + if (!isLiveCapturing()) { + if ((is_err = is_EnableEvent(cam_handle_, IS_SET_EVENT_FRAME)) != IS_SUCCESS) { + ERROR_STREAM("Could not enable frame event for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + if ((is_err = is_CaptureVideo(cam_handle_, IS_WAIT)) != IS_SUCCESS) { + ERROR_STREAM("Could not start live video mode on UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + DEBUG_STREAM("Started live video mode on UEye camera '" + cam_name_ + "'"); + } + + return is_err; +}; + + +INT UEyeCamDriver::stopLiveCapture() { + if (!isConnected()) return IS_INVALID_CAMERA_HANDLE; + + INT is_err = IS_SUCCESS; + + if (isLiveCapturing()) { + if ((is_err = is_DisableEvent(cam_handle_, IS_SET_EVENT_FRAME)) != IS_SUCCESS) { + ERROR_STREAM("Could not disable frame event for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + if ((is_err = is_StopLiveVideo(cam_handle_, IS_WAIT)) != IS_SUCCESS) { + ERROR_STREAM("Could not stop live video mode on UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + DEBUG_STREAM("Stopped live video mode on UEye camera '" + cam_name_ + "'"); + } + + return is_err; +}; + + +const char* UEyeCamDriver::processNextFrame(INT timeout_ms) { + if (!isLiveCapturing()) return NULL; + + INT is_err = IS_SUCCESS; + + // Wait for frame event + if ((is_err = is_WaitEvent(cam_handle_, IS_SET_EVENT_FRAME, + timeout_ms)) != IS_SUCCESS) { + ERROR_STREAM("Failed to acquire image from UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + } + + return cam_buffer_; +}; + + +INT UEyeCamDriver::reallocateCamBuffer() { + INT is_err = IS_SUCCESS; + + stopLiveCapture(); + + if (cam_buffer_ != NULL) { + is_err = is_FreeImageMem(cam_handle_, cam_buffer_, cam_buffer_id_); + cam_buffer_ = NULL; + } + if ((is_err = is_AllocImageMem(cam_handle_, + cam_aoi_.s32Width / (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_), + cam_aoi_.s32Height / (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_), + bits_per_pixel_, &cam_buffer_, &cam_buffer_id_)) != IS_SUCCESS) { + ERROR_STREAM("Failed to allocate " << + cam_aoi_.s32Width / (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_) << + " x " << + cam_aoi_.s32Height / (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_) << + " image buffer"); + return is_err; + } + if ((is_err = is_SetImageMem(cam_handle_, cam_buffer_, cam_buffer_id_)) != IS_SUCCESS) { + ERROR_STREAM("Failed to associate an image buffer to the UEye camera driver"); + return is_err; + } + if ((is_err = is_GetImageMemPitch(cam_handle_, &cam_buffer_pitch_)) != IS_SUCCESS) { + ERROR_STREAM("Failed to query UEye camera buffer's pitch (a.k.a. stride)"); + return is_err; + } + cam_buffer_size_ = cam_buffer_pitch_ * cam_aoi_.s32Height / + (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_); + DEBUG_STREAM("Allocate internal memory - width: " << + cam_aoi_.s32Width / (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_) << + "; height: " << + cam_aoi_.s32Height / (cam_sensor_scaling_rate_ * cam_subsampling_rate_ * cam_binning_rate_) << + "; fetched pitch: " << cam_buffer_pitch_ << "; expected bpp: " << bits_per_pixel_ << + "; total size: " << cam_buffer_size_); + + return is_err; +}; + + +const char* UEyeCamDriver::err2str(INT error) { +#define CASE(s) case s: return #s; break + switch (error) { + CASE(IS_NO_SUCCESS); + CASE(IS_SUCCESS); + CASE(IS_INVALID_CAMERA_HANDLE); + CASE(IS_IO_REQUEST_FAILED); + CASE(IS_CANT_OPEN_DEVICE); + CASE(IS_CANT_OPEN_REGISTRY); + CASE(IS_CANT_READ_REGISTRY); + CASE(IS_NO_IMAGE_MEM_ALLOCATED); + CASE(IS_CANT_CLEANUP_MEMORY); + CASE(IS_CANT_COMMUNICATE_WITH_DRIVER); + CASE(IS_FUNCTION_NOT_SUPPORTED_YET); + CASE(IS_INVALID_CAPTURE_MODE); + CASE(IS_INVALID_MEMORY_POINTER); + CASE(IS_FILE_WRITE_OPEN_ERROR); + CASE(IS_FILE_READ_OPEN_ERROR); + CASE(IS_FILE_READ_INVALID_BMP_ID); + CASE(IS_FILE_READ_INVALID_BMP_SIZE); + CASE(IS_NO_ACTIVE_IMG_MEM); + CASE(IS_SEQUENCE_LIST_EMPTY); + CASE(IS_CANT_ADD_TO_SEQUENCE); + CASE(IS_SEQUENCE_BUF_ALREADY_LOCKED); + CASE(IS_INVALID_DEVICE_ID); + CASE(IS_INVALID_BOARD_ID); + CASE(IS_ALL_DEVICES_BUSY); + CASE(IS_TIMED_OUT); + CASE(IS_NULL_POINTER); + CASE(IS_INVALID_PARAMETER); + CASE(IS_OUT_OF_MEMORY); + CASE(IS_ACCESS_VIOLATION); + CASE(IS_NO_USB20); + CASE(IS_CAPTURE_RUNNING); + CASE(IS_IMAGE_NOT_PRESENT); + CASE(IS_TRIGGER_ACTIVATED); + CASE(IS_CRC_ERROR); + CASE(IS_NOT_YET_RELEASED); + CASE(IS_WAITING_FOR_KERNEL); + CASE(IS_NOT_SUPPORTED); + CASE(IS_TRIGGER_NOT_ACTIVATED); + CASE(IS_OPERATION_ABORTED); + CASE(IS_BAD_STRUCTURE_SIZE); + CASE(IS_INVALID_BUFFER_SIZE); + CASE(IS_INVALID_PIXEL_CLOCK); + CASE(IS_INVALID_EXPOSURE_TIME); + CASE(IS_AUTO_EXPOSURE_RUNNING); + CASE(IS_CANNOT_CREATE_BB_SURF); + CASE(IS_CANNOT_CREATE_BB_MIX); + CASE(IS_BB_OVLMEM_NULL); + CASE(IS_CANNOT_CREATE_BB_OVL); + CASE(IS_NOT_SUPP_IN_OVL_SURF_MODE); + CASE(IS_INVALID_SURFACE); + CASE(IS_SURFACE_LOST); + CASE(IS_RELEASE_BB_OVL_DC); + CASE(IS_BB_TIMER_NOT_CREATED); + CASE(IS_BB_OVL_NOT_EN); + CASE(IS_ONLY_IN_BB_MODE); + CASE(IS_INVALID_COLOR_FORMAT); + CASE(IS_INVALID_WB_BINNING_MODE); + CASE(IS_INVALID_I2C_DEVICE_ADDRESS); + CASE(IS_COULD_NOT_CONVERT); + CASE(IS_TRANSFER_ERROR); + CASE(IS_PARAMETER_SET_NOT_PRESENT); + CASE(IS_INVALID_CAMERA_TYPE); + CASE(IS_INVALID_HOST_IP_HIBYTE); + CASE(IS_CM_NOT_SUPP_IN_CURR_DISPLAYMODE); + CASE(IS_NO_IR_FILTER); + CASE(IS_STARTER_FW_UPLOAD_NEEDED); + CASE(IS_DR_LIBRARY_NOT_FOUND); + CASE(IS_DR_DEVICE_OUT_OF_MEMORY); + CASE(IS_DR_CANNOT_CREATE_SURFACE); + CASE(IS_DR_CANNOT_CREATE_VERTEX_BUFFER); + CASE(IS_DR_CANNOT_CREATE_TEXTURE); + CASE(IS_DR_CANNOT_LOCK_OVERLAY_SURFACE); + CASE(IS_DR_CANNOT_UNLOCK_OVERLAY_SURFACE); + CASE(IS_DR_CANNOT_GET_OVERLAY_DC); + CASE(IS_DR_CANNOT_RELEASE_OVERLAY_DC); + CASE(IS_DR_DEVICE_CAPS_INSUFFICIENT); + CASE(IS_INCOMPATIBLE_SETTING); + CASE(IS_DR_NOT_ALLOWED_WHILE_DC_IS_ACTIVE); + CASE(IS_DEVICE_ALREADY_PAIRED); + CASE(IS_SUBNETMASK_MISMATCH); + CASE(IS_SUBNET_MISMATCH); + CASE(IS_INVALID_IP_CONFIGURATION); + CASE(IS_DEVICE_NOT_COMPATIBLE); + CASE(IS_NETWORK_FRAME_SIZE_INCOMPATIBLE); + CASE(IS_NETWORK_CONFIGURATION_INVALID); + CASE(IS_ERROR_CPU_IDLE_STATES_CONFIGURATION); + default: + return "UNKNOWN ERROR"; + break; + } + return "UNKNOWN ERROR"; +#undef CASE +}; + + +}; // namespace ueye_cam diff --git a/src/ueye_cam_node.cpp b/src/ueye_cam_node.cpp new file mode 100644 index 0000000..7f8e39f --- /dev/null +++ b/src/ueye_cam_node.cpp @@ -0,0 +1,63 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 OWNER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#include +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "ueye_cam"); // ros::init_options::AnonymousName + + nodelet::Loader manager(false); + nodelet::M_string mappings; + nodelet::V_string argvString(argv, argv + argc); + + manager.load(ros::this_node::getName(), "ueye_cam/ueye_cam_nodelet", mappings, argvString); + + ros::spin(); + + return 0; +}; diff --git a/src/ueye_cam_nodelet.cpp b/src/ueye_cam_nodelet.cpp new file mode 100644 index 0000000..9603efb --- /dev/null +++ b/src/ueye_cam_nodelet.cpp @@ -0,0 +1,954 @@ +/******************************************************************************* +* DO NOT MODIFY - AUTO-GENERATED +* +* +* DISCLAMER: +* +* This project was created within an academic research setting, and thus should +* be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the +* code, so please adjust expectations accordingly. With that said, we are +* intrinsically motivated to ensure its correctness (and often its performance). +* Please use the corresponding web repository tool (e.g. github/bitbucket/etc.) +* to file bugs, suggestions, pull requests; we will do our best to address them +* in a timely manner. +* +* +* SOFTWARE LICENSE AGREEMENT (BSD LICENSE): +* +* Copyright (c) 2013, Anqi Xu +* 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 School of Computer Science, McGill 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 OWNER OR CONTRIBUTORS BE LIABLE +* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#include "ueye_cam_nodelet.hpp" +#include // needed for getenv() +#include +#include +#include +#include +#include + + +//#define DEBUG_PRINTOUT_FRAME_GRAB_RATES + + +using namespace std; +using namespace sensor_msgs::image_encodings; + + +namespace ueye_cam { + + +const string UEyeCamNodelet::DEFAULT_CAMERA_NAME = "camera"; +const string UEyeCamNodelet::DEFAULT_CAMERA_TOPIC = "image_raw"; +const int UEyeCamNodelet::DEFAULT_IMAGE_WIDTH = 640; +const int UEyeCamNodelet::DEFAULT_IMAGE_HEIGHT = 480; +const string UEyeCamNodelet::DEFAULT_COLOR_MODE = ""; +const double UEyeCamNodelet::DEFAULT_EXPOSURE = 33.0; +const double UEyeCamNodelet::DEFAULT_FRAME_RATE = 10.0; +const int UEyeCamNodelet::DEFAULT_PIXEL_CLOCK = 25; + + +UEyeCamNodelet::UEyeCamNodelet() : + nodelet::Nodelet(), + UEyeCamDriver(ANY_CAMERA, DEFAULT_CAMERA_NAME), + frame_grab_alive_(false), + ros_cfg_(NULL), + cfg_sync_requested_(false), + ros_frame_count_(0), + cam_topic_(DEFAULT_CAMERA_TOPIC), + cam_intr_filename_(""), + cam_params_filename_("") { + cam_params_.image_width = DEFAULT_IMAGE_WIDTH; + cam_params_.image_height = DEFAULT_IMAGE_HEIGHT; + cam_params_.image_left = -1; + cam_params_.image_top = -1; + cam_params_.color_mode = DEFAULT_COLOR_MODE; + cam_params_.subsampling = cam_subsampling_rate_; + cam_params_.binning = cam_binning_rate_; + cam_params_.sensor_scaling = cam_sensor_scaling_rate_; + cam_params_.auto_gain = false; + cam_params_.master_gain = 0; + cam_params_.red_gain = 0; + cam_params_.green_gain = 0; + cam_params_.blue_gain = 0; + cam_params_.gain_boost = 0; + cam_params_.auto_exposure = false; + cam_params_.exposure = DEFAULT_EXPOSURE; + cam_params_.auto_white_balance = false; + cam_params_.white_balance_red_offset = 0; + cam_params_.white_balance_blue_offset = 0; + cam_params_.auto_frame_rate = false; + cam_params_.frame_rate = DEFAULT_FRAME_RATE; + cam_params_.pixel_clock = DEFAULT_PIXEL_CLOCK; +}; + + +UEyeCamNodelet::~UEyeCamNodelet() { + disconnectCam(); + + // NOTE: sometimes deleting dynamic reconfigure object will lock up + // (suspect the scoped lock is not releasing the recursive mutex) + // + //if (ros_cfg_ != NULL) { + // delete ros_cfg_; + // ros_cfg_ = NULL; + //} +}; + + +void UEyeCamNodelet::onInit() { + ros::NodeHandle& nh = getNodeHandle(); + ros::NodeHandle& local_nh = getPrivateNodeHandle(); + image_transport::ImageTransport it(nh); + + // Load camera-agnostic ROS parameters + local_nh.param("camera_name", cam_name_, DEFAULT_CAMERA_NAME); + local_nh.param("camera_topic", cam_topic_, DEFAULT_CAMERA_TOPIC); + local_nh.param("camera_intrinsics_file", cam_intr_filename_, ""); + local_nh.param("camera_id", cam_id_, ANY_CAMERA); + local_nh.param("camera_parameters_file", cam_params_filename_, ""); + if (cam_id_ < 0) { + NODELET_WARN_STREAM("Invalid camera ID specified: " << cam_id_ << + "; setting to ANY_CAMERA"); + cam_id_ = ANY_CAMERA; + } + + loadIntrinsicsFile(); + + // Setup dynamic reconfigure server + ros_cfg_ = new ReconfigureServer(ros_cfg_mutex_, local_nh); + ReconfigureServer::CallbackType f; + f = bind(&UEyeCamNodelet::configCallback, this, _1, _2); + + // Setup publishers, subscribers, and services + ros_cam_pub_ = it.advertiseCamera("/" + cam_name_ + "/" + cam_topic_, 1); + set_cam_info_srv_ = nh.advertiseService("/" + cam_name_ + "/set_camera_info", + &UEyeCamNodelet::setCamInfo, this); + + // Initiate camera and start capture + if (connectCam() != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to initialize UEye camera '" << cam_name_ << "'"); + return; + } + ros_cfg_->setCallback(f); // this will call configCallback, which will configure the camera's parameters + startFrameGrabber(); + NODELET_INFO_STREAM( + "UEye camera '" << cam_name_ << "' initialized on topic " << ros_cam_pub_.getTopic() << endl << + "Width:\t\t\t" << cam_params_.image_width << endl << + "Height:\t\t\t" << cam_params_.image_height << endl << + "Left Pos.:\t\t" << cam_params_.image_left << endl << + "Top Pos.:\t\t" << cam_params_.image_top << endl << + "Color Mode:\t\t" << cam_params_.color_mode << endl << + "Subsampling:\t\t" << cam_params_.subsampling << endl << + "Binning:\t\t" << cam_params_.binning << endl << + "Sensor Scaling:\t\t" << cam_params_.sensor_scaling << endl << + "Auto Gain:\t\t" << cam_params_.auto_gain << endl << + "Master Gain:\t\t" << cam_params_.master_gain << endl << + "Red Gain:\t\t" << cam_params_.red_gain << endl << + "Green Gain:\t\t" << cam_params_.green_gain << endl << + "Blue Gain:\t\t" << cam_params_.blue_gain << endl << + "Gain Boost:\t\t" << cam_params_.gain_boost << endl << + "Auto Exposure:\t\t" << cam_params_.auto_exposure << endl << + "Exposure (ms):\t\t" << cam_params_.exposure << endl << + "Auto White Balance:\t" << cam_params_.auto_white_balance << endl << + "WB Red Offset:\t\t" << cam_params_.white_balance_red_offset << endl << + "WB Blue Offset:\t\t" << cam_params_.white_balance_blue_offset << endl << + "Auto Frame Rate:\t" << cam_params_.auto_frame_rate << endl << + "Frame Rate (Hz):\t" << cam_params_.frame_rate << endl << + "Pixel Clock (MHz):\t" << cam_params_.pixel_clock << endl + ); +}; + + +INT UEyeCamNodelet::parseROSParams(ros::NodeHandle& local_nh) { + bool hasNewParams = false; + ueye_cam::UEyeCamConfig prevCamParams = cam_params_; + INT is_err = IS_SUCCESS; + + if (local_nh.hasParam("image_width")) { + local_nh.getParam("image_width", cam_params_.image_width); + if (cam_params_.image_width != prevCamParams.image_width) { + if (cam_params_.image_width <= 0) { + NODELET_WARN_STREAM("Invalid requested image width: " << cam_params_.image_width << + "; using current width: " << prevCamParams.image_width); + cam_params_.image_width = prevCamParams.image_width; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("image_height")) { + local_nh.getParam("image_height", cam_params_.image_height); + if (cam_params_.image_height != prevCamParams.image_height) { + if (cam_params_.image_height <= 0) { + NODELET_WARN_STREAM("Invalid requested image height: " << cam_params_.image_height << + "; using current height: " << prevCamParams.image_height); + cam_params_.image_height = prevCamParams.image_height; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("image_top")) { + local_nh.getParam("image_top", cam_params_.image_top); + if (cam_params_.image_top != prevCamParams.image_top) { + hasNewParams = true; + } + } + if (local_nh.hasParam("image_left")) { + local_nh.getParam("image_left", cam_params_.image_left); + if (cam_params_.image_left != prevCamParams.image_left) { + hasNewParams = true; + } + } + if (local_nh.hasParam("color_mode")) { + local_nh.getParam("color_mode", cam_params_.color_mode); + if (cam_params_.color_mode != prevCamParams.color_mode) { + if (cam_params_.color_mode.length() > 0) { + transform(cam_params_.color_mode.begin(), + cam_params_.color_mode.end(), + cam_params_.color_mode.begin(), + ::tolower); + if (cam_params_.color_mode != RGB8 && + cam_params_.color_mode != MONO8 && + cam_params_.color_mode != BAYER_RGGB8) { + NODELET_WARN_STREAM("Invalid requested color mode: " << cam_params_.color_mode << + "; using current mode: " << prevCamParams.color_mode); + cam_params_.color_mode = prevCamParams.color_mode; + } else { + hasNewParams = true; + } + } else { // Empty requested color mode string + cam_params_.color_mode = prevCamParams.color_mode; + } + } + } + if (local_nh.hasParam("subsampling")) { + local_nh.getParam("subsampling", cam_params_.subsampling); + if (cam_params_.subsampling != prevCamParams.subsampling) { + if (!(cam_params_.subsampling == 1 || + cam_params_.subsampling == 2 || + cam_params_.subsampling == 4 || + cam_params_.subsampling == 8 || + cam_params_.subsampling == 16)) { + NODELET_WARN_STREAM("Invalid or unsupported requested subsampling rate: " << cam_params_.subsampling << + "; using current rate: " << prevCamParams.subsampling); + cam_params_.subsampling = prevCamParams.subsampling; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("auto_gain")) { + local_nh.getParam("auto_gain", cam_params_.auto_gain); + if (cam_params_.auto_gain != prevCamParams.auto_gain) { + hasNewParams = true; + } + } + if (local_nh.hasParam("master_gain")) { + local_nh.getParam("master_gain", cam_params_.master_gain); + if (cam_params_.master_gain != prevCamParams.master_gain) { + if (cam_params_.master_gain < 0 || cam_params_.master_gain > 100) { + NODELET_WARN_STREAM("Invalid master gain: " << cam_params_.master_gain << + "; using current master gain: " << prevCamParams.master_gain); + cam_params_.master_gain = prevCamParams.master_gain; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("red_gain")) { + local_nh.getParam("red_gain", cam_params_.red_gain); + if (cam_params_.red_gain != prevCamParams.red_gain) { + if (cam_params_.red_gain < 0 || cam_params_.red_gain > 100) { + NODELET_WARN_STREAM("Invalid red gain: " << cam_params_.red_gain << + "; using current red gain: " << prevCamParams.red_gain); + cam_params_.red_gain = prevCamParams.red_gain; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("green_gain")) { + local_nh.getParam("green_gain", cam_params_.green_gain); + if (cam_params_.green_gain != prevCamParams.green_gain) { + if (cam_params_.green_gain < 0 || cam_params_.green_gain > 100) { + NODELET_WARN_STREAM("Invalid green gain: " << cam_params_.green_gain << + "; using current green gain: " << prevCamParams.green_gain); + cam_params_.green_gain = prevCamParams.green_gain; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("blue_gain")) { + local_nh.getParam("blue_gain", cam_params_.blue_gain); + if (cam_params_.blue_gain != prevCamParams.blue_gain) { + if (cam_params_.blue_gain < 0 || cam_params_.blue_gain > 100) { + NODELET_WARN_STREAM("Invalid blue gain: " << cam_params_.blue_gain << + "; using current blue gain: " << prevCamParams.blue_gain); + cam_params_.blue_gain = prevCamParams.blue_gain; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("gain_boost")) { + local_nh.getParam("gain_boost", cam_params_.gain_boost); + if (cam_params_.gain_boost != prevCamParams.gain_boost) { + hasNewParams = true; + } + } + if (local_nh.hasParam("auto_exposure")) { + local_nh.getParam("auto_exposure", cam_params_.auto_exposure); + if (cam_params_.auto_exposure != prevCamParams.auto_exposure) { + hasNewParams = true; + } + } + if (local_nh.hasParam("exposure")) { + local_nh.getParam("exposure", cam_params_.exposure); + if (cam_params_.exposure != prevCamParams.exposure) { + if (cam_params_.exposure < 0.0) { + NODELET_WARN_STREAM("Invalid requested exposure: " << cam_params_.exposure << + "; using current exposure: " << prevCamParams.exposure); + cam_params_.exposure = prevCamParams.exposure; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("auto_white_balance")) { + local_nh.getParam("auto_white_balance", cam_params_.auto_white_balance); + if (cam_params_.auto_white_balance != prevCamParams.auto_white_balance) { + hasNewParams = true; + } + } + if (local_nh.hasParam("white_balance_red_offset")) { + local_nh.getParam("white_balance_red_offset", cam_params_.white_balance_red_offset); + if (cam_params_.white_balance_red_offset != prevCamParams.white_balance_red_offset) { + if (cam_params_.white_balance_red_offset < -50 || cam_params_.white_balance_red_offset > 50) { + NODELET_WARN_STREAM("Invalid white balance red offset: " << cam_params_.white_balance_red_offset << + "; using current white balance red offset: " << prevCamParams.white_balance_red_offset); + cam_params_.white_balance_red_offset = prevCamParams.white_balance_red_offset; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("white_balance_blue_offset")) { + local_nh.getParam("white_balance_blue_offset", cam_params_.white_balance_blue_offset); + if (cam_params_.white_balance_blue_offset != prevCamParams.white_balance_blue_offset) { + if (cam_params_.white_balance_blue_offset < -50 || cam_params_.white_balance_blue_offset > 50) { + NODELET_WARN_STREAM("Invalid white balance blue offset: " << cam_params_.white_balance_blue_offset << + "; using current white balance blue offset: " << prevCamParams.white_balance_blue_offset); + cam_params_.white_balance_blue_offset = prevCamParams.white_balance_blue_offset; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("auto_frame_rate")) { + local_nh.getParam("auto_frame_rate", cam_params_.auto_frame_rate); + if (cam_params_.auto_frame_rate != prevCamParams.auto_frame_rate) { + hasNewParams = true; + } + } + if (local_nh.hasParam("frame_rate")) { + local_nh.getParam("frame_rate", cam_params_.frame_rate); + if (cam_params_.frame_rate != prevCamParams.frame_rate) { + if (cam_params_.frame_rate <= 0.0) { + NODELET_WARN_STREAM("Invalid requested frame rate: " << cam_params_.frame_rate << + "; using current frame rate: " << prevCamParams.frame_rate); + cam_params_.frame_rate = prevCamParams.frame_rate; + } else { + hasNewParams = true; + } + } + } + if (local_nh.hasParam("pixel_clock")) { + local_nh.getParam("pixel_clock", cam_params_.pixel_clock); + if (cam_params_.pixel_clock != prevCamParams.pixel_clock) { + if (cam_params_.pixel_clock < 0) { + NODELET_WARN_STREAM("Invalid requested pixel clock: " << cam_params_.pixel_clock << + "; using current pixel clock: " << prevCamParams.pixel_clock); + cam_params_.pixel_clock = prevCamParams.pixel_clock; + } else { + hasNewParams = true; + } + } + } + + if (hasNewParams) { + // Configure color mode, resolution, and subsampling rate + if ((is_err = setColorMode(cam_params_.color_mode, true)) != IS_SUCCESS) return is_err; + if ((is_err = setResolution(cam_params_.image_width, cam_params_.image_height, + cam_params_.image_left, cam_params_.image_top, false)) != IS_SUCCESS) return is_err; + if ((is_err = setSubsampling(cam_params_.subsampling, false)) != IS_SUCCESS) return is_err; + if ((is_err = setBinning(cam_params_.binning, false)) != IS_SUCCESS) return is_err; + if ((is_err = setSensorScaling(cam_params_.sensor_scaling, true)) != IS_SUCCESS) return is_err; + + // (Re-)populate ROS image message + // NOTE: the non-ROS UEye parameters and buffers have been updated by setColorMode, setResolution(), and setSubsampling() + ros_image_.header.frame_id = "/" + cam_name_; + ros_image_.height = cam_params_.image_height / (cam_params_.sensor_scaling * cam_params_.subsampling * cam_params_.binning); + ros_image_.width = cam_params_.image_width / (cam_params_.sensor_scaling * cam_params_.subsampling * cam_params_.binning); + ros_image_.encoding = cam_params_.color_mode; + ros_image_.step = cam_buffer_pitch_; + ros_image_.is_bigendian = 0; + ros_image_.data.resize(cam_buffer_size_); + + // Check for mutual exclusivity among requested sensor parameters + if (!cam_params_.auto_exposure) { // Auto frame rate requires auto shutter + cam_params_.auto_frame_rate = false; + } + if (cam_params_.auto_frame_rate) { // Auto frame rate has precedence over auto gain + cam_params_.auto_gain = false; + } + + // Configure camera sensor parameters + if ((is_err = setGain(cam_params_.auto_gain, cam_params_.master_gain, + cam_params_.red_gain, cam_params_.green_gain, + cam_params_.blue_gain, cam_params_.gain_boost)) != IS_SUCCESS) return is_err; + if ((is_err = setPixelClockRate(cam_params_.pixel_clock)) != IS_SUCCESS) return is_err; + if ((is_err = setFrameRate(cam_params_.auto_frame_rate, cam_params_.frame_rate)) != IS_SUCCESS) return is_err; + if ((is_err = setExposure(cam_params_.auto_exposure, cam_params_.exposure)) != IS_SUCCESS) return is_err; + if ((is_err = setWhiteBalance(cam_params_.auto_white_balance, cam_params_.white_balance_red_offset, + cam_params_.white_balance_blue_offset)) != IS_SUCCESS) return is_err; + } + + return is_err; +}; + + +void UEyeCamNodelet::configCallback(ueye_cam::UEyeCamConfig& config, uint32_t level) { + if (!isConnected()) return; + + // See if frame grabber needs to be restarted + bool restartFrameGrabber = false; + bool needToReallocateBuffer = false; + if (level == driver_base::SensorLevels::RECONFIGURE_STOP && frame_grab_alive_) { + restartFrameGrabber = true; + stopFrameGrabber(); + } + + // Configure color mode, resolution, and subsampling rate + if (config.color_mode != cam_params_.color_mode) { + needToReallocateBuffer = true; + if (setColorMode(config.color_mode, false) != IS_SUCCESS) return; + } + + if (config.image_width != cam_params_.image_width || + config.image_height != cam_params_.image_height || + config.image_left != cam_params_.image_left || + config.image_top != cam_params_.image_top) { + needToReallocateBuffer = true; + if (setResolution(config.image_width, config.image_height, + config.image_left, config.image_top, false) != IS_SUCCESS) { + // Attempt to restore previous (working) resolution + config.image_width = cam_params_.image_width; + config.image_height = cam_params_.image_height; + config.image_left = cam_params_.image_left; + config.image_top = cam_params_.image_top; + if (setResolution(config.image_width, config.image_height, + config.image_left, config.image_top) != IS_SUCCESS) return; + } + } + + if (config.subsampling != cam_params_.subsampling) { + needToReallocateBuffer = true; + if (setSubsampling(config.subsampling, false) != IS_SUCCESS) return; + } + + if (config.binning != cam_params_.binning) { + needToReallocateBuffer = true; + if (setBinning(config.binning, false) != IS_SUCCESS) return; + } + + if (config.sensor_scaling != cam_params_.sensor_scaling) { + needToReallocateBuffer = true; + if (setSensorScaling(config.sensor_scaling, false) != IS_SUCCESS) return; + } + + if (needToReallocateBuffer) { + if (reallocateCamBuffer() != IS_SUCCESS) return; + needToReallocateBuffer = false; + } + + // (Re-)populate ROS image message + // NOTE: the non-ROS UEye parameters and buffers have been updated by setColorMode(), + // setResolution(), setSubsampling(), setBinning(), and setSensorScaling() + ros_image_.header.frame_id = "/" + cam_name_; + ros_image_.height = config.image_height / (config.sensor_scaling * config.subsampling * config.binning); + ros_image_.width = config.image_width / (config.sensor_scaling * config.subsampling * config.binning); + ros_image_.encoding = config.color_mode; + ros_image_.step = cam_buffer_pitch_; + ros_image_.is_bigendian = 0; + ros_image_.data.resize(cam_buffer_size_); + + // Check for mutual exclusivity among requested sensor parameters + if (!config.auto_exposure) { // Auto frame rate requires auto shutter + config.auto_frame_rate = false; + } + if (config.auto_frame_rate) { // Auto frame rate has precedence over auto gain + config.auto_gain = false; + } + + // Configure camera sensor parameters + if (config.auto_gain != cam_params_.auto_gain || + config.master_gain != cam_params_.master_gain || + config.red_gain != cam_params_.red_gain || + config.green_gain != cam_params_.green_gain || + config.blue_gain != cam_params_.blue_gain || + config.gain_boost != cam_params_.gain_boost) { + if (setGain(config.auto_gain, config.master_gain, + config.red_gain, config.green_gain, + config.blue_gain, config.gain_boost) != IS_SUCCESS) return; + } + + if (config.pixel_clock != cam_params_.pixel_clock) { + if (setPixelClockRate(config.pixel_clock) != IS_SUCCESS) return; + } + + if (config.auto_frame_rate != cam_params_.auto_frame_rate || + config.frame_rate != cam_params_.frame_rate) { + if (setFrameRate(config.auto_frame_rate, config.frame_rate) != IS_SUCCESS) return; + } + + if (config.auto_exposure != cam_params_.auto_exposure || + config.exposure != cam_params_.exposure) { + if (setExposure(config.auto_exposure, config.exposure) != IS_SUCCESS) return; + } + + if (config.auto_white_balance != cam_params_.auto_white_balance || + config.white_balance_red_offset != cam_params_.white_balance_red_offset || + config.white_balance_blue_offset != cam_params_.white_balance_blue_offset) { + if (setWhiteBalance(config.auto_white_balance, config.white_balance_red_offset, + config.white_balance_blue_offset) != IS_SUCCESS) return; + } + + // Update local copy of parameter set to newly updated set + cam_params_ = config; + + // Restart frame grabber if needed + cfg_sync_requested_ = true; + if (restartFrameGrabber) { + startFrameGrabber(); + } +}; + + +INT UEyeCamNodelet::queryCamParams() { + INT is_err = IS_SUCCESS; + INT query; + double pval1, pval2; + + query = is_SetColorMode(cam_handle_, IS_GET_COLOR_MODE); + if (query == IS_CM_MONO8) cam_params_.color_mode = MONO8; + else if (query == IS_CM_SENSOR_RAW8) cam_params_.color_mode = BAYER_RGGB8; + else if (query == IS_CM_RGB8_PACKED) cam_params_.color_mode = RGB8; + else { + NODELET_WARN_STREAM("Camera configuration loaded into an unsupported color mode; switching to MONO8."); + cam_params_.color_mode = MONO8; + setColorMode(cam_params_.color_mode); + } + + if ((is_err = is_AOI(cam_handle_, IS_AOI_IMAGE_GET_AOI, + (void*) &cam_aoi_, sizeof(cam_aoi_))) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Could not retrieve Area Of Interest from UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + disconnectCam(); + return is_err; + } + cam_params_.image_width = cam_aoi_.s32Width; + cam_params_.image_height = cam_aoi_.s32Height; + cam_params_.image_left = cam_aoi_.s32X; + cam_params_.image_top = cam_aoi_.s32Y; + + query = is_SetSubSampling(cam_handle_, IS_GET_SUBSAMPLING); + switch (query) { + case IS_SUBSAMPLING_DISABLE: + cam_params_.subsampling = 1; + break; + case IS_SUBSAMPLING_2X: + cam_params_.subsampling = 2; + break; + case IS_SUBSAMPLING_4X: + cam_params_.subsampling = 4; + break; + case IS_SUBSAMPLING_8X: + cam_params_.subsampling = 8; + break; + case IS_SUBSAMPLING_16X: + cam_params_.subsampling = 16; + break; + default: + NODELET_WARN_STREAM("Query returned unsupported subsampling rate; resetting to 1X."); + cam_params_.subsampling = 1; + if ((is_err = setSubsampling(cam_params_.subsampling)) != IS_SUCCESS) return is_err; + break; + } + + query = is_SetBinning(cam_handle_, IS_GET_BINNING); + switch (query) { + case IS_BINNING_DISABLE: + cam_params_.binning = 1; + break; + case IS_BINNING_2X: + cam_params_.binning = 2; + break; + case IS_BINNING_4X: + cam_params_.binning = 4; + break; + case IS_BINNING_8X: + cam_params_.binning = 8; + break; + case IS_BINNING_16X: + cam_params_.binning = 16; + break; + default: + NODELET_WARN_STREAM("Query returned unsupported binning rate; resetting to 1X."); + cam_params_.binning = 1; + if ((is_err = setBinning(cam_params_.binning)) != IS_SUCCESS) return is_err; + break; + } + + SENSORSCALERINFO sensorScalerInfo; + is_err = is_GetSensorScalerInfo(cam_handle_, &sensorScalerInfo, sizeof(sensorScalerInfo)); + if (is_err == IS_NOT_SUPPORTED) { + cam_params_.sensor_scaling = 1.0; + } else if (is_err != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query sensor scaler info (" << err2str(is_err) << ")"); + return is_err; + } else { + cam_params_.sensor_scaling = sensorScalerInfo.dblCurrFactor; + if (!(cam_params_.sensor_scaling == 1.0 || + cam_params_.sensor_scaling == 2.0 || + cam_params_.sensor_scaling == 4.0 || + cam_params_.sensor_scaling == 8.0 || + cam_params_.sensor_scaling == 16.0)) { + NODELET_WARN_STREAM("Unsupported sensor scaling rate: " << cam_params_.sensor_scaling << + "; resetting to 1X."); + cam_params_.sensor_scaling = 1.0; + if ((is_err = setSensorScaling(cam_params_.sensor_scaling)) != IS_SUCCESS) return is_err; + } + } + + if ((is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_SENSOR_GAIN, &pval1, &pval2)) != IS_SUCCESS && + (is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_GAIN, &pval1, &pval2)) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query auto gain mode for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + cam_params_.auto_gain = (pval1 != 0); + + cam_params_.master_gain = is_SetHardwareGain(cam_handle_, IS_GET_MASTER_GAIN, + IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER); + cam_params_.red_gain = is_SetHardwareGain(cam_handle_, IS_GET_RED_GAIN, + IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER); + cam_params_.green_gain = is_SetHardwareGain(cam_handle_, IS_GET_GREEN_GAIN, + IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER); + cam_params_.blue_gain = is_SetHardwareGain(cam_handle_, IS_GET_BLUE_GAIN, + IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER); + + query = is_SetGainBoost(cam_handle_, IS_GET_GAINBOOST); + if (query == IS_SET_GAINBOOST_ON) { + cam_params_.gain_boost = true; + } else if (query == IS_SET_GAINBOOST_OFF) { + cam_params_.gain_boost = false; + } else { + NODELET_ERROR_STREAM("Failed to query gain boost for UEye camera '" << + cam_name_ << "' (" << err2str(query) << ")"); + return query; + } + + if ((is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_SENSOR_SHUTTER, &pval1, &pval2)) != IS_SUCCESS && + (is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_SHUTTER, &pval1, &pval2)) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query auto shutter mode for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + cam_params_.auto_exposure = (pval1 != 0); + + if ((is_err = is_Exposure(cam_handle_, IS_EXPOSURE_CMD_GET_EXPOSURE, + &cam_params_.exposure, sizeof(cam_params_.exposure))) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query exposure timing for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + + if ((is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_SENSOR_WHITEBALANCE, &pval1, &pval2)) != IS_SUCCESS && + (is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_WHITEBALANCE, &pval1, &pval2)) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query auto white balance mode for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + cam_params_.auto_white_balance = (pval1 != 0); + + if ((is_err = is_SetAutoParameter(cam_handle_, + IS_GET_AUTO_WB_OFFSET, &pval1, &pval2)) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query auto white balance red/blue channel offsets for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + cam_params_.white_balance_red_offset = pval1; + cam_params_.white_balance_blue_offset = pval2; + + if ((is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_SENSOR_FRAMERATE, &pval1, &pval2)) != IS_SUCCESS && + (is_err = is_SetAutoParameter(cam_handle_, + IS_GET_ENABLE_AUTO_FRAMERATE, &pval1, &pval2)) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query auto frame rate mode for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + cam_params_.auto_frame_rate = (pval1 != 0); + + if ((is_err = is_SetFrameRate(cam_handle_, IS_GET_FRAMERATE, &cam_params_.frame_rate)) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query frame rate for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + + UINT currPixelClock; + if ((is_err = is_PixelClock(cam_handle_, IS_PIXELCLOCK_CMD_GET, + (void*) &currPixelClock, sizeof(currPixelClock))) != IS_SUCCESS) { + NODELET_ERROR_STREAM("Failed to query pixel clock rate for UEye camera '" << + cam_name_ << "' (" << err2str(is_err) << ")"); + return is_err; + } + cam_params_.pixel_clock = currPixelClock; + + // Populate ROS image message + // NOTE: the non-ROS UEye parameters and buffers have been updated by setColorMode, setResolution(), and setSubsampling() + ros_image_.header.frame_id = "/" + cam_name_; + ros_image_.height = cam_params_.image_height / + (cam_params_.sensor_scaling * cam_params_.subsampling * cam_params_.binning); + ros_image_.width = cam_params_.image_width / + (cam_params_.sensor_scaling * cam_params_.subsampling * cam_params_.binning); + ros_image_.encoding = cam_params_.color_mode; + ros_image_.step = cam_buffer_pitch_; + ros_image_.is_bigendian = 0; + ros_image_.data.resize(cam_buffer_size_); + + return is_err; +}; + + +INT UEyeCamNodelet::connectCam() { + INT is_err = IS_SUCCESS; + + if ((is_err = UEyeCamDriver::connectCam()) != IS_SUCCESS) return is_err; + + // (Attempt to) load UEye camera parameter configuration file + if (cam_params_filename_.length() <= 0) { // Use default filename + cam_params_filename_ = string(getenv("HOME")) + "/.ros/camera_conf/" + cam_name_ + ".ini"; + } + loadCamConfig(cam_params_filename_); + + // Query existing configuration parameters from camera + if ((is_err = queryCamParams()) != IS_SUCCESS) return is_err; + + // Parse and load ROS camera settings + if ((is_err = parseROSParams(getPrivateNodeHandle())) != IS_SUCCESS) return is_err; + + return IS_SUCCESS; +}; + + +INT UEyeCamNodelet::disconnectCam() { + INT is_err = IS_SUCCESS; + + if (isConnected()) { + stopFrameGrabber(); + is_err = UEyeCamDriver::disconnectCam(); + } + + return is_err; +}; + + +bool UEyeCamNodelet::setCamInfo(sensor_msgs::SetCameraInfo::Request& req, + sensor_msgs::SetCameraInfo::Response& rsp) { + ros_cam_info_ = req.camera_info; + ros_cam_info_.header.frame_id = "/" + cam_name_; + rsp.success = saveIntrinsicsFile(); + rsp.status_message = (rsp.success) ? "successfully wrote to file" : "failed to write to file"; + return true; +}; + + +void UEyeCamNodelet::frameGrabLoop() { +#ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES + ros::Time prevStartGrab = ros::Time::now(); + ros::Time prevGrabbedFrame = ros::Time::now(); + ros::Time currStartGrab; + ros::Time currGrabbedFrame; + double startGrabSum = 0; + double grabbedFrameSum = 0; + double startGrabSumSqrd = 0; + double grabbedFrameSumSqrd = 0; + unsigned int startGrabCount = 0; + unsigned int grabbedFrameCount = 0; +#endif + + ros::Rate idleDelay(200); + + while (frame_grab_alive_ && ros::ok()) { + // Initialize live video mode if camera was previously asleep, and ROS image topic has subscribers; + // and stop live video mode if ROS image topic no longer has any subscribers + if (ros_cam_pub_.getNumSubscribers() > 0) { + if (startLiveCapture() != IS_SUCCESS) { + ERROR_STREAM("Shutting down UEye camera interface..."); + ros::shutdown(); + return; + } + } else if (ros_cam_pub_.getNumSubscribers() <= 0) { + if (stopLiveCapture() != IS_SUCCESS) { + ERROR_STREAM("Shutting down UEye camera interface..."); + ros::shutdown(); + return; + } + } + + // Send updated dyncfg parameters if previously changed + if (cfg_sync_requested_) { + if (ros_cfg_mutex_.try_lock()) { // Make sure that dynamic reconfigure server or config callback is not active + ros_cfg_mutex_.unlock(); + ros_cfg_->updateConfig(cam_params_); + cfg_sync_requested_ = false; + } + } + + +#ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES + startGrabCount++; + currStartGrab = ros::Time::now(); + if (startGrabCount > 1) { + startGrabSum += (currStartGrab - prevStartGrab).toSec() * 1000.0; + startGrabSumSqrd += ((currStartGrab - prevStartGrab).toSec() * 1000.0)*((currStartGrab - prevStartGrab).toSec() * 1000.0); + } + prevStartGrab = currStartGrab; +#endif + + if (isLiveCapturing()) { + INT eventTimeout = (cam_params_.auto_frame_rate) ? + (INT) 2000 : (INT) (1000.0 / cam_params_.frame_rate * 2); + if (processNextFrame(eventTimeout) != NULL) { + // Process new frame +#ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES + grabbedFrameCount++; + currGrabbedFrame = ros::Time::now(); + if (grabbedFrameCount > 1) { + grabbedFrameSum += (currGrabbedFrame - prevGrabbedFrame).toSec() * 1000.0; + grabbedFrameSumSqrd += ((currGrabbedFrame - prevGrabbedFrame).toSec() * 1000.0)*((currGrabbedFrame - prevGrabbedFrame).toSec() * 1000.0); + } + prevGrabbedFrame = currGrabbedFrame; + + if (grabbedFrameCount > 1) { + ROS_WARN_STREAM("\nPre-Grab: " << startGrabSum/startGrabCount << " +/- " << + sqrt(startGrabSumSqrd/startGrabCount - (startGrabSum/startGrabCount)*(startGrabSum/startGrabCount)) << " ms (" << + 1000.0*startGrabCount/startGrabSum << "Hz)\n" << + "Post-Grab: " << grabbedFrameSum/grabbedFrameCount << " +/- " << + sqrt(grabbedFrameSumSqrd/grabbedFrameCount - (grabbedFrameSum/grabbedFrameCount)*(grabbedFrameSum/grabbedFrameCount)) << " ms (" << + 1000.0*grabbedFrameCount/grabbedFrameSum << "Hz)\n" << + "Target: " << cam_params_.frame_rate << "Hz"); + } +#endif + + copy((char*) cam_buffer_, + ((char*) cam_buffer_) + cam_buffer_size_, + ros_image_.data.begin()); + ros_image_.header.stamp = ros_cam_info_.header.stamp = ros::Time::now(); + ros_image_.header.seq = ros_cam_info_.header.seq = ros_frame_count_++; + ros_cam_info_.width = cam_params_.image_width; + ros_cam_info_.height = cam_params_.image_height; + ros_cam_pub_.publish(ros_image_, ros_cam_info_); + } + } + + idleDelay.sleep(); + } + + stopLiveCapture(); + frame_grab_alive_ = false; +}; + + +void UEyeCamNodelet::startFrameGrabber() { + frame_grab_alive_ = true; + frame_grab_thread_ = thread(bind(&UEyeCamNodelet::frameGrabLoop, this)); +}; + + +void UEyeCamNodelet::stopFrameGrabber() { + frame_grab_alive_ = false; + if (frame_grab_thread_.joinable()) { + frame_grab_thread_.join(); + } + frame_grab_thread_ = thread(); +}; + + +void UEyeCamNodelet::loadIntrinsicsFile() { + if (cam_intr_filename_.length() <= 0) { // Use default filename + cam_intr_filename_ = string(getenv("HOME")) + "/.ros/camera_info/" + cam_name_ + ".yaml"; + } + + std::string dummyCamName; + if (camera_calibration_parsers::readCalibrationIni(cam_intr_filename_, dummyCamName, ros_cam_info_)) { + NODELET_DEBUG_STREAM("Loaded intrinsics parameters for UEye camera " << cam_name_); + } + ros_cam_info_.header.frame_id = "/" + cam_name_; +}; + + +bool UEyeCamNodelet::saveIntrinsicsFile() { + if (camera_calibration_parsers::writeCalibrationIni(cam_intr_filename_, cam_name_, ros_cam_info_)) { + NODELET_DEBUG_STREAM("Saved intrinsics parameters for UEye camera " << cam_name_ << + " to " << cam_intr_filename_); + return true; + } + return false; +}; + + +}; // namespace ueye_cam + + +// TODO: bug: when binning (and suspect when subsampling / sensor scaling), white balance / color gains seem to have different effects + + +#include +PLUGINLIB_DECLARE_CLASS(ueye_cam, ueye_cam_nodelet, ueye_cam::UEyeCamNodelet, nodelet::Nodelet)