take_human_input_v1.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import time
00004 import rospy
00005 
00006 import roslib; roslib.load_manifest('bwi_tasks')
00007 
00008 import actionlib
00009 import actionlib_msgs.msg
00010 from bwi_kr_execution.msg import *
00011 import segbot_gui.srv
00012 import bwi_rlg.srv
00013 import os.path
00014 import string
00015 
00016 from multiprocessing import Process, Value, Array
00017 
00018 from PIL import Image
00019 import subprocess
00020 
00021 # human_waiting = False
00022 # curr_goal = []
00023 # next_room = None
00024 
00025 person_door = {'peter'      : 'd3_508', 
00026                'dana'       : 'd3_510',
00027                'ray'        : 'd3_512',
00028                'raymond'    : 'd3_512',
00029                'stacy'      : 'd3_502',
00030                'kazunori'   : 'd3_402',
00031                'matteo'     : 'd3_418',
00032                'jivko'      : 'd3_432',
00033                'shiqi'      : 'd3_420',
00034                'piyush'     : 'd3_416',
00035                'daniel'     : 'd3_436'}
00036 
00037 room_list = ['l3_502', 'l3_504', 'l3_508', 'l3_510', 'l3_512', 'l3_516',
00038              'l3_436', 'l3_402', 'l3_404', 'l3_416', 'l3_418', 'l3_420',
00039              'l3_422', 'l3_430', 'l3_432', 'l3_414a', 'l3_414b']
00040 
00041 door_list = ['d3_502', 'd3_504', 'd3_508', 'd3_510', 'd3_512', 'd3_516',
00042              'd3_436', 'd3_402', 'd3_404', 'd3_416', 'd3_418', 'd3_420',
00043              'd3_422', 'd3_430', 'd3_432', 'd3_414a1', 'd3_414a2', 'd3_414b1',
00044              'd3_414b2']
00045 
00046 resting_time = 120
00047 cnt = 0
00048 last_loc = ''
00049 
00050 def task_guiding(doorname, client, dialog_handle):
00051 
00052     roomname = doorname[doorname.find('_')+1 : ]
00053     
00054     if roomname.find('414') >= 0:
00055         roomname = roomname[:-1]
00056 
00057     roomname = '3.' + roomname
00058 
00059     dialog_handle(0, "I am going to " + roomname + '. ', [], 0)
00060 
00061     goal = ExecutePlanGoal()
00062     rule = AspRule()
00063     fluent = AspFluent()
00064     
00065     fluent.name = "not facing"
00066     fluent.variables = [doorname]
00067     rule.body = [fluent]
00068     goal.aspGoal = [rule]
00069     
00070     rospy.loginfo("Sending goal (doorname): " + doorname)
00071     client.send_goal(goal)
00072     client.wait_for_result()
00073 
00074 def task_delivery(person, item, client, dialog_handle):
00075 
00076     global person_door
00077 
00078     upper_person = person[0].upper() + person[1:]
00079     dialog_handle(0, "I am going to pick up " + item + " for " + \
00080                   upper_person + '. ', [], 0)
00081 
00082     goal = ExecutePlanGoal()
00083     rule = AspRule()
00084     fluent = AspFluent()
00085     
00086     # going to the shop first - there is no shop - going to the kitchen: 520
00087     fluent.name = "not facing"
00088     fluent.variables = ["d3_520"]
00089     rule.body = [fluent]
00090     goal.aspGoal = [rule]
00091     
00092     rospy.loginfo("Sending goal (doorname): " + fluent.variables[0])
00093     client.send_goal(goal)
00094 
00095     client.wait_for_result()
00096 
00097     res = dialog_handle(1, "May I have " + item + " please?", \
00098                  ["Sorry, we do not have that", "Loaded"], 60)
00099 
00100     hasLoaded = (res.index == 1)
00101     
00102     if res.index == 1:
00103         res = dialog_handle(0, "I am taking " + item + " to " + upper_person + \
00104                         ". ", [""], 0)
00105     else:
00106         res = dialog_handle(0, "I am going to tell " + upper_person + \
00107                             " that " + item + " is not available. ", [""], 0)
00108 
00109     fluent.name = "not facing"
00110     fluent.variables = [person_door[person]]
00111     rule.body = [fluent]
00112     goal.aspGoal = [rule]
00113     
00114     # loaded item, now going to the person's place
00115 
00116     rospy.loginfo("Sending goal (doorname): " + person_door[person])
00117     client.send_goal(goal)
00118     client.wait_for_result()
00119 
00120     if hasLoaded == True:
00121         res = dialog_handle(1, "Someone requested this " + item + \
00122                             " to be delivered to you. Please take it. ", \
00123                             ["Unloaded"], 60)
00124     else:
00125         res = dialog_handle(1, "Someone requested " + item + \
00126                             " be delivered to you, but the store did not give it to me. ", \
00127                             ["I see."], 60)
00128 
00129 def process_request(query, client, dialog_handle):
00130 
00131     global room_list
00132     global door_list
00133     global person_door
00134 
00135     rospy.loginfo("query: " + query)
00136 
00137     if (query.find("at(") >= 0): # this is a guiding task! 
00138 
00139         room = query[query.find('l') : query.find(',')]
00140 
00141         if room in room_list:
00142 
00143             query = query.replace("at(l", "d")
00144             query = query[:query.find(",")]
00145 
00146             # in case there are multiple doors to a room, select the first one
00147             if query.find("d3_414") > 0:
00148                 query += "1"
00149 
00150             task_guiding(query, client, dialog_handle)
00151 
00152         else:
00153 
00154             rospy.logwarn("This does not seem to be a room name: " + room)
00155 
00156     elif (query.find("query(") >= 0): # this is a question-asking task! 
00157 
00158         rospy.logwarn("Query from semantic parser: " + query)
00159         # query = query.replace("query(l", "d")
00160         # query = query[:query.find(":")]
00161 
00162         # if query.find("d3_414") > 0:
00163         #     query += "1"
00164 
00165         # task_guiding(query, client, dialog_handle)
00166 
00167         return False
00168 
00169     elif (query.find("served(") >= 0): # this is a delivery task! 
00170 
00171         # served(shiqi,coffee,n)
00172 
00173         person = query[query.find('(')+1 : query.find(',')]
00174 
00175         if person in person_door: 
00176 
00177             # remove the person name -> coffee,n)
00178             query = query[query.find(',')+1 : ]
00179             item = query[: query.find(',')]
00180 
00181             task_delivery(person, item, client, dialog_handle)
00182 
00183         else:
00184 
00185             rospy.logwarn("This does not seem to be a person name: " + person)
00186     
00187     return True
00188 # option 
00189 # 1: click me if you need help
00190 # 2: please let me know your goal place
00191 def gui_thread(human_waiting, curr_goal):
00192   
00193     rospy.init_node('human_input_gui_thread')
00194 
00195     rospy.loginfo("gui_thread started")
00196 
00197     rospy.wait_for_service('question_dialog')
00198     
00199     handle = rospy.ServiceProxy('question_dialog', \
00200                                 segbot_gui.srv.QuestionDialog)
00201 
00202     while not rospy.is_shutdown():
00203 
00204         if human_waiting.value == True:
00205             rospy.sleep(2)
00206             continue
00207 
00208         res = handle(1, "", ["Button"], 2)
00209 
00210         while (res.index < 0):
00211 
00212             if len(curr_goal.value) > 6:
00213                 g = "3." + curr_goal.value[3:-1]
00214             else:
00215                 g = "3." + curr_goal.value[3:]
00216 
00217             res = handle(1, "Please click the button, if you need my help." + \
00218                          "\n\nI am moving to room " + g, ["Button"], 2)
00219 
00220         human_waiting.value = True
00221         res = handle(0, "I have to go to room " + g + " first." + \
00222                         "\n\nWe can chat when I get there.",\
00223                      ["Button"], 0)
00224 
00225     return True
00226 
00227 def platform_thread(human_waiting, curr_goal):
00228 
00229     global cnt
00230     global resting_time
00231     global last_loc
00232 
00233     rospy.init_node('human_input_platform_thread')
00234 
00235     rospy.loginfo("platform_thread started")
00236 
00237     rospy.wait_for_service('question_dialog')
00238 
00239     rooms = ["414a", "414b", "414b", "418", "420"]
00240     doors = ["d3_414a2", "d3_414b1", "d3_414b2", "d3_418", "d3_420"]
00241     
00242     client = actionlib.SimpleActionClient('/action_executor/execute_plan',\
00243                                           ExecutePlanAction)
00244     client.wait_for_server()
00245 
00246     dialog_handle = rospy.ServiceProxy('question_dialog', \
00247                                        segbot_gui.srv.QuestionDialog)
00248 
00249     parser_handle = rospy.ServiceProxy('semantic_parser', \
00250                                        bwi_rlg.srv.SemanticParser)
00251 
00252     path_rlg = rospy.get_param("/semantic_parser_server/path_to_bwi_rlg")
00253     filepath = path_rlg + "/agent/dialog/list_of_bad_data.txt"
00254 
00255     while not rospy.is_shutdown():
00256 
00257         if human_waiting.value == True:
00258 
00259             # a human may give goals to the robot one after another
00260             while True:
00261 
00262                 # commumication......
00263                 # robot speaks first
00264 
00265                 res_qd = dialog_handle(0, \
00266                          'At this time, I can only help with navigation and item delivery to named people. Please try not to change your mind about what you want while we chat. ', \
00267                          [""], 5)
00268                 rospy.sleep(6)
00269 
00270                 # img = subprocess.Popen(["eog", \
00271                 #                       path_rlg + '/images/unnamed.jpg'])
00272 
00273                 res_sp = parser_handle(0, "STARTING-KEYWORD")
00274 
00275                 while len(res_sp.query) == 0 and res_qd.index != -2:
00276                     
00277                     # take human feedback
00278                     res_qd = dialog_handle(2, res_sp.output_text, doors, 60)
00279 
00280                     # robot speaks back
00281                     res_sp = parser_handle(0, res_qd.text)
00282 
00283                     # the robot prints: I am thinking...
00284                     dialog_handle(0, "I am thinking...", doors, 2)
00285                     rospy.sleep(2)
00286 
00287                 # now the robot has found the query from semantic parser
00288 
00289                 hasSucceeded = process_request(res_sp.query, \
00290                                                client, dialog_handle)
00291                 
00292                 # img.kill()
00293                 # identify good (and bad) data
00294                 res = dialog_handle(1, "Did I accomplish the goal you were trying to convey?", \
00295                                     ["Yes", "No"], 60)
00296 
00297                 # if no response, assuming it's a wrong one
00298                 if res.index == 0:
00299 
00300                     # incrementally learning
00301                     dialog_handle(0, "I am learning...", doors, 10)
00302                     subprocess.call("python " + path_rlg +\
00303                                     "/agent/dialog/main.py " +\
00304                                      path_rlg + "/agent/dialog/ " +\
00305                                      "retrain -exclude_test_goals", shell=True)
00306 
00307                 else:
00308 
00309                     if os.path.exists(filepath):
00310                         f = open(filepath, 'a')
00311                     else:
00312                         f = open(filepath, 'w+')
00313 
00314                     res_sp = parser_handle(3, res_qd.text)
00315                     f.write(res_sp.output_text + '---' + res_sp.query + '\n')
00316                     f.close()
00317 
00318                 # anything else for the same user? 
00319                 res = dialog_handle(1, "Do you need anything else?", \
00320                                     ["Yes", "No"], 30)
00321 
00322                 if res.index < 0 or res.index == 1:
00323 
00324                     human_waiting.value = False
00325                     break
00326 
00327         else:
00328 
00329             rospy.loginfo("No one needs me. I will do a random walk.")
00330             rospy.sleep(1)
00331 
00332             if (cnt % resting_time) == 0 or last_loc == '': 
00333 
00334                 loc = doors[int(time.time()) % len(rooms)]
00335                 last_loc = loc
00336 
00337             else:
00338 
00339                 loc = last_loc
00340 
00341             cnt += 1
00342 
00343             curr_goal.value = loc
00344             goal = ExecutePlanGoal()
00345             rule = AspRule()
00346             fluent = AspFluent()
00347             
00348             fluent.name = "not beside"
00349             fluent.variables = [loc]
00350             rule.body = [fluent]
00351             goal.aspGoal = [rule]
00352             
00353             rospy.loginfo("sending goal: " + loc)
00354             client.send_goal(goal)
00355             
00356             while client.wait_for_result(
00357                     timeout = rospy.Duration.from_sec(1.0)) == False:
00358                 if human_waiting.value:
00359                     break
00360 
00361             client.cancel_goal()
00362 
00363             move_base_pub = rospy.Publisher('move_base/cancel',\
00364                                             actionlib_msgs.msg.GoalID)
00365 
00366             # for unknown reasons, we have to publish the message at least twice
00367             # to make it work
00368             for i in range(5):
00369                 move_base_pub.publish(rospy.Time.now(), '')
00370                 rospy.sleep(0.2)
00371 
00372     return 1
00373 
00374 if __name__ == '__main__':
00375 
00376     human_waiting = Value('b', False)
00377     curr_goal = Array('c', "This is a very very very long string for nothing.")
00378 
00379     p1 = Process(target = gui_thread, args = (human_waiting, curr_goal))
00380     p2 = Process(target = platform_thread, args = (human_waiting, curr_goal))
00381 
00382     p1.start()
00383     p2.start()
00384 
00385     p1.join()
00386     p2.join()
00387 


bwi_tasks
Author(s): Matteo Leonetti, Shiqi Zhang
autogenerated on Fri Aug 28 2015 10:14:58