bool Camera::setup()
{
  ///////////////////////////////////////////////////////
  // Get a camera:
  FlyCapture2::Error error;
  FlyCapture2::BusManager busMgr;

  unsigned int N;
  if ((error = busMgr.GetNumOfCameras(&N)) != PGRERROR_OK)
    ROS_ERROR (error.GetDescription ());
  if (camSerNo == 0)
  {
    if ((error = busMgr.GetCameraFromIndex(0, &guid_)) != PGRERROR_OK)
      PRINT_ERROR_AND_RETURN_FALSE;
    ROS_INFO ("did busMgr.GetCameraFromIndex(0, &guid)");
  }
  else
  {
    if ((error = busMgr.GetCameraFromSerialNumber(camSerNo, &guid_)) != PGRERROR_OK)
      PRINT_ERROR_AND_RETURN_FALSE;
  }
  ROS_INFO ("Setup: Camera GUID = %u %u %u %u", guid_.value[0], guid_.value[1], guid_.value[2], guid_.value[3]);

  ROS_INFO ("Setup successful");
  return true;

}

/*!
  Acquire a color image from the active camera.

  \param I : Image data structure (RGBa image).

  \param timestamp : The acquisition timestamp.
*/
void vpFlyCaptureGrabber::acquire(vpImage<vpRGBa> &I, FlyCapture2::TimeStamp &timestamp)
{
  this->open();

  FlyCapture2::Error error;
  // Retrieve an image
  error = m_camera.RetrieveBuffer( &m_rawImage );
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw (vpException(vpException::fatalError,
                       "Cannot retrieve image for camera with guid 0x%lx",
                       m_guid) );
  }
  timestamp = m_rawImage.GetTimeStamp();

  // Create a converted image
  FlyCapture2::Image convertedImage;

  // Convert the raw image
  error = m_rawImage.Convert( FlyCapture2::PIXEL_FORMAT_RGBU, &convertedImage );
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw (vpException(vpException::fatalError,
                       "Cannot convert image for camera with guid 0x%lx",
                       m_guid) );
  }
  height = convertedImage.GetRows();
  width = convertedImage.GetCols();
  unsigned char *data = convertedImage.GetData();
  I.resize(height, width);
  unsigned int bps = convertedImage.GetBitsPerPixel();
  memcpy(I.bitmap, data, width*height*bps/8);
}

/*!
   Connect the active camera.

   \sa disconnect()
 */
void vpFlyCaptureGrabber::connect()
{
  if (m_connected == false) {
    FlyCapture2::Error error;
    m_numCameras = this->getNumCameras();
    if (m_numCameras == 0) {
      throw (vpException(vpException::fatalError, "No camera found on the bus"));
    }

    FlyCapture2::BusManager busMgr;

    error = busMgr.GetCameraFromIndex(m_index, &m_guid);
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw (vpException(vpException::fatalError,
                         "Cannot retrieve guid of camera with index %u.",
                         m_index) );
    }
    // Connect to a camera
    error = m_camera.Connect(&m_guid);
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw (vpException(vpException::fatalError,
                         "Cannot connect to camera with guid 0x%lx", m_guid));
    }
    m_connected = true;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

  static bool frameToImage (FlyCapture2::Image * frame, sensor_msgs::Image & image)
  {
    // Get the raw image dimensions
    FlyCapture2::PixelFormat pixFormat;
    uint32_t rows, cols, stride;

    FlyCapture2::Image convertedImage;
    FlyCapture2::Error error;

    error = frame->Convert(FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage );

    if (error != FlyCapture2::PGRERROR_OK)
    {
        error.PrintErrorTrace();
        return false;
    }

    // Get the raw image dimensions
    convertedImage.GetDimensions( &rows, &cols, &stride, &pixFormat );

    return sensor_msgs::fillImage (image, sensor_msgs::image_encodings::RGB8,
                                   rows, cols, stride,
                                   convertedImage.GetData ());

  }

/*!
  Set video mode and framerate of the active camera.
  \param video_mode : Camera video mode.
  \param frame_rate : Camera frame rate.

  The following example shows how to use this function to set the camera image resolution to
  1280 x 960, pixel format to Y8 and capture framerate to 60 fps.
  \code
#include <visp3/core/vpImage.h>
#include <visp3/flycapture/vpFlyCaptureGrabber.h>

int main()
{
#if defined(VISP_HAVE_FLYCAPTURE)
  int nframes = 100;
  vpImage<unsigned char> I;
  vpFlyCaptureGrabber g;

  g.setCameraIndex(0); // Default camera is the first on the bus
  g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60);
  g.open(I);
  g.getCameraInfo(std::cout);

  for(int i=0; i< nframes; i++) {
    g.acquire(I);
  }
#endif
}
  \endcode
 */
void vpFlyCaptureGrabber::setVideoModeAndFrameRate(FlyCapture2::VideoMode video_mode,
                                                   FlyCapture2::FrameRate frame_rate)
{
  this->connect();

  FlyCapture2::Error error;
  error = m_camera.SetVideoModeAndFrameRate(video_mode, frame_rate);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw (vpException(vpException::fatalError, "Cannot set video mode and framerate.") );
  }
}

int VideoHandler::start() {

  if (m_stopped) {
    if ( m_camera) {
      FlyCapture2::BusManager busMgr;
      FlyCapture2::Error error;
      FlyCapture2::PGRGuid guid;
      
      cout << m_camIdx << endl;
      error = busMgr.GetCameraFromIndex( m_camIdx, &guid );
      if (error != PGRERROR_OK)
      {
	error.PrintErrorTrace();
	return -1;
      }
      cout << "got camera from index" << endl;
      pVideoSaver = new VideoSaver();
      cout << "created VideoSaver" << endl;
      if (pVideoSaver->init(guid)!=0){
	cout << "Warning: Error in initializing the camera\n" ;
	return -1;
      }
      if (fname.empty()) {
	cout << "start capture thread" << endl;
	if (pVideoSaver->startCapture()!=0) {
	  cout << "Warning: Error in starting the capture thread the camera \n";
	  return -1;
	}
      }
      else {
	cout << "start capture and write threads" << endl;
	if (pVideoSaver->startCaptureAndWrite(fname,string("X264"))!=0) {
	  cout << "Warning: Error in starting the writing thread the camera \n";
	  return -1;
	}
      }
    }
      else  {
      pVideoCapture = new VideoCapture(fname);
      //pVideoCapture->set(cv::CAP_PROP_CONVERT_RGB,true); // output RGB
    }

    if (knnMethod)
      pBackgroundSubtractor =  cv::createBackgroundSubtractorKNN(250,400,false);
    else
      pBackgroundThresholder =  new BackgroundThresholder();

    m_stopped=false;
    startThreads();
  }
  return 0;
}

void Camera::SetExposure(bool _auto, bool onoff, unsigned int value)
{
  FlyCapture2::Property prop;
  FlyCapture2::Error error;
  prop.type = FlyCapture2::AUTO_EXPOSURE;
  prop.autoManualMode = _auto;
  prop.onOff = onoff;
  prop.valueA = value;
  if ((error = camPGR_.SetProperty(&prop)) != PGRERROR_OK)
  {
    ROS_ERROR (error.GetDescription ());
  }
}

/*!
  Return true if video mode and framerate is supported.
  */
bool vpFlyCaptureGrabber::isVideoModeAndFrameRateSupported(FlyCapture2::VideoMode video_mode,
                                                           FlyCapture2::FrameRate frame_rate)
{
  this->connect();

  FlyCapture2::Error error;
  bool supported = false;
  error = m_camera.GetVideoModeAndFrameRateInfo(video_mode, frame_rate, &supported);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw (vpException(vpException::fatalError, "Cannot get video mode and framerate.") );
  }
  return supported;
}

void PointGreyCamera::handleError(const std::string &prefix, const FlyCapture2::Error &error)
{
  if(error == PGRERROR_TIMEOUT)
  {
    throw CameraTimeoutException("PointGreyCamera: Failed to retrieve buffer within timeout.");
  }
  else if(error != PGRERROR_OK)     // If there is actually an error (PGRERROR_OK means the function worked as intended...)
  {
    std::string start(" | FlyCapture2::ErrorType ");
    std::stringstream out;
    out << error.GetType();
    std::string desc(error.GetDescription());
    throw std::runtime_error(prefix + start + out.str() + " " + desc);
  }
}

void Camera::SetShutter (bool _auto, float value)
{
  FlyCapture2::Property prop;
  FlyCapture2::Error error;
  prop.type = FlyCapture2::AUTO_EXPOSURE;
  prop.autoManualMode = _auto;
  prop.onOff = true;
  prop.absValue = value;
  if ((error = camPGR_.SetProperty(&prop)) != PGRERROR_OK)
  {
    ROS_ERROR (error.GetDescription ());
  }

  return;
}

/*!
  Return property values.
  \param prop_type : Property type.
 */
FlyCapture2::Property vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType prop_type)
{
  this->connect();

  FlyCapture2::Property prop;
  prop.type = prop_type;
  FlyCapture2::Error error;
  error = m_camera.GetProperty( &prop );
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw (vpException(vpException::fatalError,
                       "Cannot get property %d value.", (int)prop_type));
  }
  return prop;
}

int VideoSaverFlyCapture::stopCamera() {

  if (isFlyCapture()) {
    if (m_Camera.IsConnected()) {    
      FlyCapture2::Error error = m_Camera.Disconnect();
      if ( error != FlyCapture2::PGRERROR_OK ) {
	error.PrintErrorTrace();
	return -1;
      }
    }
    return 0;
  } else {
    return VideoSaver::stopCamera();
  }
}

static bool check_success(FlyCapture2::Error error) {
    if(error != FlyCapture2::PGRERROR_OK) {
        ROS_ERROR ("%s", error.GetDescription());
        return false;
    }
    return true;
}

// Print point grey errors.
void TrackerDataSource::ptgr_err( FlyCapture2::Error error ){
  if(error != FlyCapture2::PGRERROR_OK){
    error.PrintErrorTrace();
    std::cout << "Press Enter to continue" << std::endl;
    getchar();
  }
}

/*!
   Disconnect the active camera.

   \sa connect()
 */
void vpFlyCaptureGrabber::disconnect()
{
  if (m_connected == true) {

    FlyCapture2::Error error;
    error = m_camera.Disconnect();
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw (vpException(vpException::fatalError, "Cannot stop capture.") );
    }
    m_connected = false;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

/*!
  Return true if format7 mode is supported.
  */
bool vpFlyCaptureGrabber::isFormat7Supported(FlyCapture2::Mode format7_mode)
{
  this->connect();

  FlyCapture2::Format7Info fmt7_info;
  bool supported = false;
  FlyCapture2::Error error;

  fmt7_info.mode = format7_mode;
  error = m_camera.GetFormat7Info(&fmt7_info, &supported);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw (vpException(vpException::fatalError, "Cannot get format7 info.") );
  }

  return supported;
}

void _pgrSafeCall(FlyCapture2::Error err, std::string fileName, int lineNum)
{
	if(err != FlyCapture2::PGRERROR_OK)
	{	
		std::cout << "File: " << fileName << " Line: " << lineNum << std::endl;
		err.PrintErrorTrace();
		std::cout << std::endl;
	}
}

/*!
   Start active camera capturing images.

   \sa stopCapture()
 */
void vpFlyCaptureGrabber::startCapture()
{
  this->connect();

  if (m_capture == false) {

    FlyCapture2::Error error;
    error = m_camera.StartCapture();
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw (vpException(vpException::fatalError,
                         "Cannot start capture for camera with guid 0x%lx", m_guid));
    }
    m_capture = true;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

void Camera::start()
{
  FlyCapture2::Error error;
  if (camPGR_.IsConnected())
  {
    ROS_INFO ("IsConnected returned true");
  }
  else
    ROS_INFO ("IsConnected returned false");

  if ((error = camPGR_.StartCapture(frameDone, (void *) this)) != PGRERROR_OK)
  { //frameDone, (void*) this)) != PGRERROR_OK) {
    ROS_ERROR (error.GetDescription ());
  }
  else
  {
    ROS_INFO ("StartCapture succeeded.");
  }

}

/*!
  Print to the output stream active camera information (serial number, camera model,
  camera vendor, sensor, resolution, firmaware version, ...).
  */
std::ostream &vpFlyCaptureGrabber::getCameraInfo(std::ostream & os)
{
  this->connect();

  FlyCapture2::CameraInfo camInfo;
  FlyCapture2::Error error = m_camera.GetCameraInfo(&camInfo);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
  }

  os << "Camera information:   " << std::endl;
  os << " Serial number      : " << camInfo.serialNumber << std::endl;
  os << " Camera model       : " << camInfo.modelName << std::endl;
  os << " Camera vendor      : " << camInfo.vendorName << std::endl;
  os << " Sensor             : " << camInfo.sensorInfo << std::endl;
  os << " Resolution         : " << camInfo.sensorResolution << std::endl;
  os << " Firmware version   : " << camInfo.firmwareVersion << std::endl;
  os << " Firmware build time: " << camInfo.firmwareBuildTime << std::endl;
  return os;
}