bumblebee2_driver.cpp
Go to the documentation of this file.
00001 #include "bumblebee2_driver.h"
00002 
00003 Bumblebee2Driver::Bumblebee2Driver(void)
00004 {
00005   //setDriverId(driver string id);
00006   this->server=CFirewireServer::instance();
00007   this->server->init();
00008   this->camera=NULL;
00009   this->camera_id=-1;
00010   // desired configuration
00011   this->desired_conf.width=1024;
00012   this->desired_conf.height=768;
00013   this->desired_conf.left_offset=0;
00014   this->desired_conf.top_offset=0;
00015   this->desired_conf.depth=DEPTH16;
00016   this->desired_conf.coding=RAW;
00017   this->desired_conf.framerate=0.0;
00018   // current configuration
00019   this->current_conf.width=1024;
00020   this->current_conf.height=768;
00021   this->current_conf.left_offset=0;
00022   this->current_conf.top_offset=0;
00023   this->current_conf.depth=DEPTH16;
00024   this->current_conf.coding=RAW;
00025   this->current_conf.framerate=0.0;
00026   // default configuration
00027   this->default_conf.width=1024;
00028   this->default_conf.height=768;
00029   this->default_conf.left_offset=0;
00030   this->default_conf.top_offset=0;
00031   this->default_conf.depth=DEPTH16;
00032   this->default_conf.coding=RAW;
00033   this->default_conf.framerate=0.0;
00034   // default calibration file
00035   this->calibration_files.resize(NUM_CAM);
00036   // default frame id
00037   this->frame_ids.resize(NUM_CAM);
00038 }
00039 
00040 bool Bumblebee2Driver::openDriver(void)
00041 {
00042   //setDriverId(driver string id);
00043   if(this->camera_id!=(uint64_t)-1)
00044   {
00045     this->server->init();
00046     if(this->server->get_num_cameras()>0)
00047     {
00048       this->server->get_bumblebee_camera(this->camera_id,&this->camera);
00049       this->camera->get_config(&this->default_conf.left_offset,&this->default_conf.top_offset,&this->default_conf.width,
00050                                &this->default_conf.height,&this->default_conf.framerate,&this->default_conf.depth,
00051                                &this->default_conf.coding);
00052       this->desired_conf.coding=this->default_conf.coding;
00053       this->change_config(&this->desired_conf);
00054       ROS_INFO("Driver opened");
00055       return true;
00056     }
00057     else
00058     {
00059       ROS_INFO("No cameras available");
00060       return false;
00061     }
00062   }
00063   else
00064   {
00065     ROS_INFO("Waiting for a valid id ...");
00066     return false;
00067   }
00068 }
00069 
00070 bool Bumblebee2Driver::closeDriver(void)
00071 {
00072   if(this->camera!=NULL)
00073   {
00074     delete this->camera;
00075     this->camera=NULL;
00076   }
00077 
00078   return true;
00079 }
00080 
00081 bool Bumblebee2Driver::startDriver(void)
00082 {
00083   if(this->camera!=NULL)
00084   {
00085     try{
00086       this->lock();
00087       this->camera->start();
00088       this->unlock();
00089       ROS_INFO("Driver started");
00090       return true;
00091     }catch(CException &e){
00092       this->unlock();
00093       //ROS_INFO(e.what().data());
00094       return false;
00095     }
00096   }
00097   else
00098     return false;
00099 }
00100 
00101 bool Bumblebee2Driver::stopDriver(void)
00102 {
00103   if(this->camera!=NULL)
00104   {
00105     try{
00106       this->lock();
00107       this->camera->stop();
00108       this->unlock();
00109       ROS_INFO("Driver stopped");
00110       return true;
00111     }catch(CException &e){
00112       this->unlock();
00113       //ROS_INFO(e.what().data());
00114       return false;
00115     }
00116   }
00117   else
00118     return false;
00119 }
00120 
00121 void Bumblebee2Driver::config_update(Config& new_cfg, uint32_t level)
00122 {
00123   TCameraConfig conf;
00124 
00125   // depending on current state
00126   // update driver with new_cfg data
00127   switch(this->getState())
00128   {
00129     case Bumblebee2Driver::CLOSED:
00130       this->lock();
00131       this->camera_id=new_cfg.Camera_node;
00132       this->desired_conf.framerate=new_cfg.Framerate;
00133       // change the calibration file
00134       this->calibration_files[LEFT]=new_cfg.left_cal_file;
00135       this->calibration_files[RIGHT]=new_cfg.right_cal_file;
00136       // update the frame identifier
00137       this->frame_ids[LEFT]=new_cfg.left_frame_id;
00138       this->frame_ids[RIGHT]=new_cfg.right_frame_id;
00139       this->unlock();
00140       break;
00141 
00142     case Bumblebee2Driver::OPENED:
00143       if(new_cfg.Camera_node!=-1)
00144       {
00145         try{
00146           this->lock();
00147           if(this->camera_id!=(uint64_t)new_cfg.Camera_node)
00148           {
00149             this->close();
00150             this->camera_id=new_cfg.Camera_node;
00151             this->open();
00152           }
00153           conf.framerate=new_cfg.Framerate;
00154           this->change_config(&conf);
00155           // write the configuration back to the dynamic reconfigure window
00156           new_cfg.Framerate=conf.framerate;
00157           // change the calibration file
00158           this->calibration_files[LEFT]=new_cfg.left_cal_file;
00159           this->calibration_files[RIGHT]=new_cfg.right_cal_file;
00160           // update the frame identifier
00161           this->frame_ids[LEFT]=new_cfg.left_frame_id;
00162           this->frame_ids[RIGHT]=new_cfg.right_frame_id;
00163           this->unlock();
00164         }catch(CFirewireCameraException &e){
00165           ROS_INFO("Invalid configuration. Setting back the default configuration ...");
00166           this->change_config(&this->default_conf);
00167           new_cfg.Framerate=this->default_conf.framerate;
00168           this->unlock();
00169         }
00170       }
00171       break;
00172     
00173     case Bumblebee2Driver::RUNNING:
00174       break;
00175   }
00176 
00177   // save the current configuration
00178   this->config_=new_cfg;
00179 }
00180 
00181 void Bumblebee2Driver::change_config(TCameraConfig *new_conf)
00182 {
00183   try{
00184     if(this->camera!=NULL)
00185     {
00186       this->current_conf.left_offset=new_conf->left_offset;
00187       this->current_conf.top_offset=new_conf->top_offset;
00188       this->current_conf.width=new_conf->width;
00189       this->current_conf.height=new_conf->height;
00190       this->current_conf.framerate=new_conf->framerate;
00191       this->current_conf.depth=new_conf->depth;
00192       this->current_conf.coding=new_conf->coding;
00193       this->camera->set_config(&this->current_conf.left_offset,&this->current_conf.top_offset,&this->current_conf.width,
00194                                &this->current_conf.height,&this->current_conf.framerate,this->current_conf.depth,
00195                                this->current_conf.coding);
00196       this->desired_conf.left_offset=new_conf->left_offset;
00197       new_conf->left_offset=current_conf.left_offset;
00198       this->desired_conf.top_offset=new_conf->top_offset;
00199       new_conf->top_offset=current_conf.top_offset;
00200       this->desired_conf.width=new_conf->width;
00201       new_conf->width=current_conf.width;
00202       this->desired_conf.height=new_conf->height;
00203       new_conf->height=current_conf.height;
00204       this->desired_conf.framerate=new_conf->framerate;
00205       new_conf->framerate=current_conf.framerate;
00206       this->desired_conf.depth=new_conf->depth;
00207       new_conf->depth=current_conf.depth;
00208       this->desired_conf.coding=new_conf->coding;
00209       new_conf->coding=current_conf.coding;
00210     }
00211   }catch(CFirewireCameraException &e){
00212     ROS_INFO("Invalid configuration. Setting back the default configuration ...");
00213     this->change_config(&this->default_conf);
00214     new_conf->left_offset=this->default_conf.left_offset;
00215     new_conf->top_offset=this->default_conf.top_offset;
00216     new_conf->width=this->default_conf.width;
00217     new_conf->height=this->default_conf.height;
00218     new_conf->framerate=this->default_conf.framerate;
00219     new_conf->depth=this->default_conf.depth;
00220     new_conf->coding=this->default_conf.coding;
00221   }catch(CFirewireInternalException &e){
00222     throw;
00223   }
00224 }
00225 
00226 void Bumblebee2Driver::get_current_config(TCameraConfig *current_conf)
00227 {
00228   this->camera->get_stereo_config(&(current_conf->width),&(current_conf->height),&(current_conf->framerate),&(current_conf->depth),&(current_conf->coding));
00229 
00230   current_conf->left_offset=0;
00231   current_conf->top_offset=0;
00232 }
00233 
00234 void Bumblebee2Driver::get_images(char **left_image,char **right_image)
00235 {
00236   if(this->camera!=NULL)
00237     this->camera->get_stereo_image(left_image,right_image);
00238   else
00239   {
00240     *left_image=NULL;
00241     *right_image=NULL;
00242   }
00243 }
00244 
00245 std::string Bumblebee2Driver::get_new_frame_event(void)
00246 {
00247   std::string event="";
00248 
00249   if(this->camera!=NULL)
00250     this->camera->get_new_frame_event_id(event);
00251 
00252   return event;
00253 }
00254 
00255 void Bumblebee2Driver::get_calibration_files(std::vector<std::string> &calibration_files)
00256 {
00257   calibration_files.resize(NUM_CAM);
00258   calibration_files[LEFT]=this->calibration_files[LEFT];
00259   calibration_files[RIGHT]=this->calibration_files[RIGHT];
00260 }
00261 
00262 void Bumblebee2Driver::get_frame_ids(std::vector<std::string> &frame_ids)
00263 {
00264   frame_ids.resize(NUM_CAM);
00265   frame_ids[LEFT]=this->frame_ids[LEFT];
00266   frame_ids[RIGHT]=this->frame_ids[RIGHT];
00267 }
00268 
00269 Bumblebee2Driver::~Bumblebee2Driver(void)
00270 {
00271   this->server->close();
00272 }


iri_bumblebee2
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 22:29:51