Realsense Class Reference

#include <DepthCamera.hpp>

Public Member Functions
	Realsense ()
void	set_node (rclcpp::Node *ptr)
void	init (std::string config)
	initializes camera with json settings More...
std::shared_ptr< std::pair< rclcpp::Time, pcl::PointCloud< pcl::PointXYZ >::Ptr > >	pop_last_grabbed_pcl (bool blocking)
std::shared_ptr< std::pair< rclcpp::Time, pcl::PointCloud< pcl::PointXYZ >::Ptr > >	get_last_grabbed_pcl ()

Private Member Functions
pcl::PointCloud< pcl::PointXYZ >::Ptr	points_to_pcl (const rs2::points &points)

Private Attributes
rs2::config	m_cfg
rs2::pipeline	m_pipe
rs2::pipeline_profile	m_profile
std::shared_ptr< std::pair< rclcpp::Time, pcl::PointCloud< pcl::PointXYZ >::Ptr > >	m_currentPointCloudPairSPtr = nullptr
bool	m_currentPointCloudPairNewData = false
rclcpp::Node *	m_nodePtr
std::mutex	m_mtx
std::condition_variable	m_newPCLConditionVar

Detailed Description

Definition at line 24 of file DepthCamera.hpp.

Constructor & Destructor Documentation

Realsense()

Realsense::Realsense ( )

inline

Definition at line 40 of file DepthCamera.hpp.

{};

Member Function Documentation

get_last_grabbed_pcl()

std::shared_ptr< std::pair< rclcpp::Time, pcl::PointCloud< pcl::PointXYZ >::Ptr > > Realsense::get_last_grabbed_pcl

(

)

Definition at line 156 of file DepthCamera.cpp.

                                                                                                  {
  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;
}

init()

void Realsense::init

(

std::string

config

)

initializes camera with json settings

Definition at line 14 of file DepthCamera.cpp.

                                     {
  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
}

References galaxis::utils::log(), galaxis::utils::log_debug(), and galaxis::utils::log_warning().

Referenced by RealSenseNode::RealSenseNode().

Here is the call graph for this function:

Here is the caller graph for this function:

points_to_pcl()

pcl::PointCloud< pcl::PointXYZ >::Ptr Realsense::points_to_pcl ( const rs2::points &  points )

private

Definition at line 162 of file DepthCamera.cpp.

                                                                              {
  // 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;
}

pop_last_grabbed_pcl()

std::shared_ptr< std::pair< rclcpp::Time, pcl::PointCloud< pcl::PointXYZ >::Ptr > > Realsense::pop_last_grabbed_pcl

(

bool

blocking

)

Definition at line 137 of file DepthCamera.cpp.

                                                                                                               {
  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;
}

Referenced by RealSenseNode::object_detection_loop().

Here is the caller graph for this function:

set_node()

void Realsense::set_node

(

rclcpp::Node *

ptr

)

Definition at line 10 of file DepthCamera.cpp.

                                        {
  m_nodePtr = ptr;
}

Referenced by RealSenseNode::RealSenseNode().

Here is the caller graph for this function:

Member Data Documentation

m_cfg

rs2::config Realsense::m_cfg

private

Definition at line 26 of file DepthCamera.hpp.

m_currentPointCloudPairNewData

bool Realsense::m_currentPointCloudPairNewData = false

private

Definition at line 30 of file DepthCamera.hpp.

m_currentPointCloudPairSPtr

std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr> > Realsense::m_currentPointCloudPairSPtr = nullptr

private

Definition at line 29 of file DepthCamera.hpp.

m_mtx

std::mutex Realsense::m_mtx

private

Definition at line 34 of file DepthCamera.hpp.

m_newPCLConditionVar

std::condition_variable Realsense::m_newPCLConditionVar

private

Definition at line 35 of file DepthCamera.hpp.

m_nodePtr

rclcpp::Node* Realsense::m_nodePtr

private

Definition at line 32 of file DepthCamera.hpp.

m_pipe

rs2::pipeline Realsense::m_pipe

private

Definition at line 27 of file DepthCamera.hpp.

m_profile

rs2::pipeline_profile Realsense::m_profile

private

Definition at line 28 of file DepthCamera.hpp.

---

The documentation for this class was generated from the following files:

• nodes/RealSenseNode/include/DepthCamera.hpp
• nodes/RealSenseNode/src/DepthCamera.cpp