Source code for pointcloud2objects_lib

#!/usr/bin/env python
'''
Created on 2016-01-20
@author: Andrew H. Fagg
'''
import rospy

#######################################################################################################
# Preparation: Add the following to setup.sh (or execute by hand):
# source ~/robotics_resources/karpal/setup.sh
#
# Execute:
# roslaunch openni_launch openni.launch __ns:=karpal tf_prefix:=karpal  (DO THIS ON ijiraq)
# OLD: rosrun tf static_transform_publisher 0.180000 0.100000 0.560000 0.004264 0.452811 -0.046775 0.890369 /world karpal/camera_link 50
# rosrun pointcloud_test pointcloud_test_node __ns:=karpal
# rosrun pour pourTest.py  (OR YOUR OWN VERSION)
#
# Checks:
# Using rviz, you can subscribe to /karpal/pointcloud_test/objects_segmented as a PointCloud2 object
#
##########################################################################################

#from pointcloud2objects.msg import ObjectDescription, ObjectDescriptionSet
from pointcloud2objects.msg import *

class PointCloud2Objects:
    '''
    **PointCloud to Objects Tools**
    '''
    
    def __init__(self):
        '''
        Initialize the objects: connect to the ObjectDescriptionSet publisher
        '''
        rospy.Subscriber("karpal/pointcloud2objects/objectSet", ObjectDescriptionSet, self.callback)

        self.receivedObjects = None
        self.objectSet = None

    def callback(self, data):
        """
        ObjectDescriptionSet message callback.

        If we are waiting for an object set to arrive (indicated by
        receivedObjects=False), then we store the object set.  In this
        case, receivedObjects is set to True (indicating that the set has
        arrived) and objectSet is set to the most recent set of objects.
        Note that the latter will not be overwritten until receivedObjects
        is set to False again.

        :param data: Data message received (of type ObjectDescriptionSet)

        .. note:: This method is not intended to be called by a user program.
        """

        if not self.receivedObjects:
            self.receivedObjects = True
            self.objectSet = data
            rospy.loginfo("Received object set...")

    def waitForObjects(self):
        """
        Wait for an object message to arrive.  Returns once the message
        has arrived.

        :returns: The set of objects that has most recently been received.

        .. warning:: If you are actively changing the configuration of the objects, then it is wise to wait for two messages before acting on the data.  The first one could contain a mix of new and old object configurations (or human arms).
        """

        # Force reception of the next set of objects
        self.receivedObjects = False

        # Loop until the set of objects has arrived
        while not self.receivedObjects and not rospy.is_shutdown():
            rospy.sleep(0.1)

        # Done: return
        if rospy.is_shutdown():
            return None
        else:
            return self.objectSet