test_zyonz_trajectory_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

from trajectory import *
from trajectory import ZYONZGetArcTrajectory


def construct_main_sm():

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

    user_position = [ 3.0, 3.0, 3.0 ]
    user_quaternion = quaternion_from_euler(math.pi/8, 0, 0, 'rxyz')

    sm.userdata.pose_st = PoseStamped();
    sm.userdata.pose_st.header.frame_id = '/wam_link0';
    sm.userdata.pose_st.pose.position.x = user_position[0] 
    sm.userdata.pose_st.pose.position.y = user_position[1]
    sm.userdata.pose_st.pose.position.z = user_position[2]
    sm.userdata.pose_st.pose.orientation.x = user_quaternion[0]
    sm.userdata.pose_st.pose.orientation.y = user_quaternion[1]
    sm.userdata.pose_st.pose.orientation.z = user_quaternion[2]
    sm.userdata.pose_st.pose.orientation.w = user_quaternion[3]
    sm.userdata.distance = 0.3
    sm.userdata.num_views = 10
    

    with sm:
        smach.StateMachine.add('GET_TRAJECTORY', ZYONZGetArcTrajectory(),
                          transitions  = {'success' : 'succeeded',
                                          'fail'    : 'aborted'},
                          remapping    = {'pose_st'   : 'pose_st',
                                          'distance'  : 'distance',
                                          'num_views' : 'num_views',
                                          'list_pose' : 'list_pose'})

    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()