pmdcamera_driver.cpp
Go to the documentation of this file.
00001 #include "pmdcamera_driver.h"
00002 #include "pmdcamera_exceptions.h"
00003 
00004 PmdcameraDriver::PmdcameraDriver(): PmdCC_(), cameraType_(pmd_camera::invalid) 
00005 {
00006   //setDriverId(driver string id);  
00007 }
00008 
00009 bool PmdcameraDriver::openDriver(void)
00010 {
00011   //setDriverId(driver string id);
00012 
00013   return true;
00014 }
00015 
00016 bool PmdcameraDriver::closeDriver(void)
00017 {
00018   return true;
00019 }
00020 
00021 bool PmdcameraDriver::startDriver(void)
00022 {
00023   ROS_INFO ("Opening camera connection....");
00024   if (cameraType_!=pmd_camera::invalid)
00025   {
00026     try
00027     {
00028       //PmdCC_.open (true, cameraType_); // This is not working for new camboard.
00029       PmdCC_.open (false, cameraType_, int_time_, mod_freq_);
00030       ROS_INFO ("Open camera connection");
00031       return true;
00032     }
00033     catch (CException & e)
00034     {
00035       ROS_ERROR ("Open camera connection FAILED!!! ");
00036       std::cout << e.what () << std::endl;
00037       return false;
00038     }
00039   }    
00040   return false;
00041 }
00042 
00043 bool PmdcameraDriver::stopDriver(void)
00044 {
00045   PmdCC_.close ();
00046   ROS_INFO ("Close camera connection");
00047   return true;
00048 }
00049 
00050 void PmdcameraDriver::config_update(const Config& new_cfg, uint32_t level)
00051 {
00052   this->lock();
00053   
00054   //update driver with new_cfg data
00055   if (this->getState () == 2)   //running
00056   {
00057     ROS_INFO ("Reconfigure request :%d, level %d", new_cfg.integration_time, level);
00058     try
00059     {
00060       this->PmdCC_.set_integration_time (new_cfg.integration_time);
00061       this->PmdCC_.set_min_depth_limit(new_cfg.min_dist);
00062       this->PmdCC_.set_max_depth_limit(new_cfg.max_dist);
00063       this->PmdCC_.set_modulation_frequency(new_cfg.modulation_freq);
00064     }
00065     catch (CException & e)
00066     {
00067       std::cout << e.what () << std::endl;
00068     }
00069   }
00070   // save the current configuration
00071   this->config_ = new_cfg;
00072 
00073   this->unlock();
00074 }
00075 
00076 PmdcameraDriver::~PmdcameraDriver()
00077 {  
00078 
00079 }
00080 
00081 //void CCamcubeDriver::readData (sensor_msgs::PointCloud &cloud)
00082 void
00083 PmdcameraDriver::readData ()
00084 {
00085   try {
00086     this->PmdCC_.readData ();    
00087   }
00088   catch (CException & e)
00089   {
00090     ROS_ERROR ("Read camera: FAILED!!! ");
00091     std::cout << e.what () << std::endl;
00092   }
00093 }
00094 
00095 float *PmdcameraDriver::get3DImage (bool &is_dense)
00096 {
00097     return (PmdCC_.get3DData (is_dense));
00098 }
00099 
00100 float *PmdcameraDriver::getFlagsImage()
00101 {
00102     return (PmdCC_.getInvalidFlagsData());
00103 }
00104 
00105 float *PmdcameraDriver::getIntensityImage()
00106 {
00107     return (PmdCC_.getIntensityData());
00108 }
00109 
00110 float *PmdcameraDriver::getAmplitudeImage()
00111 {
00112     return (PmdCC_.getAmplitudeData());
00113 }
00114 
00115 float *PmdcameraDriver::getRangeImage()
00116 {
00117     return (PmdCC_.getDepthData());
00118 }
00119 
00120 float *PmdcameraDriver::getDepthImage()
00121 {
00122     return (PmdCC_.getDepthImage());
00123 }
00124 
00125 int PmdcameraDriver::getWidth ()
00126 {
00127     return (this->PmdCC_.get_width ());
00128 }
00129 
00130 int PmdcameraDriver::getHeight ()
00131 {
00132     return (this->PmdCC_.get_height ());
00133 }
00134 
00135 void PmdcameraDriver::setCameraType(pmd_camera::cameraType cameraType)
00136 {
00137   cameraType_ = cameraType;
00138 }
00139 
00140 void PmdcameraDriver::setIntegrationTime(int int_time)
00141 {
00142   int_time_ = int_time;
00143 }
00144 
00145 void PmdcameraDriver::setModulationFrequency(int mod_freq)
00146 {
00147   mod_freq_ = mod_freq;
00148 }
00149 


iri_pmdcamera
Author(s): Guillem Alenya - IRI Robotics Lab
autogenerated on Fri Dec 6 2013 20:10:05