pmdcamera_driver.cpp

Go to the documentation of this file.

#include "pmdcamera_driver.h"
#include "pmdcamera_exceptions.h"

PmdcameraDriver::PmdcameraDriver(): PmdCC_(), cameraType_(pmd_camera::invalid) 
{
  //setDriverId(driver string id);  
}

bool PmdcameraDriver::openDriver(void)
{
  //setDriverId(driver string id);

  return true;
}

bool PmdcameraDriver::closeDriver(void)
{
  return true;
}

bool PmdcameraDriver::startDriver(void)
{
  ROS_INFO ("Opening camera connection....");
  if (cameraType_!=pmd_camera::invalid)
  {
    try
    {
      //PmdCC_.open (true, cameraType_); // This is not working for new camboard.
      PmdCC_.open (false, cameraType_, int_time_, mod_freq_);
      ROS_INFO ("Open camera connection");
      return true;
    }
    catch (CException & e)
    {
      ROS_ERROR ("Open camera connection FAILED!!! ");
      std::cout << e.what () << std::endl;
      return false;
    }
  }    
  return false;
}

bool PmdcameraDriver::stopDriver(void)
{
  PmdCC_.close ();
  ROS_INFO ("Close camera connection");
  return true;
}

void PmdcameraDriver::config_update(const Config& new_cfg, uint32_t level)
{
  this->lock();
  
  //update driver with new_cfg data
  if (this->getState () == 2)   //running
  {
    ROS_INFO ("Reconfigure request :%d, level %d", new_cfg.integration_time, level);
    try
    {
      this->PmdCC_.set_integration_time (new_cfg.integration_time);
      this->PmdCC_.set_min_depth_limit(new_cfg.min_dist);
      this->PmdCC_.set_max_depth_limit(new_cfg.max_dist);
      this->PmdCC_.set_modulation_frequency(new_cfg.modulation_freq);
    }
    catch (CException & e)
    {
      std::cout << e.what () << std::endl;
    }
  }
  // save the current configuration
  this->config_ = new_cfg;

  this->unlock();
}

PmdcameraDriver::~PmdcameraDriver()
{  

}

//void CCamcubeDriver::readData (sensor_msgs::PointCloud &cloud)
void
PmdcameraDriver::readData ()
{
  try {
    this->PmdCC_.readData ();    
  }
  catch (CException & e)
  {
    ROS_ERROR ("Read camera: FAILED!!! ");
    std::cout << e.what () << std::endl;
  }
}

float *PmdcameraDriver::get3DImage (bool &is_dense)
{
    return (PmdCC_.get3DData (is_dense));
}

float *PmdcameraDriver::getFlagsImage()
{
    return (PmdCC_.getInvalidFlagsData());
}

float *PmdcameraDriver::getIntensityImage()
{
    return (PmdCC_.getIntensityData());
}

float *PmdcameraDriver::getAmplitudeImage()
{
    return (PmdCC_.getAmplitudeData());
}

float *PmdcameraDriver::getRangeImage()
{
    return (PmdCC_.getDepthData());
}

float *PmdcameraDriver::getDepthImage()
{
    return (PmdCC_.getDepthImage());
}

int PmdcameraDriver::getWidth ()
{
    return (this->PmdCC_.get_width ());
}

int PmdcameraDriver::getHeight ()
{
    return (this->PmdCC_.get_height ());
}

void PmdcameraDriver::setCameraType(pmd_camera::cameraType cameraType)
{
  cameraType_ = cameraType;
}

void PmdcameraDriver::setIntegrationTime(int int_time)
{
  int_time_ = int_time;
}

void PmdcameraDriver::setModulationFrequency(int mod_freq)
{
  mod_freq_ = mod_freq;
}