RealSenseNode.hpp

Go to the documentation of this file.

#ifndef PARAM_IDENT_NODE_HPP_
#define PARAM_IDENT_NODE_HPP_
 
#include <vector>
#include <string>
#include <mutex>
#include <opencv2/core.hpp>
#include <opencv2/objdetect.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <DepthCamera.hpp>
 
#include <galaxis/GalaxisNode.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
 
#include <iostream>
 
#include <PCLObjectDetector.hpp>
 
class RealSenseNode : public GalaxisNode {
private:
  bool m_debugMode = false;
  bool m_listenToROSPCL = false;
  
  Realsense m_cam;  
  std::string m_settingsJsonFilename;
 
  // ros2
  rclcpp::Publisher<ObstacleListType>::SharedPtr m_obstaclePublisher;
 
  // debug
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_depthPublisherSPtr;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_debugPclPublisherFiltered;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_debugPclPublisherResult;
 
  // ros2 replay
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr m_depthSubscriberSPtr;
 
  EuclideanDetector m_objectDetector;
 
  std::mutex m_settingsMtx;
 
  // settings are overwritten by runtime_system.json
  
  /* NICE PARAMS -- NOT REALLY
 
    "cluster_extraction": {
      "cluster_size_max": 5000,
      "cluster_size_min": 300,
      "cluster_tolerance": 0.02,
      "leaf_size": 0.015
  },
  "postprocessing": {
      "object_min_size_x": 0.1,
      "object_min_size_y": 0.2,
      "object_min_size_z": 0.001
  }
  */
 
  // GOOD PARAMETERS -- NOT REALLY
  float m_clusterTolerance      = 0.02f;
  unsigned int m_minClusterSize = 300;
  unsigned int m_maxClusterSize = 5000;  
  float m_leafSize              = 0.015;
  /* vertical axis points to bottom */
  float m_yMin = -1.00;
  float m_yMax = 1.00;
 
 
public:
  RealSenseNode();
 
private:
  void on_ros_pointcloud_callback(sensor_msgs::msg::PointCloud2::SharedPtr pclMsg);
  void object_detection_loop();
  void object_detection(std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>> pointCloudRawPair);
  // void pointcloud_publish_loop();
 
  rcl_interfaces::msg::SetParametersResult new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
};
 
#endif //PARAM_IDENT_NODE_HPP_