def call_fd(domain, problem, args='--search "astar(lmcut())"', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move_to_grasp', 'arm1', 'box1', 'base1', 'box2'), ('move_to_grasp', 'arm2', 'box2', 'cap1', 'box1')],
[('vacuum_object', 'arm2', 'cap1', 'box1'), ('vacuum_object', 'arm1', 'base1', 'box2')],
...]
'''
cmd = 'timeout 1800 ./planners/fd/fast-downward.py --search-memory-limit 4G --sas-file /tmp/output.sas --plan-file /tmp/plan.txt {} {} {} & echo $! >> {}/fd-pid.txt'.format(domain, problem, args, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
## if there is shared library errors ##
if "error while loading shared libraries" in to_str(err):
return -1
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/fd/fast-downward.py\''))
return -1
## if no solution exists try the next domain ##
if not "translate exit code: 0" in shell or\
"configuration does not support" in to_str(err):
# if not verbose:
# print(color.fg_voilet('-- \'fd\' does not support this pddl configuration -- run with \'-v 2\''))
return -1
## problem proved unsolvable ##
if "search exit code: 11" in shell or "search exit code: 12" in shell:
return None
## if no solution exists try the next domain ##
if not "search exit code: 0" in shell:
return -1
## read the solution file
plan = list()
try:
with open('/tmp/plan.txt') as f:
for line in f:
## extract concurrent action at each step
if '; cost =' in line: continue
step = re.split('[, ) (]+',line)[1:-1]
plan.append([tuple(step)])
except FileNotFoundError as fnf_error:
print(shell)
# exit()
return -1
return plan
def call_vhpop(domain, problem, args='-g -f DSep-LIFO -s HC -w 5 -l 1500000', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move-car', 'l1', 'l4')], [('changetire', 'l4')]]
'''
cmd = 'timeout 1800 ./planners/vhpop {} {} {} & echo $! >> {}/vhpop-pid.txt'.format(args, domain, problem, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# extract plan
shell = shell[shell.find(';'):shell.find('Time')].strip()
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/vhpop\''))
return -1
## if not supported some PDDL features by planner ##
if "undeclared type" in to_str(err) or\
"type mismatch" in to_str(err):
# print(color.fg_yellow("[planning failed due to some error in the pddl description]"))
# print(color.fg_voilet('\n-- planner stdout'))
# print(shell)
# if to_str(err):
# print(color.fg_voilet('-- planner stderr'))
# print(to_str(err))
# exit()
return -1
## if no solution exists try the next domain ##
if ";Problem has no solution." in shell or "no plan" in shell or ";Search limit reached." in shell:
return None
# split shell into a list of actions and ignore '; Time'
shell = shell.split('\n')[1:]
plan = OrderedDict()
for action in shell:
action = re.split('[, ) (]+', action)[:-1]
plan.setdefault(action[0], []).append(tuple(action[1:]))
# print(list(plan.values()))
return list(plan.values())
def call_lpg(domain, problem, args='-n 1', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move_to_grasp', 'arm1', 'box1', 'base1', 'box2'), ('move_to_grasp', 'arm2', 'box2', 'cap1', 'box1')],
[('vacuum_object', 'arm2', 'cap1', 'box1'), ('vacuum_object', 'arm1', 'base1', 'box2')],
...]
'''
cmd = 'timeout 1800 ./planners/lpg -o {} -f {} {} & echo $! >> {}/lpg-pid.txt'.format(domain, problem, args, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/lpg\''))
return -1
## if no solution exists try the next domain ##
if "Goals of the planning problem can not be reached." in shell \
or "goal can be simplified to FALSE" in shell \
or "The problem is unsolvable" in shell \
or "No plan will solve it" in shell:
return None
## if not supported some PDDL features by planner ##
if "not supported by this exp version" in shell \
or "type mismatch" in shell \
or "Unexpected node:" in shell \
or "Segmentation Fault" in shell:
# print(color.fg_yellow("[planning failed due to some error in the pddl description]"))
# print(color.fg_voilet('\n-- planner stdout'))
# print(shell)
# if to_str(err):
# print(color.fg_voilet('-- planner stderr'))
# print(to_str(err))
# exit()
return -1
## if solution already exists in the problem ##
if "No action in solution" in shell and "Plan computed:" in shell:
return list()
## refine the output screen and build a plan of actions' signatures ##
# extract plan from ';<problem_name>' to 'Time:<value>'
shell = shell[shell.find('Time: (ACTION) [action Duration; action Cost]'):shell.rfind('Solution number:')].strip()
# split shell into a list of actions and ignore ';<problem_name>'
shell = shell.lower().split('\n')[1:]
plan = OrderedDict()
for action in shell:
action = re.split('[, ) (]+', action)[1:-2]
plan.setdefault(action[0], []).append(tuple(action[1:]))
# print(list(plan.values()))
return list(plan.values())
def call_probe(domain, problem, args='', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move-car', 'l1', 'l4')], [('changetire', 'l4')]]
'''
cmd = 'timeout 1800 ./planners/probe {0} -d {1} -i {2} -o {2}.soln & echo $! >> {3}/prob-pid.txt'.format(args, domain, problem, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
try: print('\n'+open(problem+'.soln.1').read())
except: pass
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/m\''))
return -1
## if no solution exists try the next domain ##
if "Planning task not solvable" in shell or\
"goal can be simplified to FALSE." in shell or\
"Could not solve problem" in shell:
return None
## read the solution file
plan = list()
try:
with open(problem+'.soln.1') as f:
for line in f:
## extract concurrent action at each step
step = re.split('[, ) (]+',line.lower())[1:-1]
plan.append([tuple(step)])
except: return -1
return plan
def call_m(domain, problem, args='-P 1 -t 5 -N', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move-car', 'l1', 'l4')], [('changetire', 'l4')]]
'''
cmd = 'timeout 1800 ./planners/m {0} {1} {2} -o {2}.soln & echo $! >> {3}/m-pid.txt'.format(args, domain, problem, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
try: print('\n'+open(problem+'.soln').read())
except: pass
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/m\''))
return -1
## if no solution exists try the next domain ##
if "Timeout after" in shell:
return None
## if solution already exists in the problem ##
if "PLAN FOUND: 0 steps" in shell:
return list()
## read the solution file
plan = list()
try:
with open(problem+'.soln') as f:
for line in enumerate(f):
## extract concurrent action at each step
step = line[1].split()[2:]
plan.append([tuple(re.split('[, ) (]+',s)[:-1]) for s in step])
except: return -1
return plan
def call_optic_clp(domain, problem, args='-b -N', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move_to_grasp', 'arm1', 'box1', 'base1', 'box2'), ('move_to_grasp', 'arm2', 'box2', 'cap1', 'box1')],
[('vacuum_object', 'arm2', 'cap1', 'box1'), ('vacuum_object', 'arm1', 'base1', 'box2')],
...]
'''
cmd = 'timeout 1800 ./planners/optic-clp {} {} {} & echo $! >> {}/optic-clp-pid.txt'.format(args, domain, problem, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/optic-clp\''))
return -1
## if solution already exists in the problem ##
if "has to terminate" in to_str(err) \
or "error while loading shared libraries" in to_str(err) \
or "Segmentation Fault" in shell:
# print(color.fg_yellow("[planning failed due to some error in the pddl description]"))
# print(color.fg_voilet('\n-- planner stdout'))
# print(shell)
# if to_str(err):
# print(color.fg_voilet('-- planner stderr'))
# print(to_str(err))
# # exit()
return -1
## if problem is unsolvable by EHC remove -b (activate best-first search)
if "Problem unsolvable by EHC, and best-first search has been disabled" in shell:
## calling 'optic-clp' again removing '-b' option ##
cmd = 'timeout 1800 ./planners/optic-clp -N {0} {1}'.format(domain, problem)
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
# shell = ''.join(map(chr, output))
shell = to_str(output)
## if no solution exists try the next domain ##
if "problem has been deemed unsolvable" in shell or "Problem unsolvable" in shell:
return None
## if no solution exists try the next domain ##
if "file appear to violate part of the PDDL" in to_str(err) or\
"problem has been encountered, and the planner has to terminate" in to_str(err):
if not verbose == 2:
print(color.fg_red('[error by external planner: run with \'-v 2\''))
return None
## if solution already exists in the problem ##
if ("; Plan empty" in shell and "; Plan found" in shell) \
or "The empty plan is optimal" in shell:
return list()
## refine the output screen and build a plan of actions' signatures ##
# extract plan from ';<problem_name>' to 'Time:<value>'
shell = shell[shell.find('; Time'):].strip()
# split shell into a list of actions and ignore ';<problem_name>'
shell = shell.split('\n')[1:]
plan = OrderedDict()
for action in shell:
action = re.split('[, ) (]+', action)[:-1]
plan.setdefault(action[0], []).append(tuple(action[1:]))
# print(list(plan.values()))
return list(plan.values())
def call_ff(domain, problem, args='', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@args : the default args of the external planner
@pwd : the current working directory
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move-car', 'l1', 'l4')], [('changetire', 'l4')]]
'''
cmd = 'timeout 1800 ./planners/ff -o {} -f {} {} & echo $! >> {}/ff-pid.txt'.format(domain, problem, args, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/ff\''))
return -1
## if no solution exists try the next domain ##
if "goal can be simplified to FALSE" in shell or "problem proven unsolvable" in shell:
return None
## if solution already exists in the problem ##
if "goal can be simplified to TRUE" in shell:
return list()
## if solution already exists in the problem ##
if 'predicate' in shell or 'type mismatch' in shell or\
'undeclared variable' in shell or 'declared to use unknown' in shell or\
'unknown constant' in shell or 'check input files' in shell or\
'increase MAX_PLAN_LENGTH!' in shell or\
'too many constants!' in shell or 'too many operators!' in shell or\
'syntax error in line' in to_str(err):
# print(color.fg_yellow('[planning failed due to some error in the pddl description]'))
# print(color.fg_voilet('\n-- planner stdout'))
# print(shell)
# if to_str(err):
# print(color.fg_voilet('-- planner stderr'))
# print(to_str(err))
# exit()
return -1
## refine the output screen and build a plan of actions' signatures ##
shell = shell[shell.find('step')+len('step'):shell.rfind('time spent')].strip() # extract plan
return [[tuple(l.lower().split()[1:])] for l in shell.splitlines()] # string to list of tuples
def action_execution_verification(action_msg):
global args, domain, problem, state, plan, action_ids
#############################
## simulate and execute the plan
for level, step in plan.items():
## if all actions in the plan are visited
if step == 'GOAL':
## check if goal is achieved then terminate planner
if state.is_true(problem.goals):
# goal state is achieved
print(color.fg_voilet('@ GOAL'))
sub_proc.unregister()
rospy.signal_shutdown('finished')
else:
# unfold step into a tuple of actions and outcomes
(actions, outcomes) = step
for action in actions:
## find an action matching the received action
if '_'.join(action.sig) == action_msg.id:
# if action was already visited
if '_'.join(action.sig) in action_ids: return
# add action's id to the visited action_ids
action_ids.append('_'.join(action.sig))
# check if action succeeded
if action_msg.succeed:
## print out succeeded action
print(color.fg_yellow(' + ') + str(action))
# apply action to the state and update the state
state = state.apply(action)
## check if goal is achieved then terminate planner
if state.is_true(problem.goals):
# goal state is achieved
print(color.fg_voilet('@ GOAL'))
sub_proc.unregister()
rospy.signal_shutdown('finished')
else:
## print out failed action
print(color.fg_red(' - ') + str(action))
for monitor in action_msg.monitors:
print(
color.fg_red(' ---- {} {}'.format(
monitor.predicate, monitor.arguments[0])))
## update the state with the action violence and make a re-plane
## convert the predicates frozenset to a list and update the state
## i.e., remove ('collision_free', 'left_arm')
state_predicates = list(state.predicates)
for monitor in action_msg.monitors:
if 'collision' in monitor.predicate:
if ('collision_free', monitor.arguments[0]
) in state_predicates:
state_predicates.remove(
('collision_free',
monitor.arguments[0]))
if not ('collision_detected', monitor.
arguments[0]) in state_predicates:
state_predicates.append(
('collision_detected',
monitor.arguments[0]))
elif 'admittance' in monitor.predicate:
if ('admittance_free', monitor.arguments[0]
) in state_predicates:
state_predicates.remove(
('admittance_free',
monitor.arguments[0]))
if not ('admittance_detected', monitor.
arguments[0]) in state_predicates:
state_predicates.append(
('admittance_detected',
monitor.arguments[0]))
## convert back to frozenset
state.predicates = frozenset(state_predicates)
#############################
## create a new pddl problem file at /tmp/safe-planner
problem_pddl = pddl.pddl(problem, state=state)
## call planner to make an initial policy given the domain, problem and planners
(policy, plan, plan_json_file, actions_json_file) = \
make_plan(domain = domain, \
problem = problem_pddl, \
planners = args.planners, \
agents = args.agents, \
temporal_actions = args.temporal_actions, \
rank=False, \
verbose=args.verbose)
#############################
## call the execution engine giving the initial plan
# plan_json_file actions_json_file
send_json_plan(plan_json_file, actions_json_file)
return
return
