Go to the documentation of this file.00001 #include "ladybug2_driver.h"
00002
00003 Ladybug2Driver::Ladybug2Driver(void)
00004 {
00005
00006 this->server=CFirewireServer::instance();
00007 this->server->init();
00008 this->camera=NULL;
00009 this->camera_id=-1;
00010
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=DEPTH8;
00016 this->desired_conf.coding=RGB;
00017 this->desired_conf.framerate=0.0;
00018
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=DEPTH8;
00024 this->current_conf.coding=RGB;
00025 this->current_conf.framerate=0.0;
00026
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=DEPTH8;
00032 this->default_conf.coding=RGB;
00033 this->default_conf.framerate=0.0;
00034
00035 this->calibration_files.resize(NUM_CAM);
00036
00037 this->frame_ids.resize(NUM_CAM);
00038 }
00039
00040 bool Ladybug2Driver::openDriver(void)
00041 {
00042
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_ladybug_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->change_config(&this->desired_conf);
00053 ROS_INFO("Driver opened");
00054 return true;
00055 }
00056 else
00057 {
00058 ROS_INFO("No cameras available");
00059 return false;
00060 }
00061 }
00062 else
00063 {
00064 ROS_INFO("Waiting for a valid id ...");
00065 return false;
00066 }
00067 }
00068
00069 bool Ladybug2Driver::closeDriver(void)
00070 {
00071 if(this->camera!=NULL)
00072 {
00073 delete this->camera;
00074 this->camera=NULL;
00075 }
00076
00077 return true;
00078 }
00079
00080 bool Ladybug2Driver::startDriver(void)
00081 {
00082 if(this->camera!=NULL)
00083 {
00084 try{
00085 this->lock();
00086 this->camera->start();
00087 this->unlock();
00088 ROS_INFO("Driver started");
00089 return true;
00090 }catch(CException &e){
00091 this->unlock();
00092
00093 return false;
00094 }
00095 }
00096 else
00097 return false;
00098 }
00099
00100 bool Ladybug2Driver::stopDriver(void)
00101 {
00102 if(this->camera!=NULL)
00103 {
00104 try{
00105 this->lock();
00106 this->camera->stop();
00107 this->unlock();
00108 ROS_INFO("Driver stopped");
00109 return true;
00110 }catch(CException &e){
00111 this->unlock();
00112
00113 return false;
00114 }
00115 }
00116 else
00117 return false;
00118 }
00119
00120 void Ladybug2Driver::config_update(Config& new_cfg, uint32_t level)
00121 {
00122 TCameraConfig conf;
00123
00124
00125
00126 switch(this->getState())
00127 {
00128 case Ladybug2Driver::CLOSED:
00129 this->lock();
00130 this->camera_id=new_cfg.Camera_node;
00131 this->desired_conf.framerate=new_cfg.Framerate;
00132
00133 this->calibration_files[FRONT]=new_cfg.front_cal_file;
00134 this->calibration_files[FRONT_RIGHT]=new_cfg.front_right_cal_file;
00135 this->calibration_files[REAR_RIGHT]=new_cfg.rear_right_cal_file;
00136 this->calibration_files[REAR_LEFT]=new_cfg.rear_left_cal_file;
00137 this->calibration_files[FRONT_LEFT]=new_cfg.front_left_cal_file;
00138 this->calibration_files[TOP]=new_cfg.top_cal_file;
00139
00140 this->frame_ids[FRONT]=new_cfg.front_frame_id;
00141 this->frame_ids[FRONT_RIGHT]=new_cfg.front_right_frame_id;
00142 this->frame_ids[REAR_RIGHT]=new_cfg.rear_right_frame_id;
00143 this->frame_ids[REAR_LEFT]=new_cfg.rear_left_frame_id;
00144 this->frame_ids[FRONT_LEFT]=new_cfg.front_left_frame_id;
00145 this->frame_ids[TOP]=new_cfg.top_frame_id;
00146 this->unlock();
00147 break;
00148
00149 case Ladybug2Driver::OPENED:
00150 if(new_cfg.Camera_node!=-1)
00151 {
00152 try{
00153 this->lock();
00154 if(this->camera_id!=(uint64_t)new_cfg.Camera_node)
00155 {
00156 this->close();
00157 this->camera_id=new_cfg.Camera_node;
00158 this->open();
00159 }
00160 conf.framerate=new_cfg.Framerate;
00161 this->change_config(&conf);
00162
00163 new_cfg.Framerate=conf.framerate;
00164
00165 this->calibration_files[FRONT]=new_cfg.front_cal_file;
00166 this->calibration_files[FRONT_RIGHT]=new_cfg.front_right_cal_file;
00167 this->calibration_files[REAR_RIGHT]=new_cfg.rear_right_cal_file;
00168 this->calibration_files[REAR_LEFT]=new_cfg.rear_left_cal_file;
00169 this->calibration_files[FRONT_LEFT]=new_cfg.front_left_cal_file;
00170 this->calibration_files[TOP]=new_cfg.top_cal_file;
00171
00172 this->frame_ids[FRONT]=new_cfg.front_frame_id;
00173 this->frame_ids[FRONT_RIGHT]=new_cfg.front_right_frame_id;
00174 this->frame_ids[REAR_RIGHT]=new_cfg.rear_right_frame_id;
00175 this->frame_ids[REAR_LEFT]=new_cfg.rear_left_frame_id;
00176 this->frame_ids[FRONT_LEFT]=new_cfg.front_left_frame_id;
00177 this->frame_ids[TOP]=new_cfg.top_frame_id;
00178 this->unlock();
00179 }catch(CFirewireCameraException &e){
00180 ROS_INFO("Invalid configuration. Setting back the default configuration ...");
00181 this->change_config(&this->default_conf);
00182 new_cfg.Framerate=this->default_conf.framerate;
00183 this->unlock();
00184 }
00185 }
00186 break;
00187
00188 case Ladybug2Driver::RUNNING:
00189 break;
00190 }
00191
00192
00193 this->config_=new_cfg;
00194 }
00195
00196 void Ladybug2Driver::change_config(TCameraConfig *new_conf)
00197 {
00198 try{
00199 if(this->camera!=NULL)
00200 {
00201 this->current_conf.left_offset=new_conf->left_offset;
00202 this->current_conf.top_offset=new_conf->top_offset;
00203 this->current_conf.width=new_conf->width;
00204 this->current_conf.height=new_conf->height;
00205 this->current_conf.framerate=new_conf->framerate;
00206 this->current_conf.depth=new_conf->depth;
00207 this->current_conf.coding=new_conf->coding;
00208 this->camera->set_config(&this->current_conf.left_offset,&this->current_conf.top_offset,&this->current_conf.width,
00209 &this->current_conf.height,&this->current_conf.framerate,this->current_conf.depth,
00210 this->current_conf.coding);
00211 this->desired_conf.left_offset=new_conf->left_offset;
00212 new_conf->left_offset=current_conf.left_offset;
00213 this->desired_conf.top_offset=new_conf->top_offset;
00214 new_conf->top_offset=current_conf.top_offset;
00215 this->desired_conf.width=new_conf->width;
00216 new_conf->width=current_conf.width;
00217 this->desired_conf.height=new_conf->height;
00218 new_conf->height=current_conf.height;
00219 this->desired_conf.framerate=new_conf->framerate;
00220 new_conf->framerate=current_conf.framerate;
00221 this->desired_conf.depth=new_conf->depth;
00222 new_conf->depth=current_conf.depth;
00223 this->desired_conf.coding=new_conf->coding;
00224 new_conf->coding=current_conf.coding;
00225 }
00226 }catch(CFirewireCameraException &e){
00227 ROS_INFO("Invalid configuration. Setting back the default configuration ...");
00228 this->change_config(&this->default_conf);
00229 new_conf->left_offset=this->default_conf.left_offset;
00230 new_conf->top_offset=this->default_conf.top_offset;
00231 new_conf->width=this->default_conf.width;
00232 new_conf->height=this->default_conf.height;
00233 new_conf->framerate=this->default_conf.framerate;
00234 new_conf->depth=this->default_conf.depth;
00235 new_conf->coding=this->default_conf.coding;
00236 }catch(CFirewireInternalException &e){
00237 throw;
00238 }
00239 }
00240
00241 void Ladybug2Driver::get_current_config(TCameraConfig *current_conf)
00242 {
00243 this->camera->get_multi_config(&(current_conf->width),&(current_conf->height),&(current_conf->framerate),&(current_conf->depth),&(current_conf->coding));
00244
00245 current_conf->left_offset=0;
00246 current_conf->top_offset=0;
00247 }
00248
00249 void Ladybug2Driver::get_images(char **front,char **front_right,char **rear_right, char **rear_left,char **front_left,char **top)
00250 {
00251 if(this->camera!=NULL)
00252 this->camera->get_images(front,front_right,rear_right,rear_left,front_left,top);
00253 else
00254 {
00255 *front=NULL;
00256 *front_right=NULL;
00257 *rear_right=NULL;
00258 *rear_left=NULL;
00259 *front_left=NULL;
00260 *top=NULL;
00261 }
00262 }
00263
00264 std::string Ladybug2Driver::get_new_frame_event(void)
00265 {
00266 std::string event="";
00267
00268 if(this->camera!=NULL)
00269 this->camera->get_new_frame_event_id(event);
00270
00271 return event;
00272 }
00273
00274 void Ladybug2Driver::get_calibration_files(std::vector<std::string> &calibration_files)
00275 {
00276 int i=0;
00277
00278 calibration_files.resize(NUM_CAM);
00279 for(i=0;i<NUM_CAM;i++)
00280 calibration_files[i]=this->calibration_files[i];
00281 }
00282
00283 void Ladybug2Driver::get_frame_ids(std::vector<std::string> &frame_ids)
00284 {
00285 int i=0;
00286
00287 frame_ids.resize(NUM_CAM);
00288 for(i=0;i<NUM_CAM;i++)
00289 frame_ids[i]=this->frame_ids[i];
00290 }
00291
00292 Ladybug2Driver::~Ladybug2Driver(void)
00293 {
00294 this->server->close();
00295 }