def forget_plan(plan):
try:
query_results = global_vars.prolog_handle ('combinedPlan', [plan]).ris
if len(query_results) != 0:
kbPlanName = plan[0].lower() + plan[1:]
planName = kbPlanName[0].upper() + kbPlanName[1:]
retract = 'combinedPlan(%s)'%kbPlanName
retract2 = 'plan(%s,X,Y)'%kbPlanName
retract3 = 'pcl(%s,X)'%kbPlanName
global_vars.prolog_retractAll_handle(retract)
global_vars.prolog_retractAll_handle(retract2)
global_vars.prolog_retractAll_handle(retract3)
remove_from_plan_prolog_file(kbPlanName)
remove_all_relatives_of_all_elements_named_from(planName + ".exec", "ChooseAction.pnml")
remove_file(global_vars.plan_directory + "/" + planName + ".pnml")
return "Done"
else:
return "Plan Not Known"
except:
rospy.logerr("Could not contact prolog node")
return "False"
def forget_plan(plan):
try:
query_results = global_vars.prolog_handle('combinedPlan', [plan]).ris
if len(query_results) != 0:
kbPlanName = plan[0].lower() + plan[1:]
planName = kbPlanName[0].upper() + kbPlanName[1:]
retract = 'combinedPlan(%s)' % kbPlanName
retract2 = 'plan(%s,X,Y)' % kbPlanName
retract3 = 'pcl(%s,X)' % kbPlanName
global_vars.prolog_retractAll_handle(retract)
global_vars.prolog_retractAll_handle(retract2)
global_vars.prolog_retractAll_handle(retract3)
remove_from_plan_prolog_file(kbPlanName)
remove_all_relatives_of_all_elements_named_from(
planName + ".exec", "ChooseAction.pnml")
remove_file(global_vars.plan_directory + "/" + planName + ".pnml")
return "Done"
else:
return "Plan Not Known"
except:
rospy.logerr("Could not contact prolog node")
return "False"
def getParamNames(action):
print "looking for parameters of plan %s" % action
kbPlanName = action
try:
query_results = global_vars.prolog_handle(
'plan', ["%s" % kbPlanName, 'X', 'Y']).ris
if len(query_results) == 1:
paramList = query_results[0].atoms[1].replace("[", "").replace(
"]", "").replace(" ", "").split(",")
for i in xrange(len(paramList)):
paramList[i] = paramList[i].title()
paramList = [value for value in paramList if value != ""]
print "parameters retrieved for plan %s: %s" % (kbPlanName,
paramList)
return paramList
else:
return []
except:
rospy.logerr("Could not retrieve details for plan named %s" %
(kbPlanName))
return []
def getOutputName(action):
kbPlanName = action
try:
query_results = global_vars.prolog_handle ('plan', ["%s"%kbPlanName, 'X', 'Y']).ris
if len(query_results) == 1:
paramList = query_results[0].atoms[2].replace("[","").replace("]","").replace(" ","").split(",")
for i in xrange(len(paramList)):
paramList[i] = paramList[i][0].upper() + paramList[i][1:]
return paramList
else:
return []
except:
rospy.logerr("Could not retrieve details for plan named %s"%(kbPlanName))
return []
def getOutputName(action):
kbPlanName = action
try:
query_results = global_vars.prolog_handle(
'plan', ["%s" % kbPlanName, 'X', 'Y']).ris
if len(query_results) == 1:
paramList = query_results[0].atoms[2].replace("[", "").replace(
"]", "").replace(" ", "").split(",")
for i in xrange(len(paramList)):
paramList[i] = paramList[i][0].upper() + paramList[i][1:]
return paramList
else:
return []
except:
rospy.logerr("Could not retrieve details for plan named %s" %
(kbPlanName))
return []
def generate_plan(name, pcl):
rospy.loginfo("received PCL %s for plan %s"%(pcl, name))
kbPlanName = name[0].lower() + name[1:]
pcl = normalize_pcl(str(pcl))
query_results = global_vars.prolog_handle('combinedPlan', [kbPlanName])
if len(query_results.ris) != 0:
rospy.logwarn("Plan name %s already used for another plan learnt previously. Not learning this new plan."%kbPlanName)
else:
tree = getTreeFromFile('InitialPlan.pnml')
string_map = {}
root = parse(pcl.replace(" ",""), string_map)[0]
tree = create_plan(root, string_map)
save_plan_in_SapienzBot(tree, name, pcl)
global_vars.log.publish("Generated pnp for plan %s from pcl %s"%(name,pcl))
return
def getParamNames(action):
print "looking for parameters of plan %s"%action
kbPlanName = action
try:
query_results = global_vars.prolog_handle ('plan', ["%s"%kbPlanName, 'X', 'Y']).ris
if len(query_results) == 1:
paramList = query_results[0].atoms[1].replace("[","").replace("]","").replace(" ","").split(",")
for i in xrange(len(paramList)):
paramList[i] = paramList[i].title()
paramList = [value for value in paramList if value != ""]
print "parameters retrieved for plan %s: %s"%(kbPlanName, paramList)
return paramList
else:
return []
except:
rospy.logerr("Could not retrieve details for plan named %s"%(kbPlanName))
return []
def generate_plan(name, pcl):
rospy.loginfo("received PCL %s for plan %s" % (pcl, name))
kbPlanName = name[0].lower() + name[1:]
pcl = normalize_pcl(str(pcl))
query_results = global_vars.prolog_handle('combinedPlan', [kbPlanName])
if len(query_results.ris) != 0:
rospy.logwarn(
"Plan name %s already used for another plan learnt previously. Not learning this new plan."
% kbPlanName)
else:
tree = getTreeFromFile('InitialPlan.pnml')
string_map = {}
root = parse(pcl.replace(" ", ""), string_map)[0]
tree = create_plan(root, string_map)
save_plan_in_SapienzBot(tree, name, pcl)
global_vars.log.publish("Generated pnp for plan %s from pcl %s" %
(name, pcl))
return
def reset(req):
try:
actions_learnt_list = []
query_results = global_vars.prolog_handle ('combinedPlan', ["X"]).ris
for i in query_results:
kbPlanName = i.atoms[0]
planName = kbPlanName[0].upper() + kbPlanName[1:]
actions_learnt_list.append(kbPlanName)
global_vars.prolog_retract_handle('plan(%s,X,Y)'%kbPlanName)
global_vars.prolog_retract_handle('pcl(%s,X)'%kbPlanName)
remove_file(global_vars.plan_directory + "/" + planName + ".pnml")
remove_all_relatives_of_all_elements_named_from(planName + ".exec", "ChooseAction.pnml")
global_vars.prolog_retractAll_handle('combinedPlan(X)')
reset_plan_prolog_file(actions_learnt_list)
except:
rospy.logerr("Could not contact the PrologInterface node")
rospy.loginfo("Combine_pnp resetted")
return "Done"
def reset(req):
try:
actions_learnt_list = []
query_results = global_vars.prolog_handle('combinedPlan', ["X"]).ris
for i in query_results:
kbPlanName = i.atoms[0]
planName = kbPlanName[0].upper() + kbPlanName[1:]
actions_learnt_list.append(kbPlanName)
global_vars.prolog_retract_handle('plan(%s,X,Y)' % kbPlanName)
global_vars.prolog_retract_handle('pcl(%s,X)' % kbPlanName)
remove_file(global_vars.plan_directory + "/" + planName + ".pnml")
remove_all_relatives_of_all_elements_named_from(
planName + ".exec", "ChooseAction.pnml")
global_vars.prolog_retractAll_handle('combinedPlan(X)')
reset_plan_prolog_file(actions_learnt_list)
except:
rospy.logerr("Could not contact the PrologInterface node")
rospy.loginfo("Combine_pnp resetted")
return "Done"
def insert_action_in_plan(req):
rospy.loginfo("Insert action in plan request received")
kbPlanName = req.planName[0].lower() + req.planName[1:]
where = req.update_command.lower()
pcl = global_vars.prolog_handle('pcl',
[kbPlanName, 'X']).ris[0].atoms[1].replace(
"\'", "")
input_pcl = pcl
second_action_pcl = " " + normalize_pcl(req.action_to_be_added) + " "
number = -1
rospy.loginfo("Retrieved pcl %s for plan %s." % (pcl, kbPlanName))
try:
number = re.findall("\d+-", req.action_to_be_modified)[0][:-1]
first_action_pcl = normalize_pcl(
req.action_to_be_modified.split("-")[1])
except:
first_action_pcl = normalize_pcl(req.action_to_be_modified)
rospy.loginfo("I could not find any number in the input action")
rospy.loginfo("Inserting the action %s %s the action %s..." %
(second_action_pcl, where, first_action_pcl))
action_name = first_action_pcl.split(":")[0].replace("(",
"").replace(" ", "")
n_action_name_occurrencies = len(
[i for i in range(len(pcl)) if pcl.startswith(action_name, i)])
n_action_occurrencies = len([
i for i in range(len(pcl)) if pcl.startswith(
first_action_pcl.replace("(", "").replace(" ", "").replace(
")", ""), i)
])
print first_action_pcl.replace("(", "").replace(" ", "")
print number
print n_action_name_occurrencies
print n_action_occurrencies
if (number != -1) and (int(number) > int(n_action_name_occurrencies)) and (
n_action_name_occurrencies == 1):
global_vars.event_pub.publish(
"CouldNotGround_There is only one %s action in this plan. Please tell me again where I should insert the new action."
% action_name)
return "Done"
elif (number != -1) and (int(number) > int(n_action_name_occurrencies)):
global_vars.event_pub.publish(
"CouldNotGround_There are only %s %s actions in this plan. Please tell me again where I should insert the new action."
% (n_action_name_occurrencies, action_name))
return "Done"
elif (number == -1) and (
("[]" in first_action_pcl and n_action_name_occurrencies > 1) or
(not "[]" in first_action_pcl and n_action_occurrencies > 1)):
global_vars.event_pub.publish(
"CouldNotGround_There are multiple %s actions in this plan. Please tell me which one you are referring to."
% action_name)
return "Done"
elif ("[]" in first_action_pcl) and (n_action_name_occurrencies < 1):
global_vars.event_pub.publish(
"CouldNotGround_There are no %s actions in this plan. Please tell me again where I should insert the new action."
% action_name)
return "Done"
elif (not "[]" in first_action_pcl) and (n_action_occurrencies < 1):
global_vars.event_pub.publish(
"CouldNotGround_There are no %s actions of that kind in this plan. Please tell me again where I should insert the new action."
% action_name)
return "Done"
global_vars.currentPlanParamList = getParamNames(kbPlanName)
if number == -1:
number = 1
if "[]" in first_action_pcl:
i = find_nth_substring_start_index(pcl, action_name, int(number))
else:
i = find_nth_substring_start_index(
pcl,
first_action_pcl.replace("(", "").replace(" ",
"").replace(")", ""),
int(number))
k = pcl[:i].rfind("(")
j = pcl[i:].find(")")
if where == "after":
pcl = pcl[:i + j + 2] + " " + second_action_pcl + " " + pcl[i + j + 2:]
else:
pcl = pcl[:k] + " " + second_action_pcl + " " + pcl[k:]
rospy.loginfo("Processing the new pcl %s" % pcl)
forget_plan(kbPlanName)
generate_plan(kbPlanName, pcl)
global_vars.log.publish(
"Inserted action %s %s action %s in %s plan pcl %s obtaining pcl %s " %
(second_action_pcl, where, first_action_pcl, req.planName, input_pcl,
pcl))
global_vars.event_pub.publish("ActionInserted_I updated the %s plan" %
req.planName)
return "Done"
def insert_action_in_plan(req):
rospy.loginfo("Insert action in plan request received")
kbPlanName = req.planName[0].lower() + req.planName[1:]
where = req.update_command.lower()
pcl = global_vars.prolog_handle('pcl', [kbPlanName, 'X']).ris[0].atoms[1].replace("\'","")
input_pcl = pcl
second_action_pcl = " " + normalize_pcl(req.action_to_be_added) + " "
number = -1
rospy.loginfo("Retrieved pcl %s for plan %s."%(pcl, kbPlanName))
try:
number = re.findall("\d+-", req.action_to_be_modified)[0][:-1]
first_action_pcl = normalize_pcl(req.action_to_be_modified.split("-")[1])
except:
first_action_pcl = normalize_pcl(req.action_to_be_modified)
rospy.loginfo("I could not find any number in the input action")
rospy.loginfo("Inserting the action %s %s the action %s..."%(second_action_pcl, where, first_action_pcl))
action_name = first_action_pcl.split(":")[0].replace("(","").replace(" ","")
n_action_name_occurrencies = len([i for i in range(len(pcl)) if pcl.startswith(action_name, i)])
n_action_occurrencies = len([i for i in range(len(pcl)) if pcl.startswith(first_action_pcl.replace("(","").replace(" ","").replace(")",""), i)])
print first_action_pcl.replace("(","").replace(" ","")
print number
print n_action_name_occurrencies
print n_action_occurrencies
if (number != -1) and (int(number) > int(n_action_name_occurrencies)) and (n_action_name_occurrencies == 1):
global_vars.event_pub.publish("CouldNotGround_There is only one %s action in this plan. Please tell me again where I should insert the new action."%action_name)
return "Done"
elif (number != -1) and (int(number) > int(n_action_name_occurrencies)):
global_vars.event_pub.publish("CouldNotGround_There are only %s %s actions in this plan. Please tell me again where I should insert the new action."%(n_action_name_occurrencies,action_name))
return "Done"
elif (number == -1) and (("[]" in first_action_pcl and n_action_name_occurrencies > 1) or (not "[]" in first_action_pcl and n_action_occurrencies > 1)):
global_vars.event_pub.publish("CouldNotGround_There are multiple %s actions in this plan. Please tell me which one you are referring to."%action_name)
return "Done"
elif ("[]" in first_action_pcl) and (n_action_name_occurrencies < 1):
global_vars.event_pub.publish("CouldNotGround_There are no %s actions in this plan. Please tell me again where I should insert the new action."%action_name)
return "Done"
elif (not "[]" in first_action_pcl) and (n_action_occurrencies < 1):
global_vars.event_pub.publish("CouldNotGround_There are no %s actions of that kind in this plan. Please tell me again where I should insert the new action."%action_name)
return "Done"
global_vars.currentPlanParamList = getParamNames(kbPlanName)
if number == -1:
number = 1
if "[]" in first_action_pcl:
i = find_nth_substring_start_index(pcl, action_name, int(number))
else:
i = find_nth_substring_start_index(pcl, first_action_pcl.replace("(","").replace(" ","").replace(")",""), int(number))
k = pcl[:i].rfind("(")
j = pcl[i:].find(")")
if where == "after":
pcl = pcl[:i+j+2] + " " + second_action_pcl + " " + pcl[i+j+2:]
else:
pcl = pcl[:k] + " " + second_action_pcl + " " + pcl[k:]
rospy.loginfo("Processing the new pcl %s"%pcl)
forget_plan(kbPlanName)
generate_plan(kbPlanName, pcl)
global_vars.log.publish("Inserted action %s %s action %s in %s plan pcl %s obtaining pcl %s "%(second_action_pcl, where, first_action_pcl, req.planName,input_pcl,pcl))
global_vars.event_pub.publish("ActionInserted_I updated the %s plan"%req.planName)
return "Done"
