test_zyonz_probing_sm.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 pprint import pprint
import copy

# Import the parent directory for common
import os, sys
_root_dir = os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
sys.path.insert(0, _root_dir)

#from zyonz_common import *

from smach import StateMachine, CBState, Iterator
from smach_ros import ServiceState
from sensor_msgs.msg import PointCloud2, JointState
from geometry_msgs.msg import Transform, PoseStamped, Pose
from zyonz_msgs.msg import chlorophyll_sampleAction, chlorophyll_sampleGoal, ProbingSteps
from zyonz_msgs.srv import GetProbingStepsResponse
from probing import ZyonzProbingSMFactory

class GetTestMsg(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['ok'], output_keys=['msg'])

    def execute(self, userdata):
        msg = GetProbingStepsResponse()
        step = ProbingSteps()
        step.probing.header.frame_id = 'yos'
        msg.steps.append(step)
        step = ProbingSteps()
        step.probing.header.frame_id = 'foo'
        msg.steps.append(step)
        userdata.msg = msg
        return 'ok'

def construct_main_sm():

    factory    = ZyonzProbingSMFactory()
    sm_probing = factory.build_sm()

    sm = StateMachine(outcomes = ['succeeded','aborted'])

    with sm:
        smach.StateMachine.add('GET_PROBING_MSG', GetTestMsg(),
                          transitions  = {'ok'     : 'ACTION'},
                          remapping    = {'msg'    : 'sm_probing_msg'})

        smach.StateMachine.add('ACTION', sm_probing,
                          transitions  = {'succeeded'     : 'succeeded'},
                          remapping    = {'probing_steps' : 'sm_probing_msg'})

    return sm

def main():
    rospy.init_node("base_probing_sm")
    sm_main = construct_main_sm()

    # Run state machine introspection server for smach viewer
    sis = smach_ros.IntrospectionServer('zyonz_smach_base_probing', sm_main,
                                        '/zyonz_image_based_leaf_probing')

    sis.start()

    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == "__main__":
    main()