00001
00002
00003 import roslib; roslib.load_manifest('estirabot_apps')
00004 import rospy
00005 import smach
00006 import smach_ros
00007 import tf
00008
00009
00010 import os, sys
00011 _root_dir = os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
00012 sys.path.insert(0, _root_dir)
00013
00014 from estirabot_common import *
00015 from pomdp_create_model import *
00016 import random
00017
00018 from smach import StateMachine, CBState, Iterator
00019 from smach_ros import ServiceState
00020 from actionlib import *
00021 from actionlib.msg import *
00022 from std_msgs.msg import String
00023 from object_manipulation_msgs.msg import PickupAction,PickupGoal,PlaceAction,PlaceGoal
00024 from sensor_msgs.msg import PointCloud2, JointState
00025 from iri_deformable_analysis.srv import DoDeformableAnalysis, DoDeformableAnalysisRequest
00026 from estirabot_msgs.msg import PomdpGraspConfig
00027 from estirabot_msgs.srv import obs2action, obs2actionRequest
00028 from geometry_msgs.msg import Transform, PoseStamped, Pose
00029 from pprint import pprint
00030 from std_msgs.msg import Int8, Int32
00031
00032 class ExperimentData():
00033 def __init__(self):
00034
00035
00036 self.iteration_number = 0
00037
00038 self.algorithms = [ 1, 2, 3, 4, 5, 6,
00039 11, 12, 13, 14, 15, 16,
00040 101, 102, 103, 104, 105, 106,
00041 111, 112, 113, 114, 115, 116]
00042
00043 self.algorithms_index_list = []
00044 self.total_algs = len(self.algorithms)
00045
00046
00047 self.numrounds = 10
00048 for i in range(self.numrounds):
00049 self.algorithms_index_list.extend(range(self.total_algs))
00050 random.shuffle(self.algorithms_index_list)
00051 print(self.algorithms_index_list)
00052 self.numexperiments = len(self.algorithms_index_list)
00053
00054
00055
00056 class DummyState(smach.State):
00057 def __init__(self):
00058 smach.State.__init__(self, outcomes=['success', 'fail'])
00059
00060 def execute(self, userdata):
00061 return 'success'
00062
00063 class InputResult(smach.State):
00064 def __init__(self):
00065 smach.State.__init__(self, outcomes=['success'])
00066
00067 def execute(self, userdata):
00068 keyserver = GetKeyStroke()
00069 c = -1
00070 print "Press [0-5] for clothes grabbed, 9 for error:"
00071 while ((c < 0) or (c > 5) or (c == 9)):
00072 c = keyserver()
00073 try:
00074 c = int(c)
00075 except ValueError:
00076 print "Error. Press [0-5] for clothes grabbed:"
00077 continue
00078
00079 print "Your value was: " + str(c)
00080 pub = rospy.Publisher('/log/number_of_clothes', Int8, None, False, True)
00081 pub.publish(c)
00082
00083 return 'success'
00084
00085 class PlaceHandler():
00086 def __init__(self):
00087 self.next_left_zone = 1
00088 self.next_right_zone = 1
00089
00090 def calculate_place_pose(self, pomdp_config):
00091
00092 if (pomdp_config.place_point == None):
00093 return self.get_basket_pose()
00094
00095
00096 if (pomdp_config.grabbing_zone == pomdp_config.LEFT_ZONE):
00097 return self.get_left_place_pose()
00098 elif (pomdp_config.grabbing_zone == pomdp_config.RIGHT_ZONE):
00099 return self.get_right_place_pose()
00100 else:
00101 rospy.logerr("Value for graspping zone is not LEFT or RIGHT")
00102 return build_home_pose()
00103
00104 def get_basket_pose(self):
00105 return build_pose(-0.165, 0.514, -0.188, 1, 0, 0, 0)
00106
00107 def get_right_place_pose(self):
00108 if (self.next_right_zone == 1):
00109 pose = build_pose(0.422, -0.390, -0.234, 1, 0, 0, 0)
00110 elif (self.next_right_zone == 2):
00111 pose = build_pose(0.584, -0.390, -0.234, 1, 0, 0, 0)
00112 elif (self.next_right_zone == 3):
00113 pose = build_pose(0.633, -0.160, -0.234, 1, 0, 0, 0)
00114 elif (self.next_right_zone == 4):
00115 pose = build_pose(0.476, -0.160, -0.234, 1, 0, 0, 0)
00116 else:
00117 rospy.logerr("Righ pose next zone is out of range: " + str(self.next_right_zone))
00118
00119 self.next_right_zone = self.next_right_zone + 1
00120
00121
00122 if (self.next_right_zone > 4):
00123 self.next_right_zone = 1
00124
00125 return pose
00126
00127 def get_left_place_pose(self):
00128 if (self.next_left_zone == 1):
00129 pose = build_pose(0.422, 0.390, -0.234, 1, 0, 0, 0)
00130 elif (self.next_left_zone == 2):
00131 pose = build_pose(0.584, 0.390, -0.234, 1, 0, 0, 0)
00132 elif (self.next_left_zone == 3):
00133 pose = build_pose(0.633, 0.160, -0.234, 1, 0, 0, 0)
00134 elif (self.next_left_zone == 4):
00135 pose = build_pose(0.476, 0.160, -0.234, 1, 0, 0, 0)
00136 else:
00137 rospy.logerr("Left pose next zone is out of range: " + str(self.next_left_zone))
00138
00139 self.next_left_zone = self.next_left_zone + 1
00140
00141
00142 if (self.next_left_zone > 4):
00143 self.next_left_zone = 1
00144
00145 return pose
00146
00147 def get_config_from_pomdp_action(pomdp_action):
00148
00149 if (pomdp_action > 21):
00150 rospy.logfatal("Pomdp action id out of range: " + str(pomdp_action))
00151
00152 server = PomdpConfigFactory()
00153
00154 if (pomdp_action == 20):
00155 magic_number = 114
00156 elif (pomdp_action == 21):
00157 magic_number = 114
00158 elif (pomdp_action % 10 == 0):
00159 magic_number = 1
00160 elif (pomdp_action % 10 == 1):
00161 magic_number = 3
00162 elif (pomdp_action % 10 == 2):
00163 magic_number = 5
00164 elif (pomdp_action % 10 == 3):
00165 magic_number = 11
00166 elif (pomdp_action % 10 == 4):
00167 magic_number = 14
00168 elif (pomdp_action % 10 == 5):
00169 magic_number = 101
00170 elif (pomdp_action % 10 == 6):
00171 magic_number = 103
00172 elif (pomdp_action % 10 == 7):
00173 magic_number = 105
00174 elif (pomdp_action % 10 == 8):
00175 magic_number = 111
00176 elif (pomdp_action % 10 == 9):
00177 magic_number = 114
00178
00179
00180 config = server.get_instance(magic_number)
00181
00182
00183 if (pomdp_action == 20):
00184 config.grabbing_zone = config.RIGHT_ZONE
00185 elif (pomdp_action == 21):
00186 config.grabbing_zone = config.LEFT_ZONE
00187 elif (pomdp_action > 9):
00188 config.grabbing_zone = config.LEFT_ZONE
00189 else:
00190 config.grabbing_zone = config.RIGHT_ZONE
00191
00192
00193
00194
00195 if (pomdp_action == 20) or (pomdp_action == 21):
00196 config.place_point = None
00197
00198 return config
00199
00200 class FirstAction(smach.State):
00201 def __init__(self):
00202 smach.State.__init__(self, outcomes = ['ok','fail'],
00203 output_keys = ['pomdp_action','place_handler'])
00204
00205 def execute(self, userdata):
00206
00207
00208 randaction = random.randint(0,userdata.total_algs)
00209 print("initial random action: " % randaction)
00210 userdata.pomdp_action = randaction
00211 pub = rospy.Publisher('/log/pomdp/action', Int8, None, False, True)
00212 pub.publish(response.action)
00213
00214 if (not response):
00215 return 'fail'
00216
00217
00218 userdata.place_handler = PlaceHandler()
00219
00220 return 'ok'
00221
00222 class LogPCL(smach.State):
00223 def __init__(self):
00224 smach.State.__init__(self, outcomes=['success','fail'],
00225 input_keys=['sm_pcl_to_log'],
00226 output_keys=[''])
00227
00228 def execute(self, userdata):
00229
00230 pub = rospy.Publisher('/log/pcl_table', PointCloud2, None, False, True)
00231 pub.publish(userdata.sm_pcl_to_log)
00232 return 'success'
00233
00234
00235 class TranslatePomdp(smach.State):
00236 def __init__(self):
00237 smach.State.__init__(self, outcomes = ['fail','image_from_left','image_from_right'],
00238 input_keys = ['pomdp_action','place_handler'],
00239 output_keys = ['sm_pomdp_config','place_handler'])
00240
00241 def execute(self, userdata):
00242
00243 config = get_config_from_pomdp_action(userdata.pomdp_action)
00244 config.place_point = userdata.place_handler.calculate_place_pose(config)
00245
00246 userdata.sm_pomdp_config = config
00247
00248 pub_config = rospy.Publisher('/debug/pomdp_config', PomdpGraspConfig, None, False, True)
00249 pub_config.publish(config)
00250 pprint(config)
00251
00252 if (config.grabbing_zone == config.LEFT_ZONE):
00253 return 'image_from_left'
00254 else:
00255 return 'image_from_right'
00256
00257 class CountObjects(smach.State):
00258 def __init__(self):
00259 smach.State.__init__(self, outcomes = ['fail','success'],
00260 input_keys = ['sm_pcl_table_left','sm_pcl_table_right'],
00261 output_keys = ['left_objects','right_objects'])
00262 self.number = -1
00263
00264 sub = rospy.Subscriber("/estirabot/skills/iri_textile_count/num_objects_found",
00265 Int32, self.recieve_number)
00266
00267 def execute(self, userdata):
00268
00269 self.calculate_objects(userdata.sm_pcl_table_left)
00270 userdata.left_objects = self.number
00271 pprint ("Left objects: " + str(self.number))
00272 pub = rospy.Publisher('/debug/observation/left_objects', Int8, None, False, True)
00273 pub.publish(self.number)
00274
00275
00276 rospy.sleep(2)
00277
00278
00279 self.calculate_objects(userdata.sm_pcl_table_right)
00280 userdata.right_objects = self.number
00281 pprint ("Right objects: " + str(self.number))
00282 pub = rospy.Publisher('/debug/observation/right_objects', Int8, None, False, True)
00283 pub.publish(self.number)
00284
00285 return 'success'
00286
00287 def recieve_number(self, data):
00288 self.number = data.data
00289
00290 def calculate_objects(self, pcl):
00291 self.number = -1
00292
00293 pub = rospy.Publisher('/estirabot/skills/table/pcl', PointCloud2, None, False, True)
00294 pub.publish(pcl)
00295
00296 while (self.number == -1 and (not rospy.is_shutdown())):
00297 pprint("waiting")
00298 time.sleep(.1)
00299
00300 class PrePlace(smach.State):
00301 def __init__(self):
00302 smach.State.__init__(self, outcomes = ['fail','success'],
00303 input_keys = ['sm_pomdp_config'])
00304
00305 def execute(self, userdata):
00306 tf = Transform()
00307 tf.translation.z = -0.4
00308
00309 return wam_cartesian_move(homogeneous_product_pose_transform(userdata.sm_pomdp_config.place_point,
00310 tf))
00311 class GetNextAction(smach.State):
00312 def __init__(self):
00313 smach.State.__init__(self, outcomes = ['fail','finish','continue'],
00314 input_keys = ['experiment_data'],
00315 output_keys = ['pomdp_action', 'experiment_data'])
00316
00317
00318 def execute(self, userdata):
00319
00320
00321 pprint("iteration number = %d" % userdata.experiment_data.iteration_number)
00322 userdata.experiment_data.iteration_number += 1
00323 if (userdata.experiment_data.iteration_number == userdata.experiment_data.numexperiments):
00324 return 'finish'
00325 action = userdata.experiment_data.algorithms_index_list[userdata.experiment_data.iteration_number]
00326
00327
00328 pub = rospy.Publisher('/log/pomdp/action', Int8, None, False, True)
00329 pub.publish(action)
00330 userdata.pomdp_action = action
00331 return 'continue'
00332
00333 def construct_main_sm():
00334 sm = StateMachine(outcomes = ['succeeded','aborted'])
00335
00336
00337 sm.userdata.experiment_data = ExperimentData()
00338
00339 sm.userdata.place_handler = PlaceHandler()
00340
00341 with sm:
00342 smach.StateMachine.add('START_ROUND', EstirabotGoHome(),
00343 transitions={'success': 'LOG_PCL',
00344 'fail' : 'aborted'})
00345 smach.StateMachine.add('LOG_PCL',
00346 EstirabotGetPCL("/estirabot/ceiling_kinect/camera/rgb/points"),
00347 transitions = {'success' : 'LOG_IMAGE',
00348 'fail' : 'aborted'},
00349 remapping = {'pcl_RGB' : 'sm_pcl_to_log'})
00350 smach.StateMachine.add('LOG_IMAGE', LogPCL(),
00351 transitions={'success': 'GET_NEXT_ACTION',
00352 'fail' : 'aborted'})
00353
00354
00355 smach.StateMachine.add('GET_NEXT_ACTION', GetNextAction(),
00356 transitions={'continue' : 'TRANSLATE_POMDP_ACTION',
00357 'finish' : 'succeeded',
00358 'fail' : 'aborted'})
00359
00360
00361 smach.StateMachine.add('TRANSLATE_POMDP_ACTION', TranslatePomdp(),
00362 transitions = { 'image_from_left' : 'GET_KINECT_IMAGE_FROM_LEFT',
00363 'image_from_right' : 'GET_KINECT_IMAGE_FROM_RIGHT',
00364 'fail':'aborted'},
00365 remapping = { 'sm_pomdp_config' : 'sm_pomdp_config'})
00366
00367
00368
00369 smach.StateMachine.add('GET_KINECT_IMAGE_FROM_LEFT',
00370 EstirabotGetPCL("/estirabot/skills/table/left/pcl"),
00371 transitions = {'success' : 'PERFORM_ANALISYS',
00372 'fail' : 'aborted'},
00373 remapping = {'pcl_RGB' : 'sm_pcl_output'})
00374
00375
00376 smach.StateMachine.add('GET_KINECT_IMAGE_FROM_RIGHT',
00377 EstirabotGetPCL("/estirabot/skills/table/right/pcl"),
00378 transitions = {'success' : 'PERFORM_ANALISYS',
00379 'fail' : 'aborted'},
00380 remapping = {'pcl_RGB' : 'sm_pcl_output'})
00381
00382
00383 smach.StateMachine.add('PERFORM_ANALISYS', PerformAnalysis(),
00384 transitions = {'success' : 'CALCULATE_GRASP',
00385 'timeout' : 'PERFORM_ANALISYS',
00386 'fail' : 'aborted'},
00387 remapping = {'pcl_RGB' : 'sm_pcl_output',
00388 'best_grasp_pose' : 'sm_best_grasp_pose'})
00389
00390 smach.StateMachine.add('CALCULATE_GRASP', CalculateGrasp(),
00391 transitions = {'success' : 'DO_GRASP',
00392 'no_ik_solution' : 'CALCULATE_GRASP',
00393 'fail' : 'aborted',
00394 'all_ik_fail' : 'GET_TABLE_IMAGE_FROM_LEFT'},
00395 remapping = {'pcl_RGB' : 'sm_pcl_output',
00396 'best_grasp_pose' : 'sm_best_grasp_pose',
00397 'grasp_goal' : 'sm_grasp_goal'})
00398
00399 smach.StateMachine.add('DO_GRASP',
00400 smach_ros.SimpleActionState('/estirabot/skills/grasp/pickup',
00401 PickupAction,
00402 goal_cb = get_sm_grasp_goal_callback,
00403 input_keys=['sm_grasp_goal']),
00404 {'succeeded': 'DO_LIFT',
00405 'preempted': 'aborted',
00406 'aborted' : 'aborted'})
00407
00408 smach.StateMachine.add('DO_LIFT', EstirabotGoHome(),
00409 transitions={'success' : 'CHECK_GRASP',
00410 'fail' : 'aborted'})
00411
00412 smach.StateMachine.add('CHECK_GRASP', InputResult(),
00413 transitions={'success' : 'DO_PRE_PLACE'})
00414
00415
00416 smach.StateMachine.add('DO_PRE_PLACE', PrePlace(),
00417 transitions={'success' : 'DO_PLACE',
00418 'fail' : 'aborted'})
00419
00420
00421 smach.StateMachine.add('DO_PLACE',
00422 smach_ros.SimpleActionState('/estirabot/skills/iri_place/place',
00423 PlaceAction,
00424 goal_cb = get_sm_place_goal_callback,
00425 input_keys=['sm_pomdp_config']),
00426 {'succeeded': 'BACK_TO_HOME',
00427 'preempted': 'aborted',
00428 'aborted' : 'aborted'})
00429
00430 smach.StateMachine.add('BACK_TO_HOME', EstirabotGoHome(),
00431 transitions={'success': 'GET_TABLE_IMAGE_FROM_LEFT',
00432 'fail' : 'aborted'})
00433
00434
00435
00436 smach.StateMachine.add('GET_TABLE_IMAGE_FROM_LEFT',
00437 EstirabotGetPCL("/estirabot/skills/table/left/pcl"),
00438 transitions = {'success' : 'GET_TABLE_IMAGE_FROM_RIGHT',
00439 'fail' : 'aborted'},
00440 remapping = {'pcl_RGB' : 'sm_pcl_table_left'})
00441
00442 smach.StateMachine.add('GET_TABLE_IMAGE_FROM_RIGHT',
00443 EstirabotGetPCL("/estirabot/skills/table/right/pcl"),
00444 transitions = {'success' : 'COUNT_OBJECTS',
00445 'fail' : 'aborted'},
00446 remapping = {'pcl_RGB' : 'sm_pcl_table_right'})
00447
00448 smach.StateMachine.add('COUNT_OBJECTS', CountObjects(),
00449 transitions={'success' : 'START_ROUND',
00450 'fail' : 'aborted'})
00451
00452 return sm
00453
00454 def main():
00455 rospy.init_node("pomdp_sm")
00456 sm_main = construct_main_sm()
00457
00458
00459 sis = smach_ros.IntrospectionServer('estirabot_smach_replicable', sm_main,
00460 '/estirabot_smach_replicable')
00461
00462 sis.start()
00463
00464 outcome = sm_main.execute()
00465
00466 rospy.spin()
00467 sis.stop()
00468
00469 if __name__ == "__main__":
00470 main()