spadmeter.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('zyonz_apps_base')
import rospy
import smach
import smach_ros

from zyonz_msgs.msg            import chlorophyll_sampleAction, chlorophyll_sampleGoal
from iri_common_smach.st_sleep import WaitSeconds

class SM_ZYONZ_CloseGripper():
    def __init__(self):
        self.sm = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])

    def build_sm(self):
        with self.sm:
            smach.StateMachine.add('DO_SPADMETER_TEST_CLOSE',
                         smach_ros.SimpleActionState('/zyonz/skills/chlorophyll_meter/take_sample',
                         chlorophyll_sampleAction,
                         goal = chlorophyll_sampleGoal(close=True)),
                                   {'succeeded': 'WAIT_SPADMETER',
                                    'preempted': 'preempted',
                                    'aborted'  : 'aborted'})

            smach.StateMachine.add('WAIT_SPADMETER', WaitSeconds(1),
                         transitions={'finish': 'DO_SPADMETER_TEST_OPEN'})

            smach.StateMachine.add('DO_SPADMETER_TEST_OPEN',
                         smach_ros.SimpleActionState('/zyonz/skills/chlorophyll_meter/take_sample',
                         chlorophyll_sampleAction,
                         goal = chlorophyll_sampleGoal(close=False)),
                                   {'succeeded': 'succeeded',
                                    'preempted': 'preempted',
                                    'aborted'  : 'aborted'})
        return self.sm