RealSenseNode.cpp

Go to the documentation of this file.

#include <iostream>
#include <string>
#include <cmath>
#include <iostream>
#include <chrono>
#include <thread>
 
#include "RealSenseNode.hpp"
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include "utils/logger.hpp"
#include <memory>
#include <DepthCamera.hpp>
#include <utils/settings.hpp>
 
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
 
using namespace galaxis::utils;
using namespace std::chrono_literals;
using std::placeholders::_1;
//ros-eloquent-rosbag2-storage-default-plugins // old?
 
RealSenseNode::RealSenseNode() : GalaxisNode("RealSenseNode") {
  try {
    std::string configPath = "settings/onstartup_realsense.json";
    auto settingsJsonSPtr = read_json_file(configPath);
    json& settingsJson = (*settingsJsonSPtr)["settings"];
 
    m_debugMode = settingsJson["debug_mode"];
    m_listenToROSPCL = settingsJson["listen_to_ros_pointcloud"];
    m_settingsJsonFilename = settingsJson["camera_config_path"];
  }
  catch(std::exception& e) {
    log_error("[RealSenseNode] Failed to read config file. ", e.what());
  }
 
  // init camera
  m_cam.set_node(this); // required
  m_cam.init(m_settingsJsonFilename);
  
  // obstacle publisher
  rclcpp::QoS qos = galaxis::ros::get_qos_profile(galaxis::ros::QoS_Profile_t::QOS_RELIABLE);
  galaxis::ros::create_publisher(this, m_obstaclePublisher);
 
  if(m_listenToROSPCL) {
    m_depthSubscriberSPtr = this->create_subscription<sensor_msgs::msg::PointCloud2>("/galaxis/debug/realsense/pointcloud/raw",
      qos,
      std::bind(&RealSenseNode::on_ros_pointcloud_callback, this, std::placeholders::_1)
    );
  }
 
  if(m_debugMode) {
    if(!m_listenToROSPCL) m_depthPublisherSPtr = this->create_publisher<sensor_msgs::msg::PointCloud2>("/galaxis/debug/realsense/pointcloud/raw", qos);
    m_debugPclPublisherFiltered = this->create_publisher<sensor_msgs::msg::PointCloud2>("galaxis/debug/realsense/pointcloud/filtered", qos);
    m_debugPclPublisherResult = this->create_publisher<sensor_msgs::msg::PointCloud2>("galaxis/debug/realsense/pointcloud/objects", qos);
  }
 
  this->enable_parameters(std::bind(&RealSenseNode::new_parameters_callback, this, std::placeholders::_1));
  
  std::thread(&RealSenseNode::object_detection_loop, this).detach(); 
  // std::thread(&RealSenseNode::pointcloud_publish_loop, this).detach(); 
}
 
void RealSenseNode::on_ros_pointcloud_callback(sensor_msgs::msg::PointCloud2::SharedPtr pclMsg) {
  auto timePclTupleSPtr = std::make_shared<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>>();
 
  pcl::fromROSMsg(*pclMsg, *(timePclTupleSPtr->second));
  timePclTupleSPtr->first = pclMsg->header.stamp;
 
  this->object_detection(timePclTupleSPtr);
}
 
void RealSenseNode::object_detection_loop(){
  while(1) {
 
    auto pointCloudRawPair = m_cam.pop_last_grabbed_pcl(true);
    if(pointCloudRawPair == nullptr) {
      log_warning("[RealSenseNode] pop_last_grabbed_pcl is nullptr");
      std::this_thread::sleep_for(std::chrono::milliseconds(5));
      continue;
    }
 
    this->object_detection(pointCloudRawPair);
  }
}
 
void RealSenseNode::object_detection(std::shared_ptr<std::pair<rclcpp::Time, pcl::PointCloud<pcl::PointXYZ>::Ptr>> pointCloudRawPair) {
  m_settingsMtx.lock();
  pclPtrType cloudFiltered(new pclType);
  m_objectDetector.filter_pcl(pointCloudRawPair->second, cloudFiltered, m_leafSize, m_yMin, m_yMax);
 
  std::vector<Object> objectsVec;
  auto detectedObjectsVecSPtr = m_objectDetector.cluster_data(cloudFiltered, objectsVec, m_clusterTolerance, m_minClusterSize, m_maxClusterSize);
  m_settingsMtx.unlock();
 
  std::unique_ptr<ObstacleListType> msgObstacle = std::make_unique<ObstacleListType>();
  std_msgs::msg::Header header;
  header.stamp = pointCloudRawPair->first;
  msgObstacle->header = header;
 
  float sizeX, sizeY, sizeZ;
  for(uint32_t i = 0; i < objectsVec.size(); i ++){
    sizeX = std::fabs(objectsVec.at(i).get_max_dimensions().x - objectsVec.at(i).get_min_dimensions().x);
    sizeY = std::fabs(objectsVec.at(i).get_max_dimensions().y - objectsVec.at(i).get_min_dimensions().y);
    sizeZ = std::fabs(objectsVec.at(i).get_max_dimensions().z - objectsVec.at(i).get_min_dimensions().z);
 
    //TODO: ueberpruefen ob ungefähr rechteck?
    msgObstacle->obstacles.push_back(ObstacleType());
    msgObstacle->obstacles.back().pos_x = objectsVec.at(i).get_center_point().x;
    msgObstacle->obstacles.back().pos_y = objectsVec.at(i).get_center_point().z;
    msgObstacle->obstacles.back().width = 0.2;
 
    if(m_debugMode) {
      log("object ", std::to_string(objectsVec.at(i).get_center_point().x), " : ", std::to_string(objectsVec.at(i).get_center_point().y), " : ", std::to_string(objectsVec.at(i).get_center_point().z), " | SIZE: ", objectsVec.at(i).size, "\n");
      log("min Dimensions (x | y | z) ", std::to_string(objectsVec.at(i).get_min_dimensions().x), " | ", std::to_string(objectsVec.at(i).get_min_dimensions().y), " | ", std::to_string(objectsVec.at(i).get_min_dimensions().y), "\n");
      log("max Dimensions (x | y | z) ", std::to_string(objectsVec.at(i).get_max_dimensions().x), " | ", std::to_string(objectsVec.at(i).get_max_dimensions().y), " | ", std::to_string(objectsVec.at(i).get_max_dimensions().y), "\n");
      log("object dims (x | y | z): ", sizeX, " | ", sizeY, " | ", sizeZ, "\n");
      log("SIZE Y: ", std::to_string(objectsVec.at(i).get_max_dimensions().y - objectsVec.at(i).get_min_dimensions().y), "\n");
      log("SIZE Z: ", std::to_string(objectsVec.at(i).get_max_dimensions().z - objectsVec.at(i).get_min_dimensions().z), "\n");
    }
  }
 
  if(msgObstacle->obstacles.size() > 0) {
    m_obstaclePublisher->publish(std::move(msgObstacle));
  }
 
  if(m_debugMode) {
    // debug publisher
    std::unique_ptr<sensor_msgs::msg::PointCloud2> pointCloudMsgUPtr = std::make_unique<sensor_msgs::msg::PointCloud2>();
    if(m_depthPublisherSPtr->get_subscription_count() > 0) {
      pcl::toROSMsg(*(std::get<1>(*pointCloudRawPair)), *pointCloudMsgUPtr);
      pointCloudMsgUPtr->header.frame_id = "map";
      pointCloudMsgUPtr->header.stamp = pointCloudRawPair->first;
    }
 
    std::unique_ptr<sensor_msgs::msg::PointCloud2> pointCloudFilteredMsgUPtr = std::make_unique<sensor_msgs::msg::PointCloud2>();
    if(m_debugPclPublisherFiltered->get_subscription_count() > 0) {
      pcl::toROSMsg(*cloudFiltered, *pointCloudFilteredMsgUPtr);
      pointCloudFilteredMsgUPtr->header.frame_id = "map";
      pointCloudFilteredMsgUPtr->header.stamp = pointCloudRawPair->first;
    }
 
    std::unique_ptr<sensor_msgs::msg::PointCloud2> pointCloudDetectedObjectsUPtr = std::make_unique<sensor_msgs::msg::PointCloud2>();
    if(m_debugPclPublisherResult->get_subscription_count() > 0) {
      pcl::PointCloud<pcl::PointXYZ>::Ptr allObjectsCloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
      // merge pcls
      for (auto pclPtr : *detectedObjectsVecSPtr) {
        *allObjectsCloudPtr += *pclPtr;
      }
 
      pcl::toROSMsg(*allObjectsCloudPtr, *pointCloudDetectedObjectsUPtr);
      pointCloudDetectedObjectsUPtr->header.frame_id = "map";
      pointCloudDetectedObjectsUPtr->header.stamp = pointCloudRawPair->first;
    }
 
    if(m_depthPublisherSPtr->get_subscription_count() > 0) {
      m_depthPublisherSPtr->publish(std::move(pointCloudMsgUPtr));
    }
 
    if(m_debugPclPublisherFiltered->get_subscription_count() > 0) {
      m_debugPclPublisherFiltered->publish(std::move(pointCloudFilteredMsgUPtr));
    }
    if(m_debugPclPublisherResult->get_subscription_count() > 0) {
      m_debugPclPublisherResult->publish(std::move(pointCloudDetectedObjectsUPtr));
    }
  }
}
 
// void RealSenseNode::pointcloud_publish_loop(){
//   while(1) {
//     auto msg = m_cam.get_last_grabbed_pcl();
//     if(msg == nullptr) {
//       log_warning("[RealSenseNode] get_last_grabbed_pcl is nullptr");
//       continue;
//     }
 
//     std::unique_ptr<sensor_msgs::msg::PointCloud2> pointCloudMsgSPtr = std::make_unique<sensor_msgs::msg::PointCloud2>();
//     pcl::toROSMsg(*(std::get<1>(*msg)), *pointCloudMsgSPtr);
//     pointCloudMsgSPtr->header.frame_id = "map";
//     pointCloudMsgSPtr->header.stamp = std::get<0>(*msg);
 
//     m_depthPublisherSPtr->publish(std::move(pointCloudMsgSPtr));
//     log("[RealSenseNode] pointcloud_publish_loop: Published. :)");
//   }
// }
 
rcl_interfaces::msg::SetParametersResult RealSenseNode::new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
  log("[RealSenseNode] NEW PARAMETERS");
  
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "";
 
  m_settingsMtx.lock();
  for (const auto &param: parameters) {
    if (param.get_name() == "cluster_extraction.leaf_size") {
      if(check_parameter<double>(param, result)) {
        m_leafSize = param.as_double();
      }
    }
    else if (param.get_name() == "cluster_extraction.cluster_tolerance") {
      if(check_parameter<double>(param, result)) {
        m_clusterTolerance = param.as_double();
      }
    }
    else if (param.get_name() == "cluster_extraction.cluster_size_min") {
      if(check_parameter<int>(param, result, 0)) {
        m_minClusterSize = param.as_int();
      }
    }
    else if (param.get_name() == "cluster_extraction.cluster_size_max") {
      if(check_parameter<int>(param, result, 0)) {
        m_maxClusterSize = param.as_int();
      }
    }
    // else if (param.get_name() == "region_of_interest.y_min") {
    //   if(check_parameter<double>(param, result, -10000, 10000)) {
    //     m_yMin = param.as_double();
    //   }
    // }
    // else if (param.get_name() == "region_of_interest.y_max") {
    //   if(check_parameter<double>(param, result, -10000, 10000)) {
    //     m_yMax = param.as_double();
    //   }
    // }
    else if (param.get_name() == "postprocessing.object_min_size_x") {
      if(check_parameter<double>(param, result, 0, 100)) {
        m_objectDetector.m_minSizeX = param.as_double();
      }
    }
    else if (param.get_name() == "postprocessing.object_min_size_y") {
      if(check_parameter<double>(param, result, 0, 100)) {
        m_objectDetector.m_minSizeY = param.as_double();
      }
    }
    else if (param.get_name() == "postprocessing.object_min_size_z") {
      if(check_parameter<double>(param, result, 0, 100)) {
        m_objectDetector.m_minSizeZ = param.as_double();
      }
    }
    else if (param.get_name() == "postprocessing.object_max_size_x") {
      if(check_parameter<double>(param, result, 0, 100)) {
        m_objectDetector.m_maxSizeX = param.as_double();
      }
    }
    else if (param.get_name() == "postprocessing.object_max_size_y") {
      if(check_parameter<double>(param, result, 0, 100)) {
        m_objectDetector.m_maxSizeY = param.as_double();
      }
    }
    else if (param.get_name() == "postprocessing.object_max_size_z") {
      if(check_parameter<double>(param, result, 0, 100)) {
        m_objectDetector.m_maxSizeZ = param.as_double();
      }
    }
    else if (param.get_name() == "postprocessing.cluster_size_min") {
      if(check_parameter<int>(param, result, 0, 1000000)) {
        m_objectDetector.m_postProcessingClusterSizeMin = param.as_int();
      }
    }
    else if (param.get_name() == "postprocessing.cluster_size_max") {
      if(check_parameter<int>(param, result, 0, 1000000)) {
        m_objectDetector.m_postProcessingClusterSizeMax = param.as_int();
      }
    }
    else {
      result.successful = false;
      result.reason += "[" + param.get_name() + "] UNKNOWN param or NOT IMPLEMENTED; ";
    }
  }
  m_settingsMtx.unlock();
 
 
  if(result.successful == false) {
    log_warning("[RealSenseNode] Failed to set some parameters. Reasons: ", result.reason);
  }
 
  log("[RealSenseNode] NEW PARAMETERS PROCESSED");
  return result;
}