learning_server_dynamic.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #################################################################
00003 # \note historical_data record subscriber
00004 # \input(from DEM to learning) command and candidates
00005 # \output(from learning to DEM) candidate and correspomding likelihood
00006 #   Project name: srs learning service for choosing priority 
00007 # \author
00008 #   Tao Cao, email:itaocao@gmail.com
00009 #
00010 # \date Date of creation: Dec 2011
00011 #################################################################
00012 
00013 import roslib; roslib.load_manifest('srs_likelihood_calculation')
00014 
00015 from srs_likelihood_calculation.srv import *
00016 
00017 from std_msgs.msg import String
00018 import rospy
00019 
00020 import actionlib
00021 
00022 from srs_decision_making.msg import *
00023 
00024 import fileinput
00025 
00026 import json # or `import simplejson as json` if on Python < 2.6
00027 
00028 #randomly generate the location
00029 
00030 from random import choice
00031 
00032 foo = ['table2', 'table1','fridge']
00033 
00034 historical_data_LW=[]
00035 
00036 #global action_sequences_global
00037 
00038 action_sequences_global=[]# action sequence for taks instance
00039 
00040 task_flag=""
00041         
00042 #using learning window to read historical data 
00043 #this function will be updated 
00044 #the learning_window_width will be decided based on the learning experience
00045 #this function should only need the historical_data file name, and will search this file itself
00046 
00047 def data_from_learning_window(learning_window_width,historical_full_path):
00048     #learning_window_width=learning_window_width
00049     line_offset = []
00050     historical_data_LW=[]
00051     
00052     num_lines=0 #the number of lines of the historical_data(how big the experience is)
00053     offset = 0 #the offset for a line
00054 
00055     fo=open(historical_full_path,'r')#open the historical_data
00056     
00057     #calculate the num_lines and the offset for every line
00058     for line in fo:
00059         line_offset.append(offset)
00060         offset += len(line)
00061         num_lines+=1
00062     #file.seek(0) # reset offset to the begining of the file
00063     #n+learning_window_width=num_lines 
00064     #the n is the line number, so n >=0, this means, num_lines>=learning_window_width, the program should make sure this is true
00065     if num_lines>=learning_window_width:
00066       #learning according the learning_window_width
00067       n=num_lines-learning_window_width
00068     else:
00069       n=0 #using the whole historical_data
00070     
00071     # Now, to skip to line n (with the first line being line 0), just do
00072     fo.seek(line_offset[n])   
00073     
00074 
00075     for line in fo:
00076         
00077         #remove the end-line character from the action sequence
00078         if line[-1] == '\n':
00079            line1=line[:-1]
00080         else:
00081            line1=line        
00082         historical_data_LW.append(line1)
00083         
00084     # Close opend file
00085     fo.close()
00086     return historical_data_LW #return the historical_data list based on the learning_window_width
00087 
00088 def calculate_likelihood(data_for_likelihood,candidate_list):
00089     #task_success_flag=[]#flag to mark the task is sucessful or not
00090     candidates_list=candidate_list.split()#store all the candidates into a list
00091     #set up a dict structure to save candidates and correspomding frenquency
00092     candidate_frenquence={}
00093     candidate_weight={}
00094     candidate_likelihood={}
00095     initial_weight=0.1#in this stage, the weight is from 0.1,0.2,..., 1, totally 10 weights
00096     #the initial frequence and weight is 0
00097     for candi in candidates_list:
00098       candidate_frenquence[candi]=0
00099       candidate_weight[candi]=0
00100       candidate_likelihood[candi]=0
00101     
00102     for action in data_for_likelihood:#data_for_likelihood is a list, with action sequence as items
00103       # action in data_for_likelihood list is a string, which has the find method
00104       if action.find('place_on_tray'):
00105         #find the action marker, the task is sucessful
00106         #task_success_flag.append(1) this task_success_flag is not needed
00107         #find the nearest move(base,position) action to match the candidates
00108         location_flag=action.rfind('move(base,')# mark where is the location for grasp
00109         sub_actions=action[location_flag:]#sub action sequence begining with move action
00110         sub_actions_list=sub_actions.split(', ')#the delimiter is', ', this delimiter will split action sequence into action list
00111         print "the location is in %s"%sub_actions_list[0]
00112         #check which candidate is the location
00113         #comparing the candidates in candidates_list with the current loation
00114    
00115         for candidate in candidates_list:#choosing candidates from dict candidate_frenquence
00116           
00117           if sub_actions_list[0].find(','+candidate+')')>0:#the candidate is the location, correspomding frequence and weight will be updated
00118             #sub_actions_list[0].find(','+candidate+')') make sure the candidate can be exactly matched
00119             print "the location is %s"%candidate
00120             candidate_frenquence[candidate]+=1/10.0#10.0 is the totally number of historical_data
00121             print "the candidate_frenquence for %s is %s" %(candidate,candidate_frenquence[candidate])
00122             candidate_weight[candidate]+=initial_weight/5.5#5.5=(0.1+0.2+...+1)
00123             print "the candidate_weight for %s is %s" %(candidate,candidate_weight[candidate])
00124             candidate_likelihood[candidate]=(candidate_frenquence[candidate]+candidate_weight[candidate])/2.0
00125             #candidate_likelihood[candidate]+=candidate_frenquence[candidate]+candidate_weight[candidate]
00126             print "the candidate_likelihood for %s is %s" %(candidate,candidate_likelihood[candidate])
00127             initial_weight+=0.1
00128             
00129           
00130       else:
00131         #we assumed all action sequences for a task is successful in this stage, this else here is to handle exceptions of receiving failed tasks
00132         #without the action marker,the task is failed
00133         #there will be no frenquecy and weight for any candidate
00134         #task_success.append(0)
00135         initial_weight+=0.1
00136     #
00137     #after above for loop, all the historical_data_LW had been used for calculating frenquence and weight
00138     #now use the frenquence and weight to calculate the likelihood
00139     #
00140     #likelihood=(weight+frenquence)/2  this also can be done in the for loop as shown above   
00141     print "candidate_frenquence is: %s" %candidate_frenquence
00142     print "candidate_weight is: %s" %candidate_weight
00143     print "candidate_likelihood is %s" %candidate_likelihood
00144     candidate_likelihood_list=candidate_likelihood.items()
00145     return str(candidate_likelihood_list)
00146         
00147 
00148 # callback function for receive consulting command and candidates
00149 def handle_consulting(req):
00150     #req is the consulting information command+candidates from the DEM to Learning_service
00151     #both the command and candidates will be used for likelihood calculating based on the historical data of learning
00152     #command used for task classification
00153     #cadidates used for activity cluster
00154     print "consulting command is: %s "%req.command
00155     print "consulting candidates are: %s" %req.candidate
00156     #here we just read the historical_data from the txt file
00157     #the final data should come from a search based on the command (task classification) and candidates(for activity cluster)
00158     #!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00159     data_for_likelihood=data_from_learning_window(10,'historical_data.txt')
00160     print"the historical_data based on learning_window_width=10 is: "
00161     print data_for_likelihood
00162     print "the number of historical_data is %d"%len(data_for_likelihood)
00163     #calculating the likelihood
00164     #likelihoodresponse="likelihood for %s from %s is ??? "%(req.command,req.candidate)
00165     likelihoodresponse=calculate_likelihood(data_for_likelihood,req.candidate)
00166     return LikelihoodResponse(likelihoodresponse)
00167 
00168 
00169 #definition of the find_ralation function
00170 
00171 def find_ralation(actions,results,objects):
00172     
00173     #define the on relationship
00174     on_something={}
00175     on_something['move']='3'
00176     on_something['detect']='3'
00177     on_something['grasp']='3'
00178     on_something['place_on_tray']='3'
00179     
00180     #define the in relationship
00181     in_something={}
00182     in_something['move']='3'
00183     in_something['open']='3'
00184     in_something['detect']='3'
00185     in_something['grasp']='3'    
00186     in_something['place_on_tray']='3'
00187     in_something['close']='3'
00188     
00189     action_list=actions.split()
00190     print action_list
00191     result_list=results.split()
00192     print result_list
00193     # build up the action_result dictionary to use the results of actions
00194     actions_result={}
00195     for action in action_list:
00196       actions_result[action]=result_list[int(action_list.index(action))]
00197     #print 'actions_result is: ',actions_result.items()
00198     #return str(actions_result)
00199     #print 'actions_result.keys()',actions_result.keys()
00200     
00201     for action in action_list:
00202       #for relationship detect action is the marker of the relationship
00203       print action
00204       
00205       if action.find('detect')>=0:
00206           
00207          print 'detect action is found!'
00208           #find the action mark
00209           #then check the detect action successful or not
00210          if actions_result[action]=='3':
00211             #detect action is successful
00212             #then find the open action
00213             print 'detect action is successful!'
00214             if actions.rfind('open')>=0:
00215               for action1 in action_list:
00216                if action1.find('open')>=0:
00217                 print 'is open'
00218                 #find the open action
00219                 #then check the open is successful
00220                 if actions_result[action1]=='3':
00221                   return 'in_something'
00222                 else:
00223                   print 'unknown relationship'
00224                   return 'unknow relationship'
00225                   
00226             elif actions.rfind('move')>=0:
00227                for action1 in action_list:
00228                  if action1.find('move')>=0:
00229                   print 'is move'
00230                   if actions_result[action1]=='3':
00231                       return 'on_something'
00232                   else:
00233                       print 'unknown relationship'
00234                       return 'unknow relationship'
00235             else:
00236               print 'unknown relationship'
00237               return 'unknow relationship'
00238          else:
00239            print 'detect action is failed, can not detect object!'
00240            return 'can not detect object!'      
00241    
00242 
00243 # callback function for receive consulting for ontology expanding
00244 def handle_consulting2(req):
00245     #req is the consulting information command+candidates from the DEM to Learning_service
00246     #both the command and candidates will be used for likelihood calculating based on the historical data of learning
00247     #command used for task classification
00248     #cadidates used for activity cluster
00249     print "consulting command is: %s "%req.command
00250     print "consulting actions are: %s" %req.actions
00251     print "consulting actions results are: %s" %req.results
00252     if req.objects!="":
00253       print "consulting objects are: %s" %req.objects
00254     #else:
00255       #find objects from the commands
00256     
00257     
00258     ontologyresponse=find_ralation(req.actions,req.results,req.objects)
00259     #ontologyresponse="ontology for %s  is ??? "%req.objects
00260     
00261     return OntologyResponse(ontologyresponse)
00262 
00263 def callback_record_data(data):
00264   
00265     #rospy.loginfo(rospy.get_name()+"Action sequence is: %s",data.data)
00266     #print rospy.get_name()+" Action sequence is: ",data.data
00267     print "task instance result is:",data.result
00268     global action_sequences_global
00269     # Open a file
00270     #action_sequences=action_sequences_global
00271     if data.result==' return_value: 3':#task successfully executed return_value: 3
00272       print "Task successfully finished\n"
00273       task_action_sequence=','.join(action_sequences_global)
00274       fo = open("dynamic_historical_data.txt", "ab")
00275       fo.write( task_action_sequence +"\n");
00276       action_sequences_global=[]
00277 
00278       # Close opend file
00279       fo.close()
00280     
00281 
00282 def historical_data_recorder(data): #publish the data
00283     #print "Json feedback is: ",data
00284     global action_sequences_global
00285     
00286     print "action_sequences_global is %s" %action_sequences_global
00287     
00288     feedback_temp=str(data)
00289     
00290     location_flag_jsonfeedback=feedback_temp.find('json_feedback:')# mark where is the location for grasp
00291     sub_feedback_temp=feedback_temp[location_flag_jsonfeedback+len('json_feedback:'):]#sub action sequence begining with move action
00292     if sub_feedback_temp.find('name')>0:
00293       print "sub_feedback_temp is: %s" %sub_feedback_temp      
00294       
00295       feedback_dict=json.loads(sub_feedback_temp) #type of feedback_dict: <type 'unicode'>
00296       
00297       
00298       
00299       #print "feedback_dict is: %s" %feedback_dict
00300       
00301       #print "type of feedback_dict: %s" %type(feedback_dict)
00302       
00303       feedback_dict1=json.loads(feedback_dict)#type of feedback_dict1: <type 'dict'>
00304       
00305       #print "type of feedback_dict1: %s" %type(feedback_dict1)
00306       
00307       actions_doing=feedback_dict1["current_action"]
00308       
00309       task_info=feedback_dict1["task"]
00310       
00311       task_flag_temp=task_info["task_id"]
00312             
00313         
00314       if actions_doing["state"]=='succeeded': # only record the successful actions
00315       
00316           actionis=actions_doing["name"]#get the action
00317           action_with_para_li=[actionis]
00318           ## get the action target
00319           
00320           #action_with_para_li.append(actions_doing["target"])
00321           action_target=actions_doing["target_object"]
00322           
00323           action_with_para_li.append(action_target)
00324           
00325           action_with_para_str=' '.join(action_with_para_li)
00326         
00327           print "curret action is: %s" %action_with_para_str
00328           
00329           ### this action_sequences is used as a stack for storing action sequence of a task instance
00330           ## the following two lines update action_sequences
00331           
00332           if  action_sequences_global==[]:#this action_sequences is null
00333             action_sequences_global=[action_with_para_str]
00334             print "action %s is recorded into historical data\n" %action_with_para_str
00335             print "the first action_sequences_global is %s" %action_sequences_global
00336             #action_sequences_global = action_sequences
00337           
00338           else:
00339             print "the non_null action_sequences_global list is %s" %action_sequences_global
00340             #####action_sequences_global=action_sequences_global.append(action_with_para_str)#update action sequence
00341             action_sequences_global.append(action_with_para_str)#update action sequence
00342             print "action %s is recorded into historical data\n" %action_with_para_str
00343             #action_sequences_global = action_sequences
00344           
00345          # Open a file
00346           fo = open("dict_dynamic_feedback_DM_historical_data.txt", "ab")       
00347           fo.write(action_with_para_str+"\n")
00348           fo.close()
00349           
00350     else:
00351         print 'This is init state'
00352   
00353     
00354     
00355 def callback_record_data2(data):
00356     
00357     temp_feed= data.feedback
00358     
00359     
00360     
00361     task_actions=temp_feed
00362     
00363     historical_data_recorder(task_actions)
00364 
00365 def consulting_server():
00366     
00367     rospy.init_node('likelyhood_server', anonymous=True)    
00368     
00369     rospy.Subscriber("srs_decision_making_actions/feedback", ExecutionActionFeedback, callback_record_data2) #record the historical data
00370     rospy.Subscriber("srs_decision_making_actions/result", ExecutionActionResult, callback_record_data) #record the historical data 
00371     
00372     rospy.Service('likelihood', Likelihood, handle_consulting) #provide service
00373     rospy.Service('ontology', Ontology, handle_consulting2) #provide service
00374     print "Ready to receive consulting."
00375     rospy.spin()
00376 
00377 if __name__ == "__main__":
00378     
00379     consulting_server()
00380     


srs_likelihood_calculation
Author(s): Administrator
autogenerated on Sun Jan 5 2014 12:09:54