def init_planner(self):
logger.info("adding atomic operators to pyhop")
self.init_state()
# note: can call declare_operators multiple times for current situation
hop.declare_operators(self.move_forward, self.turn_left,
self.turn_right, self.break_block)
hop.print_operators(hop.get_operators())
# for the main task
hop.declare_methods('get_resource', self.get_resource)
# for each sub-task of the main task
hop.declare_methods('find_route', self.find_route_to_resource)
hop.declare_methods('navigate', self.navigate_to_resource)
hop.declare_methods('acquire', self.acquire_resource)
hop.print_methods(hop.get_methods())
def init_planner(self):
logger.info("adding atomic operators to pyhop")
self.init_state()
# note: can call declare_operators multiple times for current situation
hop.declare_operators(
self.move_forward,
self.turn_left,
self.turn_right,
self.break_block,
self.search_placeholder,
)
# self.equip_agent)
hop.print_operators(hop.get_operators())
# for the main task
hop.declare_methods('get_resource', self.get_resource)
# for each sub-task of the main task
hop.declare_methods('search_for_gold', self.search_for_gold)
hop.declare_methods('move_closer', self.move_closer)
hop.declare_methods('acquire_gold', self.acquire_resource)
# hop.declare_methods('break_wall', self.break_wall)
hop.print_methods(hop.get_methods())
def place(state, r, o, x):
# print(state.objects['tray'])
# pick_from_tray(state, r, o)
if (state.loc[o] == r and state.gripperfree == False and state.loc[r] == x
and state.cupboardisopen == True):
state.gripperfree = True
state.loc[o] = x
state.objects['cupboard'] += 1
else:
return False
return state
#initial state
hop.declare_operators(look_cupboard, open, look_table, goto, pick, place)
print('')
hop.print_operators(hop.get_operators())
#x and y are locations, o is object and r is robot
def task1(state, r, o, x, y):
tasklist = [('look_cupboard', r), ('goto', r, '', x), ('open', r, x),
('look_table', r), ('goto', r, x, y)]
X1 = [('pick', r, o, y), ('goto', r, y, x), ('place', r, o, x),
('goto', r, x, y)]
for k in range(5):
for j in range(len(X1)):
tasklist.append(X1[j])
return tasklist
and state.loc[r] == x and state.cupboardisopen == True):
state.gripperfree = True
state.loc[o] = x
state.objects[x] += 1
state.objectsLocation[key_objects[state.i]] = x
else:
return False
objs[key_objects[state.i]] = x
state.i += 1
# state.loc['objects'] = 'cupboard'
state.isTable = False
return state
#initial state
hop.declare_operators(look_cupboard, open, look_table, goto, pick, place,
perceiveObjects)
print('')
hop.print_operators(hop.get_operators())
#x and y are locations, o is object and r is robot
def task1(state, r, o, x, y, i):
tasklist = [('look_cupboard', r), ('goto', r, '', x), ('open', r, x),
('look_table', r), ('goto', r, x, y), ('perceiveObjects', y)]
X1 = [('pick', r, o, y), ('goto', r, y, x), ('place', r, o, x),
('goto', r, x, y)]
for k in range(len(state.objectsLocation)):
for j in range(len(X1)):
tasklist.append(X1[j])
return tasklist
def ride_taxi(state,a,x,y):
if state.loc['taxi']==x and state.loc[a]==x:
state.loc['taxi'] = y
state.loc[a] = y
state.owe[a] = taxi_rate(state.dist[x][y])
return state
else: return False
def pay_driver(state,a):
if state.cash[a] >= state.owe[a]:
state.cash[a] = state.cash[a] - state.owe[a]
state.owe[a] = 0
return state
else: return False
hop.declare_operators(walk, call_taxi, ride_taxi, pay_driver)
print('')
hop.print_operators(hop.get_operators())
def travel_by_foot(state,a,x,y):
if state.dist[x][y] <= 2:
return [('walk',a,x,y)]
return False
def travel_by_taxi(state,a,x,y):
if state.cash[a] >= taxi_rate(state.dist[x][y]):
return [('call_taxi',a,x), ('ride_taxi',a,x,y), ('pay_driver',a)]
return False
hop.declare_methods('travel',travel_by_foot,travel_by_taxi)
print('')
return False
# state.gripper = True
# state.loc[o] = tray
# return state
def open(state, r, x):
if (state.gripper == True and state.cupDoorisopen == False
and state.loc[r] == x):
state.cupDoorisopen = True
return state
#initial state
hop.declare_operators(open, goto, pick, place)
print('')
hop.print_operators(hop.get_operators())
def task1(state, r, o, x, y):
return [('open', r, x), ('goto', r, x, y), ('pick', r, o, y),
('goto', r, y, x), ('place', r, o, x)]
#def task2(state, r,o,x,y):
#return [('goto',r, x, y), ('pick',r,o,y), ('place',r,o,x)]
hop.declare_methods('task1', task1)
print('')
hop.print_methods(hop.get_methods())
def runRobot():
"""Runs the robot
Initializes all the variables needed for the application, calls for a plan,
makes the behaviour tree and runs it.
"""
domain = parser.parse(sys.argv[1], sys.argv[2])
hop.declare_operators(*(domain.taskList))
for k in domain.methodList.keys():
hop.declare_methods(k, domain.methodList[k])
#Calculates the plan for the given state and goal
plan = hop.plan(copy.deepcopy(domain.state),domain.getGoals(),
hop.get_operators(),hop.get_methods(),verbose=0)
if plan != None:
print('PLAN =',plan,'\n')
#Launchs the ros node, and set it the shutdown function
rospy.init_node("planner_node", anonymous=False)
rospy.on_shutdown(shutdown)
#Sets the Global variables and the user defined robot configuration
global_vars.init()
getConfig()
#Initializes the simulator
init_environment()
initBattery = rospy.ServiceProxy("battery_simulator/set_battery_level", SetBatteryLevel)
initBattery(100)
#Computes the behaviour tree
global_vars.black_board.setWorld(copy.deepcopy(domain.state))
Tree = makeTree(plan, domain.state)
print_tree(Tree)
#Runs the tree
while not rospy.is_shutdown() and global_vars.black_board.finished() == False:
while not rospy.is_shutdown() and global_vars.black_board.finished() == False and global_vars.black_board.rePlanNeeded() == False:
Tree.run()
rospy.sleep(0.1)
if global_vars.black_board.rePlanNeeded() == True:
print ("Replanning...")
global_vars.move_base.cancel_all_goals()
global_vars.cmd_vel_pub.publish(Twist())
plan = hop.plan(copy.deepcopy(global_vars.black_board.getWorld()),
domain.getGoals(),hop.get_operators(),hop.get_methods(),verbose=0)
if plan != None:
print('** New plan =',plan,'\n')
Tree = makeTree(plan, copy.deepcopy(global_vars.black_board.getWorld()))
print_tree(Tree)
else:
print("Empty plan generated")
raise ValueError("Empty plan generated")
else:
print("Empty plan generated")
raise ValueError("Empty plan generated")
state.pos[b] = 'hand'
state.clear[b] = False
state.holding = b
state.clear[c] = True
return state
else: return False
def putdown(state,b):
if state.pos[b] == 'hand':
state.pos[b] = 'table'
state.clear[b] = True
state.holding = False
return state
else: return False
def stack(state,b,c):
if state.pos[b] == 'hand' and state.clear[c] == True:
state.pos[b] = c
state.clear[b] = True
state.holding = False
state.clear[c] = False
return state
else: return False
"""
Below, 'declare_operators(pickup, unstack, putdown, stack)' tells Pyhop
what the operators are. Note that the operator names are *not* quoted.
"""
hop.declare_operators(pickup, unstack, putdown, stack)
if __name__ == '__main__':
if len(sys.argv) < 3:
print(
"Usage: rosrun pfg_utils domain_checker.py domain.pddl problem.pddl"
)
else:
domPddl = sys.argv[1]
probPddl = sys.argv[2]
print "\nParsing domain: " + domPddl
print "Parsing problem: " + probPddl
domain = parser.parse(domPddl, probPddl)
print "\nINITIAL STATE: \n"
domain.printState()
print "\n"
hop.declare_operators(*(domain.taskList))
hop.print_operators(hop.get_operators())
for k in domain.methodList.keys():
hop.declare_methods(k, domain.methodList[k])
hop.print_methods(hop.get_methods())
print "\n"
plan = hop.plan(domain.state,
domain.getGoals(),
hop.get_operators(),
hop.get_methods(),
verbose=2)
