FLIR相機ROS驅動

程序來源Autoware包，詳細參考Autoware包中的spinnaker相機驅動，源程序獲得圖像顏色不正確，稍作修改先轉成opencv格式再轉爲ros消息即可，直接上相關程序，另外CMakeLists.txt的配置可以參考Autoware的寫法，積累一下。

/*
 * Copyright 2015-2019 Autoware Foundation. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thread>
#include <sstream>
#include <iostream>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include "SpinGenApi/SpinnakerGenApi.h"
#include "Spinnaker.h"

using namespace Spinnaker;
using namespace Spinnaker::GenApi;
using namespace Spinnaker::GenICam;
using namespace std;

#define __APP_NAME__ "autoware_spinnaker"

class SpinnakerCamera
{
public:
  SpinnakerCamera();
  ~SpinnakerCamera();
  void spin();
  void publish_image(int);

private:
  ros::NodeHandle nh_, private_nh_;
  image_transport::ImageTransport* image_transport_;
  std::vector<image_transport::Publisher> image_pubs_;
  Spinnaker::SystemPtr system_;
  Spinnaker::CameraList camList_;
  CameraPtr* pCamList_;

  // config
  int width_;
  int height_;
  double fps_;
  int dltl_;
  std::string format_;

  Spinnaker::GenApi::INodeMap* node_map_;
};

SpinnakerCamera::SpinnakerCamera()
  : nh_()
  , private_nh_("~")
  , system_(Spinnaker::System::GetInstance())
  , camList_(system_->GetCameras())
  , width_(0)
  , height_(0)
  , fps_(0)
  , dltl_(0)
{
  private_nh_.param("width", width_, 1440);
  private_nh_.param("height", height_, 1080);
  private_nh_.param("fps", fps_, 20.0);
  private_nh_.param("dltl", dltl_, 100000000);
  unsigned int num_cameras = camList_.GetSize();
  ROS_INFO_STREAM("[" << __APP_NAME__ << "] Number of cameras detected: " << num_cameras);
  if (num_cameras < 1)
  {
    ROS_ERROR("[%s] Error: This program requires at least 1 camera.", __APP_NAME__);
    camList_.Clear();
    system_->ReleaseInstance();
    std::exit(-1);
  }
  image_transport_ = new image_transport::ImageTransport(nh_);
  pCamList_ = new CameraPtr[num_cameras];

  try
  {
    vector<gcstring> strSerialNumbers(camList_.GetSize());
    for (uint i = 0; i < camList_.GetSize(); i++)
    {
      pCamList_[i] = camList_.GetByIndex(i);
      pCamList_[i]->Init();
      node_map_ = &pCamList_[i]->GetNodeMap();
      // config
      pCamList_[i]->Width.SetValue(width_);
      pCamList_[i]->Height.SetValue(height_);
      CEnumerationPtr ptrDeviceType = pCamList_[i]->GetTLDeviceNodeMap().GetNode("DeviceType");

      if (IsAvailable(ptrDeviceType) && ptrDeviceType->GetCurrentEntry()->GetSymbolic() == "GEV")
      {
        ///////////////////////                 DeviceLinkThroughputLimit   /////////////////////////////
        CIntegerPtr ptrDeviceLinkThroughputLimit = node_map_->GetNode("DeviceLinkThroughputLimit");
        if (IsAvailable(ptrDeviceLinkThroughputLimit) && IsWritable(ptrDeviceLinkThroughputLimit))
        {
          ROS_INFO_STREAM("[" << __APP_NAME__
                              << "] DeviceLinkThroughputLimit: " << ptrDeviceLinkThroughputLimit->GetValue());
          ptrDeviceLinkThroughputLimit->SetValue(dltl_);
        }
        else
        {
          ROS_WARN("[%s] This camera does not support DeviceLinkThroughputLimit, using default value.", __APP_NAME__);
        }
      }

      ///////////////////////                 FrameRate                   /////////////////////////////
      CFloatPtr ptrAcquisitionFrameRate = node_map_->GetNode("AcquisitionFrameRate");
      CBooleanPtr ptrAcquisitionFrameRateEnable = node_map_->GetNode("AcquisitionFrameRateEnable");
      CEnumerationPtr ptrAcquisitionFrameRateAuto = pCamList_[i]->GetNodeMap().GetNode("AcquisitionFrameRateAuto");
      if (IsAvailable(ptrAcquisitionFrameRateAuto) && IsWritable(ptrAcquisitionFrameRateAuto))
      {
        CEnumEntryPtr ptrAcquisitionFrameRateAutoOff = ptrAcquisitionFrameRateAuto->GetEntryByName("Off");
        if (IsAvailable(ptrAcquisitionFrameRateAutoOff) && IsReadable(ptrAcquisitionFrameRateAutoOff))
        {
          int64_t FrameRateAutoOff = ptrAcquisitionFrameRateAutoOff->GetValue();
          ptrAcquisitionFrameRateAuto->SetIntValue(FrameRateAutoOff);
          ROS_INFO_STREAM("[" << __APP_NAME__ << "] Updated FrameRateAuto to Off");
        }
        else
        {
          ROS_INFO_STREAM("[" << __APP_NAME__ << "] Cannot update FrameRateAuto to Off");
        }
      }
      if (IsAvailable(ptrAcquisitionFrameRateEnable) && IsWritable(ptrAcquisitionFrameRateEnable))
      {
        ptrAcquisitionFrameRateEnable->SetValue(true);
      }
      if (IsAvailable(ptrAcquisitionFrameRate) && IsWritable(ptrAcquisitionFrameRate))
      {
        ptrAcquisitionFrameRate->SetValue(fps_);
        ROS_INFO_STREAM("[" << __APP_NAME__ << "] Set FrameRate to " << fps_);
      }
      else
      {
        ROS_WARN("[%s] This camera does not support AcquisitionFrameRate, using default value.", __APP_NAME__);
      }

      ///////////////////////                 PixelFormat                 /////////////////////////////
      CEnumerationPtr ptrPixelFormat = node_map_->GetNode("PixelFormat");
      if (IsAvailable(ptrPixelFormat) && IsWritable(ptrPixelFormat))
      {
        ROS_INFO_STREAM("[" << __APP_NAME__ << "] Current pixel Format"
                            << ptrPixelFormat->GetCurrentEntry()->GetSymbolic());
        /*gcstring pixel_format = format_.c_str();

        CEnumEntryPtr ptrPixelFormatSetup = ptrPixelFormat->GetEntryByName(pixel_format);
        if (IsAvailable(ptrPixelFormatSetup) && IsReadable(ptrPixelFormatSetup))
        {
          int64_t pixelFormatSetup = ptrPixelFormatSetup->GetValue();
          ptrPixelFormat->SetIntValue(ptrPixelFormatSetup);

          ROS_INFO_STREAM("[" << __APP_NAME__ << "] Pixel format set to " <<
                                            ptrPixelFormat->GetCurrentEntry()->GetSymbolic());
        }
        else
        {
          ROS_WARN("[%s] %s pixel format not available. Using default.", __APP_NAME__, format_.c_str());
        }*/
      }
      else
      {
        ROS_WARN("[%s] Pixel format cannot be changed on this camera. Using default.", __APP_NAME__);
      }

      ///////////////////////                 AcquisitionMode                 /////////////////////////////
      CEnumerationPtr ptrAcquisitionMode = pCamList_[i]->GetNodeMap().GetNode("AcquisitionMode");
      if (!IsAvailable(ptrAcquisitionMode) || !IsWritable(ptrAcquisitionMode))
      {
        ROS_FATAL("[%s] Unable to set acquisition mode to continuous (node retrieval; "
                  "camera ",
                  __APP_NAME__);
      }

      ///////////////////////                 ContinuousMode                  /////////////////////////////
      CEnumEntryPtr ptrAcquisitionModeContinuous = ptrAcquisitionMode->GetEntryByName("Continuous");
      if (!IsAvailable(ptrAcquisitionModeContinuous) || !IsReadable(ptrAcquisitionModeContinuous))
      {
        ROS_FATAL("[%s] Unable to set acquisition mode to continuous (entry "
                  "'continuous' retrieval. Aborting...",
                  __APP_NAME__);
      }

      int64_t acquisitionModeContinuous = ptrAcquisitionModeContinuous->GetValue();
      ptrAcquisitionMode->SetIntValue(acquisitionModeContinuous);

      ROS_INFO("[%s] [camera %d] acquisition mode set to continuous...", __APP_NAME__, i);

      strSerialNumbers[i] = "";
      CStringPtr ptrStringSerial = pCamList_[i]->GetTLDeviceNodeMap().GetNode("DeviceSerialNumber");
      if (IsAvailable(ptrStringSerial) && IsReadable(ptrStringSerial))
      {
        strSerialNumbers[i] = ptrStringSerial->GetValue();
        ROS_INFO("[%s] [camera %d] serial number set to %s", __APP_NAME__, i, strSerialNumbers[i].c_str());
      }
      std::string topic("/usb_cam/image_raw");
      if (camList_.GetSize() > 1)
        topic = "camera" + std::to_string(i) + "/" + topic;
      image_pubs_.push_back(image_transport_->advertise(topic, 100));
    }
  }
  catch (Spinnaker::Exception& e)
  {
    ROS_ERROR("[%s] Error: %s", __APP_NAME__, e.what());
  }
}

SpinnakerCamera::~SpinnakerCamera()
{
  for (uint i = 0; i < camList_.GetSize(); i++)
  {
    pCamList_[i]->EndAcquisition();
    pCamList_[i]->DeInit();
    pCamList_[i] = NULL;
    ROS_INFO("[%s] Shutting down camera %d", __APP_NAME__, i);
  }
  delete[] pCamList_;
  node_map_ = NULL;
  camList_.Clear();
  system_->ReleaseInstance();
}

void SpinnakerCamera::publish_image(int index)
{
  while (ros::ok())
  {
    try
    {
      ImagePtr pResultImage = pCamList_[index]->GetNextImage();
      if (pResultImage->IsIncomplete())
      {
        ROS_WARN("[%s] [camera %d] Image incomplete with image status %d", __APP_NAME__, index,
                 pResultImage->GetImageStatus());
      }
      else
      {
        ros::Time receive_time = ros::Time::now();

//        // create opencv mat
//        int pixel_format = pResultImage->GetPixelFormat();
//        std::string encoding_pattern;
//        switch (pixel_format)
//        {
//          case PixelFormatEnums::PixelFormat_BayerRG8:
//            encoding_pattern = "bayer_rggb8";
//            break;
//          case PixelFormatEnums::PixelFormat_BayerGR8:
//            encoding_pattern = "bayer_grbg8";
//            break;
//          case PixelFormatEnums::PixelFormat_BayerGB8:
//            encoding_pattern = "bayer_gbrg8";
//            break;
//          case PixelFormatEnums::PixelFormat_BayerBG8:
//            encoding_pattern = "bayer_bggr8";
//            break;
//          case PixelFormatEnums::PixelFormat_RGB8:
//            encoding_pattern = "rgb8";
//            break;
//          case PixelFormatEnums::PixelFormat_BGR8:
//            encoding_pattern = "bgr8";
//            break;
//          default:
//            encoding_pattern = "mono8";
//        }

        ImagePtr convertedImage = pResultImage->Convert(PixelFormat_BGR8, HQ_LINEAR);

        int cvFormat = CV_8UC3;
        unsigned int XPadding = convertedImage->GetXPadding();
        unsigned int YPadding = convertedImage->GetYPadding();
        unsigned int rowsize = convertedImage->GetWidth();
        unsigned int colsize = convertedImage->GetHeight();
        cv::Mat cvmat;
        cvmat= cv::Mat(colsize + YPadding, rowsize + XPadding, cvFormat, convertedImage->GetData(), convertedImage->GetStride());
        cv::imshow("cvMat", cvmat);
        if (cv::waitKey(1) == 27)//按ESC鍵
        {
            cout << "Program Stopped！" << endl;
        }
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cvmat).toImageMsg();
        std_msgs::Header header;
        header.stamp = receive_time;
        header.frame_id = (camList_.GetSize() > 1) ? "camera" + std::to_string(index) : "camera";
        msg->header = header;

//        // create and publish ros message
//        std_msgs::Header header;
//        sensor_msgs::Image msg;
//        header.stamp = receive_time;
//        header.frame_id = (camList_.GetSize() > 1) ? "camera" + std::to_string(index) : "camera";

//        msg.header = header;
//        msg.height = pResultImage->GetHeight();
//        msg.width = pResultImage->GetWidth();
//        msg.encoding = encoding_pattern;
//        msg.step = pResultImage->GetStride();

//        size_t image_size = pResultImage->GetImageSize();
//        msg.data.resize(image_size);
//        memcpy(msg.data.data(), pResultImage->GetData(), image_size);

        image_pubs_[index].publish(msg);
      }
      pResultImage->Release();
    }
    catch (Spinnaker::Exception& e)
    {
      ROS_ERROR("[%s] Error: %s", __APP_NAME__, e.what());
    }
  }
}
void SpinnakerCamera::spin()
{
  for (uint i = 0; i < camList_.GetSize(); i++)
  {
    pCamList_[i]->BeginAcquisition();
    ROS_INFO("[%s] [camera %d] started acquisition", __APP_NAME__, i);
  }
  vector<std::shared_ptr<std::thread>> threads(camList_.GetSize());
  for (uint i = 0; i < camList_.GetSize(); i++)
  {
    threads[i] = make_shared<thread>(&SpinnakerCamera::publish_image, this, i);
  }
  for (auto& thread : threads)
  {
    thread->join();
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "spinnaker");
  SpinnakerCamera node;
  node.spin();
  return 0;
}

cmake文件配置

cmake_minimum_required(VERSION 2.8.12)

project(FlirCameraProject)
add_compile_options(-std=c++11)
find_package(OpenCV 3.3 REQUIRED)
find_package(catkin REQUIRED COMPONENTS
        #autoware_build_flags
        roscpp
        std_msgs
        message_generation
        tf
        sensor_msgs
        camera_info_manager
    	image_transport
    	cv_bridge
)

set(CMAKE_CXX_FLAGS "-O2 -Wall ${CMAKE_CXX_FLAGS}")


catkin_package()

###########
## Build ##
###########

if(EXISTS "/usr/include/spinnaker")
	include(FindSpinnaker.cmake)
	include_directories(
                spinnaker
		${Spinnaker_INCLUDE_DIRS}
		${catkin_INCLUDE_DIRS}
		${OpenCV_INCLUDE_DIRS}
	)

	add_executable(spinnaker_camera
                spinnaker/spinnaker.cpp
	)

	target_link_libraries(spinnaker_camera
		${catkin_LIBRARIES}
		${OpenCV_LIBS}
		${Spinnaker_LIBRARIES}
	)
	install(TARGETS spinnaker_camera
			ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
			LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
			RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
			)
	install(FILES
			scripts/spinnaker.launch
			DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
			)
else()
	message("'SDK for Spinnaker' is not installed. 'spinnaker_camera' will not be built.")
endif()

FindSpinnaker.cmake直接把Autoware中的拷過來即可，以後其他設備自帶驅動也可以參考着這麼寫。

unset(Spinnaker_FOUND)
unset(Spinnaker_INCLUDE_DIRS)
unset(Spinnaker_LIBRARIES)

find_path(Spinnaker_INCLUDE_DIRS NAMES
  Spinnaker.h
  PATHS
  /usr/include/spinnaker/
  /usr/local/include/spinnaker/
)

find_library(Spinnaker_LIBRARIES NAMES Spinnaker
  PATHS
  /usr/lib
  /usr/local/lib
)

if (Spinnaker_INCLUDE_DIRS AND Spinnaker_LIBRARIES)
  set(Spinnaker_FOUND 1)
endif (Spinnaker_INCLUDE_DIRS AND Spinnaker_LIBRARIES)