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"