├── CMakeLists.txt ├── README.md ├── cfg └── Flea3Dyn.cfg ├── cmake ├── FindFlyCapture2.cmake └── flea3.cmake ├── flycapture ├── include │ └── flycapture │ │ ├── AVIRecorder.h │ │ ├── BusManager.h │ │ ├── Camera.h │ │ ├── CameraBase.h │ │ ├── Error.h │ │ ├── FlyCapture2.h │ │ ├── FlyCapture2Defs.h │ │ ├── FlyCapture2Platform.h │ │ ├── GigECamera.h │ │ ├── Image.h │ │ ├── ImageStatistics.h │ │ ├── TopologyNode.h │ │ └── Utilities.h └── lib │ ├── libflycapture.so │ ├── libflycapture.so.2 │ └── libflycapture.so.2.11.3.121 ├── include └── flea3 │ ├── flea3_camera.h │ ├── flea3_ros.h │ ├── flea3_setting.h │ ├── single_node.h │ └── stereo_node.h ├── launch ├── single_node.launch ├── single_nodelet.launch └── stereo_node.launch ├── msg └── ImageMetadata.msg ├── nodelet_plugins.xml ├── package.xml └── src ├── flea3_camera.cpp ├── flea3_ros.cpp ├── flea3_setting.cpp ├── flycapture ├── async_trigger.cpp ├── custom_image.cpp └── extended_shutter.cpp ├── list_cameras.cpp ├── single ├── single_main.cpp ├── single_node.cpp └── single_nodelet.cpp └── stereo ├── stereo_main.cpp ├── stereo_node.cpp └── stereo_nodelet.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(flea3) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp nodelet camera_base 5 | dynamic_reconfigure message_generation) 6 | 7 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) 8 | set(CMAKE_CXX_STANDARD 11) 9 | 10 | find_package(FlyCapture2 REQUIRED) 11 | 12 | # Dynamic reconfigure 13 | generate_dynamic_reconfigure_options(cfg/Flea3Dyn.cfg) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS 17 | include 18 | LIBRARIES 19 | ${PROJECT_NAME} 20 | CATKIN_DEPENDS 21 | roscpp 22 | nodelet 23 | camera_base 24 | dynamic_reconfigure 25 | message_runtime # std_msgs 26 | DEPENDS 27 | FlyCapture2) 28 | 29 | include_directories(include ${catkin_INCLUDE_DIRS} ${FlyCapture2_INCLUDE_DIRS}) 30 | 31 | add_library( 32 | ${PROJECT_NAME} 33 | src/flea3_camera.cpp src/flea3_setting.cpp src/flea3_ros.cpp 34 | src/single/single_node.cpp src/stereo/stereo_node.cpp 35 | src/single/single_nodelet.cpp) 36 | target_include_directories( 37 | ${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS} 38 | ${FlyCapture2_INCLUDE_DIRS}) 39 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} 40 | ${FlyCapture2_LIBRARIES}) 41 | 42 | add_executable(${PROJECT_NAME}_single_node src/single/single_main.cpp) 43 | target_link_libraries(${PROJECT_NAME}_single_node PUBLIC ${PROJECT_NAME}) 44 | 45 | add_executable(${PROJECT_NAME}_stereo_node src/stereo/stereo_main.cpp) 46 | target_link_libraries(${PROJECT_NAME}_stereo_node PUBLIC ${PROJECT_NAME}) 47 | 48 | install( 49 | TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_single_node 50 | ${PROJECT_NAME}_stereo_node 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 54 | 55 | install( 56 | DIRECTORY include/${PROJECT_NAME}/ 57 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 58 | FILES_MATCHING 59 | PATTERN "*.h") 60 | 61 | install( 62 | DIRECTORY flycapture/include 63 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 64 | FILES_MATCHING 65 | PATTERN "*.h") 66 | 67 | install(DIRECTORY flycapture/lib/ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 68 | 69 | install(FILES nodelet_plugins.xml 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 71 | 72 | install( 73 | DIRECTORY launch 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 75 | PATTERN "*.*~" EXCLUDE) 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # flea3 (not maintained anymore, use https://github.com/ros-drivers/flir_camera_driver instead) 2 | 3 | ![image](https://s-media-cache-ak0.pinimg.com/736x/93/3b/2b/933b2b91ddf297db234e2f6d1e046e5c.jpg) 4 | 5 | Another ROS driver for Point Grey USB3 camera. 6 | 7 | **Note**: 8 | 9 | The driver works with the following cameras: 10 | 11 | [Flea3](http://www.ptgrey.com/flea3-usb3-vision-cameras) 12 | 13 | [Grasshopper3](http://www.ptgrey.com/grasshopper3-usb3-vision-cameras) 14 | 15 | [Chameleon3](http://http://www.ptgrey.com/chameleon3-usb3-vision-cameras) 16 | 17 | Dependency: 18 | [`camera_base`](https://github.com/KumarRobotics/camera_base) 19 | 20 | See the dynamic reconfigure file for all reconfigurable parameters. 21 | 22 | # Supported hardware 23 | 24 | These are the point-grey usb3 cameras that I have and tested with. 25 | 26 | [CM3-U3-13Y3M-CS](http://www.ptgrey.com/chameleon3-13-mp-mono-usb3-vision-on-semi-python-1300) 27 | 28 | [GS3-U3-23S6C-C](http://www.ptgrey.com/grasshopper3-23-mp-color-usb3-vision-sony-pregius-imx174) 29 | 30 | [FL3-U3-13E4C-C](http://www.ptgrey.com/flea3-13-mp-color-usb3-vision-e2v-ev76c560-camera) 31 | 32 | ## API Stability 33 | 34 | The ROS API of this driver should be considered unstable. 35 | 36 | ## ROS API 37 | 38 | `single_node` is a node for a single flea3 camera. 39 | 40 | #### Published topics 41 | 42 | `~image_raw` ([sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)) 43 | 44 | The unprocessed image data. 45 | 46 | `~camera_info` ([sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)) 47 | 48 | Contains the camera calibration (if calibrated). 49 | 50 | #### Parameters 51 | 52 | See the dynamic reconfigure file for all reconfigurable parameters. 53 | 54 | Usage: 55 | * single camera 56 | ``` 57 | roslaunch flea3 single_node.launch device:=13344889 58 | ``` 59 | 60 | * stereo camera 61 | ``` 62 | roslaunch flea3 stereo_node.launch left:=13344889 right:=14472242 63 | ``` 64 | Note that the `stereo_node` uses software trigger to synchronize two cameras and the delay is not compensated. 65 | 66 | * Check available cameras 67 | ``` 68 | rosrun flea3 flea3_list_cameras 69 | ``` 70 | 71 | ## FlyCapture2 72 | 73 | FlyCapture2 can be downloaded from [here](http://www.ptgrey.com/support/downloads) 74 | 75 | ## Documentation 76 | 77 | [Flea3 Technical Manual](http://www.ptgrey.com/support/downloads/10120) 78 | 79 | [Grasshopper3 Technical Manual](http://www.ptgrey.com/support/downloads/10125) 80 | 81 | [Chameleon3 Technical Manual](http://www.ptgrey.com/support/downloads/10431) 82 | 83 | [Register Reference](http://www.ptgrey.com/support/downloads/10130/) 84 | 85 | ## Known Issues 86 | 87 | ## Flycapture 2.8.3 issue 88 | 89 | Flycapture 2.8.3 generally works, so you can upgrade to it if you are using only 1 camera. The ros driver using Flycapture 2.8.3 does not work well with more than 1 cameras as explained below. 90 | 91 | The issue is that when launching the second camera, the `Camera.StartCapture()` method creates a new thread and never exits, thus blocking the following acquisition. PtGrey is working on a new release to fix this issue. 92 | 93 | Flycapture 2.7.3 also generally works, but it has issues with USB3 on Intel chips under Ubuntu system. You can get around with just using USB2 cable, but if you require high fps + high resolution, then you should use Flycapture 2.6.4. 94 | 95 | What a mess. 96 | 97 | ## Optimizing USB performance under Linux 98 | 99 | Here is an [article](http://www.matrix-vision.com/manuals/mvBlueFOX3/mvBC_page_quickstart.html#mvBC_subsubsection_quickstart_linux_requirements_optimising_usb) from matrix-vision on how to optimize USB performance. 100 | 101 | Another [article](http://www.ptgrey.com/KB/10685) from point-grey on how to optimize USB performance. 102 | 103 | In `/etc/default/grub`, change 104 | ``` 105 | GRUB_CMDLINE_LINUX_DEFAULT="quiet splash" 106 | ``` 107 | to 108 | ``` 109 | GRUB_CMDLINE_LINUX_DEFAULT="quiet splash usbcore.usbfs_memory_mb=1024" 110 | ``` 111 | then do 112 | ``` 113 | sudo update-grub 114 | ``` 115 | then restart and verify that 116 | ``` 117 | cat /sys/module/usbcore/parameters/usbfs_memory_mb 118 | ``` 119 | is `1024` 120 | 121 | Also it is recommended to upgrade your kernel to 3.16 by doing 122 | ``` 123 | sudo apt-get install linux-signed-generic-lts-utopic 124 | ``` 125 | -------------------------------------------------------------------------------- /cfg/Flea3Dyn.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | PACKAGE = "flea3" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # fps/frame_rate provides control over the frame rate of the camera. 9 | gen.add("fps", double_t, 0, 10 | "Frames per second", 11 | 7.5, 1, 240) 12 | 13 | # Video Mode controls setting of standard video mode or format 7 video mode 14 | video_mode_enum = gen.enum([gen.const("rgb_640x480", int_t, 4, ""), 15 | gen.const("y8_640x480", int_t, 5, ""), 16 | gen.const("rgb_800x600", int_t, 8, ""), 17 | gen.const("y8_800x600", int_t, 9, ""), 18 | gen.const("rgb_1024x768", int_t, 12, ""), 19 | gen.const("y8_1024x768", int_t, 13, ""), 20 | gen.const("rgb_1280x960", int_t, 16, ""), 21 | gen.const("y8_1280x960", int_t, 17, ""), 22 | gen.const("rgb_1600x1200", int_t, 20, ""), 23 | gen.const("y8_1600x1200", int_t, 21, ""), 24 | gen.const("format7", int_t, 23, "")], 25 | "An enum to set video mode") 26 | gen.add("video_mode", int_t, 0, 27 | "Video mode for camera", 28 | 23, 4, 23, edit_method=video_mode_enum) 29 | gen.add("format7_mode", int_t, 0, 30 | "Camera modes for format 7", 31 | 0, 0, 8) 32 | 33 | # ROI controls region of intreset in format 7 mode. Assumes centered ROI 34 | # Set to 0 to reset ROI 35 | gen.add("width", int_t, 0, 36 | "ROI width", 37 | 0, 0, 4096) 38 | gen.add("height", int_t, 0, 39 | "ROI height", 40 | 0, 0, 4096) 41 | 42 | # Pixel format for format 7 mode 43 | pixel_format_enum = gen.enum([gen.const("unspecified", int_t, 0, "unspecified"), 44 | gen.const("mono8", int_t, 31, "mono8"), 45 | gen.const("rgb8", int_t, 27, "rgb8"), 46 | gen.const("mono16", int_t, 26, "mono16"), 47 | gen.const("raw8", int_t, 22, "raw8"), 48 | gen.const("raw16", int_t, 21, "raw16")], 49 | "An enum to set pixel format") 50 | gen.add("pixel_format", int_t, 0, 51 | "Pixel format for format 7", 52 | 0, 0, 31, edit_method=pixel_format_enum) 53 | 54 | gen.add("raw_bayer_output", bool_t, 0, 55 | "Enable raw bayer output in standard video mode Y8 and Y16", 56 | False) 57 | 58 | # Trigger control 59 | trigger_source_enum = gen.enum([gen.const("ts_off", int_t, -1, "off"), 60 | gen.const("ts_gpio_0", int_t, 0, "GPIO0"), 61 | gen.const("ts_gpio_2", int_t, 2, "GPIO2"), 62 | gen.const("ts_gpio_3", int_t, 3, "GPIO3"), 63 | gen.const("ts_sw", int_t, 7, "software")], 64 | "Defines valid trigger source") 65 | gen.add("trigger_source", int_t, 0, 66 | "Specifies the trigger source", 67 | -1, -1, 7, edit_method=trigger_source_enum) 68 | trigger_polarity_enum = gen.enum([gen.const("ts_low", int_t, 0, "low"), 69 | gen.const("ts_high", int_t, 1, "high")], 70 | "Trigger polarity") 71 | gen.add("trigger_polarity", int_t, 0, 72 | "Trigger polarity", 73 | 0, 0, 1, edit_method=trigger_polarity_enum) 74 | # trigger modes 75 | trigger_mode_enum = gen.enum([gen.const("mode_0", int_t, 0, "standard ext (0)"), 76 | gen.const("mode_1", int_t, 1, "bulb shutter (1)"), 77 | gen.const("mode_3", int_t, 3, "skip frams (3)"), 78 | gen.const("mode_4", int_t, 4, "multi preset (4)"), 79 | gen.const("mode_5", int_t, 5, "multi pwidth (5)"), 80 | gen.const("mode_13", int_t, 13, "low smear (13)"), 81 | gen.const("mode_14", int_t, 14, "overlapped (14)"), 82 | gen.const("mode_15", int_t, 15, "multi-shot (15)")], 83 | "An enum to set trigger modes") 84 | gen.add("trigger_mode", int_t, 0, 85 | "Specifies the trigger mode", 86 | 0, 0, 15, edit_method=trigger_mode_enum) 87 | 88 | # Strobe control for now it's gpio 2 89 | strobe_control_enum = gen.enum([gen.const("sc_off", int_t, -1, "off"), 90 | gen.const("sc_gpio_1", int_t, 1, "GPIO1"), 91 | gen.const("sc_gpio_2", int_t, 2, "GPIO2"), 92 | gen.const("sc_gpio_3", int_t, 3, "GPIO3")], 93 | "Define valid strobe control") 94 | gen.add("strobe_control", int_t, 0, 95 | "Specifies the stroble control", 96 | -1, -1, 3, edit_method=strobe_control_enum) 97 | strobe_polarity_enum = gen.enum([gen.const("sc_low", int_t, 0, "low"), 98 | gen.const("sc_high", int_t, 1, "high")], 99 | "Strobe polarity") 100 | gen.add("strobe_polarity", int_t, 0, 101 | "Strobe polarity", 102 | 0, 0, 1, edit_method=strobe_polarity_enum) 103 | gen.add("enable_output_voltage", bool_t, 0, 104 | "Enable 3.3V output voltage on GPIO pin3 (blackfly)", 105 | False) 106 | 107 | # Exposure 108 | # 1. Off: Control of the exposure is achieved via setting both Exposure and 109 | # Gain. This mode is achieved by setting Auto Exposure to Off, or by 110 | # setting Exposure and Gain to Off. 111 | # 2. Manual: The camera automatically modifies Exposure and Gain to try to match 112 | # the average image intensity to the Auto Exposure value. This mode 113 | # is achieved by setting Auto Exposure to Off and either/both of 114 | # Exposure and Gain to Continuous 115 | # 3. Auto: The camera automatically modifies the value in order to produce an 116 | # image that is visually pleasing. This mode is achieved by setting 117 | # the all three of Auto Exposure, Exposure, and Gain to Continuous. 118 | # In this mode, the value reflects the average image intensity 119 | gen.add("exposure", bool_t, 0, 120 | "Enable/disable exposure", 121 | True) 122 | gen.add("auto_exposure", bool_t, 0, 123 | "Automatically change exposure (Gain, IRis & Shutter control)", 124 | True) 125 | gen.add("exposure_value", double_t, 0, 126 | "Auto exposure value", 127 | 1.35, -10.0, 10.0) 128 | 129 | gen.add("auto_shutter", bool_t, 0, 130 | "Auomatically change shutter speed", 131 | True) 132 | gen.add("shutter_ms", double_t, 0, 133 | "Amount of time (in milliseconds) for the apeture to remain open", 134 | 0.0, 0.0, 100) 135 | 136 | gen.add("auto_gain", bool_t, 0, 137 | "Automatically change gain", 138 | True) 139 | gen.add("gain_db", double_t, 0, 140 | "Relative circuit gain", 141 | 0.0, 0.0, 30.0) 142 | 143 | # White Balance 144 | # When white balance is off, 145 | # it is effectively the same as setting both red and blue to 512 146 | gen.add("white_balance", bool_t, 0, 147 | "Enable/disable white balance", 148 | False) 149 | gen.add("auto_white_balance", bool_t, 0, 150 | "Automatically change white balance", 151 | True) 152 | gen.add("wb_blue", int_t, 0, 153 | "White balance blue component", 154 | 800, 0, 1023) 155 | gen.add("wb_red", int_t, 0, 156 | "White balance red component", 157 | 550, 0, 1023) 158 | 159 | gen.add("brightness", double_t, 0, 160 | "Black level offset", 161 | 0.0, 0.0, 10.0) 162 | 163 | gen.add("gamma", double_t, 0, 164 | "Gamma expansion exponent", 165 | 1.0, 0.5, 4.0) 166 | 167 | exit(gen.generate(PACKAGE, "flea3", "Flea3Dyn")) 168 | -------------------------------------------------------------------------------- /cmake/FindFlyCapture2.cmake: -------------------------------------------------------------------------------- 1 | # FindFlyCapture2.cmake - Find FlyCapture2 SDK. 2 | # Modified from FindEigen.cmake by alexs.mac@gmail.com (Alex Stewart) 3 | # 4 | # This module defines the following variables: 5 | # 6 | # FlyCapture2_FOUND: TRUE if flycapture is found. 7 | # FlyCapture2_INCLUDE_DIRS: Include directories for flycapture. 8 | # FlyCapture2_LIBRARIES: Libraries for all flycapture component libraries and 9 | # dependencies. 10 | # 11 | # Additionally, in Windows, it also defines the following variables 12 | # 13 | # FlyCapture2_LIBRARIES_DEBUG: Debug libraries for flycapture 14 | # FlyCapture2_DLL: The DLL corresponding to the found library, for deployment 15 | # purposes 16 | #FlyCapture2_DLL_DEBUG : The DLL corresponding to the found debug library, 17 | # for deployment purposes 18 | # 19 | # FlyCapture2_VERSION: Extracted from lib/libflycapture.so.x.y.z 20 | # FlyCapture2_WORLD_VERSION: Equal to 2 if FlyCapture2_VERSION = 2.8.3 21 | # FlyCapture2_MAJOR_VERSION: Equal to 8 if FlyCapture2_VERSION = 2.8.3 22 | # FlyCapture2_MINOR_VERSION: Equal to 3 if FlyCapture2_VERSION = 2.8.3 23 | # 24 | # The following variables control the behaviour of this module: 25 | # 26 | # FlyCapture2_INCLUDE_DIR_HINTS: List of additional directories in which to 27 | # search for flycapture includes, e.g: /foo/include. 28 | # FlyCapture2_LIBRARY_DIR_HINTS: List of additional directories in which to 29 | # search for flycapture libraries, e.g: /bar/lib. 30 | # 31 | # The following variables are also defined by this module, but in line with 32 | # CMake recommended FindPackage() module style should NOT be referenced directly 33 | # by callers (use the plural variables detailed above instead). These variables 34 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 35 | # are NOT re-called (i.e. search for library is not repeated) if these variables 36 | # are set with valid values _in the CMake cache_. This means that if these 37 | # variables are set directly in the cache, either by the user in the CMake GUI, 38 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 39 | # explicitly defines a cache variable), then they will be used verbatim, 40 | # bypassing the HINTS variables and other hard-coded search locations. 41 | # 42 | # FlyCapture2_INCLUDE_DIR: Include directory for flycapture, not including the 43 | # include directory of any dependencies. 44 | # FlyCapture2_LIBRARY: flycapture library, not including the libraries of any 45 | # dependencies. 46 | 47 | # Called if we failed to find flycapture or any of it's required dependencies, 48 | # unsets all public (designed to be used externally) variables and reports 49 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 50 | macro(FlyCapture2_REPORT_NOT_FOUND REASON_MSG) 51 | unset(FlyCapture2_FOUND) 52 | unset(FlyCapture2_INCLUDE_DIRS) 53 | unset(FlyCapture2_LIBRARIES) 54 | unset(FlyCapture2_WORLD_VERSION) 55 | unset(FlyCapture2_MAJOR_VERSION) 56 | unset(FlyCapture2_MINOR_VERSION) 57 | # Make results of search visible in the CMake GUI if flycapture has not 58 | # been found so that user does not have to toggle to advanced view. 59 | mark_as_advanced(CLEAR FlyCapture2_INCLUDE_DIR) 60 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 61 | # use the camelcase library name, not uppercase. 62 | if(FlyCapture2_FIND_QUIETLY) 63 | message(STATUS "Failed to find flycapture - " ${REASON_MSG} ${ARGN}) 64 | elseif(FlyCapture2_FIND_REQUIRED) 65 | message(FATAL_ERROR "Failed to find flycapture - " ${REASON_MSG} ${ARGN}) 66 | else() 67 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 68 | # but continues configuration and allows generation. 69 | message("-- Failed to find flycapture - " ${REASON_MSG} ${ARGN}) 70 | endif() 71 | endmacro(FlyCapture2_REPORT_NOT_FOUND) 72 | 73 | # Search user-installed locations first, so that we prefer user installs 74 | # to system installs where both exist. 75 | get_filename_component(FLYCAPTURE_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH) 76 | list(APPEND FlyCapture2_CHECK_INCLUDE_DIRS 77 | ${FLYCAPTURE_DIR}/flycapture/include 78 | ) 79 | list(APPEND FlyCapture2_CHECK_LIBRARY_DIRS 80 | ${FLYCAPTURE_DIR}/flycapture/lib 81 | ) 82 | if(WIN32) 83 | set(FlyCapture2_HINTS "C:/Program Files/Point Grey Research/FlyCapture2" CACHE PATH "Path to FlyCapture2 SDK" FORCE) 84 | endif() 85 | 86 | 87 | # Check general hints 88 | if(FlyCapture2_HINTS AND EXISTS ${FlyCapture2_HINTS}) 89 | set(FlyCapture2_INCLUDE_DIR_HINTS ${FlyCapture2_HINTS}/include) 90 | set(FlyCapture2_LIBRARY_DIR_HINTS ${FlyCapture2_HINTS}/lib) 91 | if(WIN32) 92 | set(FlyCapture2_LIBRARY_DIR_HINTS ${FlyCapture2_HINTS}/lib64) 93 | set(FlyCapture2_DLL_DIR_HINTS ${FlyCapture2_HINTS}/bin64) 94 | if(MSVC) 95 | if(MSVC_VERSION GREATER_EQUAL "1800") 96 | if(MSVC_VERSION LESS "1900") 97 | set(FlyCapture2_LIBRARY_DIR_HINTS ${FlyCapture2_LIBRARY_DIR_HINTS}/vs2013) 98 | set(FlyCapture2_DLL_DIR_HINTS ${FlyCapture2_DLL_DIR_HINTS}/vs2013) 99 | else() 100 | set(FlyCapture2_LIBRARY_DIR_HINTS ${FlyCapture2_LIBRARY_DIR_HINTS}/vs2015) 101 | set(FlyCapture2_DLL_DIR_HINTS ${FlyCapture2_DLL_DIR_HINTS}/vs2015) 102 | endif() 103 | endif() 104 | else() 105 | message(WARNING "You don't seem to be using MSVC. FlyCapture is likely to not link!!!") 106 | message(WARNING "Consider using the C library instead?") 107 | #This script would need further work to point you to that library 108 | set(FlyCapture2_LIBRARY_DIR_HINTS ${FlyCapture2_LIBRARY_DIR_HINTS}/vs2015) 109 | set(FlyCapture2_DLL_DIR_HINTS ${FlyCapture2_DLL_DIR_HINTS}/vs2015) 110 | endif() 111 | endif() 112 | 113 | endif() 114 | 115 | # Mark internally as found, then verify. FlyCapture2_REPORT_NOT_FOUND() unsets if 116 | # called. 117 | set(FlyCapture2_FOUND TRUE) 118 | 119 | # Search supplied hint directories first if supplied. 120 | # Find include directory for flycapture 121 | if(WIN32) 122 | #unfortunately Flycapture installs itself in an un-prepended include path in Windows 123 | find_path(FlyCapture2_INCLUDE_DIR 124 | NAMES FlyCapture2.h 125 | PATHS ${FlyCapture2_INCLUDE_DIR_HINTS} 126 | ${FlyCapture2_CHECK_INCLUDE_DIRS} 127 | ) 128 | else() 129 | find_path(FlyCapture2_INCLUDE_DIR 130 | NAMES flycapture/FlyCapture2.h 131 | PATHS ${FlyCapture2_INCLUDE_DIR_HINTS} 132 | ${FlyCapture2_CHECK_INCLUDE_DIRS} 133 | ) 134 | endif() 135 | if(NOT FlyCapture2_INCLUDE_DIR OR NOT EXISTS ${FlyCapture2_INCLUDE_DIR}) 136 | FlyCapture2_REPORT_NOT_FOUND( 137 | "Could not find flycapture include directory, set FlyCapture2_INCLUDE_DIR to " 138 | "path to flycapture include directory," 139 | "e.g. /opt/flycapture.") 140 | else() 141 | message(STATUS "flycapture include dir found: " ${FlyCapture2_INCLUDE_DIR}) 142 | endif() 143 | 144 | if(NOT WIN32) 145 | # Find library directory for flycapture 146 | find_library(FlyCapture2_LIBRARY 147 | NAMES libflycapture.so 148 | PATHS ${FlyCapture2_LIBRARY_DIR_HINTS} 149 | ${FlyCapture2_CHECK_LIBRARY_DIRS} 150 | ) 151 | else() 152 | 153 | if(MSVC) 154 | if(MSVC_VERSION LESS "1400") 155 | FlyCapture2_REPORT_NOT_FOUND( 156 | "FlyCapture2 SDK not available for VC earlier than VC9" 157 | "Please use an SDK newer than Visual Studio 2005") 158 | elseif(MSVC_VERSION LESS "1500") 159 | set(vTail "") 160 | elseif(MSVC_VERSION LESS "1600") 161 | set(vTail "_v90") 162 | elseif(MSVC_VERSION LESS "1700") 163 | set(vTail "_v100") 164 | elseif(MSVC_VERSION LESS "1800") 165 | set(vTail "_v110") 166 | elseif(MSVC_VERSION LESS "1900") 167 | set(vTail "_v120") 168 | elseif(MSVC_VERSION LESS_EQUAL "1910") 169 | if(MSVC_VERSION EQUAL "1910") 170 | message(WARNING "I'm assuming from version numbers that VC15 will link" 171 | "But this is nothing but a guess") 172 | endif() 173 | 174 | set(vTail "_v140") 175 | endif() 176 | else() 177 | #Have the latest one and hope for the best 178 | set(vTail "_v140") 179 | endif() 180 | 181 | # message("FlyCapture2_LIBRARY_DIR_HINTS: ${FlyCapture2_LIBRARY_DIR_HINTS} ") 182 | # message("FlyCapture2_LIB_NAME: FlyCapture2${vTail}.lib") 183 | # message("FlyCapture2_DLL_NAME: FlyCapture2${vTail}.dll") 184 | # message("FlyCapture2_LIB_NAME_DEBUG: FlyCapture2d${vTail}.lib") 185 | # message("FlyCapture2_DLL_NAME_DEBUG: FlyCapture2d${vTail}.dll") 186 | 187 | find_library(FlyCapture2_LIBRARY 188 | NAMES FlyCapture2${vTail}.lib 189 | PATHS ${FlyCapture2_LIBRARY_DIR_HINTS} 190 | ${FlyCapture2_CHECK_LIBRARY_DIRS} 191 | ) 192 | find_library(FlyCapture2_LIBRARY_DEBUG 193 | NAMES FlyCapture2d${vTail}.lib 194 | PATHS ${FlyCapture2_LIBRARY_DIR_HINTS} 195 | ${FlyCapture2_CHECK_LIBRARY_DIRS} 196 | ) 197 | find_file(FlyCapture2_RUNTIME 198 | NAMES FlyCapture2${vTail}.dll 199 | PATHS ${FlyCapture2_DLL_DIR_HINTS} 200 | ${FlyCapture2_CHECK_LIBRARY_DIRS} 201 | ) 202 | find_file(FlyCapture2_RUNTIME_DEBUG 203 | NAMES FlyCapture2d${vTail}.dll 204 | PATHS ${FlyCapture2_DLL_DIR_HINTS} 205 | ${FlyCapture2_CHECK_LIBRARY_DIRS} 206 | ) 207 | # message("FlyCapture2_LIBRARY: ${FlyCapture2_LIBRARY}") 208 | # message("FlyCapture2_LIBRARY_DEBUG: ${FlyCapture2_LIBRARY_DEBUG}") 209 | # message("FlyCapture2_RUNTIME: ${FlyCapture2_RUNTIME}") 210 | # message("FlyCapture2_RUNTIME_DEBUG: ${FlyCapture2_RUNTIME_DEBUG}") 211 | 212 | endif() 213 | 214 | if(NOT FlyCapture2_LIBRARY OR NOT EXISTS ${FlyCapture2_LIBRARY}) 215 | FlyCapture2_REPORT_NOT_FOUND( 216 | "Could not find flycapture library, set FlyCapture2_LIBRARY " 217 | "to full path to flycapture library directory.") 218 | elseif(MSVC AND MSVC_VERSION LESS "1400") 219 | FlyCapture2_REPORT_NOT_FOUND( 220 | "FlyCapture2 SDK not available for VC earlier than VC9" 221 | "Please use an SDK newer than Visual Studio 2005") 222 | else() 223 | 224 | get_filename_component(FlyCapture2_LIBRARY_DIR ${FlyCapture2_LIBRARY} DIRECTORY) 225 | message(STATUS "flycapture library dir found: " ${FlyCapture2_LIBRARY_DIR}) 226 | 227 | endif() 228 | 229 | # Extract flycapture version 230 | if(FlyCapture2_LIBRARY_DIR) 231 | if(NOT WIN32) 232 | file(GLOB FlyCapture2_LIBS 233 | RELATIVE ${FlyCapture2_LIBRARY_DIR} 234 | ${FlyCapture2_LIBRARY_DIR}/libflycapture.so.[0-9].[0-9].[0-9]) 235 | # TODO: add version support 236 | # string(REGEX MATCH "" 237 | # FlyCapture2_WORLD_VERSION ${FlyCapture2_PVBASE}) 238 | # message(STATUS "flycapture world version: " ${FlyCapture2_WORLD_VERSION}) 239 | endif() 240 | endif() 241 | 242 | # Catch case when caller has set FlyCapture2_INCLUDE_DIR in the cache / GUI and 243 | # thus FIND_[PATH/LIBRARY] are not called, but specified locations are 244 | # invalid, otherwise we would report the library as found. 245 | if(WIN32) 246 | if(FlyCapture2_INCLUDE_DIR AND NOT EXISTS ${FlyCapture2_INCLUDE_DIR}/FlyCapture2.h) 247 | FlyCapture2_REPORT_NOT_FOUND("Caller defined FlyCapture2_INCLUDE_DIR: " 248 | ${FlyCapture2_INCLUDE_DIR} 249 | " does not contain FlyCapture2.h header.") 250 | endif() 251 | else() 252 | if(FlyCapture2_INCLUDE_DIR AND NOT EXISTS ${FlyCapture2_INCLUDE_DIR}/flycapture/FlyCapture2.h) 253 | FlyCapture2_REPORT_NOT_FOUND("Caller defined FlyCapture2_INCLUDE_DIR: " 254 | ${FlyCapture2_INCLUDE_DIR} 255 | " does not contain flycapture/FlyCapture2.h header.") 256 | endif() 257 | endif() 258 | 259 | # Set standard CMake FindPackage variables if found. 260 | if(FlyCapture2_FOUND) 261 | set(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR}) 262 | if(WIN32) 263 | get_filename_component(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} NAME) 264 | get_filename_component(FlyCapture2_LIBRARIES_DEBUG ${FlyCapture2_LIBRARY_DEBUG} NAME) 265 | #The use for these is "install(FILES)" so full paths are fine 266 | set(FlyCapture2_DLL ${FlyCapture2_RUNTIME}) 267 | set(FlyCapture2_DLL_DEBUG ${FlyCapture2_RUNTIME_DEBUG}) 268 | else() 269 | file(GLOB FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY}) 270 | endif() 271 | endif() 272 | 273 | # Handle REQUIRED / QUIET optional arguments. 274 | include(FindPackageHandleStandardArgs) 275 | if(FlyCapture2_FOUND) 276 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(FlyCapture2 DEFAULT_MSG 277 | FlyCapture2_INCLUDE_DIRS FlyCapture2_LIBRARIES) 278 | endif() 279 | 280 | # Only mark internal variables as advanced if we found flycapture, otherwise 281 | # leave it visible in the standard GUI for the user to set manually. 282 | if(FlyCapture2_FOUND) 283 | mark_as_advanced(FORCE FlyCapture2_INCLUDE_DIR FlyCapture2_LIBRARY) 284 | endif() 285 | -------------------------------------------------------------------------------- /cmake/flea3.cmake: -------------------------------------------------------------------------------- 1 | 2 | macro(FLEA3_ADD_EXE DIR NAME) 3 | add_executable(${PROJECT_NAME}_${NAME} src/${DIR}/${NAME}.cpp) 4 | target_link_libraries(${PROJECT_NAME}_${NAME} ${PROJECT_NAME}) 5 | # set_target_properties(${NAME} PROPERTIES OUTPUT_NAME ${NAME} PREFIX ${PROJECT_NAME}) 6 | endmacro() 7 | -------------------------------------------------------------------------------- /flycapture/include/flycapture/AVIRecorder.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright © 2017 FLIR Integrated Imaging Solutions, Inc. All Rights Reserved. 3 | // 4 | // This software is the confidential and proprietary information of FLIR 5 | // Integrated Imaging Solutions, Inc. ("Confidential Information"). You 6 | // shall not disclose such Confidential Information and shall use it only in 7 | // accordance with the terms of the license agreement you entered into 8 | // with FLIR Integrated Imaging Solutions, Inc. (FLIR). 9 | // 10 | // FLIR MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF THE 11 | // SOFTWARE, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE 12 | // IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 13 | // PURPOSE, OR NON-INFRINGEMENT. FLIR SHALL NOT BE LIABLE FOR ANY DAMAGES 14 | // SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR DISTRIBUTING 15 | // THIS SOFTWARE OR ITS DERIVATIVES. 16 | //============================================================================= 17 | 18 | //============================================================================= 19 | // $Id: AVIRecorder.h 316355 2017-02-20 23:02:28Z alin $ 20 | //============================================================================= 21 | 22 | #ifndef PGR_FC2_AVIRECORDER_H 23 | #define PGR_FC2_AVIRECORDER_H 24 | 25 | #include "FlyCapture2Platform.h" 26 | #include "FlyCapture2Defs.h" 27 | 28 | namespace FlyCapture2 29 | { 30 | class Error; 31 | class Image; 32 | 33 | /** 34 | * The AVIRecorder class provides the functionality for the user to record 35 | * images to an AVI file. 36 | */ 37 | class FLYCAPTURE2_API AVIRecorder 38 | { 39 | public: 40 | 41 | /** 42 | * Default constructor. 43 | */ 44 | AVIRecorder(); 45 | 46 | /** 47 | * Default destructor. 48 | */ 49 | virtual ~AVIRecorder(); 50 | 51 | /** 52 | * Open an AVI file in preparation for writing Images to disk. 53 | * The size of AVI files is limited to 2GB. The filenames are 54 | * automatically generated using the filename specified. 55 | * 56 | * @param pFileName The filename of the AVI file. 57 | * @param pOption Options to apply to the AVI file. 58 | * 59 | * @see AVIClose() 60 | * 61 | * @return An Error indicating the success or failure of the function. 62 | */ 63 | virtual Error AVIOpen( 64 | const char* pFileName, 65 | AVIOption* pOption ); 66 | 67 | /** 68 | * Open an MJPEG AVI file in preparation for writing Images to disk. 69 | * The size of AVI files is limited to 2GB. The filenames are 70 | * automatically generated using the filename specified. 71 | * 72 | * @param pFileName The filename of the AVI file. 73 | * @param pOption MJPEG options to apply to the AVI file. 74 | * 75 | * @see AVIClose() 76 | * @see MJPGOption 77 | * 78 | * @return An Error indicating the success or failure of the function. 79 | */ 80 | virtual Error AVIOpen( 81 | const char* pFileName, 82 | MJPGOption* pOption ); 83 | 84 | 85 | /** 86 | * Open an H264 MP4 file in preparation for writing Images to disk. 87 | * The size of MP4 files is limited to 2GB. The filenames are 88 | * automatically generated using the filename specified. 89 | * 90 | * @param pFileName The filename of the MP4 file. 91 | * @param pOption H264 options to apply to the MP4 file. 92 | * 93 | * @see AVIClose() 94 | * @see H264Option 95 | * 96 | * @return An Error indicating the success or failure of the function. 97 | */ 98 | virtual Error AVIOpen( 99 | const char* pFileName, 100 | H264Option* pOption ); 101 | 102 | 103 | /** 104 | * Append an image to the AVI/MP4 file. 105 | * 106 | * @param pImage The image to append. 107 | * 108 | * @return An Error indicating the success or failure of the function. 109 | */ 110 | virtual Error AVIAppend( Image* pImage); 111 | 112 | /** 113 | * Close the AVI/MP4 file. 114 | * 115 | * @see AVIOpen() 116 | * 117 | * @return An Error indicating the success or failure of the function. 118 | */ 119 | virtual Error AVIClose( ); 120 | 121 | private: 122 | 123 | AVIRecorder( const AVIRecorder& ); 124 | AVIRecorder& operator=( const AVIRecorder& ); 125 | 126 | struct AVIRecorderData; // Forward declaration 127 | 128 | AVIRecorderData* m_pAVIRecorderData; 129 | }; 130 | } 131 | 132 | #endif //PGR_FC2_AVIRECORDER_H 133 | -------------------------------------------------------------------------------- /flycapture/include/flycapture/BusManager.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright © 2017 FLIR Integrated Imaging Solutions, Inc. All Rights Reserved. 3 | // 4 | // This software is the confidential and proprietary information of FLIR 5 | // Integrated Imaging Solutions, Inc. ("Confidential Information"). You 6 | // shall not disclose such Confidential Information and shall use it only in 7 | // accordance with the terms of the license agreement you entered into 8 | // with FLIR Integrated Imaging Solutions, Inc. (FLIR). 9 | // 10 | // FLIR MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF THE 11 | // SOFTWARE, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE 12 | // IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 13 | // PURPOSE, OR NON-INFRINGEMENT. FLIR SHALL NOT BE LIABLE FOR ANY DAMAGES 14 | // SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR DISTRIBUTING 15 | // THIS SOFTWARE OR ITS DERIVATIVES. 16 | //============================================================================= 17 | 18 | //============================================================================= 19 | // $Id: BusManager.h 317385 2017-02-28 18:28:29Z corinal $ 20 | //============================================================================= 21 | 22 | #ifndef PGR_FC2_BUSMANAGER_H 23 | #define PGR_FC2_BUSMANAGER_H 24 | 25 | #include "FlyCapture2Platform.h" 26 | #include "FlyCapture2Defs.h" 27 | 28 | namespace FlyCapture2 29 | { 30 | class Error; 31 | class TopologyNode; 32 | 33 | /** 34 | * Bus event callback function prototype. Defines the syntax of the 35 | * callback function that is passed into RegisterCallback() and 36 | * UnregisterCallback(). It is recommended that minimal handling be 37 | * performed in this callback as it will block internal processing 38 | * of bus events until it returns. 39 | */ 40 | typedef void (*BusEventCallback)( void* pParameter, unsigned int serialNumber ); 41 | 42 | /** 43 | * Handle that is returned when registering a callback. It is required 44 | * when unregistering the callback. 45 | */ 46 | typedef void* CallbackHandle; 47 | 48 | /** 49 | * The BusManager class provides the functionality for the user to get an 50 | * PGRGuid for a desired camera or device easily. Once the 51 | * camera or device token is found, it can then be used to connect to the 52 | * camera or device through the camera class or device class. In addition, 53 | * the BusManager class provides the ability to be notified when a camera 54 | * or device is added or removed or some event occurs on the PC. 55 | */ 56 | class FLYCAPTURE2_API BusManager 57 | { 58 | public: 59 | 60 | /** 61 | * Default constructor. 62 | */ 63 | BusManager(); 64 | 65 | /** 66 | * Default destructor. 67 | */ 68 | virtual ~BusManager(); 69 | 70 | /** 71 | * Fire a bus reset. The actual bus reset is only fired for the 72 | * specified 1394 bus, but it will effectively cause a global bus 73 | * reset for the library. 74 | * 75 | * @param pGuid PGRGuid of the camera or the device to cause bus reset. 76 | * 77 | * @return An Error indicating the success or failure of the function. 78 | */ 79 | virtual Error FireBusReset( PGRGuid* pGuid); 80 | 81 | /** 82 | * Gets the number of cameras attached to the PC. 83 | * 84 | * @param pNumCameras The number of cameras attached. 85 | * 86 | * @return An Error indicating the success or failure of the function. 87 | */ 88 | virtual Error GetNumOfCameras( unsigned int* pNumCameras ); 89 | 90 | /** 91 | * Gets the PGRGuid for a camera with the specified IPv4 address. 92 | * 93 | * @param ipAddress IP address to get GUID for. 94 | * @param pGuid Unique PGRGuid for the camera. 95 | * 96 | * @return An Error indicating the success or failure of the function. 97 | */ 98 | virtual Error GetCameraFromIPAddress( 99 | IPAddress ipAddress, 100 | PGRGuid* pGuid ); 101 | 102 | /** 103 | * Gets the PGRGuid for a camera on the PC. It uniquely identifies 104 | * the camera specified by the index and is used to identify the camera 105 | * during a Camera::Connect() call. 106 | * 107 | * @param index Zero based index of camera. 108 | * @param pGuid Unique PGRGuid for the camera. 109 | * 110 | * @see GetCameraFromSerialNumber() 111 | * 112 | * @return An Error indicating the success or failure of the function. 113 | */ 114 | virtual Error GetCameraFromIndex( 115 | unsigned int index, 116 | PGRGuid* pGuid ); 117 | 118 | /** 119 | * Gets the PGRGuid for a camera on the PC. It uniquely identifies 120 | * the camera specified by the serial number and is used to identify the camera 121 | * during a Camera::Connect() call. 122 | * 123 | * @param serialNumber Serial number of camera. 124 | * @param pGuid Unique PGRGuid for the camera. 125 | * 126 | * @see GetCameraFromIndex() 127 | * 128 | * @return An Error indicating the success or failure of the function. 129 | */ 130 | virtual Error GetCameraFromSerialNumber( 131 | unsigned int serialNumber, 132 | PGRGuid* pGuid ); 133 | 134 | /** 135 | * Gets the serial number of the camera with the specified index. 136 | * 137 | * @param index Zero based index of desired camera. 138 | * @param pSerialNumber Serial number of camera. 139 | * 140 | * @return An Error indicating the success or failure of the function. 141 | */ 142 | virtual Error GetCameraSerialNumberFromIndex( 143 | unsigned int index, 144 | unsigned int* pSerialNumber ); 145 | 146 | /** 147 | * Gets the interface type associated with a PGRGuid. This is useful 148 | * in situations where there is a need to enumerate all cameras 149 | * for a particular interface. 150 | * 151 | * @param pGuid The PGRGuid to get the interface for. 152 | * @param pInterfaceType The interface type of the PGRGuid. 153 | * 154 | * @return An Error indicating the success or failure of the function. 155 | */ 156 | virtual Error GetInterfaceTypeFromGuid( 157 | PGRGuid *pGuid, 158 | InterfaceType* pInterfaceType ); 159 | 160 | /** 161 | * Gets the number of devices. This may include hubs, host controllers 162 | * and other hardware devices (including cameras). 163 | * 164 | * @param pNumDevices The number of devices found. 165 | * 166 | * @return An Error indicating the success or failure of the function. 167 | */ 168 | virtual Error GetNumOfDevices( unsigned int* pNumDevices ); 169 | 170 | /** 171 | * Gets the PGRGuid for a device. It uniquely identifies the device 172 | * specified by the index. 173 | * 174 | * @param index Zero based index of device. 175 | * @param pGuid Unique PGRGuid for the device. 176 | * 177 | * @see GetNumOfDevices() 178 | * 179 | * @return An Error indicating the success or failure of the function. 180 | */ 181 | virtual Error GetDeviceFromIndex( 182 | unsigned int index, 183 | PGRGuid* pGuid ); 184 | 185 | /** 186 | * Read a phy register on the specified device. The full address 187 | * to be read from is determined by the page, port and address. 188 | * 189 | * @param guid PGRGuid of the device to read from. 190 | * @param page Page to read from. 191 | * @param port Port to read from. 192 | * @param address Address to read from. 193 | * @param pValue Value read from the phy register. 194 | * 195 | * @return An Error indicating the success or failure of the function. 196 | */ 197 | virtual Error ReadPhyRegister( 198 | PGRGuid guid, 199 | unsigned int page, 200 | unsigned int port, 201 | unsigned int address, 202 | unsigned int* pValue ); 203 | 204 | /** 205 | * Write a phy register on the specified device. The full address 206 | * to be written to is determined by the page, port and address. 207 | * 208 | * @param guid PGRGuid of the device to write to. 209 | * @param page Page to write to. 210 | * @param port Port to write to. 211 | * @param address Address to write to. 212 | * @param value Value to write to phy register. 213 | * 214 | * @return An Error indicating the success or failure of the function. 215 | */ 216 | virtual Error WritePhyRegister( 217 | PGRGuid guid, 218 | unsigned int page, 219 | unsigned int port, 220 | unsigned int address, 221 | unsigned int value ); 222 | 223 | /** 224 | * Read usb link info for the port that the specified device is connected to. 225 | * 226 | * @param guid PGRGuid of the device to read from. 227 | * @param pValue Value read from the card register. 228 | * 229 | * @return An Error indicating the success or failure of the function. 230 | */ 231 | virtual Error GetUsbLinkInfo( 232 | PGRGuid guid, 233 | unsigned int* pValue ); 234 | 235 | /** 236 | * Read usb port status for the port that the specified device is connected to. 237 | * 238 | * @param guid PGRGuid of the device to read from. 239 | * @param pValue Value read from the card register. 240 | * 241 | * @return An Error indicating the success or failure of the function. 242 | */ 243 | virtual Error GetUsbPortStatus( 244 | PGRGuid guid, 245 | unsigned int* pValue ); 246 | 247 | /** 248 | * Gets the topology information for the PC. 249 | * 250 | * @param pNode TopologyNode object that will contain the topology 251 | * information. 252 | * 253 | * @return An Error indicating the success or failure of the function. 254 | */ 255 | virtual Error GetTopology( 256 | TopologyNode* pNode ); 257 | 258 | /** 259 | * Register a callback function that will be called when the 260 | * specified callback event occurs. 261 | * 262 | * @param busEventCallback Pointer to function that will receive 263 | the callback. 264 | * @param callbackType Type of callback to register for. 265 | * @param pParameter Callback parameter to be passed to callback. 266 | * @param pCallbackHandle Unique callback handle used for 267 | * unregistering callback. 268 | * 269 | * @see UnregisterCallback() 270 | * 271 | * @return An Error indicating the success or failure of the function. 272 | */ 273 | virtual Error RegisterCallback( 274 | BusEventCallback busEventCallback, 275 | BusCallbackType callbackType, 276 | void* pParameter, 277 | CallbackHandle* pCallbackHandle ); 278 | 279 | /** 280 | * Unregister a callback function. 281 | * 282 | * @param callbackHandle Unique callback handle. 283 | * 284 | * @see RegisterCallback() 285 | * 286 | * @return An Error indicating the success or failure of the function. 287 | */ 288 | virtual Error UnregisterCallback( CallbackHandle callbackHandle ); 289 | 290 | /** 291 | * Force a rescan of the buses. This does not trigger a bus reset. 292 | * However, any current connections to a Camera object will be 293 | * invalidated. 294 | * 295 | * @return An Error indicating the success or failure of the function. 296 | */ 297 | virtual Error RescanBus(); 298 | 299 | /** 300 | * Force the camera with the specific MAC address to the specified 301 | * IP address, subnet mask and default gateway. This is useful in 302 | * situations where GigE Vision cameras are using IP addresses 303 | * in a subnet different from the host's subnet. 304 | * 305 | * @param macAddress MAC address of the camera. 306 | * @param ipAddress IP address to set on the camera. 307 | * @param subnetMask Subnet mask to set on the camera. 308 | * @param defaultGateway Default gateway to set on the camera. 309 | * 310 | * @return An Error indicating the success or failure of the function. 311 | */ 312 | static Error ForceIPAddressToCamera( 313 | MACAddress macAddress, 314 | IPAddress ipAddress, 315 | IPAddress subnetMask, 316 | IPAddress defaultGateway ); 317 | 318 | /** 319 | * Force all cameras on the network to be assigned sequential IP addresses 320 | * on the same subnet as the netowrk adapters that they are connected to. 321 | * This is useful in situations where GigE Vision cameras are using 322 | * IP addresses in a subnet different from the host's subnet. 323 | * 324 | * @return An Error indicating the success or failure of the function. 325 | */ 326 | static Error ForceAllIPAddressesAutomatically(); 327 | 328 | /** 329 | * Force a camera on the network to be assigned an IP address 330 | * on the same subnet as the netowrk adapters that it is connected to. 331 | * This is useful in situations where GigE Vision cameras are using IP 332 | * addresses in a subnet different from the host's subnet. 333 | * 334 | * @return An Error indicating the success or failure of the function. 335 | */ 336 | static Error ForceAllIPAddressesAutomatically(unsigned int serialNumber); 337 | 338 | /** 339 | * Discover all cameras connected to the network even if they reside 340 | * on a different subnet. This is useful in situations where GigE Vision 341 | * cameras are using IP addresses in a subnet different from the host's 342 | * subnet. After discovering the camera, it is easy to use 343 | * ForceIPAddressToCamera() to set a different IP configuration. 344 | * 345 | * @param gigECameras Pointer to an array of CameraInfo structures. 346 | * @param arraySize Size of the array. Number of discovered cameras 347 | * is returned in the same value. 348 | * 349 | * @return An Error indicating the success or failure of the function. 350 | * If the error is PGRERROR_BUFFER_TOO_SMALL then arraySize will 351 | * contain the minimum size needed for gigECameras array. 352 | */ 353 | static Error DiscoverGigECameras( 354 | CameraInfo* gigECameras, 355 | unsigned int* arraySize ); 356 | 357 | /** 358 | * Query CCP status on camera with corresponding PGRGuid. This is 359 | * useful to determine if a GigE camera can be controlled. 360 | * 361 | * @param pGuid PGRGuid of the camera 362 | * @param pControlable Indicates whether camera is controllable 363 | * 364 | * @return An Error indicating the success or failure of the function. 365 | */ 366 | Error IsCameraControlable( 367 | PGRGuid* pGuid, bool* pControlable ); 368 | 369 | private: 370 | 371 | BusManager( const BusManager& ); 372 | BusManager& operator=( const BusManager& ); 373 | 374 | struct BusManagerData; // Forward declaration 375 | 376 | BusManagerData* m_pBusManagerData; 377 | }; 378 | } 379 | 380 | #endif //PGR_FC2_BUSMANAGER_H 381 | -------------------------------------------------------------------------------- /flycapture/include/flycapture/Camera.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/Camera.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/CameraBase.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/CameraBase.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/Error.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/Error.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/FlyCapture2.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/FlyCapture2.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/FlyCapture2Defs.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/FlyCapture2Defs.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/FlyCapture2Platform.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright © 2017 FLIR Integrated Imaging Solutions, Inc. All Rights Reserved. 3 | // 4 | // This software is the confidential and proprietary information of FLIR 5 | // Integrated Imaging Solutions, Inc. ("Confidential Information"). You 6 | // shall not disclose such Confidential Information and shall use it only in 7 | // accordance with the terms of the license agreement you entered into 8 | // with FLIR Integrated Imaging Solutions, Inc. (FLIR). 9 | // 10 | // FLIR MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF THE 11 | // SOFTWARE, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE 12 | // IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 13 | // PURPOSE, OR NON-INFRINGEMENT. FLIR SHALL NOT BE LIABLE FOR ANY DAMAGES 14 | // SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR DISTRIBUTING 15 | // THIS SOFTWARE OR ITS DERIVATIVES. 16 | //============================================================================= 17 | 18 | //============================================================================= 19 | // $Id: FlyCapture2Platform.h 316355 2017-02-20 23:02:28Z alin $ 20 | //============================================================================= 21 | 22 | #ifndef PGR_FC2_FLYCAPTURE2PLATFORM_H 23 | #define PGR_FC2_FLYCAPTURE2PLATFORM_H 24 | 25 | //============================================================================= 26 | // Platform-specific header file for FlyCapture2. 27 | // 28 | // All the platform-specific code that is required by individual compilers 29 | // to produce the appropriate code for each platform. 30 | //============================================================================= 31 | 32 | #if defined(_WIN32) || defined(_WIN64) 33 | 34 | // Windows 32-bit and 64-bit 35 | #ifdef FLYCAPTURE2_EXPORT 36 | #define FLYCAPTURE2_API __declspec( dllexport ) 37 | #elif defined(FLYCAPTURE2_STATIC) 38 | #define FLYCAPTURE2_API 39 | #else 40 | #define FLYCAPTURE2_API __declspec( dllimport ) 41 | #endif 42 | 43 | #if _MSC_VER > 1000 44 | #pragma once 45 | #endif 46 | 47 | // Provide a common naming scheme for fixed-width integer types 48 | #ifdef _MSC_VER 49 | #if _MSC_VER >= 1600 50 | #include 51 | #else 52 | //typedef __int8 int8_t; 53 | typedef __int16 int16_t; 54 | typedef __int32 int32_t; 55 | typedef __int64 int64_t; 56 | //typedef unsigned __int8 uint8_t; 57 | typedef unsigned __int16 uint16_t; 58 | typedef unsigned __int32 uint32_t; 59 | typedef unsigned __int64 uint64_t; 60 | #endif 61 | #elif __GNUC__ >=3 62 | #include 63 | #endif 64 | 65 | #elif defined(MAC_OSX) 66 | 67 | // Mac OSX 68 | 69 | #else 70 | // Linux and all others 71 | 72 | // Using GCC 4 where hiding attributes is possible 73 | #define FLYCAPTURE2_API __attribute__ ((visibility ("default"))) 74 | #define FLYCAPTURE2_LOCAL __attribute__ ((visibility ("hidden"))) 75 | 76 | #endif 77 | 78 | #endif // PGR_FC2_FLYCAPTURE2PLATFORM_H 79 | 80 | -------------------------------------------------------------------------------- /flycapture/include/flycapture/GigECamera.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/GigECamera.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/Image.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/Image.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/ImageStatistics.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/ImageStatistics.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/TopologyNode.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/TopologyNode.h -------------------------------------------------------------------------------- /flycapture/include/flycapture/Utilities.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/include/flycapture/Utilities.h -------------------------------------------------------------------------------- /flycapture/lib/libflycapture.so: -------------------------------------------------------------------------------- 1 | libflycapture.so.2 -------------------------------------------------------------------------------- /flycapture/lib/libflycapture.so.2: -------------------------------------------------------------------------------- 1 | libflycapture.so.2.11.3.121 -------------------------------------------------------------------------------- /flycapture/lib/libflycapture.so.2.11.3.121: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/flycapture/lib/libflycapture.so.2.11.3.121 -------------------------------------------------------------------------------- /include/flea3/flea3_camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "flea3/Flea3DynConfig.h" 8 | //#include "flea3/ImageMetadata.h" 9 | 10 | namespace flea3 { 11 | using namespace FlyCapture2; 12 | 13 | class Flea3Camera { 14 | public: 15 | using Config = ::flea3::Flea3DynConfig; 16 | 17 | explicit Flea3Camera(const std::string& serial); 18 | ~Flea3Camera(); 19 | 20 | const std::string& serial() const { return serial_; } 21 | unsigned serial_id() const { return std::atoi(serial_.c_str()); } 22 | 23 | bool GrabImage(sensor_msgs::Image& image_msg); 24 | bool GrabImageNonBlocking(sensor_msgs::Image& image_msg); 25 | bool GrabImageWithTimestamp(sensor_msgs::Image& image_msg, double* ts); 26 | bool GrabImageNonBlockingWithTimestamp(sensor_msgs::Image& image_msg, 27 | double* ts); 28 | // void GrabImageMetadata(flea3::ImageMetadata& image_metadata_msg); 29 | 30 | bool Connect(); 31 | void Configure(Config& config); 32 | void StartCapture(); 33 | void StopCapture(); 34 | bool RequestSingle(); 35 | double GetShutterTimeSec(); 36 | double GetGain(); 37 | void SetShutter(bool& auto_shutter, double& shutter_ms); 38 | void SetGain(bool& auto_gain, double& gain_db); 39 | bool FireSoftwareTrigger(); 40 | 41 | private: 42 | std::string AvailableDevice(); 43 | void enableTimestamps(); 44 | 45 | // Start up 46 | void EnableAutoWhiteBalance(); 47 | void SetConfiguration(); 48 | 49 | // Video Mode 50 | void SetVideoMode(int& video_mode, int& format7_mode, int& pixel_format, 51 | int& width, int& height); 52 | void SetFormat7VideoMode(int format7_mode, int pixel_format, int width, 53 | int height); 54 | void SetStandardVideoMode(int video_mode); 55 | void SetRoi(const Format7Info& format7_info, 56 | Format7ImageSettings& format7_settings, int width, int height); 57 | 58 | // Frame Rate 59 | void SetFrameRate(double& frame_rate); 60 | 61 | // White Balance 62 | void SetWhiteBalanceRedBlue(bool& white_balance, bool& auto_white_balance, 63 | int& red, int& blue); 64 | 65 | // Raw Bayer 66 | void SetRawBayerOutput(bool& raw_bayer_output); 67 | 68 | void SetExposure(bool& exposure, bool& auto_exposure, double& exposure_value); 69 | void SetBrightness(double& brightness); 70 | void SetGamma(double& gamma); 71 | 72 | // Trigger 73 | void SetTrigger(int& trigger_source, int& trigger_polarity, 74 | int& trigger_mode); 75 | bool PollForTriggerReady(); 76 | 77 | // Strobe 78 | void SetStrobe(int& strobe_control, int& polarity); 79 | void TurnOffStrobe(const std::vector& strobes); 80 | 81 | // Blocking/Non-blocking 82 | void setNonBlocking(); 83 | void setBlocking(); 84 | 85 | bool capturing_{false}; 86 | BusManager bus_manager_; 87 | Camera camera_; 88 | CameraInfo camera_info_; 89 | std::string serial_; 90 | Config config_; 91 | }; 92 | 93 | } // namespace flea3 94 | -------------------------------------------------------------------------------- /include/flea3/flea3_ros.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "flea3/flea3_camera.h" 6 | 7 | namespace flea3 { 8 | 9 | class Flea3Ros : public camera_base::CameraRosBase { 10 | public: 11 | explicit Flea3Ros(const ros::NodeHandle& pnh, 12 | const std::string& prefix = std::string()); 13 | 14 | Flea3Camera& camera(); 15 | 16 | bool RequestSingle(); 17 | 18 | bool Grab(const sensor_msgs::ImagePtr& image_msg, 19 | const sensor_msgs::CameraInfoPtr& cinfo_msg = nullptr) override; 20 | bool GrabNonBlocking(const sensor_msgs::ImagePtr& image_msg, 21 | const sensor_msgs::CameraInfoPtr& cinfo_msg = nullptr) { 22 | return flea3_.GrabImageNonBlocking(*image_msg); 23 | } 24 | bool GrabWithTimestamp(const sensor_msgs::ImagePtr& image_msg, 25 | double* timestamp) { 26 | return flea3_.GrabImageWithTimestamp(*image_msg, timestamp); 27 | } 28 | bool GrabNonBlockingWithTimestamp(const sensor_msgs::ImagePtr& image_msg, 29 | double* timestamp) { 30 | return flea3_.GrabImageNonBlockingWithTimestamp(*image_msg, timestamp); 31 | } 32 | void PublishImageMetadata(const ros::Time& time); 33 | 34 | void Stop(); 35 | void Start(); 36 | 37 | private: 38 | Flea3Camera flea3_; 39 | ros::NodeHandle pnh_; 40 | // ros::Publisher image_metadata_pub_; 41 | }; 42 | 43 | } // namespace flea3 44 | -------------------------------------------------------------------------------- /include/flea3/flea3_setting.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace flea3 { 8 | 9 | using namespace FlyCapture2; 10 | 11 | void PgrError(const Error& error, const std::string& message = ""); 12 | bool PgrWarn(const Error& error, const std::string& message = ""); 13 | 14 | void PrintPropertyInfo(const PropertyInfo& prop_info, 15 | const std::string& prop_name); 16 | void PrintProperty(const Property& prop, const std::string& prop_name); 17 | 18 | std::string BayerFormatToEncoding(const BayerTileFormat& bayer_format, 19 | unsigned bits_per_pixel); 20 | std::string PixelFormatToEncoding(unsigned bits_per_pixel); 21 | 22 | PropertyInfo GetPropertyInfo(Camera& camera, const PropertyType& prop_type); 23 | Property GetProperty(Camera& camera, const PropertyType& prop_type); 24 | std::pair GetFormat7Info(Camera& camera, const Mode& mode); 25 | CameraInfo GetCameraInfo(Camera& camera); 26 | float GetCameraFrameRate(Camera& camera); 27 | float GetCameraTemperature(Camera& camera); 28 | FrameRate GetMaxFrameRate(Camera& camera, const VideoMode& video_mode); 29 | std::pair GetVideoModeAndFrameRate(Camera& camera); 30 | Format7ImageSettings GetFormat7ImageSettings(Camera& camera); 31 | 32 | void SetProperty(Camera& camera, const PropertyType& prop_type, bool on, 33 | bool auto_on, double value); 34 | 35 | unsigned ReadRegister(Camera& camera, unsigned address); 36 | void WriteRegister(Camera& camera, unsigned address, unsigned value); 37 | 38 | bool IsAutoWhiteBalanceSupported(Camera& camera); 39 | bool IsFormat7Supported(Camera& camera); 40 | std::pair IsFormat7SettingsValid( 41 | Camera& camera, Format7ImageSettings& fmt7_settings); 42 | bool IsVideoModeSupported(Camera& camera, const VideoMode& video_mode); 43 | bool IsVideoModeAndFrameRateSupported(Camera& camera, 44 | const VideoMode& video_mode, 45 | const FrameRate& frame_rate); 46 | 47 | std::pair CenterRoi(int size, int max_size, int step); 48 | 49 | } // namespace flea3 50 | -------------------------------------------------------------------------------- /include/flea3/single_node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "flea3/Flea3DynConfig.h" 6 | #include "flea3/flea3_ros.h" 7 | 8 | namespace flea3 { 9 | 10 | using Config = ::flea3::Flea3DynConfig; 11 | 12 | class SingleNode : public camera_base::CameraNodeBase { 13 | public: 14 | explicit SingleNode(const ros::NodeHandle &pnh); 15 | 16 | void Acquire() override; 17 | void Setup(Config &config) override; 18 | 19 | private: 20 | Flea3Ros flea3_ros_; 21 | }; 22 | 23 | } // namespace flea3 24 | -------------------------------------------------------------------------------- /include/flea3/stereo_node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "camera_base/camera_node_base.h" 4 | #include "flea3/Flea3DynConfig.h" 5 | #include "flea3/flea3_ros.h" 6 | 7 | namespace flea3 { 8 | 9 | class StereoNode : public camera_base::CameraNodeBase { 10 | public: 11 | explicit StereoNode(const ros::NodeHandle &pnh); 12 | 13 | virtual void Acquire() override; 14 | virtual void Setup(Flea3DynConfig &config) override; 15 | 16 | private: 17 | Flea3Ros left_ros_; 18 | Flea3Ros right_ros_; 19 | }; 20 | 21 | } // namespace flea3 22 | -------------------------------------------------------------------------------- /launch/single_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /launch/single_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/stereo_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 73 | 74 | 75 | 76 | 77 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /msg/ImageMetadata.msg: -------------------------------------------------------------------------------- 1 | # Metadata related to an image 2 | Header header 3 | float32 gain_db 4 | float32 shutter_ms 5 | float32 brightness 6 | float32 exposure_value 7 | uint32 white_balance_blue 8 | uint32 white_balance_red 9 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | This is a nodelet for a single camera 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flea3 4 | 0.1.0 5 | The flea3 package 6 | 7 | Chao Qu 8 | 9 | WTFPL 10 | catkin 11 | 12 | roscpp 13 | nodelet 14 | camera_base 15 | 16 | dynamic_reconfigure 17 | libraw1394 18 | message_generation 19 | message_runtime 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/flea3_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/flea3_camera.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "flea3/flea3_setting.h" 8 | 9 | namespace flea3 { 10 | 11 | using namespace FlyCapture2; 12 | 13 | unsigned hsb(unsigned i) { 14 | unsigned c = 0; 15 | while (i >>= 1) { 16 | c++; 17 | } 18 | return c; 19 | } 20 | 21 | union AbsValueConversion { 22 | unsigned int uint_val; 23 | float float_val; 24 | }; 25 | 26 | Flea3Camera::Flea3Camera(const std::string& serial) : serial_(serial) { 27 | // Wait for camera to power up 28 | int num_tries{3}; 29 | while (num_tries > 0) { 30 | if (Connect()) break; 31 | usleep(100000); // Sleep for 100ms 32 | --num_tries; 33 | } 34 | if (num_tries == 0) { 35 | throw std::runtime_error("Failed after multiple tries, abort."); 36 | } 37 | } 38 | 39 | Flea3Camera::~Flea3Camera() { 40 | if (camera_.IsConnected()) { 41 | TurnOffStrobe({1, 2, 3}); 42 | PgrError(camera_.Disconnect(), "Failed to disconnect camera"); 43 | } 44 | } 45 | 46 | bool Flea3Camera::Connect() { 47 | PGRGuid guid; 48 | PgrError(bus_manager_.GetCameraFromSerialNumber(serial_id(), &guid), 49 | serial_ + " not found. " + AvailableDevice()); 50 | 51 | const auto error = camera_.Connect(&guid); 52 | if (error == PGRERROR_OK) { 53 | // This is a total hack, it exists because one of my camera doesn't enable 54 | // auto white balance by default. You have to write to its presence register 55 | // to bring it online. 56 | // EnableAutoWhiteBalance(); 57 | // For now this only set the grab timeout 58 | SetConfiguration(); 59 | return true; 60 | } else { 61 | ROS_INFO("Failed to connect to camera: %s. Try again. | %s", 62 | serial().c_str(), error.GetDescription()); 63 | return false; 64 | } 65 | } 66 | 67 | void Flea3Camera::SetConfiguration() { 68 | FC2Config config; 69 | PgrError(camera_.GetConfiguration(&config), "Failed to get configuration"); 70 | // Set the grab timeout to 1 seconds 71 | config.grabTimeout = 1000; 72 | // Try 2 times before declaring failure 73 | config.registerTimeoutRetries = 2; 74 | // NOTE: Cannot do this here, will block all the following settings on format7 75 | // Maybe put this in configure 76 | // config.highPerformanceRetrieveBuffer = true; 77 | 78 | enableTimestamps(); // is this the right place to enable timestamps? 79 | 80 | // Set the camera configuration 81 | PgrError(camera_.SetConfiguration(&config), "Failed to set configuration"); 82 | } 83 | 84 | std::string Flea3Camera::AvailableDevice() { 85 | unsigned num_devices = 0; 86 | PgrError(bus_manager_.GetNumOfCameras(&num_devices), 87 | "Failed to get number for cameras"); 88 | 89 | std::string devices = std::to_string(num_devices) + " available device(s): "; 90 | for (unsigned i = 0; i < num_devices; ++i) { 91 | unsigned serial_id; 92 | PgrError(bus_manager_.GetCameraSerialNumberFromIndex(i, &serial_id), 93 | "Failed to get camera serial number from index"); 94 | devices += std::to_string(serial_id) + " "; 95 | } 96 | return devices; 97 | } 98 | 99 | void Flea3Camera::StartCapture() { 100 | if (camera_.IsConnected() && !capturing_) { 101 | PgrError(camera_.StartCapture(), "Failed to start capture"); 102 | capturing_ = true; 103 | } 104 | } 105 | 106 | void Flea3Camera::StopCapture() { 107 | if (camera_.IsConnected() && capturing_) { 108 | PgrError(camera_.StopCapture(), "Failed to stop capture"); 109 | capturing_ = false; 110 | } 111 | } 112 | 113 | void Flea3Camera::Configure(Config& config) { 114 | // Video Mode 115 | SetVideoMode(config.video_mode, config.format7_mode, config.pixel_format, 116 | config.width, config.height); 117 | 118 | // Update CameraInfo here after video mode is changed 119 | camera_info_ = GetCameraInfo(camera_); 120 | 121 | // Frame Rate 122 | SetFrameRate(config.fps); 123 | 124 | // Raw Bayer 125 | SetRawBayerOutput(config.raw_bayer_output); 126 | 127 | // White Balance 128 | SetWhiteBalanceRedBlue(config.white_balance, config.auto_white_balance, 129 | config.wb_red, config.wb_blue); 130 | 131 | // Exposure 132 | SetExposure(config.exposure, config.auto_exposure, config.exposure_value); 133 | SetShutter(config.auto_shutter, config.shutter_ms); 134 | SetGain(config.auto_gain, config.gain_db); 135 | 136 | SetBrightness(config.brightness); 137 | SetGamma(config.gamma); 138 | 139 | // Strobe 140 | SetStrobe(config.strobe_control, config.strobe_polarity); 141 | // Trigger 142 | SetTrigger(config.trigger_source, config.trigger_polarity, 143 | config.trigger_mode); 144 | 145 | // Save this config 146 | config_ = config; 147 | } 148 | 149 | // TODO: simplify logic here 150 | void Flea3Camera::SetVideoMode(int& video_mode, int& format7_mode, 151 | int& pixel_format, int& width, int& height) { 152 | // Try setting video mode based on video_mode, fail silently 153 | if (video_mode == Flea3Dyn_format7) { 154 | SetFormat7VideoMode(format7_mode, pixel_format, width, height); 155 | } else { 156 | SetStandardVideoMode(video_mode); 157 | } 158 | 159 | // Update params 160 | const auto video_mode_and_frame_rate_pg = GetVideoModeAndFrameRate(camera_); 161 | if (video_mode_and_frame_rate_pg.first == VIDEOMODE_FORMAT7) { 162 | const auto fmt7_settings = GetFormat7ImageSettings(camera_); 163 | pixel_format = hsb(fmt7_settings.pixelFormat); 164 | width = fmt7_settings.width; 165 | height = fmt7_settings.height; 166 | format7_mode = fmt7_settings.mode; 167 | video_mode = Flea3Dyn_format7; 168 | } else { 169 | video_mode = static_cast(video_mode_and_frame_rate_pg.first); 170 | format7_mode = 0; 171 | pixel_format = 0; 172 | width = 0; 173 | height = 0; 174 | } 175 | } 176 | 177 | void Flea3Camera::SetFormat7VideoMode(int format7_mode, int pixel_format, 178 | int width, int height) { 179 | const auto fmt7_mode_pg = static_cast(format7_mode); 180 | const auto fmt7_info = GetFormat7Info(camera_, fmt7_mode_pg); 181 | if (!fmt7_info.second) return; 182 | 183 | Format7ImageSettings fmt7_settings; 184 | fmt7_settings.mode = fmt7_mode_pg; 185 | // Set format7 pixel format 186 | // The 22 here corresponds to PIXEL_FORMAT_RAW8 187 | pixel_format = (pixel_format == 0) ? 22 : pixel_format; 188 | fmt7_settings.pixelFormat = static_cast(1 << pixel_format); 189 | // Set format7 ROI 190 | // NOTE: Center ROI for now 191 | SetRoi(fmt7_info.first, fmt7_settings, width, height); 192 | 193 | // Validate the settings 194 | const auto fmt7_packet_info = IsFormat7SettingsValid(camera_, fmt7_settings); 195 | if (!fmt7_packet_info.second) ROS_WARN("Format 7 Setting is not valid"); 196 | PgrWarn(camera_.SetFormat7Configuration( 197 | &fmt7_settings, fmt7_packet_info.first.recommendedBytesPerPacket), 198 | "Failed to set format7 mode"); 199 | } 200 | 201 | void Flea3Camera::SetStandardVideoMode(int video_mode) { 202 | const auto video_mode_pg = static_cast(video_mode); 203 | if (!IsVideoModeSupported(camera_, video_mode_pg)) return; 204 | const auto max_frame_rate_pg = GetMaxFrameRate(camera_, video_mode_pg); 205 | PgrWarn(camera_.SetVideoModeAndFrameRate(video_mode_pg, max_frame_rate_pg)); 206 | } 207 | 208 | void Flea3Camera::SetFrameRate(double& frame_rate) { 209 | SetProperty(camera_, FRAME_RATE, true, false, frame_rate); 210 | const auto prop = GetProperty(camera_, FRAME_RATE); 211 | if (prop.onOff) { 212 | frame_rate = prop.absValue; 213 | } 214 | } 215 | 216 | void Flea3Camera::setNonBlocking() { 217 | FC2Config config; 218 | PgrError(camera_.GetConfiguration(&config), "Failed to get configuration"); 219 | config.grabTimeout = TIMEOUT_NONE; // return immediately 220 | PgrError(camera_.SetConfiguration(&config), "Failed to set configuration"); 221 | } 222 | 223 | void Flea3Camera::setBlocking() { 224 | FC2Config config; 225 | PgrError(camera_.GetConfiguration(&config), "Failed to get configuration"); 226 | config.grabTimeout = 1000; // set to one second; 227 | PgrError(camera_.SetConfiguration(&config), "Failed to set configuration"); 228 | } 229 | 230 | void Flea3Camera::enableTimestamps() { 231 | unsigned int reg_val = 0; 232 | Error error = camera_.ReadRegister(0x12F8, ®_val); 233 | if (error == PGRERROR_OK) { 234 | WriteRegister(camera_, 0x12F8, (1 << 0) | (reg_val)); 235 | } 236 | } 237 | 238 | static inline void get_encoding(const Image& image, bool isColorCam, 239 | std::string* encoding) { 240 | // Set image encodings 241 | const auto bayer_format = image.GetBayerTileFormat(); 242 | const auto bits_per_pixel = image.GetBitsPerPixel(); 243 | if (isColorCam) { 244 | if (bayer_format != NONE) { 245 | *encoding = BayerFormatToEncoding(bayer_format, bits_per_pixel); 246 | } else if (bits_per_pixel == 24) { 247 | *encoding = sensor_msgs::image_encodings::RGB8; 248 | } else { 249 | *encoding = PixelFormatToEncoding(bits_per_pixel); 250 | } 251 | } else { 252 | *encoding = PixelFormatToEncoding(bits_per_pixel); 253 | } 254 | } 255 | 256 | bool Flea3Camera::GrabImageNonBlocking(sensor_msgs::Image& image_msg) { 257 | setNonBlocking(); 258 | bool ret = GrabImage(image_msg); 259 | setBlocking(); 260 | return (ret); 261 | } 262 | 263 | bool Flea3Camera::GrabImageNonBlockingWithTimestamp( 264 | sensor_msgs::Image& image_msg, double* ts) { 265 | setNonBlocking(); 266 | bool ret = GrabImageWithTimestamp(image_msg, ts); 267 | setBlocking(); 268 | return (ret); 269 | } 270 | 271 | bool Flea3Camera::GrabImage(sensor_msgs::Image& image_msg) { 272 | if (!(camera_.IsConnected() && capturing_)) return false; 273 | 274 | Image image; 275 | const auto error = camera_.RetrieveBuffer(&image); 276 | if (error != PGRERROR_OK) return false; 277 | 278 | std::string encoding; 279 | get_encoding(image, camera_info_.isColorCamera, &encoding); 280 | return sensor_msgs::fillImage(image_msg, encoding, image.GetRows(), 281 | image.GetCols(), image.GetStride(), 282 | image.GetData()); 283 | } 284 | 285 | bool Flea3Camera::GrabImageWithTimestamp(sensor_msgs::Image& image_msg, 286 | double* tstamp) { 287 | if (!(camera_.IsConnected() && capturing_)) return false; 288 | 289 | Image image; 290 | const auto error = camera_.RetrieveBuffer(&image); 291 | if (error != PGRERROR_OK) return false; 292 | // 2) Then read it from the image structure: 293 | // 294 | // TimeStamp ts = image.GetTimeStamp(); 295 | // now look at ptgrey manuals for meaning of fields: 296 | // ts.cycleSeconds, ts.cycleCount, ts.cycleOffset 297 | TimeStamp ts = image.GetTimeStamp(); 298 | const double CYCLE_COUNT_TO_SEC = 0.000125; // 8kHz 299 | const double CYCLE_OFFSET_TO_SEC = CYCLE_COUNT_TO_SEC / 4096; // 12 bit 300 | 301 | *tstamp = ts.cycleSeconds + ts.cycleCount * CYCLE_COUNT_TO_SEC + 302 | ts.cycleOffset * CYCLE_OFFSET_TO_SEC; 303 | 304 | std::string encoding; 305 | get_encoding(image, camera_info_.isColorCamera, &encoding); 306 | return sensor_msgs::fillImage(image_msg, encoding, image.GetRows(), 307 | image.GetCols(), image.GetStride(), 308 | image.GetData()); 309 | } 310 | 311 | // void Flea3Camera::GrabImageMetadata(ImageMetadata& image_metadata_msg) { 312 | // AbsValueConversion abs_val; 313 | // // These registers can be found in register reference manual 314 | // camera_.ReadRegister(0x908, &abs_val.uint_val); 315 | // image_metadata_msg.exposure_value = abs_val.float_val; 316 | 317 | // // The value read from abs register is in seconds 318 | // camera_.ReadRegister(0x918, &abs_val.uint_val); 319 | // image_metadata_msg.shutter_ms = abs_val.float_val * 1000; 320 | 321 | // camera_.ReadRegister(0x928, &abs_val.uint_val); 322 | // image_metadata_msg.gain_db = abs_val.float_val; 323 | 324 | // camera_.ReadRegister(0x938, &abs_val.uint_val); 325 | // image_metadata_msg.brightness = abs_val.float_val; 326 | //} 327 | 328 | void Flea3Camera::SetWhiteBalanceRedBlue(bool& white_balance, 329 | bool& auto_white_balance, int& red, 330 | int& blue) { 331 | if (!camera_info_.isColorCamera) { 332 | // Not even a color camera 333 | ROS_WARN("Camera %s is not a color camera, white balance not supported", 334 | serial().c_str()); 335 | auto_white_balance = false; 336 | white_balance = false; 337 | red = 0; 338 | blue = 0; 339 | return; 340 | } 341 | 342 | // Check if white balance is supported 343 | const auto prop_info = GetPropertyInfo(camera_, WHITE_BALANCE); 344 | if (!prop_info.present) { 345 | white_balance = false; 346 | auto_white_balance = false; 347 | blue = 0; 348 | red = 0; 349 | return; 350 | } 351 | 352 | // Set white balance 353 | Error error; 354 | Property prop; 355 | prop.type = WHITE_BALANCE; 356 | prop.onOff = white_balance; 357 | prop.autoManualMode = auto_white_balance; 358 | prop.absControl = false; 359 | prop.valueA = red; 360 | prop.valueB = blue; 361 | error = camera_.SetProperty(&prop); 362 | 363 | // Get white balance 364 | camera_.GetProperty(&prop); 365 | white_balance = prop.onOff; 366 | auto_white_balance = prop.autoManualMode; 367 | red = prop.valueA; 368 | blue = prop.valueB; 369 | } 370 | 371 | void Flea3Camera::EnableAutoWhiteBalance() { 372 | WriteRegister(camera_, 0x80C, 1 << 31); 373 | } 374 | 375 | void Flea3Camera::SetExposure(bool& exposure, bool& auto_exposure, 376 | double& exposure_value) { 377 | const auto prop_type = AUTO_EXPOSURE; 378 | SetProperty(camera_, prop_type, exposure, auto_exposure, exposure_value); 379 | const auto prop = GetProperty(camera_, prop_type); 380 | exposure = prop.onOff; 381 | auto_exposure = prop.autoManualMode; 382 | exposure_value = prop.absValue; 383 | } 384 | 385 | void Flea3Camera::SetShutter(bool& auto_shutter, double& shutter_ms) { 386 | const auto prop_type = SHUTTER; 387 | SetProperty(camera_, prop_type, true, auto_shutter, shutter_ms); 388 | const auto prop = GetProperty(camera_, prop_type); 389 | auto_shutter = prop.autoManualMode; 390 | shutter_ms = prop.absValue; 391 | } 392 | 393 | void Flea3Camera::SetGain(bool& auto_gain, double& gain_db) { 394 | const auto prop_type = GAIN; 395 | SetProperty(camera_, prop_type, true, auto_gain, gain_db); 396 | const auto prop = GetProperty(camera_, prop_type); 397 | auto_gain = prop.autoManualMode; 398 | gain_db = prop.absValue; 399 | } 400 | 401 | void Flea3Camera::SetBrightness(double& brightness) { 402 | const auto prop_type = BRIGHTNESS; 403 | SetProperty(camera_, prop_type, true, false, brightness); 404 | const auto prop = GetProperty(camera_, prop_type); 405 | brightness = prop.absValue; 406 | } 407 | 408 | void Flea3Camera::SetGamma(double& gamma) { 409 | const auto prop_type = GAMMA; 410 | SetProperty(camera_, prop_type, true, false, gamma); 411 | const auto prop = GetProperty(camera_, prop_type); 412 | gamma = prop.absValue; 413 | } 414 | 415 | void Flea3Camera::SetRawBayerOutput(bool& raw_bayer_output) { 416 | // Because this only works in standard video mode, we only enable this if 417 | // video mode is not format 7 418 | const auto video_mode_frame_rate_pg = GetVideoModeAndFrameRate(camera_); 419 | if (video_mode_frame_rate_pg.first == VIDEOMODE_FORMAT7) { 420 | raw_bayer_output = false; 421 | return; 422 | } 423 | // See Point Grey Register Reference document section 5.8 424 | WriteRegister(camera_, 0x1050, static_cast(raw_bayer_output)); 425 | } 426 | 427 | void Flea3Camera::SetRoi(const Format7Info& format7_info, 428 | Format7ImageSettings& format7_settings, int width, 429 | int height) { 430 | const auto width_setting = 431 | CenterRoi(width, format7_info.maxWidth, format7_info.imageHStepSize); 432 | const auto height_setting = 433 | CenterRoi(height, format7_info.maxHeight, format7_info.imageVStepSize); 434 | format7_settings.width = width_setting.first; 435 | format7_settings.offsetX = width_setting.second; 436 | format7_settings.height = height_setting.first; 437 | format7_settings.offsetY = height_setting.second; 438 | } 439 | 440 | void Flea3Camera::SetTrigger(int& trigger_source, int& trigger_polarity, 441 | int& trigger_mode) { 442 | TriggerModeInfo trigger_mode_info; 443 | PgrWarn(camera_.GetTriggerModeInfo(&trigger_mode_info), 444 | "Failed to get trigger mode info"); 445 | // Check if trigger is supported 446 | if (!trigger_mode_info.present) { 447 | if (trigger_source == Flea3Dyn_ts_sw && 448 | !trigger_mode_info.softwareTriggerSupported) { 449 | ROS_WARN("Camera does not support software trigger"); 450 | trigger_source = Flea3Dyn_ts_off; 451 | } 452 | ROS_WARN("Camera does not support external trigger"); 453 | trigger_source = Flea3Dyn_ts_off; 454 | return; 455 | } 456 | 457 | // Turn off external trigger 458 | TriggerMode trigger_mode_struct; 459 | if (trigger_source == Flea3Dyn_ts_off) { 460 | trigger_mode_struct.onOff = false; 461 | PgrWarn(camera_.SetTriggerMode(&trigger_mode_struct), 462 | "Failed to set trigger mode"); 463 | return; 464 | } 465 | 466 | trigger_mode_struct.onOff = true; 467 | trigger_mode_struct.mode = trigger_mode; 468 | trigger_mode_struct.parameter = 0; 469 | 470 | // Source 7 means software trigger 471 | trigger_mode_struct.source = trigger_source; 472 | trigger_mode_struct.polarity = trigger_polarity; 473 | PgrWarn(camera_.SetTriggerMode(&trigger_mode_struct), 474 | "Failed to set trigger mode"); 475 | PgrWarn(camera_.GetTriggerMode(&trigger_mode_struct), 476 | "Failed to get trigger mode"); 477 | trigger_source = trigger_mode_struct.source; 478 | trigger_polarity = trigger_mode_struct.polarity; 479 | trigger_mode = trigger_mode_struct.mode; 480 | } 481 | 482 | void Flea3Camera::SetStrobe(int& strobe_control, int& polarity) { 483 | // Turn off all strobe when we switch it off 484 | if (strobe_control == Flea3Dyn_sc_off) { 485 | TurnOffStrobe({1, 2, 3}); 486 | return; 487 | } 488 | 489 | // Check if required strobe is supported 490 | StrobeInfo strobe_info; 491 | strobe_info.source = strobe_control; 492 | PgrWarn(camera_.GetStrobeInfo(&strobe_info), "Failed to get strobe info"); 493 | if (!strobe_info.present) { 494 | ROS_WARN("Camera does not support strobe"); 495 | strobe_control = Flea3Dyn_sc_off; 496 | return; 497 | } 498 | 499 | StrobeControl strobe; 500 | strobe.source = strobe_control; 501 | strobe.onOff = true; 502 | strobe.polarity = polarity; 503 | strobe.duration = 0; 504 | PgrWarn(camera_.SetStrobe(&strobe), "Failed to set strobe"); 505 | } 506 | 507 | void Flea3Camera::TurnOffStrobe(const std::vector& strobes) { 508 | for (const auto& s : strobes) { 509 | StrobeControl strobe; 510 | strobe.source = s; 511 | strobe.onOff = false; 512 | PgrWarn(camera_.SetStrobe(&strobe), "Failed to set strobe"); 513 | } 514 | } 515 | 516 | bool Flea3Camera::PollForTriggerReady() { 517 | const unsigned int software_trigger_addr = 0x62C; 518 | unsigned int reg_val = 0; 519 | 520 | Error error; 521 | do { 522 | error = camera_.ReadRegister(software_trigger_addr, ®_val); 523 | if (error != PGRERROR_OK) { 524 | return false; 525 | } 526 | } while ((reg_val >> 31) != 0); 527 | 528 | return true; 529 | } 530 | 531 | bool Flea3Camera::FireSoftwareTrigger() { 532 | const unsigned software_trigger_addr = 0x62C; 533 | const unsigned fire = 0x80000000; 534 | return camera_.WriteRegister(software_trigger_addr, fire) == PGRERROR_OK; 535 | } 536 | 537 | bool Flea3Camera::RequestSingle() { 538 | if (config_.trigger_source == Flea3Dyn_ts_sw) { 539 | if (PollForTriggerReady()) { 540 | return FireSoftwareTrigger(); 541 | } 542 | return false; 543 | } 544 | return true; 545 | } 546 | 547 | double Flea3Camera::GetShutterTimeSec() { 548 | if (config_.auto_shutter) { 549 | AbsValueConversion abs_val; 550 | // Register for abs_shutter_val, which is already in second 551 | camera_.ReadRegister(0x918, &abs_val.uint_val); 552 | return abs_val.float_val; 553 | } 554 | return config_.shutter_ms / 1000.0; 555 | } 556 | 557 | double Flea3Camera::GetGain() { 558 | if (config_.auto_gain) { 559 | AbsValueConversion abs_val; 560 | // Register for abs_val_gain, which is in db 561 | camera_.ReadRegister(0x928, &abs_val.uint_val); 562 | return abs_val.float_val; 563 | } 564 | return config_.gain_db; 565 | } 566 | 567 | } // namespace flea3 568 | -------------------------------------------------------------------------------- /src/flea3_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/flea3_ros.h" 2 | 3 | namespace flea3 { 4 | 5 | Flea3Ros::Flea3Ros(const ros::NodeHandle& pnh, const std::string& prefix) 6 | : CameraRosBase(pnh, prefix), flea3_(identifier()), pnh_(pnh) { 7 | SetHardwareId(flea3_.serial()); 8 | } 9 | 10 | Flea3Camera& Flea3Ros::camera() { return flea3_; } 11 | 12 | bool Flea3Ros::Grab(const sensor_msgs::ImagePtr& image_msg, 13 | const sensor_msgs::CameraInfoPtr& cinfo_msg) { 14 | return flea3_.GrabImage(*image_msg); 15 | } 16 | 17 | // void Flea3Ros::PublishImageMetadata(const ros::Time& time) { 18 | // auto image_metadata_msg = boost::make_shared(); 19 | // flea3_.GrabImageMetadata(*image_metadata_msg); 20 | // image_metadata_msg->header.stamp = time; 21 | // image_metadata_msg->header.frame_id = frame_id(); 22 | // image_metadata_pub_.publish(image_metadata_msg); 23 | //} 24 | 25 | void Flea3Ros::Stop() { flea3_.StopCapture(); } 26 | 27 | void Flea3Ros::Start() { flea3_.StartCapture(); } 28 | 29 | bool Flea3Ros::RequestSingle() { return flea3_.RequestSingle(); } 30 | 31 | } // namespace flea3 32 | -------------------------------------------------------------------------------- /src/flea3_setting.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/flea3_setting.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace flea3 { 9 | 10 | std::string BayerFormatToEncoding(const BayerTileFormat& bayer_format, 11 | unsigned bits_per_pixel) { 12 | using namespace sensor_msgs::image_encodings; 13 | if (bits_per_pixel == 8) { 14 | switch (bayer_format) { 15 | case RGGB: 16 | return BAYER_RGGB8; 17 | case GRBG: 18 | return BAYER_GRBG8; 19 | case GBRG: 20 | return BAYER_GBRG8; 21 | case BGGR: 22 | return BAYER_BGGR8; 23 | case NONE: 24 | return MONO8; 25 | default: 26 | return MONO8; 27 | } 28 | } else if (bits_per_pixel == 16) { 29 | switch (bayer_format) { 30 | case RGGB: 31 | return BAYER_RGGB16; 32 | case GRBG: 33 | return BAYER_GRBG16; 34 | case GBRG: 35 | return BAYER_GBRG16; 36 | case BGGR: 37 | return BAYER_BGGR16; 38 | case NONE: 39 | return MONO16; 40 | default: 41 | return MONO16; 42 | } 43 | } 44 | return MONO8; 45 | } 46 | 47 | std::string PixelFormatToEncoding(unsigned bits_per_pixel) { 48 | using namespace sensor_msgs::image_encodings; 49 | if (bits_per_pixel == 16) { 50 | return MONO16; 51 | } else if (bits_per_pixel == 8) { 52 | return MONO8; 53 | } 54 | return MONO8; 55 | } 56 | 57 | void PgrError(const Error& error, const std::string& message) { 58 | if (error == PGRERROR_TIMEOUT) { 59 | throw std::runtime_error("Failed to retrieve buffer within timeout"); 60 | } else if (error != PGRERROR_OK) { 61 | throw std::runtime_error(message + " | " + std::to_string(error.GetType()) + 62 | " " + error.GetDescription()); 63 | } 64 | } 65 | 66 | bool PgrWarn(const Error& error, const std::string& message) { 67 | if (error != PGRERROR_OK) { 68 | ROS_WARN("%s, %s", error.GetDescription(), message.c_str()); 69 | return false; 70 | } else { 71 | return true; 72 | } 73 | } 74 | 75 | void PrintPropertyInfo(const PropertyInfo& prop_info, 76 | const std::string& prop_name) { 77 | std::cout << "* Property Info: " << prop_name; 78 | 79 | if (!prop_info.present) { 80 | std::cout << " does not exist" << std::endl; 81 | return; 82 | } 83 | 84 | std::cout << std::boolalpha; 85 | std::cout << ", on/off: " << prop_info.onOffSupported 86 | << ", auto: " << prop_info.autoSupported 87 | << ", manual: " << prop_info.manualSupported 88 | << ", one push: " << prop_info.onePushSupported; 89 | 90 | if (!prop_info.readOutSupported) return; 91 | 92 | if (prop_info.absValSupported) { 93 | std::cout << ", [abs]" 94 | << " min: " << prop_info.absMin << ", max: " << prop_info.absMax; 95 | } else { 96 | std::cout << ", [int]" 97 | << " min: " << prop_info.min << ", max: " << prop_info.max; 98 | } 99 | std::cout << std::endl; 100 | } 101 | 102 | void PrintProperty(const Property& prop, const std::string& prop_name) { 103 | std::cout << "* Property: " << prop_name; 104 | 105 | if (!prop.present) { 106 | std::cout << " does not exist" << std::endl; 107 | return; 108 | } 109 | 110 | std::cout << std::boolalpha; 111 | std::cout << ", on/off: " << prop.onOff << ", auto: " << prop.autoManualMode 112 | << ", one push: " << prop.onePush << ", value A: " << prop.valueA 113 | << ", value B: " << prop.valueB << ", abs: " << prop.absValue; 114 | 115 | std::cout << std::endl; 116 | } 117 | 118 | unsigned ReadRegister(Camera& camera, unsigned address) { 119 | unsigned reg_val = 0; 120 | PgrWarn(camera.ReadRegister(address, ®_val), "Failed to read register"); 121 | return reg_val; 122 | } 123 | 124 | PropertyInfo GetPropertyInfo(Camera& camera, const PropertyType& prop_type) { 125 | PropertyInfo prop_info; 126 | prop_info.type = prop_type; 127 | PgrWarn(camera.GetPropertyInfo(&prop_info), "Failed to get property info"); 128 | return prop_info; 129 | } 130 | 131 | Property GetProperty(Camera& camera, const PropertyType& prop_type) { 132 | Property prop; 133 | prop.type = prop_type; 134 | PgrWarn(camera.GetProperty(&prop), "Failed to get property"); 135 | return prop; 136 | } 137 | 138 | std::pair GetFormat7Info(Camera& camera, const Mode& mode) { 139 | Format7Info fmt7_info; 140 | bool supported = false; 141 | fmt7_info.mode = mode; 142 | PgrWarn(camera.GetFormat7Info(&fmt7_info, &supported), 143 | "Failed to get format 7 info"); 144 | return {fmt7_info, supported}; 145 | } 146 | 147 | CameraInfo GetCameraInfo(Camera& camera) { 148 | CameraInfo camera_info; 149 | PgrWarn(camera.GetCameraInfo(&camera_info), "Failed to get camera info"); 150 | return camera_info; 151 | } 152 | 153 | float GetCameraFrameRate(Camera& camera) { 154 | const auto prop = GetProperty(camera, FRAME_RATE); 155 | return prop.absValue; 156 | } 157 | 158 | FrameRate GetMaxFrameRate(Camera& camera, const VideoMode& video_mode) { 159 | // This magic number 2 skips VideoMode format 7 160 | for (int i = NUM_FRAMERATES - 2; i >= 0; --i) { 161 | const auto frame_rate = static_cast(i); 162 | if (IsVideoModeAndFrameRateSupported(camera, video_mode, frame_rate)) { 163 | // Whatever we get here should be the supported max frame rate 164 | return frame_rate; 165 | } 166 | } 167 | return FRAMERATE_1_875; 168 | } 169 | 170 | std::pair GetVideoModeAndFrameRate(Camera& camera) { 171 | VideoMode video_mode; 172 | FrameRate frame_rate; 173 | PgrWarn(camera.GetVideoModeAndFrameRate(&video_mode, &frame_rate), 174 | "Failed to get VideoMode and FrameRate"); 175 | return {video_mode, frame_rate}; 176 | } 177 | 178 | Format7ImageSettings GetFormat7ImageSettings(Camera& camera) { 179 | Format7ImageSettings fmt7_settings; 180 | unsigned packet_size; 181 | float percentage; 182 | PgrWarn( 183 | camera.GetFormat7Configuration(&fmt7_settings, &packet_size, &percentage), 184 | "Failed to get format7 image settings"); 185 | return fmt7_settings; 186 | } 187 | 188 | float GetCameraTemperature(Camera& camera) { 189 | const auto prop = GetProperty(camera, TEMPERATURE); 190 | // It returns values of 10 * K 191 | return prop.valueA / 10.0f - 273.15f; 192 | } 193 | 194 | void SetProperty(Camera& camera, const PropertyType& prop_type, bool on, 195 | bool auto_on, double value) { 196 | const auto prop_info = GetPropertyInfo(camera, prop_type); 197 | if (prop_info.present) { 198 | Property prop; 199 | prop.type = prop_type; 200 | prop.onOff = on && prop_info.onOffSupported; 201 | prop.autoManualMode = auto_on && prop_info.autoSupported; 202 | prop.absControl = prop_info.absValSupported; 203 | value = std::max(std::min(value, prop_info.absMax), 204 | prop_info.absMin); 205 | prop.absValue = value; 206 | PgrWarn(camera.SetProperty(&prop), 207 | "Faield to set property: " + std::to_string(prop_type)); 208 | } 209 | } 210 | 211 | void WriteRegister(Camera& camera, unsigned address, unsigned value) { 212 | PgrWarn(camera.WriteRegister(address, value), "Failed to write register"); 213 | } 214 | 215 | bool IsAutoWhiteBalanceSupported(Camera& camera) { 216 | const auto pinfo = GetPropertyInfo(camera, WHITE_BALANCE); 217 | return pinfo.autoSupported; 218 | } 219 | 220 | bool IsVideoModeSupported(Camera& camera, const VideoMode& video_mode) { 221 | return IsVideoModeAndFrameRateSupported(camera, video_mode, FRAMERATE_1_875); 222 | } 223 | 224 | bool IsVideoModeAndFrameRateSupported(Camera& camera, 225 | const VideoMode& video_mode, 226 | const FrameRate& frame_rate) { 227 | bool supported = false; 228 | PgrWarn( 229 | camera.GetVideoModeAndFrameRateInfo(video_mode, frame_rate, &supported), 230 | "Failed to get video mode and frame rate info"); 231 | return supported; 232 | } 233 | 234 | bool IsFormat7Supported(Camera& camera) { 235 | const int num_modes{NUM_MODES}; 236 | for (int i = 0; i < num_modes; ++i) { 237 | const auto mode = static_cast(i); 238 | if (GetFormat7Info(camera, mode).second) { 239 | // Supported 240 | return true; 241 | } 242 | } 243 | return false; 244 | } 245 | 246 | std::pair IsFormat7SettingsValid( 247 | Camera& camera, Format7ImageSettings& fmt7_settings) { 248 | Format7PacketInfo fmt7_packet_info; 249 | bool valid = false; 250 | PgrWarn( 251 | camera.ValidateFormat7Settings(&fmt7_settings, &valid, &fmt7_packet_info), 252 | "Failed to validate format7 settings"); 253 | return {fmt7_packet_info, valid}; 254 | } 255 | 256 | std::pair CenterRoi(int size, int max_size, int step) { 257 | if (size == 0 || size > max_size) size = max_size; 258 | // size should be a multiple of step 259 | size = size / step * step; 260 | const int offset = (max_size - size) / 2; 261 | // Return offset for centering roi 262 | return std::make_pair(size, offset); 263 | } 264 | 265 | } // namespace flea3 266 | -------------------------------------------------------------------------------- /src/flycapture/async_trigger.cpp: -------------------------------------------------------------------------------- 1 | 2 | //============================================================================= 3 | // Copyright © 2008 Point Grey Research, Inc. All Rights Reserved. 4 | // 5 | // This software is the confidential and proprietary information of Point 6 | // Grey Research, Inc. ("Confidential Information"). You shall not 7 | // disclose such Confidential Information and shall use it only in 8 | // accordance with the terms of the license agreement you entered into 9 | // with PGR. 10 | // 11 | // PGR MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF THE 12 | // SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 13 | // IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 14 | // PURPOSE, OR NON-INFRINGEMENT. PGR SHALL NOT BE LIABLE FOR ANY DAMAGES 15 | // SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR DISTRIBUTING 16 | // THIS SOFTWARE OR ITS DERIVATIVES. 17 | //============================================================================= 18 | //============================================================================= 19 | // $Id: AsyncTriggerEx.cpp,v 1.21 2010-07-22 22:51:51 soowei Exp $ 20 | //============================================================================= 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | // 28 | // Software trigger the camera instead of using an external hardware trigger 29 | // 30 | #define SOFTWARE_TRIGGER_CAMERA 31 | 32 | using namespace FlyCapture2; 33 | 34 | void PrintBuildInfo() { 35 | FC2Version fc2Version; 36 | Utilities::GetLibraryVersion(&fc2Version); 37 | char version[128]; 38 | sprintf(version, "FlyCapture2 library version: %d.%d.%d.%d\n", 39 | fc2Version.major, fc2Version.minor, fc2Version.type, 40 | fc2Version.build); 41 | 42 | printf("%s", version); 43 | 44 | char timeStamp[512]; 45 | sprintf(timeStamp, "Application build date: %s %s\n\n", __DATE__, __TIME__); 46 | 47 | printf("%s", timeStamp); 48 | } 49 | 50 | void PrintProperty(const Property& prop) { 51 | std::cout << "present: " << prop.present 52 | << "\nabsControl: " << prop.absControl 53 | << "\nonePush:" << prop.onePush << "\nonOff:" << prop.onOff 54 | << "\nautoManualMode: " << prop.autoManualMode 55 | << "\nabsValue: " << prop.absValue << std::endl; 56 | } 57 | 58 | void PrintPropertyInfo(const PropertyInfo& prop_info) {} 59 | 60 | void PrintCameraInfo(CameraInfo* pCamInfo) { 61 | printf( 62 | "\n*** CAMERA INFORMATION ***\n" 63 | "Serial number - %u\n" 64 | "Camera model - %s\n" 65 | "Camera vendor - %s\n" 66 | "Sensor - %s\n" 67 | "Resolution - %s\n" 68 | "Firmware version - %s\n" 69 | "Firmware build time - %s\n\n", 70 | pCamInfo->serialNumber, pCamInfo->modelName, pCamInfo->vendorName, 71 | pCamInfo->sensorInfo, pCamInfo->sensorResolution, 72 | pCamInfo->firmwareVersion, pCamInfo->firmwareBuildTime); 73 | } 74 | 75 | void PrintError(const Error error) { error.PrintErrorTrace(); } 76 | 77 | bool CheckSoftwareTriggerPresence(Camera* pCam) { 78 | const unsigned int k_triggerInq = 0x530; 79 | 80 | Error error; 81 | unsigned int regVal = 0; 82 | 83 | error = pCam->ReadRegister(k_triggerInq, ®Val); 84 | 85 | if (error != PGRERROR_OK) { 86 | PrintError(error); 87 | return false; 88 | } 89 | 90 | if ((regVal & 0x10000) != 0x10000) { 91 | return false; 92 | } 93 | 94 | return true; 95 | } 96 | 97 | bool PollForTriggerReady(Camera* pCam) { 98 | const unsigned int k_softwareTrigger = 0x62C; 99 | Error error; 100 | unsigned int regVal = 0; 101 | 102 | int num_polls = 0; 103 | do { 104 | error = pCam->ReadRegister(k_softwareTrigger, ®Val); 105 | if (error != PGRERROR_OK) { 106 | PrintError(error); 107 | return false; 108 | } 109 | std::cout << "polling: " << ++num_polls << std::endl; 110 | 111 | } while ((regVal >> 31) != 0); 112 | 113 | return true; 114 | } 115 | 116 | bool FireSoftwareTrigger(Camera* pCam) { 117 | const unsigned int k_softwareTrigger = 0x62C; 118 | const unsigned int k_fireVal = 0x80000000; 119 | Error error; 120 | 121 | error = pCam->WriteRegister(k_softwareTrigger, k_fireVal); 122 | if (error != PGRERROR_OK) { 123 | PrintError(error); 124 | return false; 125 | } 126 | 127 | return true; 128 | } 129 | 130 | int main(int /*argc*/, char** /*argv*/) { 131 | PrintBuildInfo(); 132 | 133 | const int k_numImages = 2; 134 | 135 | Error error; 136 | 137 | BusManager busMgr; 138 | unsigned int numCameras; 139 | error = busMgr.GetNumOfCameras(&numCameras); 140 | if (error != PGRERROR_OK) { 141 | PrintError(error); 142 | return -1; 143 | } 144 | 145 | printf("Number of cameras detected: %u\n", numCameras); 146 | 147 | if (numCameras < 1) { 148 | printf("Insufficient number of cameras... exiting\n"); 149 | return -1; 150 | } 151 | 152 | PGRGuid guid; 153 | error = busMgr.GetCameraFromIndex(0, &guid); 154 | if (error != PGRERROR_OK) { 155 | PrintError(error); 156 | return -1; 157 | } 158 | 159 | Camera cam; 160 | 161 | // Connect to a camera 162 | error = cam.Connect(&guid); 163 | if (error != PGRERROR_OK) { 164 | PrintError(error); 165 | return -1; 166 | } 167 | 168 | // Power on the camera 169 | const unsigned int k_cameraPower = 0x610; 170 | const unsigned int k_powerVal = 0x80000000; 171 | error = cam.WriteRegister(k_cameraPower, k_powerVal); 172 | if (error != PGRERROR_OK) { 173 | PrintError(error); 174 | return -1; 175 | } 176 | 177 | const unsigned int millisecondsToSleep = 100; 178 | unsigned int regVal = 0; 179 | unsigned int retries = 10; 180 | 181 | // Wait for camera to complete power-up 182 | do { 183 | #if defined(WIN32) || defined(WIN64) 184 | Sleep(millisecondsToSleep); 185 | #else 186 | usleep(millisecondsToSleep * 1000); 187 | #endif 188 | error = cam.ReadRegister(k_cameraPower, ®Val); 189 | if (error == PGRERROR_TIMEOUT) { 190 | // ignore timeout errors, camera may not be responding to 191 | // register reads during power-up 192 | } else if (error != PGRERROR_OK) { 193 | PrintError(error); 194 | return -1; 195 | } 196 | 197 | retries--; 198 | } while ((regVal & k_powerVal) == 0 && retries > 0); 199 | 200 | // Check for timeout errors after retrying 201 | if (error == PGRERROR_TIMEOUT) { 202 | PrintError(error); 203 | return -1; 204 | } 205 | 206 | // Get the camera information 207 | CameraInfo camInfo; 208 | error = cam.GetCameraInfo(&camInfo); 209 | if (error != PGRERROR_OK) { 210 | PrintError(error); 211 | return -1; 212 | } 213 | 214 | PrintCameraInfo(&camInfo); 215 | 216 | #ifndef SOFTWARE_TRIGGER_CAMERA 217 | // Check for external trigger support 218 | TriggerModeInfo triggerModeInfo; 219 | error = cam.GetTriggerModeInfo(&triggerModeInfo); 220 | if (error != PGRERROR_OK) { 221 | PrintError(error); 222 | return -1; 223 | } 224 | 225 | if (triggerModeInfo.present != true) { 226 | printf("Camera does not support external trigger! Exiting...\n"); 227 | return -1; 228 | } 229 | #endif 230 | 231 | // Get current trigger settings 232 | TriggerMode triggerMode; 233 | error = cam.GetTriggerMode(&triggerMode); 234 | if (error != PGRERROR_OK) { 235 | PrintError(error); 236 | return -1; 237 | } 238 | 239 | // Set camera to trigger mode 0 240 | triggerMode.onOff = true; 241 | triggerMode.mode = 0; 242 | triggerMode.parameter = 0; 243 | 244 | #ifdef SOFTWARE_TRIGGER_CAMERA 245 | // A source of 7 means software trigger 246 | triggerMode.source = 7; 247 | #else 248 | // Triggering the camera externally using source 0. 249 | triggerMode.source = 0; 250 | #endif 251 | 252 | error = cam.SetTriggerMode(&triggerMode); 253 | if (error != PGRERROR_OK) { 254 | PrintError(error); 255 | return -1; 256 | } 257 | 258 | // Poll to ensure camera is ready 259 | bool retVal = PollForTriggerReady(&cam); 260 | if (!retVal) { 261 | printf("\nError polling for trigger ready!\n"); 262 | return -1; 263 | } 264 | 265 | // Get the camera configuration 266 | FC2Config config; 267 | error = cam.GetConfiguration(&config); 268 | if (error != PGRERROR_OK) { 269 | PrintError(error); 270 | return -1; 271 | } 272 | 273 | // Set the grab timeout to 5 seconds 274 | config.grabTimeout = 1000; 275 | 276 | // Set the camera configuration 277 | error = cam.SetConfiguration(&config); 278 | if (error != PGRERROR_OK) { 279 | PrintError(error); 280 | return -1; 281 | } 282 | 283 | Property prop; 284 | prop.type = FRAME_RATE; 285 | error = cam.GetProperty(&prop); 286 | if (error != PGRERROR_OK) { 287 | error.PrintErrorTrace(); 288 | return -1; 289 | } 290 | std::cout << "Current frame rate: " << std::endl; 291 | PrintProperty(prop); 292 | 293 | prop.onOff = false; 294 | prop.absControl = true; 295 | prop.absValue = 100; 296 | prop.autoManualMode = false; 297 | error = cam.SetProperty(&prop); 298 | if (error != PGRERROR_OK) { 299 | error.PrintErrorTrace(); 300 | return -1; 301 | } 302 | std::cout << "Set frame rate to: " << prop.absValue << std::endl; 303 | 304 | error = cam.GetProperty(&prop); 305 | if (error != PGRERROR_OK) { 306 | error.PrintErrorTrace(); 307 | return -1; 308 | } 309 | std::cout << "After setting frame rate: " << prop.absValue << std::endl; 310 | 311 | // Camera is ready, start capturing images 312 | error = cam.StartCapture(); 313 | if (error != PGRERROR_OK) { 314 | PrintError(error); 315 | return -1; 316 | } 317 | 318 | #ifdef SOFTWARE_TRIGGER_CAMERA 319 | if (!CheckSoftwareTriggerPresence(&cam)) { 320 | printf( 321 | "SOFT_ASYNC_TRIGGER not implemented on this camera! Stopping " 322 | "application\n"); 323 | return -1; 324 | } 325 | #else 326 | printf("Trigger the camera by sending a trigger pulse to GPIO%d.\n", 327 | triggerMode.source); 328 | #endif 329 | 330 | Image image; 331 | for (int imageCount = 0; imageCount < k_numImages; imageCount++) { 332 | #ifdef SOFTWARE_TRIGGER_CAMERA 333 | // Check that the trigger is ready 334 | PollForTriggerReady(&cam); 335 | 336 | printf("Press the Enter key to initiate a software trigger.\n"); 337 | getchar(); 338 | 339 | printf("waiting for buffer 1\n"); 340 | error = cam.WaitForBufferEvent(&image, 0); 341 | if (error != PGRERROR_OK) { 342 | PrintError(error); 343 | } 344 | printf("after waiting buffer 1\n"); 345 | 346 | // Fire software trigger 347 | bool retVal = FireSoftwareTrigger(&cam); 348 | if (!retVal) { 349 | printf("\nError firing software trigger!\n"); 350 | return -1; 351 | } 352 | usleep(2e6); 353 | #endif 354 | 355 | printf("waiting for buffer 2\n"); 356 | cam.WaitForBufferEvent(&image, 0); 357 | printf("after waiting buffer 2\n"); 358 | 359 | // Grab image 360 | printf("calling retrieve buffer\n"); 361 | error = cam.RetrieveBuffer(&image); 362 | if (error != PGRERROR_OK) { 363 | PrintError(error); 364 | } 365 | printf("after retrieve buffer\n"); 366 | 367 | printf(".\n"); 368 | } 369 | 370 | // Turn trigger mode off. 371 | triggerMode.onOff = false; 372 | error = cam.SetTriggerMode(&triggerMode); 373 | if (error != PGRERROR_OK) { 374 | PrintError(error); 375 | return -1; 376 | } 377 | printf("\nFinished grabbing images\n"); 378 | 379 | // Stop capturing images 380 | error = cam.StopCapture(); 381 | if (error != PGRERROR_OK) { 382 | PrintError(error); 383 | return -1; 384 | } 385 | 386 | // Turn off trigger mode 387 | triggerMode.onOff = false; 388 | error = cam.SetTriggerMode(&triggerMode); 389 | if (error != PGRERROR_OK) { 390 | PrintError(error); 391 | return -1; 392 | } 393 | 394 | // Disconnect the camera 395 | error = cam.Disconnect(); 396 | if (error != PGRERROR_OK) { 397 | PrintError(error); 398 | return -1; 399 | } 400 | 401 | printf("Done! Press Enter to exit...\n"); 402 | getchar(); 403 | 404 | return 0; 405 | } 406 | -------------------------------------------------------------------------------- /src/flycapture/custom_image.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright © 2008 Point Grey Research, Inc. All Rights Reserved. 3 | // 4 | // This software is the confidential and proprietary information of Point 5 | // Grey Research, Inc. ("Confidential Information"). You shall not 6 | // disclose such Confidential Information and shall use it only in 7 | // accordance with the terms of the license agreement you entered into 8 | // with PGR. 9 | // 10 | // PGR MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF THE 11 | // SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 12 | // IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 13 | // PURPOSE, OR NON-INFRINGEMENT. PGR SHALL NOT BE LIABLE FOR ANY DAMAGES 14 | // SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR DISTRIBUTING 15 | // THIS SOFTWARE OR ITS DERIVATIVES. 16 | //============================================================================= 17 | //============================================================================= 18 | // $Id: CustomImageEx.cpp,v 1.20 2010-02-26 23:24:47 soowei Exp $ 19 | //============================================================================= 20 | 21 | #include "stdio.h" 22 | 23 | #include 24 | 25 | using namespace FlyCapture2; 26 | 27 | void PrintBuildInfo() { 28 | FC2Version fc2Version; 29 | Utilities::GetLibraryVersion(&fc2Version); 30 | char version[128]; 31 | sprintf(version, "FlyCapture2 library version: %d.%d.%d.%d\n", 32 | fc2Version.major, fc2Version.minor, fc2Version.type, 33 | fc2Version.build); 34 | 35 | printf("%s", version); 36 | 37 | char timeStamp[512]; 38 | sprintf(timeStamp, "Application build date: %s %s\n\n", __DATE__, __TIME__); 39 | 40 | printf("%s", timeStamp); 41 | } 42 | 43 | void PrintCameraInfo(CameraInfo* pCamInfo) { 44 | printf( 45 | "\n*** CAMERA INFORMATION ***\n" 46 | "Serial number - %u\n" 47 | "Camera model - %s\n" 48 | "Camera vendor - %s\n" 49 | "Sensor - %s\n" 50 | "Resolution - %s\n" 51 | "Firmware version - %s\n" 52 | "Firmware build time - %s\n\n", 53 | pCamInfo->serialNumber, pCamInfo->modelName, pCamInfo->vendorName, 54 | pCamInfo->sensorInfo, pCamInfo->sensorResolution, 55 | pCamInfo->firmwareVersion, pCamInfo->firmwareBuildTime); 56 | } 57 | 58 | void PrintFormat7Capabilities(Format7Info fmt7Info) { 59 | printf( 60 | "Max image pixels: (%u, %u)\n" 61 | "Image Unit size: (%u, %u)\n" 62 | "Offset Unit size: (%u, %u)\n" 63 | "Pixel format bitfield: 0x%08x\n", 64 | fmt7Info.maxWidth, fmt7Info.maxHeight, fmt7Info.imageHStepSize, 65 | fmt7Info.imageVStepSize, fmt7Info.offsetHStepSize, 66 | fmt7Info.offsetVStepSize, fmt7Info.pixelFormatBitField); 67 | } 68 | 69 | void PrintError(Error error) { error.PrintErrorTrace(); } 70 | 71 | int main(int /*argc*/, char** /*argv*/) { 72 | PrintBuildInfo(); 73 | 74 | const Mode k_fmt7Mode = MODE_0; 75 | const PixelFormat k_fmt7PixFmt = PIXEL_FORMAT_MONO8; 76 | const int k_numImages = 10; 77 | 78 | Error error; 79 | 80 | // Since this application saves images in the current folder 81 | // we must ensure that we have permission to write to this folder. 82 | // If we do not have permission, fail right away. 83 | FILE* tempFile = fopen("test.txt", "w+"); 84 | if (tempFile == NULL) { 85 | printf( 86 | "Failed to create file in current folder. Please check " 87 | "permissions.\n"); 88 | return -1; 89 | } 90 | fclose(tempFile); 91 | remove("test.txt"); 92 | 93 | BusManager busMgr; 94 | unsigned int numCameras; 95 | error = busMgr.GetNumOfCameras(&numCameras); 96 | if (error != PGRERROR_OK) { 97 | PrintError(error); 98 | return -1; 99 | } 100 | 101 | printf("Number of cameras detected: %u\n", numCameras); 102 | 103 | if (numCameras < 1) { 104 | printf("Insufficient number of cameras... exiting\n"); 105 | return -1; 106 | } 107 | 108 | PGRGuid guid; 109 | error = busMgr.GetCameraFromIndex(0, &guid); 110 | if (error != PGRERROR_OK) { 111 | PrintError(error); 112 | return -1; 113 | } 114 | 115 | Camera cam; 116 | 117 | // Connect to a camera 118 | error = cam.Connect(&guid); 119 | if (error != PGRERROR_OK) { 120 | PrintError(error); 121 | return -1; 122 | } 123 | 124 | // Get the camera information 125 | CameraInfo camInfo; 126 | error = cam.GetCameraInfo(&camInfo); 127 | if (error != PGRERROR_OK) { 128 | PrintError(error); 129 | return -1; 130 | } 131 | 132 | PrintCameraInfo(&camInfo); 133 | 134 | // Query for available Format 7 modes 135 | Format7Info fmt7Info; 136 | bool supported; 137 | fmt7Info.mode = k_fmt7Mode; 138 | error = cam.GetFormat7Info(&fmt7Info, &supported); 139 | if (error != PGRERROR_OK) { 140 | PrintError(error); 141 | return -1; 142 | } 143 | 144 | PrintFormat7Capabilities(fmt7Info); 145 | 146 | if ((k_fmt7PixFmt & fmt7Info.pixelFormatBitField) == 0) { 147 | // Pixel format not supported! 148 | printf("Pixel format is not supported\n"); 149 | return -1; 150 | } 151 | 152 | Format7ImageSettings fmt7ImageSettings; 153 | fmt7ImageSettings.mode = k_fmt7Mode; 154 | fmt7ImageSettings.offsetX = 0; 155 | fmt7ImageSettings.offsetY = 0; 156 | fmt7ImageSettings.width = fmt7Info.maxWidth; 157 | fmt7ImageSettings.height = fmt7Info.maxHeight; 158 | fmt7ImageSettings.pixelFormat = k_fmt7PixFmt; 159 | 160 | bool valid; 161 | Format7PacketInfo fmt7PacketInfo; 162 | 163 | // Validate the settings to make sure that they are valid 164 | error = 165 | cam.ValidateFormat7Settings(&fmt7ImageSettings, &valid, &fmt7PacketInfo); 166 | if (error != PGRERROR_OK) { 167 | PrintError(error); 168 | return -1; 169 | } 170 | 171 | if (!valid) { 172 | // Settings are not valid 173 | printf("Format7 settings are not valid\n"); 174 | return -1; 175 | } 176 | 177 | // Set the settings to the camera 178 | error = cam.SetFormat7Configuration(&fmt7ImageSettings, 179 | fmt7PacketInfo.recommendedBytesPerPacket); 180 | if (error != PGRERROR_OK) { 181 | PrintError(error); 182 | return -1; 183 | } 184 | 185 | // Start capturing images 186 | error = cam.StartCapture(); 187 | if (error != PGRERROR_OK) { 188 | PrintError(error); 189 | return -1; 190 | } 191 | 192 | // Retrieve frame rate property 193 | Property frmRate; 194 | frmRate.type = FRAME_RATE; 195 | error = cam.GetProperty(&frmRate); 196 | if (error != PGRERROR_OK) { 197 | PrintError(error); 198 | return -1; 199 | } 200 | 201 | printf("Frame rate is %3.2f fps\n", frmRate.absValue); 202 | 203 | printf("Grabbing %d images\n", k_numImages); 204 | 205 | Image rawImage; 206 | for (int imageCount = 0; imageCount < k_numImages; imageCount++) { 207 | // Retrieve an image 208 | error = cam.RetrieveBuffer(&rawImage); 209 | if (error != PGRERROR_OK) { 210 | PrintError(error); 211 | continue; 212 | } 213 | 214 | printf("."); 215 | 216 | // Get the raw image dimensions 217 | PixelFormat pixFormat; 218 | unsigned int rows, cols, stride; 219 | rawImage.GetDimensions(&rows, &cols, &stride, &pixFormat); 220 | 221 | // Create a converted image 222 | Image convertedImage; 223 | 224 | // Convert the raw image 225 | error = rawImage.Convert(PIXEL_FORMAT_BGRU, &convertedImage); 226 | if (error != PGRERROR_OK) { 227 | PrintError(error); 228 | return -1; 229 | } 230 | 231 | // Create a unique filename 232 | char filename[512]; 233 | sprintf(filename, "%u-%d.bmp", camInfo.serialNumber, imageCount); 234 | 235 | // Save the image. If a file format is not passed in, then the file 236 | // extension is parsed to attempt to determine the file format. 237 | error = convertedImage.Save(filename); 238 | if (error != PGRERROR_OK) { 239 | PrintError(error); 240 | return -1; 241 | } 242 | } 243 | 244 | printf("\nFinished grabbing images\n"); 245 | 246 | // Stop capturing images 247 | error = cam.StopCapture(); 248 | if (error != PGRERROR_OK) { 249 | PrintError(error); 250 | return -1; 251 | } 252 | 253 | // Disconnect the camera 254 | error = cam.Disconnect(); 255 | if (error != PGRERROR_OK) { 256 | PrintError(error); 257 | return -1; 258 | } 259 | 260 | printf("Done! Press Enter to exit...\n"); 261 | getchar(); 262 | 263 | return 0; 264 | } 265 | -------------------------------------------------------------------------------- /src/flycapture/extended_shutter.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Copyright © 2008 Point Grey Research, Inc. All Rights Reserved. 3 | // 4 | // This software is the confidential and proprietary information of Point 5 | // Grey Research, Inc. ("Confidential Information"). You shall not 6 | // disclose such Confidential Information and shall use it only in 7 | // accordance with the terms of the license agreement you entered into 8 | // with PGR. 9 | // 10 | // PGR MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF THE 11 | // SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 12 | // IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 13 | // PURPOSE, OR NON-INFRINGEMENT. PGR SHALL NOT BE LIABLE FOR ANY DAMAGES 14 | // SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR DISTRIBUTING 15 | // THIS SOFTWARE OR ITS DERIVATIVES. 16 | //============================================================================= 17 | //============================================================================= 18 | // $Id: ExtendedShutterEx.cpp,v 1.9 2009-12-08 18:58:36 soowei Exp $ 19 | //============================================================================= 20 | 21 | #include 22 | 23 | #include "flycapture/FlyCapture2.h" 24 | 25 | using namespace FlyCapture2; 26 | 27 | enum ExtendedShutterType { 28 | NO_EXTENDED_SHUTTER, 29 | DRAGONFLY_EXTENDED_SHUTTER, 30 | GENERAL_EXTENDED_SHUTTER 31 | }; 32 | 33 | void PrintBuildInfo() { 34 | FC2Version fc2Version; 35 | Utilities::GetLibraryVersion(&fc2Version); 36 | char version[128]; 37 | sprintf(version, "FlyCapture2 library version: %d.%d.%d.%d\n", 38 | fc2Version.major, fc2Version.minor, fc2Version.type, 39 | fc2Version.build); 40 | 41 | printf("%s", version); 42 | 43 | char timeStamp[512]; 44 | sprintf(timeStamp, "Application build date: %s %s\n\n", __DATE__, __TIME__); 45 | 46 | printf("%s", timeStamp); 47 | } 48 | 49 | void PrintCameraInfo(CameraInfo* pCamInfo) { 50 | printf( 51 | "\n*** CAMERA INFORMATION ***\n" 52 | "Serial number - %u\n" 53 | "Camera model - %s\n" 54 | "Camera vendor - %s\n" 55 | "Sensor - %s\n" 56 | "Resolution - %s\n" 57 | "Firmware version - %s\n" 58 | "Firmware build time - %s\n\n", 59 | pCamInfo->serialNumber, pCamInfo->modelName, pCamInfo->vendorName, 60 | pCamInfo->sensorInfo, pCamInfo->sensorResolution, 61 | pCamInfo->firmwareVersion, pCamInfo->firmwareBuildTime); 62 | } 63 | 64 | void PrintError(Error error) { error.PrintErrorTrace(); } 65 | 66 | int main(int /*argc*/, char** /*argv*/) { 67 | PrintBuildInfo(); 68 | 69 | const int k_numImages = 5; 70 | 71 | Error error; 72 | 73 | BusManager busMgr; 74 | unsigned int numCameras; 75 | error = busMgr.GetNumOfCameras(&numCameras); 76 | if (error != PGRERROR_OK) { 77 | PrintError(error); 78 | return -1; 79 | } 80 | 81 | printf("Number of cameras detected: %u\n", numCameras); 82 | 83 | if (numCameras < 1) { 84 | printf("Insufficient number of cameras... exiting\n"); 85 | return -1; 86 | } 87 | 88 | PGRGuid guid; 89 | error = busMgr.GetCameraFromIndex(0, &guid); 90 | if (error != PGRERROR_OK) { 91 | PrintError(error); 92 | return -1; 93 | } 94 | 95 | Camera cam; 96 | 97 | // Connect to a camera 98 | error = cam.Connect(&guid); 99 | if (error != PGRERROR_OK) { 100 | PrintError(error); 101 | return -1; 102 | } 103 | 104 | // Get the camera information 105 | CameraInfo camInfo; 106 | error = cam.GetCameraInfo(&camInfo); 107 | if (error != PGRERROR_OK) { 108 | PrintError(error); 109 | return -1; 110 | } 111 | 112 | PrintCameraInfo(&camInfo); 113 | 114 | // Check if the camera supports the FRAME_RATE property 115 | PropertyInfo propInfo; 116 | propInfo.type = FRAME_RATE; 117 | error = cam.GetPropertyInfo(&propInfo); 118 | if (error != PGRERROR_OK) { 119 | PrintError(error); 120 | return -1; 121 | } 122 | 123 | ExtendedShutterType shutterType = NO_EXTENDED_SHUTTER; 124 | 125 | if (propInfo.present == true) { 126 | // Turn off frame rate 127 | 128 | Property prop; 129 | prop.type = FRAME_RATE; 130 | error = cam.GetProperty(&prop); 131 | if (error != PGRERROR_OK) { 132 | PrintError(error); 133 | return -1; 134 | } 135 | 136 | prop.autoManualMode = false; 137 | prop.onOff = false; 138 | 139 | error = cam.SetProperty(&prop); 140 | if (error != PGRERROR_OK) { 141 | PrintError(error); 142 | return -1; 143 | } 144 | 145 | shutterType = GENERAL_EXTENDED_SHUTTER; 146 | } else { 147 | // Frame rate property does not appear to be supported. 148 | // Disable the extended shutter register instead. 149 | // This is only applicable for Dragonfly. 150 | 151 | const unsigned int k_extendedShutter = 0x1028; 152 | unsigned int extendedShutterRegVal = 0; 153 | 154 | error = cam.ReadRegister(k_extendedShutter, &extendedShutterRegVal); 155 | if (error != PGRERROR_OK) { 156 | PrintError(error); 157 | return -1; 158 | } 159 | 160 | std::bitset<32> extendedShutterBS((int)extendedShutterRegVal); 161 | if (extendedShutterBS[31] == true) { 162 | // Set the camera into extended shutter mode 163 | error = cam.WriteRegister(k_extendedShutter, 0x80020000); 164 | if (error != PGRERROR_OK) { 165 | PrintError(error); 166 | return -1; 167 | } 168 | } else { 169 | printf("Frame rate and extended shutter are not supported... exiting\n"); 170 | return -1; 171 | } 172 | 173 | shutterType = DRAGONFLY_EXTENDED_SHUTTER; 174 | } 175 | 176 | // Set the shutter property of the camera 177 | Property prop; 178 | prop.type = SHUTTER; 179 | error = cam.GetProperty(&prop); 180 | if (error != PGRERROR_OK) { 181 | PrintError(error); 182 | return -1; 183 | } 184 | 185 | prop.autoManualMode = false; 186 | prop.absControl = true; 187 | 188 | const float k_shutterVal = 3000.0; 189 | prop.absValue = k_shutterVal; 190 | 191 | error = cam.SetProperty(&prop); 192 | if (error != PGRERROR_OK) { 193 | PrintError(error); 194 | return -1; 195 | } 196 | 197 | printf("Shutter time set to %.2fms\n", k_shutterVal); 198 | 199 | // Enable timestamping 200 | EmbeddedImageInfo embeddedInfo; 201 | 202 | error = cam.GetEmbeddedImageInfo(&embeddedInfo); 203 | if (error != PGRERROR_OK) { 204 | PrintError(error); 205 | return -1; 206 | } 207 | 208 | if (embeddedInfo.timestamp.available != 0) { 209 | embeddedInfo.timestamp.onOff = true; 210 | } 211 | 212 | error = cam.SetEmbeddedImageInfo(&embeddedInfo); 213 | if (error != PGRERROR_OK) { 214 | PrintError(error); 215 | return -1; 216 | } 217 | 218 | // Start the camera 219 | error = cam.StartCapture(); 220 | if (error != PGRERROR_OK) { 221 | PrintError(error); 222 | return -1; 223 | } 224 | 225 | for (int i = 0; i < k_numImages; i++) { 226 | Image image; 227 | error = cam.RetrieveBuffer(&image); 228 | if (error != PGRERROR_OK) { 229 | PrintError(error); 230 | return -1; 231 | } 232 | 233 | TimeStamp timestamp = image.GetTimeStamp(); 234 | printf("TimeStamp [%d %d]\n", timestamp.cycleSeconds, timestamp.cycleCount); 235 | } 236 | 237 | // Stop capturing images 238 | error = cam.StopCapture(); 239 | if (error != PGRERROR_OK) { 240 | PrintError(error); 241 | return -1; 242 | } 243 | 244 | // Set the camera back to its original state 245 | 246 | prop.type = SHUTTER; 247 | error = cam.GetProperty(&prop); 248 | if (error != PGRERROR_OK) { 249 | PrintError(error); 250 | return -1; 251 | } 252 | 253 | prop.autoManualMode = true; 254 | 255 | error = cam.SetProperty(&prop); 256 | if (error != PGRERROR_OK) { 257 | PrintError(error); 258 | return -1; 259 | } 260 | 261 | if (shutterType == GENERAL_EXTENDED_SHUTTER) { 262 | Property prop; 263 | prop.type = FRAME_RATE; 264 | error = cam.GetProperty(&prop); 265 | if (error != PGRERROR_OK) { 266 | PrintError(error); 267 | return -1; 268 | } 269 | 270 | prop.autoManualMode = true; 271 | prop.onOff = true; 272 | 273 | error = cam.SetProperty(&prop); 274 | if (error != PGRERROR_OK) { 275 | PrintError(error); 276 | return -1; 277 | } 278 | 279 | } else if (shutterType == DRAGONFLY_EXTENDED_SHUTTER) { 280 | const unsigned int k_extendedShutter = 0x1028; 281 | unsigned int extendedShutterRegVal = 0; 282 | 283 | error = cam.ReadRegister(k_extendedShutter, &extendedShutterRegVal); 284 | if (error != PGRERROR_OK) { 285 | PrintError(error); 286 | return -1; 287 | } 288 | 289 | std::bitset<32> extendedShutterBS((int)extendedShutterRegVal); 290 | if (extendedShutterBS[31] == true) { 291 | // Set the camera into extended shutter mode 292 | error = cam.WriteRegister(k_extendedShutter, 0x80000000); 293 | if (error != PGRERROR_OK) { 294 | PrintError(error); 295 | return -1; 296 | } 297 | } 298 | } 299 | 300 | // Disconnect the camera 301 | error = cam.Disconnect(); 302 | if (error != PGRERROR_OK) { 303 | PrintError(error); 304 | return -1; 305 | } 306 | 307 | printf("Done! Press Enter to exit...\n"); 308 | getchar(); 309 | 310 | return 0; 311 | } 312 | -------------------------------------------------------------------------------- /src/list_cameras.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "flea3/flea3_setting.h" 6 | 7 | using namespace FlyCapture2; 8 | using namespace flea3; 9 | 10 | std::string InterfaceTypeToString(const InterfaceType& interface_type) { 11 | switch (interface_type) { 12 | case INTERFACE_IEEE1394: 13 | return "IEEE1394"; 14 | case INTERFACE_USB2: 15 | return "USB2.0"; 16 | case INTERFACE_USB3: 17 | return "USB3.0"; 18 | case INTERFACE_GIGE: 19 | return "GigE"; 20 | case INTERFACE_UNKNOWN: 21 | default: 22 | return "unknown"; 23 | } 24 | } 25 | 26 | void printCameraInfo(const CameraInfo& cinfo) { 27 | const auto interface_type = InterfaceTypeToString(cinfo.interfaceType); 28 | std::cout << "Serial: " << cinfo.serialNumber 29 | << ", Model: " << cinfo.modelName 30 | << ", Vendor: " << cinfo.vendorName 31 | << ", Sensor: " << cinfo.sensorInfo 32 | << ", Resolution: " << cinfo.sensorResolution 33 | << ", Color: " << std::boolalpha << cinfo.isColorCamera 34 | << ", Firmware Version: " << cinfo.firmwareVersion 35 | << ", Interface Type: " << interface_type << std::endl; 36 | } 37 | 38 | int main(int argc, char** argv) { 39 | BusManager bus_manager; 40 | 41 | try { 42 | unsigned num_devices = 0; 43 | PgrError(bus_manager.GetNumOfCameras(&num_devices), 44 | "Failed get number of cameras"); 45 | if (num_devices) { 46 | std::cout << "Number of cameras found: " << num_devices << std::endl; 47 | for (unsigned i = 0; i < num_devices; ++i) { 48 | PGRGuid guid; 49 | PgrError(bus_manager.GetCameraFromIndex(i, &guid), 50 | "Failed to get camera from index" + std::to_string(i)); 51 | 52 | Camera camera; 53 | PgrError(camera.Connect(&guid), "Failed to connect to camera"); 54 | 55 | CameraInfo cinfo; 56 | PgrError(camera.GetCameraInfo(&cinfo), "Failed to get camera info"); 57 | 58 | std::cout << "[" << i << "]"; 59 | printCameraInfo(cinfo); 60 | } 61 | } else { 62 | // No cameras found 63 | std::cout << "No PointGrey cameras detected on this computer." 64 | << std::endl 65 | << std::endl; 66 | 67 | std::cout << "Note that you may need to restart udev and " 68 | "replug your camera, eg:" 69 | << std::endl 70 | << " sudo service udev restart" << std::endl; 71 | } 72 | } catch (const std::runtime_error& e) { 73 | std::cout << "There was an error checking the active cameras: " << e.what() 74 | << std::endl; 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /src/single/single_main.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/single_node.h" 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "flea3_single"); 5 | ros::NodeHandle pnh("~"); 6 | 7 | try { 8 | flea3::SingleNode single_node(pnh); 9 | single_node.Run(); 10 | ros::spin(); 11 | single_node.End(); 12 | } catch (const std::exception& e) { 13 | ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what()); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/single/single_node.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/single_node.h" 2 | 3 | namespace flea3 { 4 | 5 | SingleNode::SingleNode(const ros::NodeHandle &pnh) 6 | : CameraNodeBase(pnh), flea3_ros_(pnh) {} 7 | 8 | void SingleNode::Acquire() { 9 | while (is_acquire() && ros::ok()) { 10 | if (flea3_ros_.RequestSingle()) { 11 | const auto expose_duration = 12 | ros::Duration(flea3_ros_.camera().GetShutterTimeSec() / 2); 13 | const auto time = ros::Time::now() + expose_duration; 14 | flea3_ros_.PublishCamera(time); 15 | // flea3_ros_.PublishImageMetadata(time); 16 | Sleep(); 17 | } 18 | } 19 | } 20 | 21 | void SingleNode::Setup(Config &config) { 22 | flea3_ros_.Stop(); 23 | flea3_ros_.camera().Configure(config); 24 | flea3_ros_.set_fps(config.fps); 25 | flea3_ros_.Start(); 26 | } 27 | 28 | } // namespace flea3 29 | -------------------------------------------------------------------------------- /src/single/single_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "flea3/single_node.h" 5 | 6 | namespace flea3 { 7 | 8 | class SingleNodelet : public nodelet::Nodelet { 9 | public: 10 | SingleNodelet() = default; 11 | ~SingleNodelet() { 12 | if (single_node_) { 13 | single_node_->End(); 14 | } 15 | } 16 | 17 | virtual void onInit() { 18 | try { 19 | single_node_.reset(new SingleNode(getPrivateNodeHandle())); 20 | single_node_->Run(); 21 | } catch (const std::exception &e) { 22 | NODELET_ERROR("%s: %s", getPrivateNodeHandle().getNamespace().c_str(), 23 | e.what()); 24 | } 25 | } 26 | 27 | private: 28 | std::unique_ptr single_node_; 29 | }; 30 | 31 | PLUGINLIB_EXPORT_CLASS(flea3::SingleNodelet, nodelet::Nodelet) 32 | 33 | } // namespace flea3 34 | -------------------------------------------------------------------------------- /src/stereo/stereo_main.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/stereo_node.h" 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "flea3_stereo"); 5 | ros::NodeHandle pnh("~"); 6 | 7 | try { 8 | flea3::StereoNode stereo_node(pnh); 9 | stereo_node.Run(); 10 | ros::spin(); 11 | stereo_node.End(); 12 | } catch (const std::exception& e) { 13 | ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what()); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/stereo/stereo_node.cpp: -------------------------------------------------------------------------------- 1 | #include "flea3/stereo_node.h" 2 | 3 | namespace flea3 { 4 | 5 | StereoNode::StereoNode(const ros::NodeHandle& pnh) 6 | : CameraNodeBase(pnh), left_ros_(pnh, "left"), right_ros_(pnh, "right") {} 7 | 8 | void StereoNode::Acquire() { 9 | while (is_acquire() && ros::ok()) { 10 | if (left_ros_.RequestSingle() && right_ros_.RequestSingle()) { 11 | const auto expose_duration = 12 | ros::Duration(left_ros_.camera().GetShutterTimeSec() / 2); 13 | const auto time = ros::Time::now() + expose_duration; 14 | left_ros_.PublishCamera(time); 15 | right_ros_.PublishCamera(time); 16 | Sleep(); 17 | } 18 | } 19 | } 20 | 21 | void StereoNode::Setup(Flea3DynConfig& config) { 22 | left_ros_.Stop(); 23 | right_ros_.Stop(); 24 | left_ros_.set_fps(config.fps); 25 | right_ros_.set_fps(config.fps); 26 | Flea3DynConfig config_cpy = config; 27 | left_ros_.camera().Configure(config_cpy); 28 | right_ros_.camera().Configure(config); 29 | left_ros_.Start(); 30 | right_ros_.Start(); 31 | } 32 | 33 | } // namespace flea3 34 | -------------------------------------------------------------------------------- /src/stereo/stereo_nodelet.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flea3/e1644f45615f05ab00f65a87a91f829d46acab3c/src/stereo/stereo_nodelet.cpp --------------------------------------------------------------------------------