def __init__(self): self.initialized = False self.dry_run = rospy.get_param("~dry_run", False) self.clingo_interface = ClingoWrapper(AtomICAPS2014) initial_file = rospy.get_param("~initial_file", None) self.query_file = rospy.get_param("~query_file") self.passto_file = rospy.get_param("~passto_file") self.knowinside_file = rospy.get_param("~knowinside_file") self.knows_file = rospy.get_param("~knows_file") self.domain_costs_file = rospy.get_param("~domain_costs_file", None) self.costs_file = rospy.get_param("~costs_file", None) self.enable_learning = rospy.get_param("~enable_learning", False) if self.dry_run and not initial_file: rospy.logfatal( "Dry run requested, but no initial state provided." + " Please set the ~initial_file param.") return self.initial_file = "/tmp/initial" if self.dry_run: rospy.loginfo("Supplied initial state written to: " + self.initial_file) shutil.copyfile(initial_file, self.initial_file) self.executor = ActionExecutorICAPS2014(self.dry_run, self.initial_file) if self.enable_learning: rospy.wait_for_service('cost_learner/increment_episode') rospy.wait_for_service('cost_learner/add_sample') self.finalize_episode = \ rospy.ServiceProxy('cost_learner/increment_episode', Empty) self.add_sample = \ rospy.ServiceProxy('cost_learner/add_sample', CostLearnerInterface) self.planning_times_file = rospy.get_param("~planning_times_file", None) self.passto_table = read_table_from_file(self.passto_file) self.knowinside_table = read_table_from_file(self.knowinside_file) self.knows_table = read_table_from_file(self.knows_file) self.initialized = True
def __init__(self): self.initialized = False self.dry_run = rospy.get_param("~dry_run", False) self.clingo_interface = ClingoWrapper(AtomICAPS2014) initial_file = rospy.get_param("~initial_file", None) self.query_file = rospy.get_param("~query_file") self.passto_file = rospy.get_param("~passto_file") self.knowinside_file = rospy.get_param("~knowinside_file") self.knows_file = rospy.get_param("~knows_file") self.domain_costs_file = rospy.get_param("~domain_costs_file", None) self.costs_file = rospy.get_param("~costs_file", None) self.enable_learning = rospy.get_param("~enable_learning", False) if self.dry_run and not initial_file: rospy.logfatal("Dry run requested, but no initial state provided." + " Please set the ~initial_file param.") return self.initial_file = "/tmp/initial" if self.dry_run: rospy.loginfo("Supplied initial state written to: " + self.initial_file) shutil.copyfile(initial_file, self.initial_file) self.executor = ActionExecutorICAPS2014(self.dry_run, self.initial_file) if self.enable_learning: rospy.wait_for_service('cost_learner/increment_episode') rospy.wait_for_service('cost_learner/add_sample') self.finalize_episode = \ rospy.ServiceProxy('cost_learner/increment_episode', Empty) self.add_sample = \ rospy.ServiceProxy('cost_learner/add_sample', CostLearnerInterface) self.planning_times_file = rospy.get_param("~planning_times_file", None) self.passto_table = read_table_from_file(self.passto_file) self.knowinside_table = read_table_from_file(self.knowinside_file) self.knows_table = read_table_from_file(self.knows_file) self.initialized = True
class PlannerICAPS2014(object): def __init__(self): self.initialized = False self.dry_run = rospy.get_param("~dry_run", False) self.clingo_interface = ClingoWrapper(AtomICAPS2014) initial_file = rospy.get_param("~initial_file", None) self.query_file = rospy.get_param("~query_file") self.passto_file = rospy.get_param("~passto_file") self.knowinside_file = rospy.get_param("~knowinside_file") self.knows_file = rospy.get_param("~knows_file") self.domain_costs_file = rospy.get_param("~domain_costs_file", None) self.costs_file = rospy.get_param("~costs_file", None) self.enable_learning = rospy.get_param("~enable_learning", False) if self.dry_run and not initial_file: rospy.logfatal( "Dry run requested, but no initial state provided." + " Please set the ~initial_file param.") return self.initial_file = "/tmp/initial" if self.dry_run: rospy.loginfo("Supplied initial state written to: " + self.initial_file) shutil.copyfile(initial_file, self.initial_file) self.executor = ActionExecutorICAPS2014(self.dry_run, self.initial_file) if self.enable_learning: rospy.wait_for_service('cost_learner/increment_episode') rospy.wait_for_service('cost_learner/add_sample') self.finalize_episode = \ rospy.ServiceProxy('cost_learner/increment_episode', Empty) self.add_sample = \ rospy.ServiceProxy('cost_learner/add_sample', CostLearnerInterface) self.planning_times_file = rospy.get_param("~planning_times_file", None) self.passto_table = read_table_from_file(self.passto_file) self.knowinside_table = read_table_from_file(self.knowinside_file) self.knows_table = read_table_from_file(self.knows_file) self.initialized = True def _construct_initial_state(self, previous_state, observations): """ This is a poor man's belief merge function between the previous_state and observations. This needs to be replaced with proper belief merge algorithms as it impossible to cover every possible situation here using if/else statements """ new_state = [] removed_atoms = [] for atom in previous_state: atom.time = 0 add_atom = True for observation in observations: if observation.conflicts_with(atom): add_atom = False if add_atom: new_state.append(atom) else: removed_atoms.append(atom) if len(removed_atoms) != 0: display_message = " Incomaptible members in previous state: " for atom in removed_atoms: display_message += str(atom) + " " rospy.logwarn(display_message) for observation in observations: observation.time = 0 new_state.append(observation) initial_file = open(self.initial_file, "w") display_message = " Constructed state: " for atom in new_state: initial_file.write(str(atom) + ".\n") display_message += str(atom) + " " initial_file.close() rospy.loginfo(display_message) rospy.loginfo("Constructed initial state written to: " + self.initial_file) def _send_finish(self): if self.enable_learning: self.finalize_episode() def start(self): if not self.initialized: return self.executor.sense_initial_state() # Write tables to file passto_file = '/tmp/passto' write_table_to_file('passto', self.passto_table, passto_file) knows_file = '/tmp/knows' write_table_to_file('knows', self.knows_table, knows_file) knowinside_file = '/tmp/knowinside' write_table_to_file('knowinside', self.knowinside_table, knowinside_file, 0) first_run = True while True: additional_files = [] if first_run: additional_files = [passto_file, knowinside_file, knows_file] first_run = False if self.domain_costs_file != None: rospy.loginfo("Planning with cost optimization!!") start_time = time.time() plan_available, optimization, plan, states = \ self.clingo_interface.get_plan_costs([self.initial_file, self.query_file, self.costs_file, self.domain_costs_file].extend(additional_files) ) duration = time.time() - start_time if self.enable_learning and self.planning_times_file: ptf = open(self.planning_times_file, "a") ptf.write(str(duration) + "\n") ptf.close() else: plan_available, optimization, plan, states = \ self.clingo_interface.get_plan([self.initial_file, self.query_file].extend(additional_files)) if not plan_available: rospy.logfatal("No plan found to complete task!") break rospy.loginfo("Found plan (optimization = %i): %s" % (optimization, plan)) need_replan = False for action in plan: current_state = [ state for state in states if state.time == action.time ] next_state = [ state for state in states if state.time == action.time + 1 ] start_time = rospy.get_time() success, observations = \ self.executor.execute_action(action, next_state, action.time+1) duration = rospy.get_time() - start_time if self.enable_learning and action.name == "approach": if success: # Check to see if we were beside some door in start # state and the location req = CostLearnerInterfaceRequest() req.location = get_location(current_state) req.door_from = get_adjacent_door(current_state) if req.door_from: req.door_to = str(action.value) req.cost = duration self.add_sample(req) else: rospy.loginfo("Executed unsuccessful approach action.") for observation in observations: if observation not in next_state: rospy.logwarn(" Unexpected observation: " + str(observation)) need_replan = True break if need_replan: break if need_replan: self._construct_initial_state(current_state, observations) continue rospy.loginfo("Plan successful!") self._send_finish() break
class PlannerICAPS2014(object): def __init__(self): self.initialized = False self.dry_run = rospy.get_param("~dry_run", False) self.clingo_interface = ClingoWrapper(AtomICAPS2014) initial_file = rospy.get_param("~initial_file", None) self.query_file = rospy.get_param("~query_file") self.passto_file = rospy.get_param("~passto_file") self.knowinside_file = rospy.get_param("~knowinside_file") self.knows_file = rospy.get_param("~knows_file") self.domain_costs_file = rospy.get_param("~domain_costs_file", None) self.costs_file = rospy.get_param("~costs_file", None) self.enable_learning = rospy.get_param("~enable_learning", False) if self.dry_run and not initial_file: rospy.logfatal("Dry run requested, but no initial state provided." + " Please set the ~initial_file param.") return self.initial_file = "/tmp/initial" if self.dry_run: rospy.loginfo("Supplied initial state written to: " + self.initial_file) shutil.copyfile(initial_file, self.initial_file) self.executor = ActionExecutorICAPS2014(self.dry_run, self.initial_file) if self.enable_learning: rospy.wait_for_service('cost_learner/increment_episode') rospy.wait_for_service('cost_learner/add_sample') self.finalize_episode = \ rospy.ServiceProxy('cost_learner/increment_episode', Empty) self.add_sample = \ rospy.ServiceProxy('cost_learner/add_sample', CostLearnerInterface) self.planning_times_file = rospy.get_param("~planning_times_file", None) self.passto_table = read_table_from_file(self.passto_file) self.knowinside_table = read_table_from_file(self.knowinside_file) self.knows_table = read_table_from_file(self.knows_file) self.initialized = True def _construct_initial_state(self, previous_state, observations): """ This is a poor man's belief merge function between the previous_state and observations. This needs to be replaced with proper belief merge algorithms as it impossible to cover every possible situation here using if/else statements """ new_state = [] removed_atoms = [] for atom in previous_state: atom.time = 0 add_atom = True for observation in observations: if observation.conflicts_with(atom): add_atom = False if add_atom: new_state.append(atom) else: removed_atoms.append(atom) if len(removed_atoms) != 0: display_message = " Incomaptible members in previous state: " for atom in removed_atoms: display_message += str(atom) + " " rospy.logwarn(display_message) for observation in observations: observation.time = 0 new_state.append(observation) initial_file = open(self.initial_file,"w") display_message = " Constructed state: " for atom in new_state: initial_file.write(str(atom) + ".\n") display_message += str(atom) + " " initial_file.close() rospy.loginfo(display_message) rospy.loginfo("Constructed initial state written to: " + self.initial_file) def _send_finish(self): if self.enable_learning: self.finalize_episode() def start(self): if not self.initialized: return self.executor.sense_initial_state() # Write tables to file passto_file = '/tmp/passto' write_table_to_file('passto', self.passto_table, passto_file) knows_file = '/tmp/knows' write_table_to_file('knows', self.knows_table, knows_file) knowinside_file = '/tmp/knowinside' write_table_to_file('knowinside', self.knowinside_table, knowinside_file, 0) first_run = True while True: additional_files = [] if first_run: additional_files = [passto_file, knowinside_file, knows_file] first_run = False if self.domain_costs_file != None: rospy.loginfo("Planning with cost optimization!!") start_time = time.time() plan_available, optimization, plan, states = \ self.clingo_interface.get_plan_costs([self.initial_file, self.query_file, self.costs_file, self.domain_costs_file].extend(additional_files) ) duration = time.time() - start_time if self.enable_learning and self.planning_times_file: ptf = open(self.planning_times_file, "a") ptf.write(str(duration) + "\n") ptf.close() else: plan_available, optimization, plan, states = \ self.clingo_interface.get_plan([self.initial_file, self.query_file].extend(additional_files)) if not plan_available: rospy.logfatal("No plan found to complete task!") break rospy.loginfo("Found plan (optimization = %i): %s"%(optimization, plan)) need_replan = False for action in plan: current_state = [state for state in states if state.time == action.time] next_state = [state for state in states if state.time == action.time+1] start_time = rospy.get_time() success, observations = \ self.executor.execute_action(action, next_state, action.time+1) duration = rospy.get_time() - start_time if self.enable_learning and action.name == "approach": if success: # Check to see if we were beside some door in start # state and the location req = CostLearnerInterfaceRequest() req.location = get_location(current_state) req.door_from = get_adjacent_door(current_state) if req.door_from: req.door_to = str(action.value) req.cost = duration self.add_sample(req) else: rospy.loginfo("Executed unsuccessful approach action.") for observation in observations: if observation not in next_state: rospy.logwarn(" Unexpected observation: " + str(observation)) need_replan = True break if need_replan: break if need_replan: self._construct_initial_state(current_state, observations) continue rospy.loginfo("Plan successful!") self._send_finish() break