pmdcamera_driver_node.cpp

Go to the documentation of this file.

#include "pmdcamera_driver_node.h"
#include <boost/make_shared.hpp>
#include "exceptions.h"

PmdcameraDriverNode::PmdcameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<PmdcameraDriver>(nh) {

  max_intensity_ = 50000.0;  // This value doesn't need to be fixed for calibration.(experimentally it didn't overpass 34539.2)
  max_amplitude_ = 50000.0;  // This value needs to be fixed for calibration.(experimentally it didn't overpass 48776.8)
  //TODO: Canviar la max_depth segons la frequencia de modulacio
  max_depth_ = 7.5;  // (experimentally it didn't overpass 7.49)

  int camera_type=0;
  private_node_handle_.param<int>("camera_type", camera_type, pmd_camera::camboard);
  std::string frame_id="/camboard_frame";
  private_node_handle_.param<std::string>("frame_id", frame_id, "/camboard_frame");

  int int_time=700;
  private_node_handle_.param<int>("integration_time", int_time, 700);
  int mod_freq=20000000;
  private_node_handle_.param<int>("modulation_freq", mod_freq, 20000000);

  if (camera_type==pmd_camera::camboard) {
    driver_.setCameraType(pmd_camera::camboard);
    ROS_INFO("Camera type CAMBOARD");
  } else  if (camera_type==pmd_camera::camcube) {
    driver_.setCameraType(pmd_camera::camcube);
    ROS_INFO("Camera type CAMCUBE");  
  } else {
    ROS_WARN("Camera type not recognized. Trying camboard...");  
  }

  driver_.setIntegrationTime(int_time);
  driver_.setModulationFrequency(mod_freq);

  // Obtain real camera width and height
  real_width_  = driver_.getWidth();  // 207
  real_height_ = driver_.getHeight(); // 204

  //CamBoard has some dead pixels and efective image size is 200x200
  ROS_INFO("Raw Image size %d %d", real_width_, real_height_);

  // Define working camera width and height
  width_  = 200;
  height_ = 200;

  // Init class attributes if necessary
  loop_rate_ = 50; //in [Hz]

  // Assemble the point cloud data
  if(camera_type==pmd_camera::camboard){
    pcl2_int_msg_.header.frame_id = std::string (frame_id);
    pcl2_amp_msg_.header.frame_id = std::string (frame_id);
    pcl2_range_msg_.header.frame_id = std::string (frame_id);
  } else if (camera_type==pmd_camera::camcube) {
    pcl2_int_msg_.header.frame_id = std::string (frame_id);
    pcl2_amp_msg_.header.frame_id = std::string (frame_id);
    pcl2_range_msg_.header.frame_id = std::string (frame_id);
  } else {
    ROS_WARN("Camera type not recognized. Trying again...");  
  }

  // PointCloud_int is XYZI
  pcl2_int_msg_.height         = height_;
  pcl2_int_msg_.width          = width_;
  pcl2_int_msg_.fields.resize (4);
  pcl2_int_msg_.fields[0].name = "x";
  pcl2_int_msg_.fields[1].name = "y";
  pcl2_int_msg_.fields[2].name = "z";
  pcl2_int_msg_.fields[3].name = "intensity";
  // PointCloud_amp is XYZI
  pcl2_amp_msg_.height         = height_;
  pcl2_amp_msg_.width          = width_;
  pcl2_amp_msg_.fields.resize (4);
  pcl2_amp_msg_.fields[0].name = "x";
  pcl2_amp_msg_.fields[1].name = "y";
  pcl2_amp_msg_.fields[2].name = "z";
  pcl2_amp_msg_.fields[3].name = "intensity";
  // PointCloud_range is PointWithRange
  pcl2_range_msg_.height         = height_;
  pcl2_range_msg_.width          = width_;
  pcl2_range_msg_.fields.resize (5);
  pcl2_range_msg_.fields[0].name = "x";
  pcl2_range_msg_.fields[1].name = "y";
  pcl2_range_msg_.fields[2].name = "z";
  pcl2_range_msg_.fields[3].name = "range";
  pcl2_range_msg_.fields[4].name = "intensity";

  // Set all the fields types accordingly
  int offset = 0;
  // pcl2_int and pcl2_amp
  for (size_t s = 0; s < pcl2_int_msg_.fields.size (); ++s, offset += 4)
  {
    pcl2_int_msg_.fields[s].offset   = offset;
    pcl2_int_msg_.fields[s].count    = 1;
    pcl2_int_msg_.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
    pcl2_amp_msg_.fields[s].offset   = offset;
    pcl2_amp_msg_.fields[s].count    = 1;
    pcl2_amp_msg_.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
  }
  pcl2_int_msg_.point_step = offset;
  pcl2_int_msg_.row_step   = pcl2_int_msg_.point_step * pcl2_int_msg_.width;
  pcl2_int_msg_.data.resize (pcl2_int_msg_.row_step   * pcl2_int_msg_.height);
  pcl2_int_msg_.is_dense   = true;
  pcl2_amp_msg_.point_step = offset;
  pcl2_amp_msg_.row_step   = pcl2_amp_msg_.point_step * pcl2_amp_msg_.width;
  pcl2_amp_msg_.data.resize (pcl2_amp_msg_.row_step   * pcl2_amp_msg_.height);
  pcl2_amp_msg_.is_dense   = true;

  offset = 0;
  // pcl2_range
  for (size_t s = 0; s < pcl2_range_msg_.fields.size (); ++s, offset += 4)
  {
    pcl2_range_msg_.fields[s].offset   = offset;
    pcl2_range_msg_.fields[s].count    = 1;
    pcl2_range_msg_.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
  }
  pcl2_range_msg_.point_step = offset;
  pcl2_range_msg_.row_step   = pcl2_range_msg_.point_step * pcl2_range_msg_.width;
  pcl2_range_msg_.data.resize (pcl2_range_msg_.row_step   * pcl2_range_msg_.height);
  pcl2_range_msg_.is_dense   = true;

  // Assemble the intensity image data
  Image_flags_msg_.header.frame_id = pcl2_int_msg_.header.frame_id;
  Image_flags_msg_.height   = height_;
  Image_flags_msg_.width    = width_;
  Image_flags_msg_.encoding = sensor_msgs::image_encodings::MONO8;
  Image_flags_msg_.step     = width_;
  Image_flags_msg_.data.resize(width_ * height_);

  // Assemble the intensity image data
  Image_int_msg_.header.frame_id = pcl2_int_msg_.header.frame_id;
  Image_int_msg_.height   = height_;
  Image_int_msg_.width    = width_;
  Image_int_msg_.encoding = sensor_msgs::image_encodings::MONO8;
  Image_int_msg_.step     = width_ * sizeof(uint8_t);
  Image_int_msg_.data.resize(Image_int_msg_.step * height_);

  // Assemble the amplitude image data
  Image_amp_msg_.header.frame_id = pcl2_int_msg_.header.frame_id;
  Image_amp_msg_.height   = height_;
  Image_amp_msg_.width    = width_;
  Image_amp_msg_.encoding = sensor_msgs::image_encodings::MONO8;
  Image_amp_msg_.step     = width_ * sizeof(uint8_t);
  Image_amp_msg_.data.resize(Image_amp_msg_.step * height_);

  // Assemble the depth image data
  Image_depth_msg_.header.frame_id = pcl2_int_msg_.header.frame_id;
  Image_depth_msg_.height          = height_;
  Image_depth_msg_.width           = width_;
  Image_depth_msg_.encoding        = sensor_msgs::image_encodings::TYPE_32FC1;
  Image_depth_msg_.step            = width_ * sizeof(float);
  Image_depth_msg_.data.resize(Image_depth_msg_.step * height_);

  //@TODO: put the paths in a configuration file?
  //@TODO: podem fer servir public_handler o private_handler 
  // Read calibration parameters from disk
  switch (camera_type) {
    case pmd_camera::camboard:
      public_node_handle_.param ("camera_name", camera_name_, std::string("camboard"));
      public_node_handle_.param ("camera_info_url", camera_info_url_, std::string("file://")+ros::package::getPath(ROS_PACKAGE_NAME)+std::string("/info/camboard_calibration.yaml"));
      break;
    case pmd_camera::camcube:
      public_node_handle_.param ("camera_name", camera_name_, std::string("camcube"));
      public_node_handle_.param ("camera_info_url", camera_info_url_, std::string("file://")+ros::package::getPath(ROS_PACKAGE_NAME)+std::string("/info/camcube_calibration.yaml"));
      break;
    default:
      public_node_handle_.param ("camera_name", camera_name_, std::string("camera"));
      public_node_handle_.param ("camera_info_url", camera_info_url_, std::string("auto"));
      break;
  }

  camera_info_manager_ = new camera_info_manager::CameraInfoManager(public_node_handle_, camera_name_, camera_info_url_);

  image_transport::ImageTransport imt(public_node_handle_);  

  // [init publishers]
  image_raw_publisher_   = imt.advertiseCamera ("intensity/image_raw", 1); 
  image_amp_publisher_   = imt.advertiseCamera ("amplitude/image_raw", 1);
  image_depth_publisher_ = imt.advertiseCamera ("distance/image_raw", 1);
  image_flags_publisher_ = imt.advertiseCamera ("flags/image_raw", 1);
  pcl_int_raw_publisher_  = public_node_handle_.advertise<sensor_msgs::PointCloud2>("pointcloud/pcl_int_raw", 1);
  pcl_amp_raw_publisher_  = public_node_handle_.advertise<sensor_msgs::PointCloud2>("pointcloud/pcl_amp_raw", 1);
  pcl_range_raw_publisher_  = public_node_handle_.advertise<sensor_msgs::PointCloud2>("pointcloud/pcl_range_raw", 1);

  ROS_INFO ("publishers initialized");

  // [init subscribers]

  // [init services]

  // [init clients]

  // [init action servers]

  // [init action clients]
}


  void 
PmdcameraDriverNode::findMaxIntensity(const float* intensity)
{
  if (max_intensity_ == 0)
    for (int v = 0; v < height_; ++v)
      for (int u = 0; u < width_; ++u) 
        if (max_intensity_ < intensity[v*(width_)+u])
          max_intensity_ = intensity[v*(width_)+u];
  //ROS_INFO("max_intensity %d",max_intensity_);
}

void PmdcameraDriverNode::findMaxAmplitude(const float* amplitude)
{
  if (max_amplitude_ == 0)
    for (int v = 0; v < height_; ++v)
      for (int u = 0; u < width_; ++u) 
        if (max_amplitude_ < amplitude[v*(width_)+u])
          max_amplitude_ = amplitude[v*(width_)+u];
  //ROS_INFO("max_amplitude %d",max_amplitude_);
}

void PmdcameraDriverNode::findMaxDepth(const float* distance)
{
  if (max_depth_ == 0)
    for (int v = 0; v < height_; ++v)
      for (int u = 0; u < width_; ++u) 
        if (max_depth_ < distance[v*(width_)+u])
          max_depth_ = distance[v*(width_)+u];
  //ROS_INFO("max_depth %d",max_depth_);
}

void PmdcameraDriverNode::assembleFlagsImage(float* flags)
{
  for (int v = 0; v < height_; ++v)
  {
    for (int u = 0; u < width_; ++u) 
    {
      int index = v * width_ + u;
      Image_flags_msg_.data[index] = flags[index];
    }
  }
}

void PmdcameraDriverNode::assembleIntensityImage(float* intensity)
{
    //we need to obtain the intensity if not done yet
    findMaxIntensity(intensity);
    if (max_intensity_ != 0)
    {
        for (unsigned index = 0; index < Image_int_msg_.height * Image_int_msg_.width; ++index){
            Image_int_msg_.data[index]=intensity[index]*255/max_intensity_;
        }
    }
}

void PmdcameraDriverNode::assembleAmplitudeImage(float* amplitude)
{
    //we need to obtain the intensity if not done yet
    //  findMaxAmplitude(amplitude);
    if (max_amplitude_ != 0)
    {
        for (unsigned index = 0; index < Image_amp_msg_.height * Image_amp_msg_.width; ++index){
            Image_amp_msg_.data[index] = amplitude[index]*255/max_amplitude_;
        }
    }
}

void PmdcameraDriverNode::assembleDepthImage(float* distance)
{
  //we need to obtain the intensity if not done yet
  //  max_depth_ = 0; //reset intensity as now we want depth
  //  findMaxDepth(distance);
  
    float* raw_data = reinterpret_cast<float*>(&distance[0]);
    float* depth_data = reinterpret_cast<float*>(&Image_depth_msg_.data[0]);
    for (unsigned index = 0; index < Image_depth_msg_.height * Image_depth_msg_.width; ++index){
        depth_data[index] = raw_data[index];
    }

}

void PmdcameraDriverNode::assemblePcl2Int(const float* const coord_3D, const float* const intensity) 
{    
  //we need to obtain the intensity if not done yet
  findMaxIntensity(intensity);
  if (max_intensity_ == 0 ) max_intensity_=1;
  // float bad_point = std::numeric_limits<float>::quiet_NaN ();
  // Assemble an awesome sensor_msgs/PointCloud2 message
  for (int v = 0; v < height_; v++)
  {
    for (int u = 0; u < width_; u++) 
    {
      int index = v * width_ + u;
      int index1 = v * real_width_ + u;
      int index3 = (v * real_width_ + u)*3;

      float *pstep = (float*)&pcl2_int_msg_.data[(index) * pcl2_int_msg_.point_step];
      //flip vertically !!!
      pstep[0] = coord_3D[index3];
      pstep[1] = coord_3D[index3+1];
      pstep[2] = coord_3D[index3+2];
      pstep[3] = intensity[index1]*255/max_intensity_;

      //RGBValue intensity_rgb;
      //intensity_rgb.Red = intensity_rgb.Green = intensity_rgb.Blue = intensity[index1]*255/max_intensity_;
      //pstep[3] = intensity_rgb.float_value;

    }
  }
}

void PmdcameraDriverNode::assemblePcl2Amp(const float* const coord_3D, const float* const amplitude) 
{    
  // float bad_point = std::numeric_limits<float>::quiet_NaN ();
  // Assemble an awesome sensor_msgs/PointCloud2 message
  for (int v = 0; v < height_; v++){
    for (int u = 0; u < width_; u++){
      int index = v * width_ + u;
      int index1 = v * real_width_ + u;
      int index3 = (v * real_width_ + u)*3;

      float *pstep = (float*)&pcl2_amp_msg_.data[(index) * pcl2_amp_msg_.point_step];
      //flip vertically !!!
      pstep[0] = coord_3D[index3];
      pstep[1] = coord_3D[index3+1];
      pstep[2] = coord_3D[index3+2];
      pstep[3] = amplitude[index1];
      
      //RGBValue amplitude_rgb;
      //amplitude_rgb.Red = amplitude_rgb.Green = amplitude_rgb.Blue = amplitude[index1];
      //pstep[3] = amplitude_rgb.float_value;
    }
  }
}

void PmdcameraDriverNode::assemblePcl2RangeImage(const float* const coord_3D, const float* const range, const float* const amp) 
{    
  // float bad_point = std::numeric_limits<float>::quiet_NaN ();
  // Assemble an awesome sensor_msgs/PointCloud2 message
  for (int v = 0; v < height_; v++){
    for (int u = 0; u < width_; u++){
      int index = v * width_ + u;
      int index1 = v * real_width_ + u;
      int index3 = (v * real_width_ + u)*3;

      float *pstep = (float*)&pcl2_range_msg_.data[(index) * pcl2_range_msg_.point_step];
      //flip vertically !!!
      pstep[0] = coord_3D[index3];
      pstep[1] = coord_3D[index3+1];
      pstep[2] = coord_3D[index3+2];
      pstep[3] = range[index1];
      pstep[4] = amp[index1];
    }
  }
}

void PmdcameraDriverNode::mainNodeThread(void)
{
  camera_info_ = camera_info_manager_->getCameraInfo ();

  //reset the intensity max value
  max_intensity_ = 0;

  //lock access to driver
  //  this->driver_.lock();

  // [fill msg Header if necessary]
  //<publisher_name>.header.frame_id = "<publisher_topic_name>";
  //<publisher_name>.header.stamp = ros::Time::now();
  PointCloud_msg_.header.frame_id = camera_info_.header.frame_id = pcl2_range_msg_.header.frame_id = pcl2_amp_msg_.header.frame_id = pcl2_int_msg_.header.frame_id;
  PointCloud_msg_.header.stamp = pcl2_range_msg_.header.stamp = pcl2_amp_msg_.header.stamp = pcl2_int_msg_.header.stamp = Image_int_msg_.header.stamp = Image_amp_msg_.header.stamp
    = Image_depth_msg_.header.stamp = camera_info_.header.stamp = ros::Time::now ();

  //TODO: blocking the reading part is enough  or all function should be under blockng?
  if ((pcl_range_raw_publisher_.getNumSubscribers () > 0) ||
      (pcl_amp_raw_publisher_.getNumSubscribers () > 0) ||
      (pcl_int_raw_publisher_.getNumSubscribers () > 0) ||
      (image_raw_publisher_.getNumSubscribers ()   > 0) ||
      (image_amp_publisher_.getNumSubscribers ()   > 0) ||
      (image_flags_publisher_.getNumSubscribers () > 0) ||
      (image_depth_publisher_.getNumSubscribers () > 0)) {
    try {
      driver_.lock();
      driver_.readData();
      //unlock access to driver if previously blocked
      driver_.unlock();

      // [fill msg structures]

      // [fill srv structure and make request to the server]

      // [fill action structure and make request to the action server]

      // [publish messages]
      if (pcl_range_raw_publisher_.getNumSubscribers () > 0) {
        bool is_dense = true;
        assemblePcl2RangeImage(driver_.get3DImage(is_dense), driver_.getRangeImage(), driver_.getAmplitudeImage());
        pcl2_range_msg_.is_dense = is_dense;
        pcl_range_raw_publisher_.publish(pcl2_range_msg_);
      }

      if (pcl_amp_raw_publisher_.getNumSubscribers () > 0) {
        bool is_dense = true;
        assemblePcl2Amp(driver_.get3DImage(is_dense), driver_.getAmplitudeImage());
        pcl2_amp_msg_.is_dense = is_dense;
        pcl_amp_raw_publisher_.publish(pcl2_amp_msg_);
      }

      if (pcl_int_raw_publisher_.getNumSubscribers () > 0) {
        bool is_dense = true;
        assemblePcl2Int(driver_.get3DImage(is_dense), driver_.getIntensityImage());
        pcl2_int_msg_.is_dense = is_dense;
        pcl_int_raw_publisher_.publish(pcl2_int_msg_);
      }

      if (image_flags_publisher_.getNumSubscribers () > 0) {
        assembleFlagsImage(driver_.getFlagsImage());
        //shared to easy the nodelet paradigm
        image_flags_publisher_.publish(boost::make_shared<const sensor_msgs::Image> (Image_int_msg_),boost::make_shared<const sensor_msgs::CameraInfo> (camera_info_));
      }

      if (image_raw_publisher_.getNumSubscribers () > 0) {
        assembleIntensityImage(driver_.getIntensityImage());
        //shared to easy the nodelet paradigm
        image_raw_publisher_.publish(boost::make_shared<const sensor_msgs::Image> (Image_int_msg_),boost::make_shared<const sensor_msgs::CameraInfo> (camera_info_));
      }

      if (image_amp_publisher_.getNumSubscribers () > 0)
      {
        assembleAmplitudeImage(driver_.getAmplitudeImage());
        //shared to easy the nodelet paradigm
        image_amp_publisher_.publish(boost::make_shared<const sensor_msgs::Image> (Image_amp_msg_),boost::make_shared<const sensor_msgs::CameraInfo> (camera_info_));
      }

      if (image_depth_publisher_.getNumSubscribers () > 0)
      {
        assembleDepthImage(driver_.getDepthImage());
        //shared to easy the nodelet paradigm
        image_depth_publisher_.publish(boost::make_shared<const sensor_msgs::Image> (Image_depth_msg_),boost::make_shared<const sensor_msgs::CameraInfo> (camera_info_));
      }
      //   ROS_INFO("new image");
    } catch(CException &e){
      driver_.unlock();
      ROS_INFO("Impossible to capture frame");
    }
    //unlock access to driver if previously blocked
    //  this->driver_.unlock();
  }
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void PmdcameraDriverNode::postNodeOpenHook(void)
{
}

void PmdcameraDriverNode::addNodeDiagnostics(void)
{
}

void PmdcameraDriverNode::addNodeOpenedTests(void)
{
}

void PmdcameraDriverNode::addNodeStoppedTests(void)
{
}

void PmdcameraDriverNode::addNodeRunningTests(void)
{
}

void PmdcameraDriverNode::reconfigureNodeHook(int level)
{
}

PmdcameraDriverNode::~PmdcameraDriverNode()
{
  //[free dynamic memory]
  delete camera_info_manager_;
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<PmdcameraDriverNode>(argc,argv,"pmdcamera_driver_node");
}