firewire_camera_driver_node.cpp
Go to the documentation of this file.
00001 #include "firewire_camera_driver_node.h"
00002 
00003 FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_))
00004 {
00005   std::string cal_file;
00006 
00007   //init class attributes if necessary
00008   this->loop_rate_ = 1000;//in [Hz]
00009   this->desired_framerate=30.0;
00010   this->diagnosed_camera_image=NULL;
00011   this->it=NULL;
00012 
00013   this->it=new image_transport::ImageTransport(this->public_node_handle_);
00014   this->diagnosed_camera_image = new diagnostic_updater::HeaderlessTopicDiagnostic("camera_image",this->diagnostic_,
00015                                      diagnostic_updater::FrequencyStatusParam(&this->desired_framerate,&this->desired_framerate,0.01,5));
00016 
00017   this->event_server=CEventServer::instance();
00018 
00019   // [init publishers]
00020   this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);
00021 
00022   // try to load the calibration file
00023   public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
00024   if(this->camera_manager.validateURL(cal_file))
00025   {
00026     if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
00027       ROS_INFO("Invalid calibration file");
00028   } 
00029   else
00030     ROS_INFO("Invalid calibration file");
00031 
00032   public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
00033   
00034   // [init subscribers]
00035   
00036   // [init services]
00037   
00038   // [init clients]
00039   
00040   // [init action servers]
00041   
00042   // [init action clients]
00043 }
00044 
00045 void FirewireCameraDriverNode::mainNodeThread(void)
00046 {
00047   sensor_msgs::CameraInfo camera_info=this->camera_manager.getCameraInfo();
00048   std::list<std::string> events;
00049   char *image_data=NULL;
00050   TCameraConfig config;
00051 
00052   try{
00053     //lock access to driver if necessary
00054     this->driver_.lock();
00055     if(this->driver_.isRunning())
00056     {
00057       this->new_frame_event=this->driver_.get_new_frame_event();
00058       if(this->new_frame_event!="")
00059       {
00060         events.push_back(this->new_frame_event);
00061         this->event_server->wait_all(events,1000);
00062         this->driver_.get_image(&image_data);
00063         if(image_data!=NULL && this->camera_image_publisher_.getNumSubscribers()>0)
00064         {
00065           this->driver_.get_current_config(&config);
00066           // update the desired framerate for the diagnostics
00067           this->desired_framerate=config.framerate;
00068           //fill msg structures
00069           this->Image_msg_.width=config.width;
00070           this->Image_msg_.height=config.height;
00071           this->Image_msg_.step=config.width*config.depth/8;
00072           this->Image_msg_.data=std::vector<unsigned char>(image_data,image_data+config.width*config.height*config.depth/8);
00073           if(config.coding==MONO || config.coding==RAW)
00074           {
00075             if(config.depth==DEPTH8)
00076               this->Image_msg_.encoding="8UC1";
00077             else if(config.depth==DEPTH16)
00078               this->Image_msg_.encoding="16UC1";
00079             else
00080               this->Image_msg_.encoding="16UC1";
00081           }
00082           else if(config.coding==YUV)
00083           {
00084             this->Image_msg_.encoding="yuv422";
00085           }
00086           else
00087           {
00088             if(config.depth==DEPTH24)
00089               this->Image_msg_.encoding="rgb8";
00090             else if(config.depth==DEPTH48)
00091               this->Image_msg_.encoding="16UC3";
00092             else
00093               this->Image_msg_.encoding="16UC3";
00094           }
00095         }
00096         delete[] image_data;
00097         //publish messages
00098         this->Image_msg_.header.stamp = ros::Time::now();
00099         this->Image_msg_.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
00100         camera_info.header.stamp = this->Image_msg_.header.stamp;
00101         camera_info.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
00102         //this->Image_msg_.header.frame_id = "<publisher_topic_name>";
00103         this->camera_image_publisher_.publish(this->Image_msg_,camera_info);
00104         this->diagnosed_camera_image->tick();
00105       }
00106     }
00107     this->driver_.unlock();
00108   }catch(CException &e){
00109     this->driver_.unlock();
00110     ROS_INFO("Impossible to capture frame");
00111   }
00112   //fill srv structure and make request to the server
00113 }
00114   // [fill msg Header if necessary]
00115 
00116 void FirewireCameraDriverNode::check_configuration(diagnostic_updater::DiagnosticStatusWrapper &stat)
00117 {
00118   TCameraConfig desired_config;
00119   TCameraConfig current_config;
00120 
00121   this->driver_.get_current_config(&current_config);
00122   this->driver_.get_desired_config(&desired_config);
00123 
00124   if(current_config.left_offset!=desired_config.left_offset)
00125   {
00126     stat.summary(1,"The current configuration differs from the desired configuration");
00127     stat.addf("Image left offset:","current value is %d,desired value is %d", current_config.left_offset,desired_config.left_offset);
00128   }
00129   else if(current_config.top_offset!=desired_config.top_offset)
00130   {
00131     stat.summary(1,"The current configuration differs from the desired configuration");
00132     stat.addf("Image top offset:","current value is %d,desired value is %d", current_config.top_offset,desired_config.top_offset);
00133   }
00134   else if(current_config.width!=desired_config.width)
00135   {
00136     stat.summary(1,"The current configuration differs from the desired configuration");
00137     stat.addf("Image width:","current value is %d,desired value is %d", current_config.width,desired_config.width);
00138   }
00139   else if(current_config.height!=desired_config.height)
00140   {
00141     stat.summary(1,"The current configuration differs from the desired configuration");
00142     stat.addf("Image height:","current value is %d,desired value is %d", current_config.height,desired_config.height);
00143   }
00144   else
00145     stat.summary(0,"The current configuration is okay");
00146 }
00147 
00148 /*  [subscriber callbacks] */
00149 
00150 /*  [service callbacks] */
00151 
00152 /*  [action callbacks] */
00153 
00154 /*  [action requests] */
00155 
00156 void FirewireCameraDriverNode::postNodeOpenHook(void)
00157 {
00158 }
00159 
00160 void FirewireCameraDriverNode::addNodeDiagnostics(void)
00161 {
00162   diagnostic_.add("Check Configuration", this, &FirewireCameraDriverNode::check_configuration);
00163 }
00164 
00165 void FirewireCameraDriverNode::addNodeOpenedTests(void)
00166 {
00167 }
00168 
00169 void FirewireCameraDriverNode::addNodeStoppedTests(void)
00170 {
00171 }
00172 
00173 void FirewireCameraDriverNode::addNodeRunningTests(void)
00174 {
00175 }
00176 
00177 void FirewireCameraDriverNode::reconfigureNodeHook(int level)
00178 {
00179   // try to load the calibration file
00180   if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
00181   {
00182     this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
00183   } 
00184   else
00185     ROS_INFO("Invalid calibration file");
00186 }
00187 
00188 FirewireCameraDriverNode::~FirewireCameraDriverNode()
00189 {
00190   // [free dynamic memory]
00191   if(this->it!=NULL)
00192   {
00193     delete this->it;
00194     this->it=NULL;
00195   }
00196   if(this->diagnosed_camera_image!=NULL)
00197   {
00198     delete this->diagnosed_camera_image;
00199     this->diagnosed_camera_image=NULL;
00200   }
00201 }
00202 
00203 /* main function */
00204 int main(int argc,char *argv[])
00205 {
00206   return driver_base::main<FirewireCameraDriverNode>(argc,argv,"firewire_camera_driver_node");
00207 }


iri_firewire_camera
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 22:28:49