Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('zyonz_geometric_based_single_leaf_probing')
00004 import rospy
00005 import sys
00006 import threading
00007 import time
00008 from pprint import pprint
00009
00010 from zyonz_msgs.srv import GetProbingSteps, GetProbingStepsRequest
00011 from sensor_msgs.msg import PointCloud2
00012 from std_msgs.msg import Int8, Int32, String
00013
00014
00015 class GetPCL():
00016 def __init__(self, pcl_topic, service_topic):
00017 self._last_pcl_RGB = None
00018 self._pcl_RGB_recieved = False
00019 self.mutex = threading.Lock()
00020 self.subscriber = rospy.Subscriber(pcl_topic, PointCloud2,
00021 self.recieve_pcl_callback)
00022 self.srv_topic = service_topic
00023
00024 def recieve_pcl_callback(self, data):
00025 self.mutex.acquire()
00026 rospy.loginfo(" <- PCL recieved from topic")
00027 self._last_pcl_RGB = data
00028 self._pcl_RGB_recieved = True
00029 self.subscriber.unregister()
00030 self.mutex.release()
00031
00032 def execute(self):
00033
00034 for i in range(0, 50):
00035 self.mutex.acquire()
00036 if self._pcl_RGB_recieved:
00037
00038 self.send_pcl(self._last_pcl_RGB)
00039 self.mutex.release()
00040 return
00041
00042 self.mutex.release()
00043 time.sleep(.1)
00044
00045
00046 rospy.loginfo("Waiting more than 5 seconds for PCL")
00047 return 'fail'
00048
00049 def send_pcl(self, pcl):
00050
00051 handler = rospy.ServiceProxy(self.srv_topic,
00052 GetProbingSteps)
00053
00054 analysis_request = GetProbingStepsRequest()
00055 analysis_request.point_cloud = pcl
00056
00057
00058
00059 try:
00060 rospy.loginfo(" -> PCL sent to service request")
00061 response = handler(analysis_request)
00062
00063 if (not response):
00064 return 'fail'
00065
00066 except rospy.ServiceException, e:
00067 return 'timeout'
00068
00069 pose_st = response
00070 pprint(pose_st)
00071
00072 return 'success'
00073
00074
00075 def main():
00076 if len(sys.argv) < 2:
00077 sys.exit('Usage: %s pcl_input_topic zyonz_image_based_leaf_probing_srv' % sys.argv[0])
00078
00079 rospy.init_node("pcl_2_zyonz_image_based_leaf_probing_request")
00080
00081 getter = GetPCL(sys.argv[1], sys.argv[2])
00082 getter.execute()
00083
00084 rospy.spin()
00085
00086 if __name__ == "__main__":
00087 main()