demoplanaction.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib;
00003 roslib.load_manifest('srs_knowledge')
00004 import sys
00005 import rospy
00006 
00007 from srs_knowledge.srv import *
00008 
00009 def testNextActionService(result):
00010     print 'Plan next Action service'
00011     rospy.wait_for_service('plan_next_action')
00012     try:
00013         next_action = rospy.ServiceProxy('plan_next_action', PlanNextAction)
00014         
00015         resp1 = next_action(1, result)
00016     
00017         return resp1.nextAction
00018     
00019     except rospy.ServiceException, e:
00020         print "Service call failed: %s"%e
00021 
00022 def planNextActionService(sessionId, result, feedback):
00023     print 'Plan next Action service'
00024     rospy.wait_for_service('plan_next_action')
00025     try:
00026         next_action = rospy.ServiceProxy('plan_next_action', PlanNextAction)
00027         
00028         resp1 = next_action(sessionId, result, feedback)
00029     
00030         return resp1.nextAction
00031     
00032     except rospy.ServiceException, e:
00033         print "Service call failed: %s"%e
00034 
00035 def terminateCurrentTask():
00036     print 'Terminate current task (due to vital problems, e.g. with hardware)'
00037     rospy.wait_for_service('plan_next_action')
00038     try:
00039         next_action = rospy.ServiceProxy('plan_next_action', PlanNextAction)
00040         res = next_action(1, [2, 2, 2])
00041         print 'here'
00042         return res
00043     except rospy.ServiceException, e:
00044         print "Service call failed: %s"%e
00045 
00046 
00047 def requestNewTask():
00048     print 'Request new task'
00049     rospy.wait_for_service('task_request')
00050     try:
00051         requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00052         res = requestNewTask()
00053         return res
00054     except rospy.ServiceException, e:
00055         print "Service call failed: %s"%e
00056 
00057 def requestNewTaskMove():
00058     print 'Request new task'
00059     rospy.wait_for_service('task_request')
00060     try:
00061         requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00062         res = requestNewTask('move', 'kitchen', None, None, None, None)
00063         print 'send task request'
00064         return res
00065     except rospy.ServiceException, e:
00066         print "Service call failed: %s"%e
00067 
00068 def requestNewTaskSearch():
00069     print 'Request new task - search'
00070     rospy.wait_for_service('task_request')
00071     try:
00072         requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00073         res = requestNewTask('search', 'Milkbox', 'order')
00074         print 'send task request'
00075         return res
00076     except rospy.ServiceException, e:
00077         print 'Service call failed: %s'%e
00078 
00079 def requestNewTaskJSONMove():
00080     print 'Request new task'
00081     rospy.wait_for_service('task_request')
00082     try:
00083         requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00084         req = TaskRequestRequest()
00085         req.task = 'move'
00086         req.content = ''
00087         req.json_parameters = '{\"time_schedule\":1263798000000,\"task\":\"move\",\"destination\":{\"predefined_pose\":\"home\"}}'
00088         req.userPose = ''
00089         res = requestNewTask(req)
00090         print 'send task request', req.json_parameters
00091         return res
00092     except rospy.ServiceException, e:
00093         print "Service call failed: %s"%e
00094 
00095 def requestNewTaskJSONFetch():
00096     print 'Request new task'
00097     rospy.wait_for_service('task_request')
00098     try:
00099         requestNewTask = rospy.ServiceProxy('task_request', TaskRequest)
00100         req = TaskRequestRequest()
00101         req.task = 'get'
00102         req.content = 'Milkbox'
00103         req.json_parameters = '{\"time_schedule\":1263798000000,\"task\":\"get\",\"deliver_destination\":{\"predefined_pose\":\"order\"},\"object\":{\"object_type\":\"Milkbox\"},\"grasping_type\":\"Simple\"}'
00104 
00105         req.userPose = ''
00106         res = requestNewTask(req)
00107         print 'send task request', req.json_parameters
00108         return res
00109     except rospy.ServiceException, e:
00110         print "Service call failed: %s"%e
00111 
00112 def planNextActionServiceJSON(sessionId, result, jsonFeedback):
00113     print 'Plan next Action service'
00114     rospy.wait_for_service('plan_next_action')
00115     try:
00116         next_action = rospy.ServiceProxy('plan_next_action', PlanNextAction)
00117         req = PlanNextActionRequest()
00118         req.sessionId = sessionId
00119         req.resultLastAction = result
00120         req.jsonFeedback = jsonFeedback
00121         resp1 = next_action(req)
00122     
00123         return resp1.nextAction
00124     
00125     except rospy.ServiceException, e:
00126         print "Service call failed: %s"%e
00127 
00128 def getObjectsOnMap():
00129     print 'test get all objects from map'
00130     try:
00131         requestNewTask = rospy.ServiceProxy('get_objects_on_map', GetObjectsOnMap)
00132         res = requestNewTask('ipa-kitchen-map', False)
00133         return res
00134     except rospy.ServiceException, e:
00135         print "Service call failed: %s"%e
00136 
00137 def getWorkspaceOnMap():
00138     print 'test get all workspace (furnitures basically here) from map'
00139     try:
00140         getWorkspace = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap)
00141         req = GetWorkspaceOnMapRequest()
00142         req.map = 'ipa-kitchen-map'
00143         req.ifGeometryInfo = True
00144         res = getWorkspace(req)
00145         #res = getWorkspace('ipa-kitchen-map', True)
00146         return res
00147     except rospy.ServiceException, e:
00148         print "Service call failed: %s"%e
00149 
00150 def test_fetch_1():
00151     res = requestNewTaskJSONFetch()
00152     sessionId = res.sessionId
00153     acts = list()
00154     act = planNextActionServiceJSON(sessionId, 0, '{}')
00155     acts.append(act)
00156 
00157     act = planNextActionServiceJSON(sessionId, 0, '{}')
00158     acts.append(act)
00159 
00160     jsonFeedback = '{"feedback":{"action":"detect", "object":{"object_type":"Milkbox"}, "pose":{"x":-3.0, "y":-0.2, "z":1.02, "rotx":0, "roty":0, "rotz":0, "rotw":1}}}'
00161     act = planNextActionServiceJSON(sessionId, 0, jsonFeedback)
00162     acts.append(act)
00163 
00164     act = planNextActionServiceJSON(sessionId, 0, '{}')
00165     acts.append(act)
00166 
00167     act = planNextActionServiceJSON(sessionId, 0, '{}')
00168     acts.append(act)
00169 
00170     return acts
00171 
00172 def test_move():
00173     res = requestNewTaskJSONMove()
00174     sessionId = res.sessionId
00175     acts = list()
00176     act = planNextActionServiceJSON(sessionId, 0, '{}')
00177     acts.append(act)
00178 
00179     act = planNextActionServiceJSON(sessionId, 1, '{}')
00180     acts.append(act)
00181 
00182     return acts
00183 
00184 def test_fetch_2():
00185     res = requestNewTaskJSONFetch()
00186     sessionId = res.sessionId
00187     acts = list()
00188     act = planNextActionServiceJSON(sessionId, 0, '{}')
00189     acts.append(act)
00190 
00191     act = planNextActionServiceJSON(sessionId, 1, '{}')
00192     acts.append(act)
00193 
00194     act = planNextActionServiceJSON(sessionId, 0, '{}')
00195     acts.append(act)
00196 
00197     jsonFeedback = '{"feedback":{"action":"detect", "object":{"object_type":"Milkbox"}, "pose":{"x":-3.0, "y":-0.2, "z":1.02, "rotx":0, "roty":0, "rotz":0, "rotw":1}}}'
00198     act = planNextActionServiceJSON(sessionId, 0, jsonFeedback)
00199     acts.append(act)
00200 
00201     act = planNextActionServiceJSON(sessionId, 0, '{}')
00202     acts.append(act)
00203 
00204     act = planNextActionServiceJSON(sessionId, 1, '{}')
00205     acts.append(act)
00206 
00207     return acts
00208 
00209 if __name__ == "__main__":
00210 
00211     # request a new task
00212     #print requestNewTask()
00213     #print requestNewTaskSearch()
00214     # get next action [0 0 0 ] is not used here as the first step. see explanation in the next call
00215     #print '1-- ', planNextActionService(16, 0, ['s'])
00216 
00217     #[0, 0, 0] means: success for move, perception, and grasp actions in the last step
00218 
00219     #print '2-- ', planNextActionService(16, 1, ['s'])
00220     #print '3-- ', planNextActionService(16, 1, ['s'])
00221     #print '4-- ', planNextActionService(16, 1, ['s'])
00222 
00223     #print '4-- ', testNextActionService([0,0,0])
00224     #print '5-- ', testNextActionService([0,0,0])
00225     #print '6-- ', testNextActionService([0,0,0])
00226     # to terminate current task, 
00227     #print terminateCurrentTask()
00228     #print getObjectsOnMap()
00229     #print getWorkspaceOnMap()
00230 
00231     #print 'Test FETCH task'
00232     #test_fetch_1()
00233 
00234     #print 'Test MOVE task'
00235     #r = test_move()
00236     #print r
00237 
00238     print 'Test FETCH task'
00239     r = test_fetch_1()
00240     print r


srs_knowledge
Author(s): Ze Ji
autogenerated on Mon Oct 6 2014 08:27:32