bumblebee2_driver.cpp

Go to the documentation of this file.

#include "bumblebee2_driver.h"

Bumblebee2Driver::Bumblebee2Driver(void)
{
  //setDriverId(driver string id);
  this->server=CFirewireServer::instance();
  this->server->init();
  this->camera=NULL;
  this->camera_id=-1;
  // desired configuration
  this->desired_conf.width=1024;
  this->desired_conf.height=768;
  this->desired_conf.left_offset=0;
  this->desired_conf.top_offset=0;
  this->desired_conf.depth=DEPTH16;
  this->desired_conf.coding=RAW;
  this->desired_conf.framerate=0.0;
  // current configuration
  this->current_conf.width=1024;
  this->current_conf.height=768;
  this->current_conf.left_offset=0;
  this->current_conf.top_offset=0;
  this->current_conf.depth=DEPTH16;
  this->current_conf.coding=RAW;
  this->current_conf.framerate=0.0;
  // default configuration
  this->default_conf.width=1024;
  this->default_conf.height=768;
  this->default_conf.left_offset=0;
  this->default_conf.top_offset=0;
  this->default_conf.depth=DEPTH16;
  this->default_conf.coding=RAW;
  this->default_conf.framerate=0.0;
  // default calibration file
  this->calibration_files.resize(NUM_CAM);
  // default frame id
  this->frame_ids.resize(NUM_CAM);
}

bool Bumblebee2Driver::openDriver(void)
{
  //setDriverId(driver string id);
  if(this->camera_id!=(uint64_t)-1)
  {
    this->server->init();
    if(this->server->get_num_cameras()>0)
    {
      this->server->get_bumblebee_camera(this->camera_id,&this->camera);
      this->camera->get_config(&this->default_conf.left_offset,&this->default_conf.top_offset,&this->default_conf.width,
                               &this->default_conf.height,&this->default_conf.framerate,&this->default_conf.depth,
                               &this->default_conf.coding);
      this->desired_conf.coding=this->default_conf.coding;
      this->change_config(&this->desired_conf);
      ROS_INFO("Driver opened");
      return true;
    }
    else
    {
      ROS_INFO("No cameras available");
      return false;
    }
  }
  else
  {
    ROS_INFO("Waiting for a valid id ...");
    return false;
  }
}

bool Bumblebee2Driver::closeDriver(void)
{
  if(this->camera!=NULL)
  {
    delete this->camera;
    this->camera=NULL;
  }

  return true;
}

bool Bumblebee2Driver::startDriver(void)
{
  if(this->camera!=NULL)
  {
    try{
      this->lock();
      this->camera->start();
      this->unlock();
      ROS_INFO("Driver started");
      return true;
    }catch(CException &e){
      this->unlock();
      //ROS_INFO(e.what().data());
      return false;
    }
  }
  else
    return false;
}

bool Bumblebee2Driver::stopDriver(void)
{
  if(this->camera!=NULL)
  {
    try{
      this->lock();
      this->camera->stop();
      this->unlock();
      ROS_INFO("Driver stopped");
      return true;
    }catch(CException &e){
      this->unlock();
      //ROS_INFO(e.what().data());
      return false;
    }
  }
  else
    return false;
}

void Bumblebee2Driver::config_update(Config& new_cfg, uint32_t level)
{
  TCameraConfig conf;

  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case Bumblebee2Driver::CLOSED:
      this->lock();
      this->camera_id=new_cfg.Camera_node;
      this->desired_conf.framerate=new_cfg.Framerate;
      // change the calibration file
      this->calibration_files[LEFT]=new_cfg.left_cal_file;
      this->calibration_files[RIGHT]=new_cfg.right_cal_file;
      // update the frame identifier
      this->frame_ids[LEFT]=new_cfg.left_frame_id;
      this->frame_ids[RIGHT]=new_cfg.right_frame_id;
      this->unlock();
      break;

    case Bumblebee2Driver::OPENED:
      if(new_cfg.Camera_node!=-1)
      {
        try{
          this->lock();
          if(this->camera_id!=(uint64_t)new_cfg.Camera_node)
          {
            this->close();
            this->camera_id=new_cfg.Camera_node;
            this->open();
          }
          conf.framerate=new_cfg.Framerate;
          this->change_config(&conf);
          // write the configuration back to the dynamic reconfigure window
          new_cfg.Framerate=conf.framerate;
          // change the calibration file
          this->calibration_files[LEFT]=new_cfg.left_cal_file;
          this->calibration_files[RIGHT]=new_cfg.right_cal_file;
          // update the frame identifier
          this->frame_ids[LEFT]=new_cfg.left_frame_id;
          this->frame_ids[RIGHT]=new_cfg.right_frame_id;
          this->unlock();
        }catch(CFirewireCameraException &e){
          ROS_INFO("Invalid configuration. Setting back the default configuration ...");
          this->change_config(&this->default_conf);
          new_cfg.Framerate=this->default_conf.framerate;
          this->unlock();
        }
      }
      break;
    
    case Bumblebee2Driver::RUNNING:
      break;
  }

  // save the current configuration
  this->config_=new_cfg;
}

void Bumblebee2Driver::change_config(TCameraConfig *new_conf)
{
  try{
    if(this->camera!=NULL)
    {
      this->current_conf.left_offset=new_conf->left_offset;
      this->current_conf.top_offset=new_conf->top_offset;
      this->current_conf.width=new_conf->width;
      this->current_conf.height=new_conf->height;
      this->current_conf.framerate=new_conf->framerate;
      this->current_conf.depth=new_conf->depth;
      this->current_conf.coding=new_conf->coding;
      this->camera->set_config(&this->current_conf.left_offset,&this->current_conf.top_offset,&this->current_conf.width,
                               &this->current_conf.height,&this->current_conf.framerate,this->current_conf.depth,
                               this->current_conf.coding);
      this->desired_conf.left_offset=new_conf->left_offset;
      new_conf->left_offset=current_conf.left_offset;
      this->desired_conf.top_offset=new_conf->top_offset;
      new_conf->top_offset=current_conf.top_offset;
      this->desired_conf.width=new_conf->width;
      new_conf->width=current_conf.width;
      this->desired_conf.height=new_conf->height;
      new_conf->height=current_conf.height;
      this->desired_conf.framerate=new_conf->framerate;
      new_conf->framerate=current_conf.framerate;
      this->desired_conf.depth=new_conf->depth;
      new_conf->depth=current_conf.depth;
      this->desired_conf.coding=new_conf->coding;
      new_conf->coding=current_conf.coding;
    }
  }catch(CFirewireCameraException &e){
    ROS_INFO("Invalid configuration. Setting back the default configuration ...");
    this->change_config(&this->default_conf);
    new_conf->left_offset=this->default_conf.left_offset;
    new_conf->top_offset=this->default_conf.top_offset;
    new_conf->width=this->default_conf.width;
    new_conf->height=this->default_conf.height;
    new_conf->framerate=this->default_conf.framerate;
    new_conf->depth=this->default_conf.depth;
    new_conf->coding=this->default_conf.coding;
  }catch(CFirewireInternalException &e){
    throw;
  }
}

void Bumblebee2Driver::get_current_config(TCameraConfig *current_conf)
{
  this->camera->get_stereo_config(&(current_conf->width),&(current_conf->height),&(current_conf->framerate),&(current_conf->depth),&(current_conf->coding));

  current_conf->left_offset=0;
  current_conf->top_offset=0;
}

void Bumblebee2Driver::get_images(char **left_image,char **right_image)
{
  if(this->camera!=NULL)
    this->camera->get_stereo_image(left_image,right_image);
  else
  {
    *left_image=NULL;
    *right_image=NULL;
  }
}

std::string Bumblebee2Driver::get_new_frame_event(void)
{
  std::string event="";

  if(this->camera!=NULL)
    this->camera->get_new_frame_event_id(event);

  return event;
}

void Bumblebee2Driver::get_calibration_files(std::vector<std::string> &calibration_files)
{
  calibration_files.resize(NUM_CAM);
  calibration_files[LEFT]=this->calibration_files[LEFT];
  calibration_files[RIGHT]=this->calibration_files[RIGHT];
}

void Bumblebee2Driver::get_frame_ids(std::vector<std::string> &frame_ids)
{
  frame_ids.resize(NUM_CAM);
  frame_ids[LEFT]=this->frame_ids[LEFT];
  frame_ids[RIGHT]=this->frame_ids[RIGHT];
}

Bumblebee2Driver::~Bumblebee2Driver(void)
{
  this->server->close();
}