pcl_topic_to_request.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('iri_deformable_analysis')
import rospy
import sys
import threading
import time

from iri_deformable_analysis.srv import DoDeformableAnalysis, DoDeformableAnalysisRequest
from sensor_msgs.msg import PointCloud2
from std_msgs.msg import Int8, Int32, String


class GetPCL():
    def __init__(self, pcl_topic, service_topic):
        self._last_pcl_RGB     = None
        self._pcl_RGB_recieved = False
        self.mutex             = threading.Lock()
        self.subscriber        = rospy.Subscriber(pcl_topic, PointCloud2,
                                                  self.recieve_pcl_callback)
        self.srv_topic         = service_topic

    def recieve_pcl_callback(self, data):
        self.mutex.acquire()
        rospy.loginfo(" <- PCL recieved from topic")
        self._last_pcl_RGB     = data
        self._pcl_RGB_recieved = True
        self.subscriber.unregister() # we only want one message
        self.mutex.release()

    def execute(self):
        # wait for a maximum of 5 seconds
        for i in range(0, 50):
            self.mutex.acquire()
            if self._pcl_RGB_recieved:
                # Ok PCL recieved
                self.send_pcl(self._last_pcl_RGB)
                self.mutex.release()
                return

            self.mutex.release()
            time.sleep(.1)

        # we didn't get PCL in the 5 sec
        rospy.loginfo("Waiting more than 5 seconds for PCL")
        return 'fail'

    def send_pcl(self, pcl):
        # Config and launch the deformable analysis
        handler = rospy.ServiceProxy(self.srv_topic,
                                     DoDeformableAnalysis)

        analysis_request = DoDeformableAnalysisRequest()
        analysis_request.pcl_to_analyze = pcl
        # TODO: define the fusion criteria here
        #analysis_request.fusion_criteria_id = userdata.sm_pomdp_config.best_pose_algorithm_id

        try:
            rospy.loginfo(" -> PCL sent to service request")
            response = handler(analysis_request)

            if (not response):
                return 'fail'

        except rospy.ServiceException, e:
            return 'timeout'

        pose_st = response.deformable_analysis.graspability.best_grasp_pose

        return 'success'


def main():
    if len(sys.argv) < 2:
        sys.exit('Usage: %s pcl_input_topic deformable_srv' % sys.argv[0])

    rospy.init_node("pcl_2_deformable_request")

    getter = GetPCL(sys.argv[1], sys.argv[2])
    getter.execute()

    rospy.spin()

if __name__ == "__main__":
    main()