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
00007
00008
00009
00010 segmented_image_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("segmented_image", 10);
00011
00012
00013 input_pcl_subscriber_ = public_node_handle_.subscribe("input_pcl", 10, &LeafProbingPointAlgNode::input_pcl_callback, this);
00014
00015
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
00021
00022
00023
00024
00025 }
00026
00027 LeafProbingPointAlgNode::~LeafProbingPointAlgNode(void)
00028 {
00029
00030 }
00031
00032 void LeafProbingPointAlgNode::mainNodeThread(void)
00033 {
00034
00035
00036
00037
00038
00039
00040
00041
00042 this->segmented_image_publisher_.publish(this->Image_msg_);
00043 }
00044
00045
00046 void LeafProbingPointAlgNode::input_pcl_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00047 {
00048
00049
00050 pcl_xyzrgb_ = msg;
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 }
00062
00063
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
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
00101
00102
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
00116 int main(int argc,char *argv[])
00117 {
00118 return algorithm_base::main<LeafProbingPointAlgNode>(argc, argv, "leaf_probing_point_alg_node");
00119 }