firewire_camera_driver.cpp

Go to the documentation of this file.

#include "firewire_camera_driver.h"

FirewireCameraDriver::FirewireCameraDriver()
{
  //setDriverId(driver string id);
  this->server=CFirewireServer::instance();
  this->server->init();
  this->camera=NULL;
  this->camera_id=-1;
  this->ISO_speed=400;
  // desired configuration
  this->desired_conf.width=-1;
  this->desired_conf.height=-1;
  this->desired_conf.left_offset=-1;
  this->desired_conf.top_offset=-1;
  this->desired_conf.depth=(depths_t)-1;
  this->desired_conf.coding=(codings_t)-1;
  this->desired_conf.framerate=0.0;
  // current configuration
  this->current_conf.width=-1;
  this->current_conf.height=-1;
  this->current_conf.left_offset=-1;
  this->current_conf.top_offset=-1;
  this->current_conf.depth=(depths_t)-1;
  this->current_conf.coding=(codings_t)-1;
  this->current_conf.framerate=0.0;
  // default configuration
  this->default_conf.width=-1;
  this->default_conf.height=-1;
  this->default_conf.left_offset=-1;
  this->default_conf.top_offset=-1;
  this->default_conf.depth=(depths_t)-1;
  this->default_conf.coding=(codings_t)-1;
  this->default_conf.framerate=0.0;
  // default calibration file
  this->calibration_file="";
  // default frame id
  this->frame_id="";
}

bool FirewireCameraDriver::openDriver(void)
{
  //setDriverId(driver string id);
  if(this->camera_id!=-1)
  {
    this->server->init();
    if(this->server->get_num_cameras()>0)
    {
      this->server->get_camera(this->camera_id,&this->camera);
      this->camera->set_ISO_speed(this->ISO_speed);
      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->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 FirewireCameraDriver::closeDriver(void)
{ 
  if(this->camera!=NULL)
  {  
    delete this->camera;
    this->camera=NULL;
  }

  return true;
}

bool FirewireCameraDriver::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 FirewireCameraDriver::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 FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
{
  TCameraConfig conf;

  // depending on current state
  // update driver with new_cfg data
  switch(this->getState())
  {
    case FirewireCameraDriver::CLOSED:
      this->lock();
      this->camera_id=new_cfg.Camera_node;
      this->ISO_speed=new_cfg.ISO_speed;
      this->dyn_rec_to_camera(new_cfg,this->desired_conf);
      // change the calibration file
      this->calibration_file=new_cfg.cal_file;
      // update the frame identifier
      this->frame_id=new_cfg.frame_id;
      this->unlock();
      break;

    case FirewireCameraDriver::OPENED:
      if(new_cfg.Camera_node!=-1)
      {
        try{
          this->lock();
          // update the desired ISO speed
          this->ISO_speed=new_cfg.ISO_speed;
          if(this->camera_id!=new_cfg.Camera_node)
          {
            this->close();
            this->camera_id=new_cfg.Camera_node;
            this->open();
          }
          this->dyn_rec_to_camera(new_cfg,conf);
          this->change_config(&conf);
          // write the configuration back to the dynamic reconfigure window
          this->camera_to_dyn_rec(conf,new_cfg);
          // white balance feature
          this->set_white_balance(&new_cfg.White_balance_enabled,&new_cfg.White_balance_mode,&new_cfg.White_balance_u_b_value,
                                  &new_cfg.White_balance_v_r_value);
          // shutter feature
          this->set_shutter(&new_cfg.Shutter_enabled,&new_cfg.Shutter_mode,&new_cfg.Shutter_value);
          // gain feature
          this->set_gain(&new_cfg.Gain_enabled,&new_cfg.Gain_mode,&new_cfg.Gain_value);
          // change the calibration file
          this->calibration_file=new_cfg.cal_file;
          // update the frame identifier
          this->frame_id=new_cfg.frame_id;
          this->unlock();
        }catch(CFirewireCameraException &e){
          ROS_INFO("Invalid configuration. Setting back the default configuration ...");
          this->change_config(&this->default_conf);
          this->camera_to_dyn_rec(this->default_conf,new_cfg);
          this->unlock();
        }
      }
      break;

    case FirewireCameraDriver::RUNNING:
      break;
  }

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

void FirewireCameraDriver::dyn_rec_to_camera(iri_firewire_camera::FirewireCameraConfig &dyn_rec_config,TCameraConfig &camera_config)
{
  camera_config.left_offset=dyn_rec_config.Left_offset;
  camera_config.top_offset=dyn_rec_config.Top_offset;
  camera_config.width=dyn_rec_config.Image_width;
  camera_config.height=dyn_rec_config.Image_height;
  camera_config.framerate=dyn_rec_config.Framerate;
  switch(dyn_rec_config.Color_coding)
  {
    case 0: camera_config.depth=DEPTH8;
            camera_config.coding=MONO;
            break;
    case 1: camera_config.depth=DEPTH16;
            camera_config.coding=YUV;
            break;
    case 2: camera_config.depth=DEPTH24;
            camera_config.coding=YUV;
            break;
    case 3: camera_config.depth=DEPTH24;
            camera_config.coding=RGB;
            break;
    case 4: camera_config.depth=DEPTH16;
            camera_config.coding=MONO;
            break;
    case 5: camera_config.depth=DEPTH48;
            camera_config.coding=RGB;
            break;
    case 6: camera_config.depth=DEPTH8;
            camera_config.coding=RAW;
            break;
    case 7: camera_config.depth=DEPTH16;
            camera_config.coding=RAW;
            break;
    default: camera_config.depth=DEPTH24;
             camera_config.coding=RGB;
             break;
  }
}

void FirewireCameraDriver::camera_to_dyn_rec(TCameraConfig &camera_config,iri_firewire_camera::FirewireCameraConfig &dyn_rec_config)
{
  dyn_rec_config.Left_offset=camera_config.left_offset;
  dyn_rec_config.Top_offset=camera_config.top_offset;
  dyn_rec_config.Image_width=camera_config.width;
  dyn_rec_config.Image_height=camera_config.height;
  dyn_rec_config.Framerate=camera_config.framerate;
  if(camera_config.depth==DEPTH8 && camera_config.coding==MONO)
    dyn_rec_config.Color_coding=0;
  else if(camera_config.depth==DEPTH16 && camera_config.coding==YUV)
    dyn_rec_config.Color_coding=1;
  else if(camera_config.depth==DEPTH24 && camera_config.coding==YUV)
    dyn_rec_config.Color_coding=2;
  else if(camera_config.depth==DEPTH24 && camera_config.coding==RGB)
    dyn_rec_config.Color_coding=3;
  else if(camera_config.depth==DEPTH16 && camera_config.coding==MONO)
    dyn_rec_config.Color_coding=4;
  else if(camera_config.depth==DEPTH48 && camera_config.coding==RGB)
    dyn_rec_config.Color_coding=5;
  else if(camera_config.depth==DEPTH8 && camera_config.coding==RAW)
    dyn_rec_config.Color_coding=6;
  else if(camera_config.depth==DEPTH16 && camera_config.coding==RAW)
    dyn_rec_config.Color_coding=0;
  else
    dyn_rec_config.Color_coding=3;
}

void FirewireCameraDriver::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 FirewireCameraDriver::set_white_balance(bool *enable,int *mode, int *u_b_value, int *v_r_value)
{
  try{
    if(this->camera->is_feature_present(DC1394_FEATURE_WHITE_BALANCE))
    {
      if(*enable)
      {
        try{
          if(!this->camera->is_feature_enabled(DC1394_FEATURE_WHITE_BALANCE))
            this->camera->enable_feature(DC1394_FEATURE_WHITE_BALANCE);
        }catch(CFirewireFeatureException &e){
          (*enable)=false;
          ROS_INFO("The white balance feature can not be enabled");
          return;
        }
        if((*mode)==0)
        {
          try{
            this->camera->set_feature_auto(DC1394_FEATURE_WHITE_BALANCE);
          }catch(CFirewireFeatureException &e){
            (*mode)=1;
            ROS_INFO("The white balance feature can not be set in auto mode");
            return;
          }
        }
        else
        {
          try{
            this->camera->set_feature_manual(DC1394_FEATURE_WHITE_BALANCE);
          }catch(CFirewireFeatureException &e){
            (*mode)=0;
            ROS_INFO("The white balance feature can not be set in manual mode");
            return;
          }
          try{
            this->camera->set_white_balance_value((*u_b_value),(*v_r_value));
          }catch(CFirewireFeatureException &e){
            this->camera->get_white_balance_value((unsigned int *)u_b_value,(unsigned int *)v_r_value);
            ROS_INFO("The white balance value is out of range");
            return;
          }
        }
      }
      else
      {
        try{
          if(this->camera->is_feature_enabled(DC1394_FEATURE_WHITE_BALANCE))
            this->camera->disable_feature(DC1394_FEATURE_WHITE_BALANCE);
        }catch(CFirewireFeatureException &e){
          (*enable)=true;
          ROS_INFO("The white balance feature can not be disabled");
          return;
        }
      }
    }
    else
    {
      (*enable)=false;
      ROS_INFO("The white balance feature is not present");
    }
  }catch(CFirewireInternalException &e){
    ROS_INFO("Impossible to set the white balance feature");
  }
}

void FirewireCameraDriver::set_shutter(bool *enable,int *mode, int *value)
{
  try{
    if(this->camera->is_feature_present(DC1394_FEATURE_SHUTTER))
    {
      if(*enable)
      {
        try{
          if(!this->camera->is_feature_enabled(DC1394_FEATURE_SHUTTER))
            this->camera->enable_feature(DC1394_FEATURE_SHUTTER);
        }catch(CFirewireFeatureException &e){
          (*enable)=false;
          ROS_INFO("The shutter feature is not present");
          return;
        }
        if((*mode)==0)
        {
          try{
            this->camera->set_feature_auto(DC1394_FEATURE_SHUTTER);
          }catch(CFirewireFeatureException &e){
            (*mode)=1;
            ROS_INFO("The shutter feature can not be set in auto mode");
            return;
          }
        }
        else
        {
          try{
            this->camera->set_feature_manual(DC1394_FEATURE_SHUTTER);
          }catch(CFirewireFeatureException &e){
            (*mode)=0;
            ROS_INFO("The shutter feature can not be set in manual mode");
            return;
          }
          try{
            this->camera->set_feature_value(DC1394_FEATURE_SHUTTER,(*value));
          }catch(CFirewireFeatureException &e){
            (*value)=this->camera->get_feature_value(DC1394_FEATURE_SHUTTER);
            ROS_INFO("The shutter value is out of range");
            return;
          }
        }
      }
      else
      {
        try{
          if(this->camera->is_feature_enabled(DC1394_FEATURE_SHUTTER))
            this->camera->disable_feature(DC1394_FEATURE_SHUTTER);
        }catch(CFirewireFeatureException &e){
          (*enable)=true;
          ROS_INFO("The shutter feature can not be disabled");
          return;
        }
      }
    }
    else
    {
      (*enable)=false;
      ROS_INFO("The shutter feature is not present");
    }
  }catch(CFirewireInternalException &e){
    ROS_INFO("Impossible to set the shutter feature");
  }
}

void FirewireCameraDriver::set_gain(bool *enable, int *mode, int *value)
{
  try{
    if(this->camera->is_feature_present(DC1394_FEATURE_GAIN))
    {
      if(*enable)
      {
        try{
          if(!this->camera->is_feature_enabled(DC1394_FEATURE_GAIN))
            this->camera->enable_feature(DC1394_FEATURE_GAIN);
        }catch(CFirewireFeatureException &e){
          (*enable)=false;
          ROS_INFO("The gain feature is not present");
          return;
        }
        if((*mode)==0)
        {
          try{
            this->camera->set_feature_auto(DC1394_FEATURE_GAIN);
          }catch(CFirewireFeatureException &e){
            (*mode)=1;
            ROS_INFO("The gain feature can not be set in auto mode");
            return;
          }
        }
        else
        {
          try{
            this->camera->set_feature_manual(DC1394_FEATURE_GAIN);
          }catch(CFirewireFeatureException &e){
            (*mode)=0;
            ROS_INFO("The gain feature can not be set in manual mode");
            return;
          }
          try{
            this->camera->set_feature_value(DC1394_FEATURE_GAIN,(*value));
          }catch(CFirewireFeatureException &e){
            (*value)=this->camera->get_feature_value(DC1394_FEATURE_GAIN);
            ROS_INFO("The gain value is out of range");
            return;
          }
        }
      }
      else
      {
        try{
          if(this->camera->is_feature_enabled(DC1394_FEATURE_GAIN))
            this->camera->disable_feature(DC1394_FEATURE_GAIN);
        }catch(CFirewireFeatureException &e){
          (*enable)=true;
          ROS_INFO("The gain feature can not be disabled");
          return;
        }
      }
    }
    else
    {
      (*enable)=false;
      ROS_INFO("The gain feature is not present");
    }
  }catch(CFirewireInternalException &e){
    ROS_INFO("Impossible to set the gain feature");
  }
}

void FirewireCameraDriver::get_image(char **image_data)
{
  if(this->camera!=NULL)
    this->camera->get_image(image_data);
  else
    *image_data=NULL;
}

void FirewireCameraDriver::set_ISO_speed(int iso_speed)
{
  this->ISO_speed=iso_speed;
}

void FirewireCameraDriver::set_config(TCameraConfig *new_conf)
{
  try{
    this->lock();
    this->change_config(new_conf);
    this->unlock();
  }catch(CException &e){
    this->unlock();
    throw;
  }
}

void FirewireCameraDriver::get_current_config(TCameraConfig *current_conf)
{
  current_conf->left_offset=this->current_conf.left_offset;
  current_conf->top_offset=this->current_conf.top_offset;
  current_conf->width=this->current_conf.width;
  current_conf->height=this->current_conf.height;
  current_conf->framerate=this->current_conf.framerate;
  current_conf->depth=this->current_conf.depth;
  current_conf->coding=this->current_conf.coding;
}

void FirewireCameraDriver::get_desired_config(TCameraConfig *desired_conf)
{
  desired_conf->left_offset=this->desired_conf.left_offset;
  desired_conf->top_offset=this->desired_conf.top_offset;
  desired_conf->width=this->desired_conf.width;
  desired_conf->height=this->desired_conf.height;
  desired_conf->framerate=this->desired_conf.framerate;
  desired_conf->depth=this->desired_conf.depth;
  desired_conf->coding=this->desired_conf.coding;
}

void FirewireCameraDriver::get_default_config(TCameraConfig *default_conf)
{
  default_conf->left_offset=this->default_conf.left_offset;
  default_conf->top_offset=this->default_conf.top_offset;
  default_conf->width=this->default_conf.width;
  default_conf->height=this->default_conf.height;
  default_conf->framerate=this->default_conf.framerate;
  default_conf->depth=this->default_conf.depth;
  default_conf->coding=this->default_conf.coding;
}

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

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

  return event;
}

std::string FirewireCameraDriver::get_calibration_file(void)
{
  return this->calibration_file;
}

std::string FirewireCameraDriver::get_frame_id(void)
{
  return this->frame_id;
}

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