DepthCamera.cpp

Go to the documentation of this file.

#include <DepthCamera.hpp>
 
#include <iostream>
#include <utils/logger.hpp>
#include <pcl_conversions/pcl_conversions.h>
 
 
using namespace galaxis::utils;
 
void Realsense::set_node(rclcpp::Node* ptr) {
  m_nodePtr = ptr;
}
 
void Realsense::init(std::string config) {
  log_warning("[Realsense::init] Configuration File ignored. (not implemented)");
 
  log("Enable depth stream...");
  m_cfg.enable_stream(RS2_STREAM_DEPTH, RS2_FORMAT_Z16);
 
  // callback for depth pointcloud
  auto callback = [&](const rs2::frame& frame) {
    log_debug("[Realsense::Callback] New Frame");
    auto now = this->m_nodePtr->get_clock()->now();
 
    // check if frame has desired data 
    if(!frame.is<rs2::frameset>()) {
      log_warning("[Realsense::Callback] Wrong Frame");
      return;
    }
 
    // retrieve point cloud
    auto frameset = frame.as<rs2::frameset>();
 
    auto it = frameset.begin();
    if (it == frameset.end()) {
      log_warning("Realsense::Callback] Frameset empty");
      return;
    }
 
    if((*it).get_profile().stream_type() != RS2_STREAM_DEPTH) {
      log_warning("[Realsense::Callback] No Depth Frame");
      return;
    }
 
    // retrieve point cloud in pcl
    rs2::depth_frame depthFrame = frameset.get_depth_frame();
    rs2::pointcloud pointcloud;
    rs2::points pc = pointcloud.calculate(depthFrame);
 
    auto pointCloudPairSPtr = std::make_shared<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>>();
    std::get<0>(*pointCloudPairSPtr) = now;
    std::get<1>(*pointCloudPairSPtr) = points_to_pcl(pc);
 
    m_mtx.lock();
    this->m_currentPointCloudPairSPtr = pointCloudPairSPtr;
    this->m_currentPointCloudPairNewData = true;
    m_newPCLConditionVar.notify_all();
    m_mtx.unlock();
 
    // log_debug("[Realsense::Callback] Frame fully processed");
    // DEBUG
    // Get the depth frame's dimensions
    // auto width = depthFrame.get_width();
    // auto height = depthFrame.get_height();
 
    // // Query the distance from the camera to the object in the center of the image
    // float dist_to_center = depthFrame.get_distance(width / 2, height / 2);
 
    // // Print the distance
    // std::cout << "The camera is facing an object " << dist_to_center << " meters away \r";
    
  };
 
  log("Pipeline started...");
  // m_profile = m_pipe.start(m_cfg, callback); // starts thread
  m_profile = m_pipe.start(callback); // starts thread without config
 
  // std::cout << "Get device" << std::endl;
  // rs2::device device = m_profile.get_device();
 
  // std::cout << "Get depth sensor" << std::endl;
  // rs2::depth_sensor depth_sensor(device.first<rs2::depth_sensor>());
  // rs400::advanced_mode advancedDevice(device);
 
  // if(advancedDevice.is_enabled() && fexists(p_jsonSettingsFilename.c_str())) {
  //   std::cout << "rs400 advanced mode" << std::endl;
  //   std::cout << "Camera Infos: " << std::endl;
  //   for(auto i = 0; i < RS2_CAMERA_INFO_COUNT; i++) {
  //     if(!advancedDevice.supports((rs2_camera_info)i))
  //       continue;
  //     std::cout << static_cast<rs2_camera_info>(i) << ": " << advancedDevice.get_info(static_cast<rs2_camera_info>(i)) << std::endl;
  //   }
 
  //   std::ifstream jsonSettingsFile(p_jsonSettingsFilename);
  //   std::stringstream jsonSettings;
  //   jsonSettings << jsonSettingsFile.rdbuf();
 
  //   std::cout << "Load json settings: " << std::endl;
  //   std::cout << jsonSettings.str() << std::endl;
  //   advancedDevice.load_json(jsonSettings.str());
  // } else {
  //   std::cout << "Error: no advanced mode toggled or json settings file not available" << std::endl;
 
  //   /** comment from rs-measure.cpp
  //    * TODO: At the moment the SDK does not offer a closed enum for D400 visual presets
  //    * (because they keep changing)
  //    * As a work-around we try to find the High-Density preset by name
  //    * We do this to reduce the number of black pixels
  //    * The hardware can perform hole-filling much better and much more power efficient then our software
  //    **/
  //   auto range = depth_sensor.get_option_range(RS2_OPTION_VISUAL_PRESET);
  //   for(auto i = range.min; i < range.max; i += range.step) {
  //     if(std::string(depth_sensor.get_option_value_description(RS2_OPTION_VISUAL_PRESET, i)) == "High Accuracy") {
  //       depth_sensor.set_option(RS2_OPTION_VISUAL_PRESET, i);
  //     }
  //   }
  // }
  // depth_sensor.set_option(RS2_OPTION_DEPTH_UNITS, 0.001);
 
  // std::cout << "Wait for autoexposure..." << std::endl;
  // // wait for autoexposure
  // for(auto i = 0; i < 10; i++) {
  //   m_pipe.wait_for_frames();
  // }
 
  // // Get stream m_profile of depthstream and show info
  // m_streamProfile = m_profile.get_stream(RS2_STREAM_DEPTH);
  // std::cout << "FPS: " << m_streamProfile.fps() << std::endl;
  // std::cout << "ID: " << m_streamProfile.unique_id() << std::endl;
  // std::cout << "Format: " << m_streamProfile.format() << std::endl;
 
  // m_running = true;
 
  // //create thread with process
}
 
std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>> Realsense::pop_last_grabbed_pcl(bool blocking) {
  std::unique_lock<std::mutex> lock(m_mtx);
  std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>> ret = nullptr;
 
  if(m_currentPointCloudPairNewData) {
    ret = m_currentPointCloudPairSPtr;
    m_currentPointCloudPairNewData = false;
  }
 
  // if return image is nullptr and blocking is enabled then wait for new image
  if(ret == nullptr && blocking) {
    m_newPCLConditionVar.wait(lock); // release lock and wait until notify_one called
    ret = m_currentPointCloudPairSPtr;
    m_currentPointCloudPairNewData = false;
  }
 
  return ret;
}
 
std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>> Realsense::get_last_grabbed_pcl() {
  std::unique_lock<std::mutex> lock(m_mtx);
  std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>> ret = m_currentPointCloudPairSPtr;
  return ret;
}
 
pcl::PointCloud<pcl::PointXYZ>::Ptr Realsense::points_to_pcl(const rs2::points& points) {
  // https://pointclouds.org/documentation/classpcl_1_1_point_cloud.html
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
  auto sp = points.get_profile().as<rs2::video_stream_profile>();
  // log_debug("[Realsense] PointCloud (W x H): ", sp.width(), " x ", sp.height());
 
  cloud->width = points.size();
  cloud->height = 1;
  cloud->is_dense = true;
  cloud->points.reserve(points.size());
 
  auto ptr = points.get_vertices();
 
  size_t pointsAppended = 0;
 
  float m_minY = -0.08; // meter
  float m_maxY = 0.08f; // meter;
  float m_maxZ = 1.8f; // 2 meter max distance
 
  for (size_t i = 0; i < points.size(); ++i) {
    if(ptr->z <= m_maxZ && ptr->y >= m_minY && ptr->y <= m_maxY) {
      cloud->push_back(pcl::PointXYZ(ptr->x, ptr->y, ptr->z));
      pointsAppended++;
    }
    ptr++;
  }
 
  // log_debug("Original Size: ", points.size());
  // log_debug("Postprocessed Size: ", pointsAppended);
  cloud->points.resize(pointsAppended);
  cloud->width = pointsAppended;
  
  return cloud;
}