test_table_object_cluster.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 detection_states import *
00014 
00015 class TestStates:
00016   def __init__(self, *args):
00017     rospy.init_node('test_states')
00018 
00019   def test_table_object_cluster(self):
00020     # create a SMACH state machine
00021     SM = smach.StateMachine(outcomes=['overall_succeeded','overall_failed'])
00022     #SM.userdata.object_id = 0
00023     SM.userdata.target_table_pose = Pose()
00024     SM.userdata.target_table_pose.position.x = 0.67
00025     SM.userdata.target_table_pose.position.y = 1.26
00026     SM.userdata.target_table_pose.position.z = 0.74
00027     SM.userdata.target_table_pose.orientation.w = 0
00028     SM.userdata.target_table_pose.orientation.x = 0
00029     SM.userdata.target_table_pose.orientation.y = 0
00030     SM.userdata.target_table_pose.orientation.z = 0
00031     SM.userdata.target_object_pose_on_table = Pose()
00032     SM.userdata.target_object_pose_on_table.position.x = 0.7
00033     SM.userdata.target_object_pose_on_table.position.y = 1.0
00034     SM.userdata.target_object_pose_on_table.position.z = 0.74
00035     SM.userdata.target_object_pose_on_table.orientation.w = 0
00036     SM.userdata.target_object_pose_on_table.orientation.x = 0
00037     SM.userdata.target_object_pose_on_table.orientation.y = 0
00038     SM.userdata.target_object_pose_on_table.orientation.z = 0
00039     #SM.userdata.verfied_target_object_pose = Pose()
00040 
00041     # open the container
00042     with SM:
00043       smach.StateMachine.add('TABLE_CLUSTER', GetTableObjectCluster(),
00044         transitions={'succeeded':'CHECK_FREE', 'failed':'overall_failed', 'not_completed':'overall_failed', 'preempted':'overall_failed'})
00045       smach.StateMachine.add('CHECK_FREE', CheckPoseOnTableFree(),
00046         transitions={'succeeded':'overall_succeeded', 'failed':'overall_failed', 'not_completed':'overall_failed', 'preempted':'overall_failed'})
00047       #smach.StateMachine.add('UPDATE', Map360(),
00048       #  transitions={'succeeded':'overall_succeeded', 'failed':'overall_failed'})
00049     try:
00050       SM.execute()
00051     except:
00052       error_message = "Unexpected error:", sys.exc_info()[0]
00053       #self.fail(error_message)
00054 
00055 # main
00056 if __name__ == '__main__':
00057     test = TestStates()
00058     test.test_table_object_cluster()


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