server_likelihood.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #################################################################
00003 # \note likelihood service
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 from std_msgs.msg import String
00017 import rospy
00018 
00019 import fileinput
00020 
00021 historical_data_LW=[]
00022         
00023 #using learning window to read historical data 
00024 #this function will be updated 
00025 #the learning_window_width will be decided based on the learning experience
00026 #this function should only need the historical_data file name, and will search this file itself
00027 
00028 def data_from_learning_window(learning_window_width,historical_full_path):
00029     #learning_window_width=learning_window_width
00030     line_offset = []
00031     historical_data_LW=[]
00032     
00033     num_lines=0 #the number of lines of the historical_data(how big the experience is)
00034     offset = 0 #the offset for a line
00035 
00036     fo=open(historical_full_path,'r')#open the historical_data
00037     
00038     #calculate the num_lines and the offset for every line
00039     for line in fo:
00040         line_offset.append(offset)
00041         offset += len(line)
00042         num_lines+=1
00043     #file.seek(0) # reset offset to the begining of the file
00044     #n+learning_window_width=num_lines 
00045     #the n is the line number, so n >=0, this means, num_lines>=learning_window_width, the program should make sure this is true
00046     if num_lines>=learning_window_width:
00047       #learning according the learning_window_width
00048       n=num_lines-learning_window_width
00049     else:
00050       n=0 #using the whole historical_data
00051     
00052     # Now, to skip to line n (with the first line being line 0), just do
00053     fo.seek(line_offset[n])   
00054     
00055 
00056     for line in fo:
00057         
00058         #remove the end-line character from the action sequence
00059         if line[-1] == '\n':
00060            line1=line[:-1]
00061         else:
00062            line1=line        
00063         historical_data_LW.append(line1)
00064         
00065     # Close opend file
00066     fo.close()
00067     return historical_data_LW #return the historical_data based on the learning_window_width
00068 
00069 def calculate_likelihood(data_for_likelihood,candidate_list):
00070     #task_success_flag=[]#flag to mark the task is sucessful or not
00071     candidates_list=candidate_list.split()#store all the candidates into a list
00072     #set up a dict structure to save candidates and correspomding frenquency
00073     candidate_frenquence={}
00074     candidate_weight={}
00075     candidate_likelihood={}
00076     initial_weight=0.1#in this stage, the weight is from 0.1,0.2,..., 1, totally 10 weights
00077     #the initial frequence and weight is 0
00078     for candi in candidates_list:
00079       candidate_frenquence[candi]=0
00080       candidate_weight[candi]=0
00081       candidate_likelihood[candi]=0
00082     
00083     for action in data_for_likelihood:#data_for_likelihood is a list, with action sequence as item
00084       if action.find('place_on_tray'):
00085         #find the action marker, the task is sucessful
00086         #task_success_flag.append(1) this task_success_flag is not needed
00087         #find the nearest move(base,position) action to match the candidates
00088         location_flat=action.rfind('move(base,')# mark where is the location for grasp
00089         sub_actions=action[location_flat:]#sub action sequence begining with move action
00090         sub_actions_list=sub_actions.split()
00091         print "the location is in %s"%sub_actions_list[0]
00092         #check which candidate is the location
00093         #comparing the candidates in candidates_list with the current loation
00094         for candidate in candidates_list:#choosing candidates from dict candidate_frenquence
00095           
00096           if sub_actions_list[0].find(candidate)>0:#the candidate is the location, correspomding frequence and weight will be updated
00097             print "the location is %s"%candidate
00098             candidate_frenquence[candidate]+=1/10.0#10.0 is the totally number of historical_data
00099             print "the candidate_frenquence for %s is %s" %(candidate,candidate_frenquence[candidate])
00100             candidate_weight[candidate]+=initial_weight/5.5#5.5=(0.1+0.2+...+1)
00101             print "the candidate_weight for %s is %s" %(candidate,candidate_weight[candidate])
00102             candidate_likelihood[candidate]=(candidate_frenquence[candidate]+candidate_weight[candidate])/2.0
00103             #candidate_likelihood[candidate]+=candidate_frenquence[candidate]+candidate_weight[candidate]
00104             print "the candidate_likelihood for %s is %s" %(candidate,candidate_likelihood[candidate])
00105             initial_weight+=0.1
00106             
00107           
00108       else:
00109         #we assumed all action sequences for a task is successful in this stage, this else here is to handle exceptions of receiving failed tasks
00110         #without the action marker,the task is failed
00111         #there will be no frenquecy and weight for any candidate
00112         #task_success.append(0)
00113         initial_weight+=0.1
00114     #
00115     #after above for loop, all the historical_data_LW had been used for calculating frenquence and weight
00116     #now use the frenquence and weight to calculate the likelihood
00117     #
00118     #likelihood=(weight+frenquence)/2  this also can be done in the for loop as shown above   
00119     print "candidate_frenquence is: %s" %candidate_frenquence
00120     print "candidate_weight is: %s" %candidate_weight
00121     print "candidate_likelihood is %s" %candidate_likelihood
00122     candidate_likelihood_list=candidate_likelihood.items()
00123     return str(candidate_likelihood_list)
00124         
00125 
00126 # callback function for receive consulting command and candidates
00127 def handle_consulting(req):
00128     #req is the consulting information command+candidates from the DEM to Learning_service
00129     #both the command and candidates will be used for likelihood calculating based on the historical data of learning 
00130     print "consulting command is: %s "%req.command
00131     print "consulting candidates are: %s" %req.candidate
00132     #here we just read the historical_data from the txt file
00133     #the final data should come from a search based on the command (task classification) and candidates(for activity cluster)
00134     #!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00135     data_for_likelihood=data_from_learning_window(10,'historical_data.txt')
00136     print"the historical_data based on learning_window_width=10 is: "
00137     print data_for_likelihood
00138     print "the number of historical_data is %d"%len(data_for_likelihood)
00139     #calculating the likelihood
00140     #likelihoodresponse="likelihood for %s from %s is ??? "%(req.command,req.candidate)
00141     likelihoodresponse=calculate_likelihood(data_for_likelihood,req.candidate)
00142     return LikelihoodResponse(likelihoodresponse)
00143 
00144 def callback_record_data(data):
00145     #rospy.loginfo(rospy.get_name()+"Action sequence is: %s",data.data)
00146     #print rospy.get_name()+" Action sequence is: ",data.data
00147     print "Action sequence is: ",data.data
00148     # Open a file
00149     fo = open("historical_data.txt", "ab")
00150     fo.write( data.data+"\n");
00151 
00152     # Close opend file
00153     fo.close()
00154 
00155 def consulting_server():
00156     
00157     rospy.init_node('likelyhood_server', anonymous=True)
00158     #rospy.init_node('recorder', anonymous=True)
00159     #rospy.loginfo("node is initialized")
00160     rospy.Subscriber("historical_data", String, callback_record_data) #record the historical data 
00161     #rospy.init_node('likelyhood_server')
00162     s = rospy.Service('likelihood', Likelihood, handle_consulting) #provide service
00163     print "Ready to receive consulting."
00164     rospy.spin()
00165 
00166 if __name__ == "__main__":
00167     consulting_server()


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