Go to the documentation of this file.00001 #include "zyonz_obtain_two_clustered_leaves_alg_node.h"
00002
00003 ZyonzObtainTwoClusteredLeavesAlgNode::ZyonzObtainTwoClusteredLeavesAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<ZyonzObtainTwoClusteredLeavesAlgorithm>()
00005 {
00006
00007
00008
00009
00010 cluster_a_img_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("cluster_a_img", 1);
00011 cluster_b_img_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("cluster_b_img", 1);
00012 cluster_a_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("cluster_a", 1);
00013 cluster_b_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("cluster_b", 1);
00014
00015
00016
00017
00018 extract_clusters_server_ = public_node_handle_.advertiseService("extract_clusters", &ZyonzObtainTwoClusteredLeavesAlgNode::extract_clustersCallback, this);
00019
00020
00021
00022
00023
00024
00025 }
00026
00027 ZyonzObtainTwoClusteredLeavesAlgNode::~ZyonzObtainTwoClusteredLeavesAlgNode(void)
00028 {
00029
00030 }
00031
00032 void ZyonzObtainTwoClusteredLeavesAlgNode::mainNodeThread(void)
00033 {
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 }
00048
00049
00050
00051
00052 bool ZyonzObtainTwoClusteredLeavesAlgNode::extract_clustersCallback(zyonz_obtain_two_clustered_leaves::ExtractClusters::Request &req, zyonz_obtain_two_clustered_leaves::ExtractClusters::Response &res)
00053 {
00054 ROS_INFO("ZyonzObtainTwoClusteredLeavesAlgNode::extract_clustersCallback: New Request Received!");
00055
00056
00057 alg_.lock();
00058 extract_clusters_mutex_.enter();
00059
00060
00061
00062 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr;
00063 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.input);
00064
00065 if(!alg_.extract_clusters(point_cloud_ConstPtr))
00066 {
00067 ROS_INFO("ZyonzObtainTwoClusteredLeavesAlgNode::extract_clustersCallback: ERROR: alg is not on run mode yet!");
00068 }
00069
00070 pcl::toROSMsg(*alg_.get_cluster_a(), res.cluster_a);
00071 pcl::toROSMsg(*alg_.get_cluster_b(), res.cluster_b);
00072 res.cluster_a_img = *alg_.get_cluster_a_img();
00073 res.cluster_b_img = *alg_.get_cluster_b_img();
00074
00075 cluster_a_publisher_.publish(*alg_.get_cluster_a());
00076 cluster_b_publisher_.publish(*alg_.get_cluster_b());
00077 cluster_a_img_publisher_.publish(*alg_.get_cluster_a_img());
00078 cluster_b_img_publisher_.publish(*alg_.get_cluster_b_img());
00079
00080
00081 alg_.unlock();
00082 extract_clusters_mutex_.exit();
00083
00084 return true;
00085 }
00086
00087
00088
00089
00090
00091 void ZyonzObtainTwoClusteredLeavesAlgNode::node_config_update(Config &config, uint32_t level)
00092 {
00093 this->alg_.lock();
00094
00095 this->alg_.unlock();
00096 }
00097
00098 void ZyonzObtainTwoClusteredLeavesAlgNode::addNodeDiagnostics(void)
00099 {
00100 }
00101
00102
00103 int main(int argc,char *argv[])
00104 {
00105 return algorithm_base::main<ZyonzObtainTwoClusteredLeavesAlgNode>(argc, argv, "zyonz_obtain_two_clustered_leaves_alg_node");
00106 }