def runPlannerOnce(self):
"""
This method runs the planner once with the currently
active PlaceNodes (active facts and goals).
"""
# write active nodes to output folder
active_facts_pdkb = output_path + "active_facts.pdkb"
active_goals_pdkb = output_path + "active_goals.pdkb"
# todo: add filtering
factsPDDL = ""
goalsPDDL = ""
for i in self.memory.retreiveActiveItems():
node = i.getObject()
factsPDDL += node.dumpFactsToPDDL()
goalsPDDL += node.dumpGoalsToPDDL()
# write the facts PDKB file
active_facts_file = open(active_facts_pdkb, 'w')
for l in factsPDDL.split("\n"):
active_facts_file.write(l + "\n")
active_facts_file.close()
# write the goals PDKB file
active_goals_file = open(active_goals_pdkb, 'w')
for l in goalsPDDL.split("\n"):
active_goals_file.write(l + "\n")
active_goals_file.close()
# insert the PDKB files into the PDDL templates
# todo: set the domain id as an option
domain_template = current_domain_path + "domain-template.pddl"
problem_template = current_domain_path + "problem-template.pddl"
# run the planner
p = Planner.run(current_domain_name, domain_template, problem_template,
[active_facts_pdkb],
[active_goals_pdkb])
return p
def runPlannerOnce(self):
"""
This method runs the planner once with the currently
active PlaceNodes (active facts and goals).
"""
# write active nodes to output folder
active_facts_pdkb = output_path + "active_facts.pdkb"
active_goals_pdkb = output_path + "active_goals.pdkb"
# todo: add filtering
factsPDDL = ""
goalsPDDL = ""
for i in self.memory.retreiveActiveItems():
node = i.getObject()
factsPDDL += node.dumpFactsToPDDL()
goalsPDDL += node.dumpGoalsToPDDL()
# write the facts PDKB file
active_facts_file = open(active_facts_pdkb, 'w')
for l in factsPDDL.split("\n"):
active_facts_file.write(l + "\n")
active_facts_file.close()
# write the goals PDKB file
active_goals_file = open(active_goals_pdkb, 'w')
for l in goalsPDDL.split("\n"):
active_goals_file.write(l + "\n")
active_goals_file.close()
# insert the PDKB files into the PDDL templates
# todo: set the domain id as an option
domain_template = current_domain_path + "domain-template.pddl"
problem_template = current_domain_path + "problem-template.pddl"
# run the planner
p = Planner.run(current_domain_name, domain_template, problem_template,
[active_facts_pdkb], [active_goals_pdkb])
return p
############### ROS setup #######################
node_name = 'mico_planner'
group_name = 'arm'
planner_name = 'RRTConnectkConfigDefault'
ee_link_name = 'mico_link_endeffector'
roscpp_initialize(sys.argv)
rospy.init_node(node_name, anonymous=True)
acHan = ActionHandler(group_name, planner_name, ee_link_name)
rospy.sleep(1)
acHan.setWorkspace(-10, -10, 0, 10, 10, 10)
#################################################
planner = Planner()
timing = Timing()
planner.run()
# Database setup
storage = FileStorage.FileStorage("pGraph.fs")
db = DB(storage)
conn = db.open()
pGraphDB = conn.root()
if not pGraphDB.has_key("graph"):
pGraphDB["graph"] = PGraph.PGraph()
# Variables
pGraph = pGraphDB["graph"]
# clear persistent program memory
pGraph.setCurrNodeNone()
def run():
Ble.init()
Ble.get_shell().load_events()
Planner.run()
print("program terminated")
