Galaxis Codebase: RealSenseNode Class Reference

#include <RealSenseNode.hpp>

Inheritance diagram for RealSenseNode:

Collaboration diagram for RealSenseNode:

[legend]

Public Member Functions
	RealSenseNode ()

Private Member Functions
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)

rcl_interfaces::msg::SetParametersResult	new_parameters_callback (const std::vector< rclcpp::Parameter > &parameters)

Private Attributes
bool	m_debugMode = false

bool	m_listenToROSPCL = false

Realsense	m_cam

std::string	m_settingsJsonFilename

rclcpp::Publisher< ObstacleListType >::SharedPtr	m_obstaclePublisher

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

rclcpp::Subscription< sensor_msgs::msg::PointCloud2 >::SharedPtr	m_depthSubscriberSPtr

EuclideanDetector	m_objectDetector

std::mutex	m_settingsMtx

float	m_clusterTolerance = 0.02f

unsigned int	m_minClusterSize = 300

unsigned int	m_maxClusterSize = 5000

float	m_leafSize = 0.015

float	m_yMin = -1.00

float	m_yMax = 1.00

Additional Inherited Members
Protected Member Functions inherited from GalaxisNode
	GalaxisNode ()=delete

	GalaxisNode (std::string p_name, unsigned int p_heartbeatTimePeriod)

	GalaxisNode (std::string p_name)

template<typename MsgSPtr >
void	tickSub (std::map< galaxis::serviceStates::State, std::function< void(const MsgSPtr)>> const *p_functionMapPtr, MsgSPtr const p_message)

template<typename... Args>
void	tickPub (std::map< galaxis::serviceStates::State, std::function< void(Args...)>> const *p_functionMapPtr, Args... p_args)

void	register_callback (std::function< void(galaxis::serviceStates::State)> p_function)

void	service_state_callback (ServiceStateType::SharedPtr const p_msgSPtr)

galaxis::serviceStates::State	get_current_service_state () const

void	enable_parameters (std::function< rcl_interfaces::msg::SetParametersResult(const std::vector< rclcpp::Parameter > &)> callbackFunc)

void	create_parameters (std::shared_ptr< json > configJsonSPtr)

json const &	get_global_settings ()

Detailed Description

Definition at line 20 of file RealSenseNode.hpp.

Constructor & Destructor Documentation

◆ RealSenseNode()

RealSenseNode::RealSenseNode ( )

Definition at line 24 of file RealSenseNode.cpp.

                              : 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(); 
 }

References GalaxisNode::enable_parameters(), Realsense::init(), galaxis::utils::log_error(), m_cam, m_debugMode, m_debugPclPublisherFiltered, m_debugPclPublisherResult, m_depthPublisherSPtr, m_depthSubscriberSPtr, m_listenToROSPCL, m_obstaclePublisher, m_settingsJsonFilename, new_parameters_callback(), object_detection_loop(), on_ros_pointcloud_callback(), galaxis::utils::read_json_file(), and Realsense::set_node().

Here is the call graph for this function:

Member Function Documentation

◆ new_parameters_callback()

rcl_interfaces::msg::SetParametersResult RealSenseNode::new_parameters_callback ( const std::vector< rclcpp::Parameter > & parameters )

private

Definition at line 188 of file RealSenseNode.cpp.

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

References galaxis::utils::log(), galaxis::utils::log_warning(), m_clusterTolerance, m_leafSize, m_maxClusterSize, EuclideanDetector::m_maxSizeX, EuclideanDetector::m_maxSizeY, EuclideanDetector::m_maxSizeZ, m_minClusterSize, EuclideanDetector::m_minSizeX, EuclideanDetector::m_minSizeY, EuclideanDetector::m_minSizeZ, m_objectDetector, EuclideanDetector::m_postProcessingClusterSizeMax, EuclideanDetector::m_postProcessingClusterSizeMin, and m_settingsMtx.

Referenced by RealSenseNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ object_detection()

void RealSenseNode::object_detection ( std::shared_ptr< std::pair< rclcpp::Time, pcl::PointCloud< pcl::PointXYZ >::Ptr >> pointCloudRawPair )

private

Definition at line 88 of file RealSenseNode.cpp.

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

References EuclideanDetector::cluster_data(), PCLObjectDetector::filter_pcl(), galaxis::utils::log(), m_clusterTolerance, m_debugMode, m_debugPclPublisherFiltered, m_debugPclPublisherResult, m_depthPublisherSPtr, m_leafSize, m_maxClusterSize, m_minClusterSize, m_objectDetector, m_obstaclePublisher, m_settingsMtx, m_yMax, and m_yMin.

Referenced by object_detection_loop(), and on_ros_pointcloud_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ object_detection_loop()

void RealSenseNode::object_detection_loop ( )

private

Definition at line 74 of file RealSenseNode.cpp.

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

References galaxis::utils::log_warning(), m_cam, object_detection(), and Realsense::pop_last_grabbed_pcl().

Referenced by RealSenseNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_ros_pointcloud_callback()

void RealSenseNode::on_ros_pointcloud_callback ( sensor_msgs::msg::PointCloud2::SharedPtr pclMsg )

private

Definition at line 65 of file RealSenseNode.cpp.

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