├── CMakeLists.txt ├── README.md ├── calib ├── circles_grid.png └── draw_circles_grid.py ├── cfg ├── CalibProcDyn.cfg ├── FlirGigeDyn.cfg └── ThermalProcDyn.cfg ├── cmake └── FindEbus.cmake ├── include ├── calib_proc │ └── calib_proc_node.h ├── flir_gige │ ├── flir_gige.h │ ├── flir_gige_node.h │ ├── flir_gige_ros.h │ └── planck.h └── thermal_proc │ └── thermal_proc_node.h ├── install ├── install.bash └── set_puregev_gen ├── launch ├── node.launch ├── nodelet.launch └── thermal_proc.launch ├── math └── conversion.py ├── nodelet_plugins.xml ├── package.xml └── src ├── calib_proc ├── calib_proc_main.cpp └── calib_proc_node.cpp ├── flir_gige ├── flir_gige.cpp ├── flir_gige_main.cpp ├── flir_gige_node.cpp ├── flir_gige_nodelet.cpp └── flir_gige_ros.cpp └── thermal_proc ├── thermal_proc_main.cpp ├── thermal_proc_node.cpp └── thermal_proc_nodelet.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_gige) 3 | 4 | add_definitions(-std=c++11 -Wall -Wno-unknown-pragmas) 5 | 6 | # Set library paths for pleora ebus sdk on 64-bit linux 7 | # For more information, see cmake/Findebus.cmake 8 | if(CMAKE_SIZEOF_VOID_P EQUAL 8) 9 | # Make CMake aware of the cmake folder for local FindXXX scripts. 10 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 11 | find_package(Ebus REQUIRED) 12 | else() 13 | message(FATAL_ERROR "This package only supports 64-bit linux") 14 | endif() 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | roscpp nodelet camera_base cv_bridge 18 | ) 19 | find_package(OpenCV) 20 | 21 | # Dynamic reconfigure option, as to be placed right before catkin_package 22 | generate_dynamic_reconfigure_options( 23 | cfg/FlirGigeDyn.cfg 24 | cfg/ThermalProcDyn.cfg 25 | cfg/CalibProcDyn.cfg 26 | ) 27 | 28 | catkin_package( 29 | # INCLUDE_DIRS include 30 | # LIBRARIES flir 31 | # CATKIN_DEPENDS roscpp sensor_msgs 32 | # DEPENDS system_lib 33 | ) 34 | 35 | include_directories( 36 | include 37 | ${catkin_INCLUDE_DIRS} 38 | ${Ebus_INCLUDE_DIRS} 39 | ${OpenCV_INCLUDE_DIRS} 40 | ) 41 | 42 | # library 43 | add_library(${PROJECT_NAME} 44 | src/flir_gige/flir_gige.cpp 45 | src/flir_gige/flir_gige_ros.cpp 46 | src/flir_gige/flir_gige_node.cpp 47 | src/flir_gige/flir_gige_nodelet.cpp 48 | src/thermal_proc/thermal_proc_node.cpp 49 | src/thermal_proc/thermal_proc_nodelet.cpp 50 | src/calib_proc/calib_proc_node.cpp 51 | ) 52 | target_link_libraries(${PROJECT_NAME} 53 | ${catkin_LIBRARIES} 54 | ${OpenCV_LIBRARIES} 55 | ${Ebus_LIBRARIES} 56 | ) 57 | 58 | # node 59 | add_executable(${PROJECT_NAME}_node 60 | src/flir_gige/flir_gige_main.cpp 61 | ) 62 | target_link_libraries(${PROJECT_NAME}_node 63 | ${PROJECT_NAME} 64 | ) 65 | add_executable(thermal_proc_node 66 | src/thermal_proc/thermal_proc_main.cpp 67 | ) 68 | target_link_libraries(thermal_proc_node 69 | ${PROJECT_NAME} 70 | ) 71 | add_executable(calib_proc_node 72 | src/calib_proc/calib_proc_main.cpp 73 | ) 74 | target_link_libraries(calib_proc_node 75 | ${PROJECT_NAME} 76 | ) 77 | 78 | ## Add cmake target dependencies of the executable/library 79 | add_dependencies(${PROJECT_NAME} 80 | ${catkin_EXPORTED_TARGETS} 81 | ${PROJECT_NAME}_gencfg 82 | ${PROJECT_NAME}_gencpp 83 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 84 | ) 85 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # flir_gige 2 | A ros driver for FLIR ax5 thermal camera using Pleora eBus SDK 4.0.5+ 3 | 4 | ![image](http://www.flir.com/uploadedImages/Thermography_USA/Industries/ATS/Products/Ax5_Series_Kits/FLIR-A-Series-Thumbnail.png) 5 | 6 | ## Supported hardware 7 | This driver should work at least with a FLIR ax5 thermal camera. 8 | 9 | ## API Stability 10 | The ROS API of this driver should be considered **unstable**. 11 | 12 | ## ROS API 13 | 14 | ### flir_gige_node 15 | 16 | `flir_gige_node` is a driver for a FLIR GigE camera. 17 | 18 | #### Published topics 19 | 20 | `~image_raw` ([sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)) 21 | The unprocessed image data. 22 | 23 | `~camera_info` ([sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)) 24 | Contains the camera calibration (if calibrated) and extra data about the camera configuration. 25 | 26 | ## Installing Pleora eBUS SDK 27 | You can find the latest version of Pleora eBus SDK from [here](http://www.pleora.com/support-center/documentation-downloads). 28 | If you couldn't download one from their website, this driver comes with version 4.0.5. 29 | To install, run the following command: 30 | 31 | ```bash 32 | cd install 33 | sudo ./eBUS_SDK_4.0.5.3150_Ubuntu-12.04-x86_64.run 34 | # accept all default options 35 | ``` 36 | 37 | This will install the eBUS SDK to `/opt/pleora`. 38 | If you are using ubuntu 14.04, you need to install `libudev-dev` and link it into `/usr/lib`, since eBUS SDK links to that version by default. 39 | 40 | ```bash 41 | sudo apt-get install libudev-dev 42 | cd /usr/lib 43 | sudo ln -s x86_64/libudev.so libudev.so.0 44 | ``` 45 | 46 | ## Running the node 47 | You need to first source the `set_puregev_gen` file in `install`. 48 | 49 | Then just do 50 | 51 | ``` 52 | roslaunch flir_gige node.launch 53 | 54 | ``` 55 | 56 | Notice that the `bit` option specifies whether to receive 8 bit or 14 bit data. 57 | `bit - 2`: This will tell the device to return data in 8 bit format, which is good for visualization 58 | `bit - 3`: This will tell the device to return data in 14 bit format, which can be used to get the temperature 59 | -------------------------------------------------------------------------------- /calib/circles_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/flir_gige/77301f24a6862b53018a02c46c3dda166155347a/calib/circles_grid.png -------------------------------------------------------------------------------- /calib/draw_circles_grid.py: -------------------------------------------------------------------------------- 1 | __author__ = 'chao' 2 | 3 | import matplotlib.pyplot as plt 4 | 5 | fig = plt.gcf() 6 | n_cols = 5 7 | n_rows = 4 8 | r = 0.1 9 | for x in range(0, n_cols): 10 | for y in range(0, n_rows): 11 | circle = plt.Circle((x, y), r, color='k') 12 | fig.gca().add_artist(circle) 13 | fig.gca().set_aspect('equal') 14 | fig.gca().axis([-1.5, n_cols + 0.5, -1.5, n_rows + 0.5]) 15 | plt.show() 16 | 17 | fig.savefig('circles_grid.png') 18 | -------------------------------------------------------------------------------- /cfg/CalibProcDyn.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "flir_gige" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # name / type / level / description / default / min / max 9 | # Flir Ax5 has a maximum resolution of 320 x 256 10 | gen.add("circles_per_row", int_t, 0, 11 | "circles per row", 4, 1, 20) 12 | gen.add("circles_per_col", int_t, 0, 13 | "circles per col", 5, 1, 20) 14 | gen.add("sigma", double_t, 0, 15 | "gaussian sigma", 1, 1, 20) 16 | gen.add("min_area", double_t, 0, 17 | "contour min area", 40, 1, 1000) 18 | gen.add("max_area", double_t, 0, 19 | "contour max area", 200, 100, 10000) 20 | gen.add("thresh_window", int_t, 0, 21 | "adaptive threshold window size", 11, 3, 100) 22 | gen.add("erosion_size", int_t, 0, 23 | "erosion size", 1, 1, 10) 24 | size_enum = gen.enum([gen.const("mean", int_t, 0, "Adaptive thresh mean"), 25 | gen.const("gaussian", int_t, 1, "Adaptive thresh gaussian")], 26 | "Adaptive thresh type ") 27 | gen.add("thresh_type", int_t, 0, 28 | "Adaptive thresh type", 0, 0, 1, edit_method=size_enum) 29 | 30 | exit(gen.generate(PACKAGE, "flir_gige", "CalibProcDyn")) 31 | -------------------------------------------------------------------------------- /cfg/FlirGigeDyn.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "flir_gige" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("fps", double_t, 0, "frame per second", 20, 1, 60) 9 | gen.add("raw", bool_t, 0, "Raw 14-bit data", False) 10 | gen.add("nuc_action", bool_t, 0, "Non-uniform image correction", False) 11 | nuc_enum = gen.enum([gen.const("nuc_manual", int_t, 0, "Manual"), 12 | gen.const("nuc_automatic", int_t, 1, "Automatic")], 13 | "An enum to set nuc mode") 14 | gen.add("nuc_mode", int_t, 0, "Non-uniform correction mode", 0, 0, 1, edit_method=nuc_enum) 15 | exit(gen.generate(PACKAGE, "flir_gige", "FlirGigeDyn")) 16 | -------------------------------------------------------------------------------- /cfg/ThermalProcDyn.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "flir_gige" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # name / type / level / description / default / min / max 9 | # Flir A35 has a maximum resolution of 320 x 256 10 | gen.add("celsius_min", double_t, 0, "min temperature", 20, 0, 70) 11 | gen.add("celsius_max", double_t, 0, "max temperature", 40, 0, 70) 12 | 13 | exit(gen.generate(PACKAGE, "flir_gige", "ThermalProcDyn")) 14 | -------------------------------------------------------------------------------- /cmake/FindEbus.cmake: -------------------------------------------------------------------------------- 1 | # FindEbus.cmake - Find ebus sdk, version >= 4. 2 | # quchao@seas.upenn.edu (Chao Qu) 3 | # Modified from FindEigen.cmake by alexs.mac@gmail.com (Alex Stewart) 4 | # 5 | # This module defines the following variables: 6 | # 7 | # Ebus_FOUND: TRUE if ebus is found. 8 | # Ebus_INCLUDE_DIRS: Include directories for ebus. 9 | # Ebus_LIBRARIES: Libraries for all ebus component libraries 10 | # and dependencies. 11 | # 12 | # Ebus_VERSION: Extracted from lib/PvBase.so.x.y.z 13 | # Ebus_WORLD_VERSION: Equal to 4 if Ebus_VERSION = 4.0.5 14 | # Ebus_MAJOR_VERSION: Equal to 0 if Ebus_VERSION = 4.0.5 15 | # Ebus_MINOR_VERSION: Equal to 5 if Ebus_VERSION = 4.0.5 16 | # 17 | # The following variables control the behaviour of this module: 18 | # 19 | # Ebus_INCLUDE_DIR_HINTS: List of additional directories in which to 20 | # search for ebus includes, e.g: /foo/include. 21 | # Ebus_LIBRARY_DIR_HINTS: List of additional directories in which to 22 | # search for ebus libraries, e.g: /bar/lib. 23 | # 24 | # The following variables are also defined by this module, but in line with 25 | # CMake recommended FindPackage() module style should NOT be referenced directly 26 | # by callers (use the plural variables detailed above instead). These variables 27 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 28 | # are NOT re-called (i.e. search for library is not repeated) if these variables 29 | # are set with valid values _in the CMake cache_. This means that if these 30 | # variables are set directly in the cache, either by the user in the CMake GUI, 31 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 32 | # explicitly defines a cache variable), then they will be used verbatim, 33 | # bypassing the HINTS variables and other hard-coded search locations. 34 | # 35 | # Ebus_INCLUDE_DIR: Include directory for ebus, not including the 36 | # include directory of any dependencies. 37 | # Ebus_LIBRARY: ebus library, not including the libraries of any 38 | # dependencies. 39 | 40 | # Called if we failed to find ebus or any of it's required dependencies, 41 | # unsets all public (designed to be used externally) variables and reports 42 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 43 | macro(Ebus_REPORT_NOT_FOUND REASON_MSG) 44 | unset(Ebus_FOUND) 45 | unset(Ebus_INCLUDE_DIRS) 46 | unset(Ebus_LIBRARIES) 47 | unset(Ebus_WORLD_VERSION) 48 | unset(Ebus_MAJOR_VERSION) 49 | unset(Ebus_MINOR_VERSION) 50 | # Make results of search visible in the CMake GUI if ebus has not 51 | # been found so that user does not have to toggle to advanced view. 52 | mark_as_advanced(CLEAR Ebus_INCLUDE_DIR) 53 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 54 | # use the camelcase library name, not uppercase. 55 | if(Ebus_FIND_QUIETLY) 56 | message(STATUS "Failed to find ebus - " ${REASON_MSG} ${ARGN}) 57 | elseif(Ebus_FIND_REQUIRED) 58 | message(FATAL_ERROR "Failed to find ebus - " ${REASON_MSG} ${ARGN}) 59 | else() 60 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 61 | # but continues configuration and allows generation. 62 | message("-- Failed to find ebus - " ${REASON_MSG} ${ARGN}) 63 | endif() 64 | endmacro(Ebus_REPORT_NOT_FOUND) 65 | 66 | # Search user-installed locations first, so that we prefer user installs 67 | # to system installs where both exist. 68 | list(APPEND Ebus_CHECK_INCLUDE_DIRS 69 | /opt/pleora/ebus_sdk/Ubuntu-12.04-x86_64/include) 70 | list(APPEND Ebus_CHECK_LIBRARY_DIRS 71 | /opt/pleora/ebus_sdk/Ubuntu-12.04-x86_64/lib) 72 | 73 | # Check general hints 74 | if(Ebus_HINTS AND EXISTS ${Ebus_HINTS}) 75 | set(Ebus_INCLUDE_DIR_HINTS ${Ebus_HINTS}/include) 76 | set(Ebus_LIBRARY_DIR_HINTS ${Ebus_HINTS}/lib) 77 | endif() 78 | 79 | # Search supplied hint directories first if supplied. 80 | # Find include directory for ebus 81 | find_path(Ebus_INCLUDE_DIR 82 | NAMES PvBase.h 83 | PATHS ${Ebus_INCLUDE_DIR_HINTS} 84 | ${Ebus_CHECK_INCLUDE_DIRS} 85 | NO_DEFAULT_PATH) 86 | if(NOT Ebus_INCLUDE_DIR OR NOT EXISTS ${Ebus_INCLUDE_DIR}) 87 | Ebus_REPORT_NOT_FOUND( 88 | "Could not find ebus include directory, set Ebus_INCLUDE_DIR to " 89 | "path to ebus include directory," 90 | "e.g. /opt/pleora/ebus_sdk/Ubuntu-12.04-x86_64/include.") 91 | else() 92 | message(STATUS "ebus include dir found: " ${Ebus_INCLUDE_DIR}) 93 | endif() 94 | 95 | # Find library directory for ebus 96 | find_library(Ebus_LIBRARY 97 | NAMES PvBase 98 | PATHS ${Ebus_LIBRARY_DIR_HINTS} 99 | ${Ebus_CHECK_LIBRARY_DIRS} 100 | NO_DEFAULT_PATH) 101 | if(NOT Ebus_LIBRARY OR NOT EXISTS ${Ebus_LIBRARY}) 102 | Ebus_REPORT_NOT_FOUND( 103 | "Could not find ebus library, set Ebus_LIBRARY " 104 | "to full path to ebus library direcotory.") 105 | else() 106 | # TODO: need to fix this hacky solution for getting Ebus_LIBRARY_DIR 107 | string(REGEX MATCH ".*/" Ebus_LIBRARY_DIR ${Ebus_LIBRARY}) 108 | endif() 109 | 110 | # Mark internally as found, then verify. Ebus_REPORT_NOT_FOUND() unsets if 111 | # called. 112 | set(Ebus_FOUND TRUE) 113 | 114 | # Extract ebus version from ebus_sdk/Ubuntu-12.04-x86_64/lib/libPvBase.so.x.y.z 115 | if(Ebus_LIBRARY_DIR) 116 | file(GLOB Ebus_PVBASE 117 | RELATIVE ${Ebus_LIBRARY_DIR} 118 | ${Ebus_LIBRARY_DIR}/libPvBase.so.[0-9].[0-9].[0-9]) 119 | # TODO: add version support 120 | # string(REGEX MATCH "" 121 | # Ebus_WORLD_VERSION ${Ebus_PVBASE}) 122 | # message(STATUS "ebus world version: " ${Ebus_WORLD_VERSION}) 123 | endif() 124 | 125 | # Catch case when caller has set Ebus_INCLUDE_DIR in the cache / GUI and 126 | # thus FIND_[PATH/LIBRARY] are not called, but specified locations are 127 | # invalid, otherwise we would report the library as found. 128 | if(Ebus_INCLUDE_DIR AND NOT EXISTS ${Ebus_INCLUDE_DIR}/PvBase.h) 129 | Ebus_REPORT_NOT_FOUND("Caller defined Ebus_INCLUDE_DIR: " 130 | ${Ebus_INCLUDE_DIR} 131 | " does not contain PvBase.h header.") 132 | endif() 133 | 134 | # Set standard CMake FindPackage variables if found. 135 | if(Ebus_FOUND) 136 | set(Ebus_INCLUDE_DIRS ${Ebus_INCLUDE_DIR}) 137 | file(GLOB Ebus_LIBRARIES ${Ebus_LIBRARY_DIR}libPv*.so) 138 | endif() 139 | 140 | # Handle REQUIRED / QUIET optional arguments. 141 | include(FindPackageHandleStandardArgs) 142 | if(Ebus_FOUND) 143 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(Ebus DEFAULT_MSG 144 | Ebus_INCLUDE_DIRS Ebus_LIBRARIES) 145 | endif() 146 | 147 | # Only mark internal variables as advanced if we found ebus, otherwise 148 | # leave it visible in the standard GUI for the user to set manually. 149 | if(Ebus_FOUND) 150 | mark_as_advanced(FORCE Ebus_INCLUDE_DIR Ebus_LIBRARY) 151 | endif() 152 | -------------------------------------------------------------------------------- /include/calib_proc/calib_proc_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIR_GIGE_CALIB_PROC_NODE_H_ 2 | #define FLIR_GIGE_CALIB_PROC_NODE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace flir_gige { 15 | 16 | class CalibProcNode { 17 | public: 18 | CalibProcNode(const ros::NodeHandle &nh, const ros::NodeHandle &pnh); 19 | 20 | private: 21 | void ImageCb(const sensor_msgs::ImageConstPtr &image_msg); 22 | void ConnectCb(); 23 | void ConfigCb(CalibProcDynConfig &config, int level); 24 | 25 | ros::NodeHandle nh_; 26 | ros::NodeHandle pnh_; 27 | image_transport::ImageTransport it_; 28 | image_transport::Subscriber sub_image_; 29 | image_transport::Publisher pub_calib_; 30 | std::mutex connect_mutex_; 31 | dynamic_reconfigure::Server cfg_server_; 32 | CalibProcDynConfig config_; 33 | }; 34 | 35 | void DetectAndDrawCriclesGrid(const cv::Mat &src, const cv::Size &size, 36 | cv::Mat &disp); 37 | 38 | } // namespace flir_gige 39 | 40 | #endif // FLIR_GIGE_CALIB_PROC_NODE_H_ 41 | -------------------------------------------------------------------------------- /include/flir_gige/flir_gige.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIR_GIGE_H_ 2 | #define FLIR_GIGE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "flir_gige/planck.h" 22 | 23 | namespace flir_gige { 24 | 25 | struct FreeDevice { 26 | void operator()(PvDevice *device) const { PvDevice::Free(device); } 27 | }; 28 | 29 | struct FreeStream { 30 | void operator()(PvStream *stream) const { PvStream::Free(stream); } 31 | }; 32 | 33 | class FlirGige { 34 | public: 35 | FlirGige(const std::string &ip_address); 36 | 37 | const std::string &ip_address() const { return ip_address_; } 38 | const std::string &display_id() const { return display_id_; } 39 | 40 | void Connect(); 41 | void Disconnect(); 42 | void StartAcquisition(); 43 | void StopAcquisition(); 44 | void Configure(FlirGigeDynConfig &config); 45 | bool GrabImage(sensor_msgs::Image &image_msg, 46 | sensor_msgs::CameraInfo &cinfo_msg); 47 | bool GrabTemprature(sensor_msgs::Temperature &temp_msg); 48 | 49 | private: 50 | using PvDevicePtr = std::unique_ptr; 51 | using PvStreamPtr = std::unique_ptr; 52 | using PvPipelinePtr = std::unique_ptr; 53 | using PvDeviceInfoGEVVec = std::vector; 54 | 55 | bool FindDevice(const std::string &ip, 56 | const PvDeviceInfoGEVVec &dinfo_gev_vec); 57 | std::string AvailableDevice(const PvDeviceInfoGEVVec &dinfo_gev_vec) const; 58 | PvDeviceInfoGEVVec GatherGevDevice() const; 59 | 60 | void ConnectDevice(); 61 | void OpenStream(); 62 | void ConfigureStream(); 63 | void CreatePipeline(); 64 | void CacheParams(); 65 | 66 | void SetAoi(int *width, int *height) const; 67 | void SetPixelFormat(bool raw) const; 68 | void SetNucMode(int nuc) const; 69 | void DoNuc(bool& nuc) const; 70 | 71 | // double GetSpotPixel(const cv::Mat &image) const; 72 | 73 | std::string ip_address_; 74 | std::string display_id_; 75 | PvSystem system_; 76 | const PvDeviceInfo *dinfo_; 77 | PvDevicePtr device_; 78 | PvStreamPtr stream_; 79 | PvPipelinePtr pipeline_; 80 | PvGenParameterArray *param_array_; 81 | struct { 82 | int height; 83 | int width; 84 | double B, F, O, R; 85 | int bit; 86 | } cache_; 87 | }; 88 | 89 | } // namespace flir_gige 90 | 91 | #endif // FLIR_GIGE_H_ 92 | -------------------------------------------------------------------------------- /include/flir_gige/flir_gige_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIR_GIGE_NODE_H_ 2 | #define FLIR_GIGE_NODE_H_ 3 | 4 | #include "flir_gige/flir_gige_ros.h" 5 | #include "flir_gige/FlirGigeDynConfig.h" 6 | #include "camera_base/camera_node_base.h" 7 | 8 | namespace flir_gige { 9 | 10 | class FlirGigeNode : public camera_base::CameraNodeBase { 11 | public: 12 | FlirGigeNode(const ros::NodeHandle &nh) 13 | : CameraNodeBase(nh), flir_gige_ros_(nh) {} 14 | 15 | virtual void Acquire() override; 16 | virtual void Setup(FlirGigeDynConfig &config) override; 17 | 18 | private: 19 | FlirGigeRos flir_gige_ros_; 20 | }; 21 | 22 | } // namespace flir_gige 23 | 24 | #endif // FLIR_GIGE_NODE_H_ 25 | -------------------------------------------------------------------------------- /include/flir_gige/flir_gige_ros.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIR_GIGE_ROS_H_ 2 | #define FLIR_GIGE_ROS_H_ 3 | 4 | #include "flir_gige/flir_gige.h" 5 | #include "camera_base/camera_ros_base.h" 6 | 7 | #include 8 | 9 | namespace flir_gige { 10 | 11 | class FlirGigeRos : public camera_base::CameraRosBase { 12 | public: 13 | FlirGigeRos(const ros::NodeHandle& nh) 14 | : CameraRosBase(nh), 15 | flir_gige_(identifier()), 16 | nh_(nh), 17 | temp_pub_(nh_.advertise("spot", 1)), 18 | temp_msg_(new sensor_msgs::Temperature()) { 19 | SetHardwareId(flir_gige_.display_id()); 20 | } 21 | 22 | FlirGige& camera() { return flir_gige_; } 23 | 24 | void Reconnect() { 25 | flir_gige_.StopAcquisition(); 26 | flir_gige_.Disconnect(); 27 | flir_gige_.Connect(); 28 | } 29 | void Start() { flir_gige_.StartAcquisition(); } 30 | 31 | virtual bool Grab(const sensor_msgs::ImagePtr& image_msg, 32 | const sensor_msgs::CameraInfoPtr& cinfo_msg) override; 33 | 34 | void PublishTemperature(const ros::Time& time); 35 | 36 | private: 37 | FlirGige flir_gige_; 38 | ros::NodeHandle nh_; 39 | ros::Publisher temp_pub_; 40 | sensor_msgs::TemperaturePtr temp_msg_; 41 | }; 42 | 43 | } // namespace flir_gige 44 | 45 | #endif // FLIR_GIGE_ROS_H_ 46 | -------------------------------------------------------------------------------- /include/flir_gige/planck.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIR_GIGE_PLANCK_H_ 2 | #define FLIR_GIGE_PLANCK_H_ 3 | 4 | #include 5 | 6 | namespace flir_gige { 7 | 8 | /** 9 | * @brief The Planck constants from flir thermal camera 10 | */ 11 | struct Planck { 12 | Planck() : B{0}, F{0}, O{0}, R{0} {} 13 | Planck(double B, double F, double O, double R) : B{B}, F{F}, O{O}, R{R} {} 14 | 15 | double B; 16 | double F; 17 | double O; 18 | double R; 19 | static constexpr double kT0{273.15}; ///< Kelvin at 0 celcius 20 | 21 | /** 22 | * @brief CelsiusToRaw Convert celsius to 16-bit raw data 23 | * @param t Celcius 24 | * @return raw data 25 | */ 26 | int CelsiusToRaw(const double t) const { 27 | return R / (std::exp(B / (t + kT0)) - F) + O; 28 | } 29 | 30 | /** 31 | * @brief RawToCelsius Convert 16-bit raw data to celsius 32 | * @param s Raw data 33 | * @return temperature in celsius 34 | */ 35 | double RawToCelsius(const int s) const { 36 | return B / std::log(R / (s - O) + F) - kT0; 37 | } 38 | }; 39 | 40 | } // namespace flir_gige 41 | 42 | #endif // FLIR_GIGE_PLANCK_H_ 43 | -------------------------------------------------------------------------------- /include/thermal_proc/thermal_proc_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIR_GIGE_THERMAL_PROC_NODE_H_ 2 | #define FLIR_GIGE_THERMAL_PROC_NODE_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "flir_gige/planck.h" 20 | #include "flir_gige/ThermalProcDynConfig.h" 21 | 22 | namespace flir_gige { 23 | 24 | class ThermalProcNode { 25 | public: 26 | ThermalProcNode(const ros::NodeHandle &nh, const ros::NodeHandle &pnh); 27 | 28 | private: 29 | void CameraCb(const sensor_msgs::ImageConstPtr &image_msg, 30 | const sensor_msgs::CameraInfoConstPtr &cinfo_msg); 31 | void ConfigCb(ThermalProcDynConfig &config, int level); 32 | void ConnectCb(); 33 | 34 | void RawToJet(const cv::Mat &raw, const Planck &planck, cv::Mat *color) const; 35 | 36 | ros::NodeHandle nh_, pnh_; 37 | image_transport::ImageTransport it_; 38 | image_transport::CameraSubscriber sub_camera_; 39 | image_transport::Publisher pub_proc_; 40 | dynamic_reconfigure::Server cfg_server_; 41 | std::mutex connect_mutex_; 42 | ThermalProcDynConfig config_; 43 | }; 44 | 45 | Planck GetPlanck(const sensor_msgs::CameraInfo &cinfo_msg); 46 | 47 | void RawToHeat(const cv::Mat &raw, const Planck &planck, cv::Mat *heat); 48 | } // namespace flir_gige 49 | 50 | #endif // FLIR_GIGE_THERMAL_PROC_NODE_H_ 51 | -------------------------------------------------------------------------------- /install/install.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | CURRENT_DIR=$(pwd) 4 | REPO_NAME=ebus_sdk 5 | CLONE_DIR=/tmp 6 | 7 | sudo apt-get install libudev-dev 8 | if [[ ! -L libudev.so.0 ]]; then 9 | sudo ln -s /usr/lib/x86_64-linux-gnu/libudev.so /usr/lib/libudev.so.0 10 | sudo ln -s /usr/lib/libudev.so.0 /usr/lib/libudev.so.1 11 | fi 12 | 13 | cd ${CLONE_DIR} 14 | git clone https://github.com/versatran01/${REPO_NAME} 15 | cd ${REPO_NAME} 16 | PLATFORM=$(uname -i) 17 | if [[ ${PLATFORM} == "x86_64" ]] ; then 18 | echo "Installing 64-bit version" 19 | sudo ./eBUS_SDK_4.0.6.3228_Ubuntu-12.04-x86_64.run 20 | else 21 | echo "Platform ${PLATFORM} not supported" 22 | fi 23 | 24 | echo "Cleaning up..." 25 | cd ${CLONE_DIR} 26 | rm -rf ${REPO_NAME} 27 | cd ${CURRENT_DIR} 28 | echo "Done." 29 | 30 | -------------------------------------------------------------------------------- /install/set_puregev_gen: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | export PUREGEV_ROOT=/opt/pleora/ebus_sdk/Ubuntu-12.04-x86_64 4 | export GENICAM_ROOT=$PUREGEV_ROOT/lib/genicam 5 | export GENICAM_ROOT_V2_4=$GENICAM_ROOT 6 | export GENICAM_LOG_CONFIG=$GENICAM_ROOT/log/config/DefaultLogging.properties 7 | export GENICAM_LOG_CONFIG_V2_4=$GENICAM_LOG_CONFIG 8 | if [ "$HOME" = "/" ]; then 9 | export GENICAM_CACHE_V2_4=/.config/Pleora/genicam_cache_v2_4 10 | else 11 | export GENICAM_CACHE_V2_4=$HOME/.config/Pleora/genicam_cache_v2_4 12 | fi 13 | export GENICAM_CACHE=$GENICAM_CACHE_V2_4 14 | export GENICAM_LIB_DIR=$GENICAM_ROOT/bin/Linux64_x64 15 | mkdir -p $GENICAM_CACHE 16 | 17 | if ! echo ${LD_LIBRARY_PATH} | grep -q ${PUREGEV_ROOT}/lib; then 18 | if [ "$LD_LIBRARY_PATH" = "" ]; then 19 | LD_LIBRARY_PATH=${PUREGEV_ROOT}/lib 20 | else 21 | LD_LIBRARY_PATH=${PUREGEV_ROOT}/lib:${LD_LIBRARY_PATH} 22 | fi 23 | fi 24 | 25 | if ! echo ${LD_LIBRARY_PATH} | grep -q ${GENICAM_LIB_DIR}; then 26 | LD_LIBRARY_PATH=${GENICAM_LIB_DIR}:${LD_LIBRARY_PATH} 27 | fi 28 | 29 | export LD_LIBRARY_PATH 30 | 31 | if ! echo ${PATH} | grep -q ${PUREGEV_ROOT}/bin; then 32 | PATH=${PUREGEV_ROOT}/bin:${PATH} 33 | fi 34 | 35 | export PATH 36 | 37 | unset GENICAM_LIB_DIR 38 | 39 | -------------------------------------------------------------------------------- /launch/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 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 38 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /launch/nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 28 | 31 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /launch/thermal_proc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /math/conversion.py: -------------------------------------------------------------------------------- 1 | from math import exp, log 2 | 3 | def raw_to_celsius(S, B, F, O, R, T0): 4 | return (B / log(R / (S - O) + F) - T0) 5 | 6 | def celsius_to_raw(t, B, F, O, R, T0): 7 | return ((R / exp(B / (t + T0)) - F) + O) 8 | 9 | B = 1428.0 10 | F = 1.0 11 | O = 118.126 12 | R = 377312.0 13 | T0 = 273.15 14 | 15 | celsius = [10, 20, 30, 40, 50] 16 | raw = []; 17 | celcius_converted = [] 18 | 19 | for t in celsius: 20 | raw.append(celsius_to_raw(t, B, F, O, R, T0)) 21 | 22 | for s in raw: 23 | celcius_converted.append(raw_to_celsius(s, B, F, O, R, T0)) 24 | 25 | for t in celcius_converted: 26 | print(t) 27 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | This is a nodelet for the Flir thermal camera 6 | 7 | 8 | 10 | 11 | This is a nodelet for the thermal image processing 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_gige 4 | 0.0.1 5 | The flir_gige package 6 | 7 | Chao Qu 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | nodelet 15 | cv_bridge 16 | camera_base 17 | 18 | roscpp 19 | nodelet 20 | cv_bridge 21 | camera_base 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/calib_proc/calib_proc_main.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_proc/calib_proc_node.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "calib_proc"); 5 | ros::NodeHandle nh; 6 | ros::NodeHandle pnh("~"); 7 | 8 | try { 9 | flir_gige::CalibProcNode calib_proc_node(nh, pnh); 10 | ros::spin(); 11 | } 12 | catch (const std::exception &e) { 13 | ROS_ERROR("%s: %s", nh.getNamespace().c_str(), e.what()); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/calib_proc/calib_proc_node.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_proc/calib_proc_node.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace flir_gige { 10 | 11 | CalibProcNode::CalibProcNode(const ros::NodeHandle &nh, 12 | const ros::NodeHandle &pnh) 13 | : nh_(nh), pnh_(pnh), it_(nh), cfg_server_(pnh) { 14 | image_transport::SubscriberStatusCallback connect_cb = 15 | boost::bind(&CalibProcNode::ConnectCb, this); 16 | pub_calib_ = it_.advertise("image_calib", 1, connect_cb, connect_cb); 17 | cfg_server_.setCallback(boost::bind(&CalibProcNode::ConfigCb, this, _1, _2)); 18 | } 19 | 20 | void CalibProcNode::ConnectCb() { 21 | std::lock_guard lock(connect_mutex_); 22 | if (!pub_calib_.getNumSubscribers()) { 23 | sub_image_.shutdown(); 24 | } else if (!sub_image_) { 25 | image_transport::TransportHints hints("raw", ros::TransportHints(), nh_); 26 | sub_image_ = 27 | it_.subscribe("image_raw", 2, &CalibProcNode::ImageCb, this, hints); 28 | } 29 | } 30 | 31 | void CalibProcNode::ConfigCb(CalibProcDynConfig &config, int level) { 32 | if (level < 0) { 33 | ROS_INFO("%s: initialize dynamic reconfigure server", 34 | pnh_.getNamespace().c_str()); 35 | } 36 | if (!(config.thresh_window % 2)) { 37 | config.thresh_window += 1; 38 | } 39 | if (config.min_area > config.max_area) { 40 | config.max_area = config.min_area + 1; 41 | } 42 | config_ = config; 43 | } 44 | 45 | void CalibProcNode::ImageCb(const sensor_msgs::ImageConstPtr &image_msg) { 46 | cv::Mat image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; 47 | 48 | cv::Mat inverted; 49 | cv::bitwise_not(image, inverted); 50 | /* 51 | // Threshold 52 | cv::Mat thresh; 53 | cv::adaptiveThreshold(image, thresh, 255, config_.thresh_type, 54 | cv::THRESH_BINARY, config_.thresh_window, 0); 55 | cv::imshow("thresh", thresh); 56 | 57 | // Gaussian blur 58 | cv::Mat gauss; 59 | cv::GaussianBlur(thresh, gauss, cv::Size(), config_.sigma, config_.sigma, 60 | cv::BORDER_DEFAULT); 61 | cv::imshow("gauss", gauss); 62 | 63 | // Erosion 64 | cv::Mat eroded; 65 | int erosion_size = config_.erosion_size; 66 | cv::Mat element = cv::getStructuringElement( 67 | cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), 68 | cv::Point(erosion_size, erosion_size)); 69 | cv::erode(thresh, eroded, element); 70 | cv::imshow("erosion", eroded); 71 | 72 | // Contour 73 | std::vector> contours; 74 | cv::findContours(eroded, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); 75 | 76 | cv::Mat ctr_image; 77 | cv::cvtColor(image, ctr_image, CV_GRAY2BGR); 78 | for (size_t i = 0; i < contours.size(); ++i) { 79 | double area = cv::contourArea(contours[i]); 80 | if (area > config_.min_area && area < config_.max_area) { 81 | cv::drawContours(ctr_image, contours, i, cv::Scalar(255, 0, 0), 2, 8); 82 | } 83 | } 84 | cv::imshow("contour", ctr_image); 85 | */ 86 | 87 | // cv::Mat display; 88 | // DetectAndDrawCriclesGrid( 89 | // inverted, cv::Size(config_.circles_per_row, config_.circles_per_col), 90 | // display); 91 | 92 | // Display 93 | // cv::imshow("display", display); 94 | // cv::waitKey(1); 95 | 96 | cv::Mat calib(inverted); 97 | // Publish processed image 98 | cv_bridge::CvImage cvimg_calib(image_msg->header, image_msg->encoding, calib); 99 | pub_calib_.publish(cvimg_calib.toImageMsg()); 100 | } 101 | 102 | void DetectAndDrawCriclesGrid(const cv::Mat &src, const cv::Size &size, 103 | cv::Mat &disp) { 104 | std::vector centers; 105 | bool found = cv::findCirclesGrid(src, size, centers); 106 | if (disp.empty()) { 107 | disp = src.clone(); 108 | } 109 | if (disp.channels() == 1) { 110 | cv::cvtColor(disp, disp, CV_GRAY2BGR); 111 | } 112 | cv::drawChessboardCorners(disp, size, cv::Mat(centers), found); 113 | } 114 | } // namespace flir_gige 115 | -------------------------------------------------------------------------------- /src/flir_gige/flir_gige.cpp: -------------------------------------------------------------------------------- 1 | #include "flir_gige/flir_gige.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace flir_gige { 14 | 15 | FlirGige::FlirGige(const std::string &ip_address) 16 | : ip_address_{ip_address}, dinfo_{nullptr}, param_array_{nullptr} { 17 | // Find all devices on the network 18 | const PvResult result = system_.Find(); 19 | if (!result.IsOK()) { 20 | throw std::runtime_error(std::string("PvSystem::Find Error: ") + 21 | result.GetCodeString().GetAscii()); 22 | } 23 | const PvDeviceInfoGEVVec dinfo_gev_vec = GatherGevDevice(); 24 | if (!FindDevice(ip_address, dinfo_gev_vec)) { 25 | throw std::runtime_error(ip_address + 26 | " not found. Available IP Address(es): " + 27 | AvailableDevice(dinfo_gev_vec)); 28 | } 29 | } 30 | 31 | void FlirGige::Connect() { 32 | ConnectDevice(); 33 | OpenStream(); 34 | ConfigureStream(); 35 | CreatePipeline(); 36 | } 37 | 38 | void FlirGige::Disconnect() { 39 | pipeline_.reset(); 40 | stream_.reset(); 41 | device_.reset(); 42 | param_array_ = nullptr; 43 | } 44 | 45 | void FlirGige::StartAcquisition() { 46 | // Note: the pipeline must be initialized before we start acquisition 47 | pipeline_->Start(); 48 | device_->StreamEnable(); 49 | param_array_->ExecuteCommand("AcquisitionStart"); 50 | CacheParams(); 51 | } 52 | 53 | void FlirGige::StopAcquisition() { 54 | // Get device parameters need to control streaming 55 | if (!param_array_) return; 56 | param_array_->ExecuteCommand("AcquisitionStop"); 57 | device_->StreamDisable(); 58 | pipeline_->Stop(); 59 | } 60 | 61 | void FlirGige::Configure(FlirGigeDynConfig &config) { 62 | SetPixelFormat(config.raw); 63 | SetNucMode(config.nuc_mode); 64 | DoNuc(config.nuc_action); 65 | } 66 | 67 | FlirGige::PvDeviceInfoGEVVec FlirGige::GatherGevDevice() const { 68 | const int interface_cnt = system_.GetInterfaceCount(); 69 | 70 | // Go through all interfaces, but we only care about network interface 71 | // For other interfaces such as usb, refer to sample code DeviceFinder.cpp 72 | std::vector dinfo_gev_vec; 73 | for (int i = 0; i < interface_cnt; ++i) { 74 | // Get pointer to the interface 75 | const PvInterface *interface = system_.GetInterface(i); 76 | // Is it a PvNetworkAdapter? 77 | const auto *nic = dynamic_cast(interface); 78 | if (nic) { 79 | // ROS_INFO("Interface: %s", interface->GetDisplayID().GetAscii()); 80 | // Go through all the devices attached to the network interface 81 | const int dev_cnt = interface->GetDeviceCount(); 82 | for (int j = 0; j < dev_cnt; ++j) { 83 | const PvDeviceInfo *dinfo = interface->GetDeviceInfo(j); 84 | // Is it a GigE Vision device? 85 | const auto *dinfo_gev = dynamic_cast(dinfo); 86 | if (dinfo_gev) { 87 | dinfo_gev_vec.push_back(dinfo_gev); 88 | } 89 | } 90 | } 91 | } 92 | return dinfo_gev_vec; 93 | } 94 | 95 | bool FlirGige::FindDevice(const std::string &ip, 96 | const PvDeviceInfoGEVVec &dinfo_gev_vec) { 97 | // Check GigE devices found on network adaptor 98 | if (dinfo_gev_vec.empty()) return false; 99 | 100 | // Try finding the device with the correct ip address 101 | const auto it = std::find_if(dinfo_gev_vec.cbegin(), dinfo_gev_vec.cend(), 102 | [&ip](const PvDeviceInfoGEV *dinfo) { 103 | return ip == dinfo->GetIPAddress().GetAscii(); 104 | }); 105 | 106 | if (it == dinfo_gev_vec.end()) return false; 107 | // Found device with given ip address 108 | const PvDeviceInfoGEV *dinfo_gev = *it; 109 | display_id_ = std::string(dinfo_gev->GetDisplayID().GetAscii()); 110 | 111 | if (!dinfo_gev->IsConfigurationValid()) return false; 112 | // Try connect and disconnect to verify 113 | dinfo_ = dinfo_gev; 114 | PvResult result; 115 | 116 | // Creates and connects the device controller 117 | PvDevice *device = PvDevice::CreateAndConnect(dinfo_, &result); 118 | if (!result.IsOK()) return false; 119 | PvDevice::Free(device); 120 | return true; 121 | } 122 | 123 | std::string FlirGige::AvailableDevice( 124 | const PvDeviceInfoGEVVec &dinfo_gev_vec) const { 125 | std::string devices; 126 | for (const PvDeviceInfoGEV *dinfo : dinfo_gev_vec) { 127 | devices += dinfo->GetIPAddress().GetAscii() + std::string(" "); 128 | } 129 | return devices; 130 | } 131 | 132 | void FlirGige::ConnectDevice() { 133 | PvResult result; 134 | // Use a unique_ptr to manage device resource 135 | device_.reset(PvDevice::CreateAndConnect(dinfo_, &result)); 136 | if (!result.IsOK()) { 137 | throw std::runtime_error("Unable to connect to " + display_id()); 138 | } 139 | param_array_ = device_->GetParameters(); 140 | } 141 | 142 | void FlirGige::OpenStream() { 143 | PvResult result; 144 | // Use a unique_ptr to manage stream resource 145 | stream_.reset(PvStream::CreateAndOpen(dinfo_->GetConnectionID(), &result)); 146 | if (!stream_) { 147 | throw std::runtime_error("Unable to stream from " + display_id()); 148 | } 149 | } 150 | 151 | void FlirGige::ConfigureStream() { 152 | // If this is a GigE Vision devie, configure GigE Vision specific parameters 153 | auto *device_gev = dynamic_cast(device_.get()); 154 | if (!device_gev) { 155 | throw std::runtime_error("Not a GigE vision device " + display_id()); 156 | } 157 | auto *stream_gev = static_cast(stream_.get()); 158 | // Negotiate packet size 159 | device_gev->NegotiatePacketSize(); 160 | // Configure device streaming destination 161 | device_gev->SetStreamDestination(stream_gev->GetLocalIPAddress(), 162 | stream_gev->GetLocalPort()); 163 | } 164 | 165 | void FlirGige::CreatePipeline() { 166 | pipeline_.reset(new PvPipeline(stream_.get())); 167 | const auto payload_size = device_->GetPayloadSize(); 168 | // Set the Buffer count and the Buffer size 169 | // BufferCount should be at least 4 170 | pipeline_->SetBufferCount(4); 171 | pipeline_->SetBufferSize(payload_size); 172 | } 173 | 174 | bool FlirGige::GrabImage(sensor_msgs::Image &image_msg, 175 | sensor_msgs::CameraInfo &cinfo_msg) { 176 | static bool skip_next_frame = false; 177 | 178 | // Start loop for acquisition 179 | PvBuffer *buffer; 180 | PvResult op_result; 181 | 182 | // Skip next frame when operation is not ok 183 | if (skip_next_frame) { 184 | skip_next_frame = false; 185 | sleep(1); 186 | } 187 | 188 | // Retrieve next buffer 189 | PvResult result = pipeline_->RetrieveNextBuffer(&buffer, 1000, &op_result); 190 | 191 | // Failed to retrieve buffer 192 | if (result.IsFailure()) { 193 | return false; 194 | } 195 | 196 | // Operation not ok, need to return buffer back to pipeline 197 | if (op_result.IsFailure()) { 198 | skip_next_frame = true; 199 | // Release the buffer back to the pipeline 200 | pipeline_->ReleaseBuffer(buffer); 201 | return false; 202 | } 203 | 204 | // Buffer is not an image 205 | if ((buffer->GetPayloadType()) != PvPayloadTypeImage) { 206 | pipeline_->ReleaseBuffer(buffer); 207 | return false; 208 | } 209 | 210 | // Get image specific buffer interface 211 | PvImage *image = buffer->GetImage(); 212 | 213 | // Get device parameters need to control streaming 214 | // Assemble cinfo msg 215 | cinfo_msg.R[0] = cache_.B; 216 | cinfo_msg.R[1] = cache_.F; 217 | cinfo_msg.R[2] = cache_.O; 218 | cinfo_msg.R[3] = cache_.R; 219 | 220 | // Assemble image msg 221 | image_msg.height = cache_.height; 222 | image_msg.width = cache_.width; 223 | if (cache_.bit == 2) { 224 | image_msg.encoding = sensor_msgs::image_encodings::MONO8; 225 | image_msg.step = image_msg.width; 226 | } else { 227 | image_msg.encoding = sensor_msgs::image_encodings::MONO16; 228 | image_msg.step = image_msg.width * 2; 229 | } 230 | 231 | const size_t data_size = image->GetImageSize(); 232 | if (image_msg.data.size() != data_size) { 233 | image_msg.data.resize(data_size); 234 | } 235 | memcpy(&image_msg.data[0], image->GetDataPointer(), image->GetImageSize()); 236 | 237 | // Release the buffer back to the pipeline 238 | pipeline_->ReleaseBuffer(buffer); 239 | return true; 240 | } 241 | 242 | void FlirGige::CacheParams() { 243 | int64_t width, height; 244 | param_array_->GetIntegerValue("Width", width); 245 | param_array_->GetIntegerValue("Height", height); 246 | 247 | int64_t bit; 248 | param_array_->GetEnumValue("DigitalOutput", bit); 249 | 250 | int64_t R; 251 | double F, B, O; 252 | param_array_->GetIntegerValue("R", R); 253 | param_array_->GetFloatValue("F", F); 254 | param_array_->GetFloatValue("B", B); 255 | param_array_->GetFloatValue("O", O); 256 | 257 | cache_.B = B; 258 | cache_.F = F; 259 | cache_.O = O; 260 | cache_.R = R; 261 | cache_.height = height; 262 | cache_.width = width; 263 | cache_.bit = bit; 264 | } 265 | 266 | bool FlirGige::GrabTemprature(sensor_msgs::Temperature &temp_msg) { 267 | temp_msg.variance = 0; 268 | return param_array_->GetFloatValue("Spot", temp_msg.temperature).IsOK(); 269 | } 270 | 271 | // This function is not intended to be used 272 | void FlirGige::SetAoi(int *width, int *height) const { 273 | // Get current width and height 274 | int64_t curr_width = 0; 275 | int64_t curr_height = 0; 276 | param_array_->GetIntegerValue("Width", curr_width); 277 | param_array_->GetIntegerValue("Height", curr_height); 278 | // Check to see if it's necessary to change width and height 279 | if (curr_width != *width) { 280 | param_array_->SetIntegerValue("Width", *width); 281 | } 282 | if (curr_height != *height) { 283 | param_array_->SetIntegerValue("Height", *height); 284 | } 285 | } 286 | 287 | void FlirGige::SetPixelFormat(bool raw) const { 288 | // Set digital output and pixel format 289 | if (raw) { 290 | param_array_->SetEnumValue("PixelFormat", PvPixelMono14); 291 | param_array_->SetEnumValue("DigitalOutput", static_cast(3)); 292 | } else { 293 | param_array_->SetEnumValue("PixelFormat", PvPixelMono8); 294 | param_array_->SetEnumValue("DigitalOutput", static_cast(2)); 295 | } 296 | } 297 | 298 | void FlirGige::SetNucMode(int nuc) const { 299 | param_array_->SetEnumValue("NUCMode", static_cast(nuc)); 300 | } 301 | 302 | void FlirGige::DoNuc(bool &nuc) const { 303 | if (nuc) { 304 | param_array_->ExecuteCommand("NUCAction"); 305 | nuc = false; 306 | } 307 | } 308 | 309 | // double FlirGige::GetSpotPixel(const cv::Mat &image) const { 310 | // auto c = image.cols / 2; 311 | // auto r = image.rows / 2; 312 | // auto s1 = image.at(r - 1, c - 1); 313 | // auto s2 = image.at(r - 1, c); 314 | // auto s3 = image.at(r, c - 1); 315 | // auto s4 = image.at(r, c); 316 | // return static_cast(s1 / 4 + s2 / 4 + s3 / 4 + s4 / 4); 317 | //} 318 | 319 | } // namespace flir_gige 320 | -------------------------------------------------------------------------------- /src/flir_gige/flir_gige_main.cpp: -------------------------------------------------------------------------------- 1 | #include "flir_gige/flir_gige_node.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "flir_gige_node"); 5 | ros::NodeHandle nh("~"); 6 | 7 | try { 8 | flir_gige::FlirGigeNode flir_gige_node(nh); 9 | flir_gige_node.Run(); 10 | ros::spin(); 11 | flir_gige_node.End(); 12 | } 13 | catch (const std::exception &e) { 14 | ROS_ERROR("%s: %s", nh.getNamespace().c_str(), e.what()); 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /src/flir_gige/flir_gige_node.cpp: -------------------------------------------------------------------------------- 1 | #include "flir_gige/flir_gige_node.h" 2 | 3 | namespace flir_gige { 4 | 5 | void FlirGigeNode::Setup(FlirGigeDynConfig &config) { 6 | flir_gige_ros_.set_fps(config.fps); 7 | flir_gige_ros_.Reconnect(); 8 | flir_gige_ros_.camera().Configure(config); 9 | flir_gige_ros_.Start(); 10 | } 11 | 12 | void FlirGigeNode::Acquire() { 13 | while (is_acquire() && ros::ok()) { 14 | const ros::Time time = ros::Time::now(); 15 | flir_gige_ros_.PublishCamera(time); 16 | flir_gige_ros_.PublishTemperature(time); 17 | Sleep(); 18 | } 19 | } 20 | 21 | } // namespace flir_gige 22 | -------------------------------------------------------------------------------- /src/flir_gige/flir_gige_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "flir_gige/flir_gige_node.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace flir_gige { 7 | 8 | class FlirGigeNodelet : public nodelet::Nodelet { 9 | public: 10 | FlirGigeNodelet() = default; 11 | ~FlirGigeNodelet() { 12 | if (flir_gige_node_) { 13 | flir_gige_node_->End(); 14 | } 15 | } 16 | 17 | virtual void onInit() { 18 | try { 19 | flir_gige_node_.reset(new FlirGigeNode(getPrivateNodeHandle())); 20 | flir_gige_node_->Run(); 21 | } 22 | catch (const std::exception &e) { 23 | NODELET_ERROR("%s: %s", getPrivateNodeHandle().getNamespace().c_str(), 24 | e.what()); 25 | } 26 | } 27 | 28 | private: 29 | std::unique_ptr flir_gige_node_; 30 | }; 31 | 32 | PLUGINLIB_EXPORT_CLASS(flir_gige::FlirGigeNodelet, nodelet::Nodelet) 33 | 34 | } // namespace flir_gige 35 | -------------------------------------------------------------------------------- /src/flir_gige/flir_gige_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "flir_gige/flir_gige_ros.h" 2 | 3 | namespace flir_gige { 4 | 5 | bool FlirGigeRos::Grab(const sensor_msgs::ImagePtr& image_msg, 6 | const sensor_msgs::CameraInfoPtr& cinfo_msg) { 7 | return flir_gige_.GrabImage(*image_msg, *cinfo_msg); 8 | } 9 | 10 | void FlirGigeRos::PublishTemperature(const ros::Time& time) { 11 | if (flir_gige_.GrabTemprature(*temp_msg_)) { 12 | temp_msg_->header.stamp = time; 13 | temp_msg_->header.frame_id = frame_id(); 14 | temp_pub_.publish(temp_msg_); 15 | } 16 | } 17 | 18 | } // namespace flir_gige 19 | -------------------------------------------------------------------------------- /src/thermal_proc/thermal_proc_main.cpp: -------------------------------------------------------------------------------- 1 | #include "thermal_proc/thermal_proc_node.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "thermal_proc"); 5 | ros::NodeHandle nh; 6 | ros::NodeHandle pnh("~"); 7 | 8 | try { 9 | flir_gige::ThermalProcNode thermal_proc_node(nh, pnh); 10 | ros::spin(); 11 | } 12 | catch (const std::exception &e) { 13 | ROS_ERROR("%s: %s", nh.getNamespace().c_str(), e.what()); 14 | } 15 | } 16 | -------------------------------------------------------------------------------- /src/thermal_proc/thermal_proc_node.cpp: -------------------------------------------------------------------------------- 1 | #include "thermal_proc/thermal_proc_node.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace flir_gige { 9 | 10 | ThermalProcNode::ThermalProcNode(const ros::NodeHandle &nh, 11 | const ros::NodeHandle &pnh) 12 | : nh_(nh), pnh_(pnh), it_(nh), cfg_server_(pnh) { 13 | image_transport::SubscriberStatusCallback connect_cb = 14 | boost::bind(&ThermalProcNode::ConnectCb, this); 15 | pub_proc_ = it_.advertise("image_proc", 1, connect_cb, connect_cb); 16 | 17 | cfg_server_.setCallback( 18 | boost::bind(&ThermalProcNode::ConfigCb, this, _1, _2)); 19 | } 20 | 21 | void ThermalProcNode::ConnectCb() { 22 | std::lock_guard lock(connect_mutex_); 23 | if (!pub_proc_.getNumSubscribers()) 24 | sub_camera_.shutdown(); 25 | else if (!sub_camera_) { 26 | image_transport::TransportHints hints("raw", ros::TransportHints(), nh_); 27 | sub_camera_ = it_.subscribeCamera("image_raw", 2, 28 | &ThermalProcNode::CameraCb, this, hints); 29 | } 30 | } 31 | 32 | void ThermalProcNode::ConfigCb(ThermalProcDynConfig &config, int level) { 33 | if (level < 0) { 34 | ROS_INFO("%s: %s", pnh_.getNamespace().c_str(), 35 | "Initializaing reconfigure server"); 36 | } 37 | // Make sure that max is greater than min 38 | config.celsius_max = (config.celsius_max > config.celsius_min) 39 | ? config.celsius_max 40 | : (config.celsius_min + 5); 41 | config_ = config; 42 | } 43 | 44 | void ThermalProcNode::CameraCb( 45 | const sensor_msgs::ImageConstPtr &image_msg, 46 | const sensor_msgs::CameraInfoConstPtr &cinfo_msg) { 47 | // Verify camera is actually calibrated 48 | if (cinfo_msg->K[0] == 0.0 || cinfo_msg->D[0] == 0.0) { 49 | ROS_ERROR_THROTTLE(5, 50 | "Topic '%s' requested but " 51 | "camera publishing '%s' is uncalibrated", 52 | pub_proc_.getTopic().c_str(), 53 | sub_camera_.getInfoTopic().c_str()); 54 | return; 55 | } 56 | 57 | if (pub_proc_.getNumSubscribers()) { 58 | const Planck planck = GetPlanck(*cinfo_msg); 59 | // Get image using cv_bridge 60 | cv_bridge::CvImagePtr raw_ptr = 61 | cv_bridge::toCvCopy(image_msg, image_msg->encoding); 62 | cv::Mat color; 63 | if (image_msg->encoding == sensor_msgs::image_encodings::MONO8) { 64 | // Just do a color map conversion for 8 bit 65 | cv::applyColorMap(raw_ptr->image, color, cv::COLORMAP_JET); 66 | } else if (image_msg->encoding == sensor_msgs::image_encodings::MONO16) { 67 | RawToJet(raw_ptr->image, planck, &color); 68 | } else { 69 | ROS_ERROR_THROTTLE(5, "Encoding not supported: %s", 70 | image_msg->encoding.c_str()); 71 | return; 72 | } 73 | cv_bridge::CvImage proc_cvimg(image_msg->header, 74 | sensor_msgs::image_encodings::BGR8, color); 75 | pub_proc_.publish(proc_cvimg.toImageMsg()); 76 | } 77 | } 78 | 79 | void ThermalProcNode::RawToJet(const cv::Mat &raw, const Planck &planck, 80 | cv::Mat *color) const { 81 | const int raw_min = planck.CelsiusToRaw(config_.celsius_min); 82 | const int raw_max = planck.CelsiusToRaw(config_.celsius_max); 83 | ROS_ASSERT_MSG(raw_max > raw_min, "max is less than min"); 84 | const double alpha = 255.0 / (raw_max - raw_min); 85 | const double beta = -alpha * raw_min; 86 | raw.convertTo(*color, CV_8UC1, alpha, beta); 87 | cv::applyColorMap(*color, *color, cv::COLORMAP_JET); 88 | } 89 | 90 | Planck GetPlanck(const sensor_msgs::CameraInfo &cinfo_msg) { 91 | return Planck(cinfo_msg.R[0], cinfo_msg.R[1], cinfo_msg.R[2], cinfo_msg.R[3]); 92 | } 93 | 94 | void RawToHeat(const cv::Mat &raw, const Planck &planck, cv::Mat *heat) { 95 | for (int i = 0; i < raw.rows; ++i) { 96 | float *pheat = heat->ptr(i); 97 | const uint16_t *praw = raw.ptr(i); 98 | for (int j = 0; j < raw.cols; ++j) { 99 | pheat[j] = static_cast(planck.RawToCelsius(praw[j])); 100 | } 101 | } 102 | } 103 | 104 | } // namespace flir_gige 105 | -------------------------------------------------------------------------------- /src/thermal_proc/thermal_proc_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "thermal_proc/thermal_proc_node.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace flir_gige { 10 | 11 | class ThermalProcNodelet : public nodelet::Nodelet { 12 | public: 13 | ThermalProcNodelet() : nodelet::Nodelet() {} 14 | ~ThermalProcNodelet() {} 15 | 16 | virtual void onInit() { 17 | try { 18 | thermal_proc_node_.reset( 19 | new ThermalProcNode(getPrivateNodeHandle(), getPrivateNodeHandle())); 20 | } 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 thermal_proc_node_; 29 | }; 30 | 31 | PLUGINLIB_EXPORT_CLASS(ThermalProcNodelet, nodelet::Nodelet) 32 | 33 | } // namespace flir_gige 34 | --------------------------------------------------------------------------------