leaf_probing_point_alg_node.cpp
Go to the documentation of this file.
00001 #include "leaf_probing_point_alg_node.h"
00002 
00003 LeafProbingPointAlgNode::LeafProbingPointAlgNode(void) :
00004   algorithm_base::IriBaseAlgorithm<LeafProbingPointAlgorithm>()
00005 {
00006   //init class attributes if necessary
00007   //this->loop_rate_ = 2;//in [Hz]
00008 
00009   // [init publishers]
00010   segmented_image_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("segmented_image", 10);
00011   
00012   // [init subscribers]
00013   input_pcl_subscriber_ = public_node_handle_.subscribe("input_pcl", 10, &LeafProbingPointAlgNode::input_pcl_callback, this);
00014   
00015   // [init services]
00016   leaf_probing_srv_ = public_node_handle_.advertiseService("leaf_probing_srv", &LeafProbingPointAlgNode::leaf_probing_srv_callback, this);
00017   leaf_segmentation_client_ = public_node_handle_.serviceClient<iri_leaf_segmentation::GetClusteredImage> ("/leaf_segmentation_alg_node/get_clustered_image_srv");
00018   leaf_fitting_client_ = public_node_handle_.serviceClient<iri_leaf_fitting::GetConfidenceLeafFitting> ("/leaf_fitting_alg_node/get_confidence_leaf_fitting_srv");
00019 
00020   // [init clients]
00021   
00022   // [init action servers]
00023   
00024   // [init action clients]
00025 }
00026 
00027 LeafProbingPointAlgNode::~LeafProbingPointAlgNode(void)
00028 {
00029   // [free dynamic memory]
00030 }
00031 
00032 void LeafProbingPointAlgNode::mainNodeThread(void)
00033 {
00034   // [fill msg structures]
00035   //this->Image_msg_.data = my_var;
00036   
00037   // [fill srv structure and make request to the server]
00038   
00039   // [fill action structure and make request to the action server]
00040 
00041   // [publish messages]
00042   this->segmented_image_publisher_.publish(this->Image_msg_);
00043 }
00044 
00045 /*  [subscriber callbacks] */
00046 void LeafProbingPointAlgNode::input_pcl_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
00047 { 
00048 //  ROS_INFO("LeafProbingPointAlgNode::input_pcl_callback: New Message Received"); 
00049   
00050   pcl_xyzrgb_ = msg;
00051 
00052   //use appropiate mutex to shared variables if necessary 
00053   //this->alg_.lock(); 
00054   //this->input_pcl_mutex_.enter(); 
00055 
00056   //std::cout << msg->data << std::endl; 
00057 
00058   //unlock previously blocked shared variables 
00059   //this->alg_.unlock(); 
00060   //this->input_pcl_mutex_.exit(); 
00061 }
00062 
00063 /*  [service callbacks] */
00064 bool
00065 LeafProbingPointAlgNode::leaf_probing_srv_callback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
00066 {
00067   ROS_INFO("LeafProbingPointAlgNode::srv_callback: New Service Request Received"); 
00068   
00069   iri_leaf_segmentation::GetClusteredImage cluster_srv;
00070 
00071   cluster_srv.request.point_cloud = *pcl_xyzrgb_;
00072   
00073   if (leaf_segmentation_client_.call(cluster_srv))
00074   {
00075     ROS_INFO("Current clusters: %d", cluster_srv.response.num_clusters.data);
00076   }
00077   else
00078   {
00079     ROS_ERROR("Failed to call service get_clustered_image_srv");
00080     return 0;
00081   }
00082 
00083   iri_leaf_fitting::GetConfidenceLeafFitting confidence_srv;
00084   confidence_srv.request.num_clusters = cluster_srv.response.num_clusters;
00085   confidence_srv.request.cluster_labels = cluster_srv.response.cluster_labels;
00086   if (leaf_fitting_client_.call(confidence_srv))
00087   {
00088   //  ROS_INFO("Current clusters: %d", confidence_srv.response.fit_score_list.data);
00089   }
00090   else
00091   {
00092     ROS_ERROR("Failed to call service get_clustered_image_srv");
00093     return 0;
00094   }
00095 
00096 
00097   return 1;
00098 }
00099 
00100 /*  [action callbacks] */
00101 
00102 /*  [action requests] */
00103 
00104 void LeafProbingPointAlgNode::node_config_update(Config &config, uint32_t level)
00105 {
00106   this->alg_.lock();
00107 
00108   this->alg_.unlock();
00109 }
00110 
00111 void LeafProbingPointAlgNode::addNodeDiagnostics(void)
00112 {
00113 }
00114 
00115 /* main function */
00116 int main(int argc,char *argv[])
00117 {
00118   return algorithm_base::main<LeafProbingPointAlgNode>(argc, argv, "leaf_probing_point_alg_node");
00119 }


iri_leaf_probing_point
Author(s): Sergi Foix
autogenerated on Fri Dec 6 2013 21:11:43