test_move_arm_state.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 PKG = 'srs_states'
00004 import roslib; roslib.load_manifest(PKG)
00005 import rospy
00006 import smach
00007 import smach_ros
00008 import sys
00009 from geometry_msgs.msg import Pose
00010 
00011 #import unittest
00012 
00013 from move_arm_states import *
00014 
00015 class TestStates:
00016   def __init__(self, *args):
00017     rospy.init_node('test_states')
00018 
00019   def test_move_arm(self,z=0.0):
00020     # create a SMACH state machine
00021     SM = smach.StateMachine(outcomes=['overall_succeeded','overall_failed'])
00022     pose1 = PoseStamped()
00023     pose1.header.frame_id = 'sdh_palm_link'
00024     pose1.header.stamp= rospy.Time.now()
00025     pose1.pose.position.x = 0
00026     pose1.pose.position.y = 0
00027     pose1.pose.position.z = z
00028     pose1.pose.orientation.w = 1
00029     pose1.pose.orientation.x = 0
00030     pose1.pose.orientation.y = 0
00031     pose1.pose.orientation.z = 0
00032     SM.userdata.poses = [pose1]
00033 
00034     # open the container
00035     with SM:
00036       smach.StateMachine.add('MOVE_ARM_PLANNED', move_arm_planned(),
00037         transitions={'succeeded':'overall_succeeded', 'failed':'overall_failed', 'not_completed':'overall_failed', 'preempted':'overall_failed'})
00038     rospy.sleep(1.0) # let script server start
00039     try:
00040       SM.execute()
00041     except:
00042       error_message = "Unexpected error:", sys.exc_info()[0]
00043       #self.fail(error_message)
00044 
00045 # main
00046 if __name__ == '__main__':
00047     test = TestStates()
00048     test.test_move_arm(-0.1)
00049     test.test_move_arm(0.1)


srs_states
Author(s): Georg Arbeiter
autogenerated on Mon Oct 6 2014 08:34:06