smach subscriber publisher

edit

Hi,

This small (and untested) example subscribes on a topic and return success if 2 is received on this topic in the next 30 seconds.

#!/usr/bin/env python
import roslib; roslib.load_manifest('smach_example')
import rospy
import smach
import smach_ros
from std_msgs.msg import Int16
import time
import threading

class WaitForTwo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['success', 'in_progress', 'failed'])

        self.mutex = threading.Lock()
        self.two_received = False

        self.subscriber = rospy.Subscriber('/test', Int16, self.callback)

    def callback(self, data):
        self.mutex.acquire()
        if data.data == 2:
            self.two_received = True
        self.mutex.release()

    def execute(self):
        #wait for a maximum of 30 seconds 
        for i in range(0, 300):
            self.mutex.acquire()
            if self.two_received:
                #ok we received 2
                return 'success'

            self.mutex.release()

            time.sleep(.1)
            #still waiting
            return 'in_progress'
        #we didn't get 2 in the 30 sec
        return 'failed'

Based on the callback-and-loop-in-execute-with-mutex scheme Ugo mentioned in his answer I made a fully generic message receiver smach.State class that helps a lot for receiving messages in a SMACH machine, as I couldn't made use of the the MonitorState.

The current feature set includes waiting, timeout, userdata access and latching, of course optional.

Class WaitForMsgState in module util.py in unreleased package uashh_smach. The class CheckSmachEnabledState right underneath is a simple usage example

As the whole class has a lot of (docstring) lines I just post the up-to-date link to the module's file in the repository (and just for read only reference the commit-static link to its code line).