Go to the documentation of this file.00001 #include "zyonz_obtain_roi_jump_edge_based_alg_node.h"
00002
00003 ZyonzObtainRoiJumpEdgeBasedAlgNode::ZyonzObtainRoiJumpEdgeBasedAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<ZyonzObtainRoiJumpEdgeBasedAlgorithm>()
00005 {
00006
00007
00008
00009
00010 roi_img_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("roi_img", 1);
00011 roi_pc_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("roi_pc", 1);
00012 roi_pose_publisher_ = public_node_handle_.advertise<geometry_msgs::PoseStamped>("roi_pose", 1);
00013
00014
00015
00016
00017 extract_two_leaves_roi_server_ = public_node_handle_.advertiseService("extract_two_leaves_roi", &ZyonzObtainRoiJumpEdgeBasedAlgNode::extract_two_leaves_roiCallback, this);
00018
00019
00020
00021
00022
00023
00024 }
00025
00026 ZyonzObtainRoiJumpEdgeBasedAlgNode::~ZyonzObtainRoiJumpEdgeBasedAlgNode(void)
00027 {
00028
00029 }
00030
00031 void ZyonzObtainRoiJumpEdgeBasedAlgNode::mainNodeThread(void)
00032 {
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 }
00043
00044
00045
00046
00047 bool ZyonzObtainRoiJumpEdgeBasedAlgNode::extract_two_leaves_roiCallback(zyonz_obtain_roi_jump_edge_based::ExtractTwoLeavesRoi::Request &req, zyonz_obtain_roi_jump_edge_based::ExtractTwoLeavesRoi::Response &res)
00048 {
00049 ROS_INFO("ZyonzObtainRoiJumpEdgeBasedAlgNode::extract_two_leaves_roiCallback: New Request Received!");
00050
00051
00052 sensor_msgs::PointCloud2::ConstPtr point_cloud_ConstPtr;
00053 point_cloud_ConstPtr = boost::make_shared<sensor_msgs::PointCloud2>(req.residual_pc);
00054 sensor_msgs::Image::ConstPtr cluster_a_img_ConstPtr, cluster_b_img_ConstPtr, residual_img_ConstPtr;
00055 cluster_a_img_ConstPtr = boost::make_shared<sensor_msgs::Image>(req.cluster_a_img);
00056 cluster_b_img_ConstPtr = boost::make_shared<sensor_msgs::Image>(req.cluster_b_img);
00057 residual_img_ConstPtr = boost::make_shared<sensor_msgs::Image>(req.residual_img);
00058
00059
00060 alg_.lock();
00061 extract_two_leaves_roi_mutex_.enter();
00062
00063 if(!alg_.isRunning(point_cloud_ConstPtr, cluster_a_img_ConstPtr, cluster_b_img_ConstPtr, residual_img_ConstPtr))
00064 {
00065 ROS_INFO("ZyonzObtainRoiJumpEdgeBasedAlgNode::extract_two_leaves_roiCallback: ERROR: alg is not on run mode yet.");
00066 }
00067
00068
00069 pcl::toROSMsg(*alg_.get_roi_pc (), res.roi_pc);
00070 res.roi_img = *alg_.get_roi_img ();
00071 res.roi_pose = *alg_.get_roi_pose ();
00072 res.viewpoint_pose = *alg_.get_viewpoint_pose ();
00073
00074
00075 roi_pc_publisher_.publish(*alg_.get_roi_pc ());
00076 roi_img_publisher_.publish(*alg_.get_roi_img ());
00077 roi_pose_publisher_.publish(*alg_.get_roi_pose ());
00078
00079
00080 alg_.unlock();
00081 extract_two_leaves_roi_mutex_.exit ();
00082
00083 return true;
00084 }
00085
00086
00087
00088
00089
00090 void ZyonzObtainRoiJumpEdgeBasedAlgNode::node_config_update(Config &config, uint32_t level)
00091 {
00092 this->alg_.lock();
00093
00094 this->alg_.unlock();
00095 }
00096
00097 void ZyonzObtainRoiJumpEdgeBasedAlgNode::addNodeDiagnostics(void)
00098 {
00099
00100 }
00101
00102
00103 int main(int argc,char *argv[])
00104 {
00105 return algorithm_base::main<ZyonzObtainRoiJumpEdgeBasedAlgNode>(argc, argv, "zyonz_obtain_roi_jump_edge_based_alg_node");
00106 }