00001 #include "zyonz_image_based_leaf_probing_alg_node.h" 00002 //#include <opencv2/highgui/highgui.hpp> 00003 //#include <opencv2/imgproc/imgproc.hpp> 00004 //#include <sensor_msgs/image_encodings.h> 00005 00006 ZyonzImageBasedLeafProbingAlgNode::ZyonzImageBasedLeafProbingAlgNode(void) : 00007 algorithm_base::IriBaseAlgorithm<ZyonzImageBasedLeafProbingAlgorithm>() 00008 { 00009 //init class attributes if necessary 00010 //this->loop_rate_ = 2;//in [Hz] 00011 00012 // [init publishers] 00013 this->seg_image_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("seg_image", 1); 00014 this->leaf_confidence_image_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("leaf_confidence_image", 1); 00015 00016 // [init subscribers] 00017 00018 // [init services] 00019 this->leaf_probing_srv_server_ = this->public_node_handle_.advertiseService("leaf_probing_srv", &ZyonzImageBasedLeafProbingAlgNode::leaf_probing_srvCallback, this); 00020 00021 // [init clients] 00022 00023 // [init action servers] 00024 00025 // [init action clients] 00026 } 00027 00028 ZyonzImageBasedLeafProbingAlgNode::~ZyonzImageBasedLeafProbingAlgNode(void) 00029 { 00030 // [free dynamic memory] 00031 } 00032 00033 void ZyonzImageBasedLeafProbingAlgNode::mainNodeThread(void) 00034 { 00035 // [fill msg structures] 00036 //this->Image_msg_.data = my_var; 00037 00038 // [fill srv structure and make request to the server] 00039 00040 // [fill action structure and make request to the action server] 00041 00042 // [publish messages] 00043 //this->seg_image_publisher_.publish(this->Image_msg_); 00044 //this->leaf_confidence_image_publisher_.publish(this->Image_msg_); 00045 } 00046 00047 /* [subscriber callbacks] */ 00048 00049 /* [service callbacks] */ 00050 bool ZyonzImageBasedLeafProbingAlgNode::leaf_probing_srvCallback(zyonz_msgs::GetProbingSteps::Request &req, zyonz_msgs::GetProbingSteps::Response &res) 00051 { 00052 //ROS_INFO("ZyonzImageBasedLeafProbingAlgNode::leaf_probing_srvCallback: New Request Received!"); 00053 00054 //use appropiate mutex to shared variables if necessary 00055 this->alg_.lock(); 00056 //this->leaf_probing_srv_mutex_.enter(); 00057 00058 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr; 00059 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.point_cloud); 00060 00061 if (!alg_.get_leaf_probing_point(point_cloud_ConstPtr)) 00062 { 00063 ROS_INFO("ZyonzImageBasedLeafProbingAlgNode::leaf_probing_srvCallback: ERROR: alg is not on run mode yet."); 00064 } 00065 //ROS_INFO("ZyonzImageBasedLeafProbingAlgNode::leaf_probing_srvCallback: Processing New Request!"); 00066 00067 // [publish messages] 00068 seg_image_publisher_.publish(alg_.get_seg_image()->toImageMsg()); 00069 leaf_confidence_image_publisher_.publish(alg_.get_confidence_image()->toImageMsg()); 00070 00071 res.steps.push_back(alg_.get_probing_step()); 00072 00073 //unlock previously blocked shared variables 00074 this->alg_.unlock(); 00075 //this->leaf_probing_srv_mutex_.exit(); 00076 00077 return true; 00078 } 00079 00080 /* [action callbacks] */ 00081 00082 /* [action requests] */ 00083 00084 void ZyonzImageBasedLeafProbingAlgNode::node_config_update(Config &config, uint32_t level) 00085 { 00086 this->alg_.lock(); 00087 00088 this->alg_.unlock(); 00089 } 00090 00091 void ZyonzImageBasedLeafProbingAlgNode::addNodeDiagnostics(void) 00092 { 00093 } 00094 00095 /* main function */ 00096 int main(int argc,char *argv[]) 00097 { 00098 return algorithm_base::main<ZyonzImageBasedLeafProbingAlgNode>(argc, argv, "zyonz_image_based_leaf_probing_alg_node"); 00099 }