def cram_plan(debug):
my_blackboard = Blackboard()
my_blackboard.set(
"current_shelf_id",
"http://knowrob.org/kb/dm-market.owl#DMShelfW100_XLUZQHSE")
layer_detection = Sequence('layer detection composite')
layer_detection.add_child(
ConfigureLayerDetectionBehavior('configure layer detection'))
layer_detection.add_child(BaseNavigationBehavior('move base'))
layer_detection.add_child(
running_is_success(LayerDetectionBehavior)('layer detection'))
layer_detection.add_child(ArmMovementBehavior('move arm'))
layer_detection.add_child(FinishPerceptionBehavior('finish perception'))
root = Sequence('root')
root.add_child(layer_detection)
tree = BehaviourTree(root)
if debug:
# TODO create data folder if it does not exist
def post_tick(snapshot_visitor, behaviour_tree):
print(u'\n' + ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = SnapshotVisitor()
tree.add_post_tick_handler(
functools.partial(post_tick, snapshot_visitor))
tree.visitors.append(snapshot_visitor)
render_dot_tree(root, name='tree')
tree.setup(3)
return tree
class MyBahaviour(Behaviour):
"""
Base Behavior that gives access to robosherlock and knowrob.
"""
def __init__(self, name=""):
super(MyBahaviour, self).__init__(name)
self.blackboard = Blackboard()
self.param_name = 'exception'
def get_knowrob(self):
"""
:rtype: KnowRob
"""
return self.blackboard.knowrob
def get_robosherlock(self):
"""
:rtype: RoboSherlock
"""
return self.blackboard.robosherlock
def get_blackboard(self):
return self.blackboard
def raise_to_blackboard(self, exception):
"""
saves an exception on the blackboard
:type exception: Exception
"""
self.blackboard.set(self.param_name, exception)
def get_exception(self):
return self.blackboard.get(self.param_name)
def setup(self, timeout):
# TODO handle timeout
self.as_handler = Blackboard().get(self.as_name)
if self.as_handler is None:
self.as_handler = ActionServerHandler(self.as_name,
self.action_type)
Blackboard().set(self.as_name, self.as_handler)
return super(ActionServerBehavior, self).setup(timeout)
def __init__(self, action_name, action_type):
self.goal_queue = Queue(1)
self.result_queue = Queue(1)
self.lock = Blackboard().lock
self.action_name = action_name
self.cancel_next_goal = False
self._as = SimpleActionServer(action_name, action_type, execute_cb=self.execute_cb, auto_start=False)
self._as.register_preempt_callback(self.cancel_cb)
self._as.start()
def __init__(self, name):
"""Init method for the Until sub-tree.
Until sub-tree has following structure
Sequence
- Selector
-p_2
-\\phi_1
- Sequence
-p_1
-\\phi_2
"""
super(UntilNode, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
# Define a sequence to combine the primitive behavior
root = Sequence('U')
selec = Selector('Se')
p2 = DummyNode('p2')
p1 = DummyNode('p1')
goal1 = DummyNode('g1')
goal2 = DummyNode('g2')
selec.add_children([p2, goal1])
seq = Sequence('S')
seq.add_children([p1, goal2])
root.add_children([selec, seq])
self.bt = BehaviourTree(root)
def __init__(
self, env, keys, goalspec, gtable=dict(), max_trace=40,
actions=[0, 1, 2, 3], epoch=10, seed=None, allkeys=None,
actionu=0.90, id=None):
super().__init__(
env, keys, goalspec, gtable, max_trace, actions, epoch, seed)
self.tcount = 0
self.door_history = 0
if allkeys is None:
self.allkeys = keys
else:
self.allkeys = allkeys
# Initialize blackboard to store data
self.blackboard = Blackboard()
# Action uncertainty
self.prob = actionu
# Id to reference the blackboard
self.id = self.goalspec if id is None else id
# Numpy array to hold competency data
# print(self.epoch)
self.tdata = np.zeros((self.epoch, max_trace+4))
self.idata = np.zeros((self.epoch, max_trace+4))
self.blackboard.shared_content[
'ctdata'][self.id] = self.tdata
self.blackboard.shared_content[
'cidata'][self.id] = self.idata
def update(self):
skip_failures = self.get_god_map().get_data(identifier.skip_failures)
Blackboard().set('exception', None) # FIXME move this to reset?
result = self.get_god_map().get_data(identifier.result_message)
trajectory = self.get_god_map().get_data(identifier.trajectory)
sample_period = self.get_god_map().get_data(identifier.sample_period)
controlled_joints = self.get_robot().controlled_joints
result.trajectory = traj_to_msg(sample_period, trajectory,
controlled_joints, True)
if result.error_codes[-1] == MoveResult.PREEMPTED:
self.get_as().send_preempted(result)
return Status.SUCCESS
if skip_failures:
if not self.any_goal_succeeded(result):
self.get_as().send_aborted(result)
return Status.SUCCESS
else:
if not self.all_goals_succeeded(result):
self.get_as().send_aborted(result)
return Status.SUCCESS
self.get_as().send_result(result)
return Status.SUCCESS
def __init__(self, name, model):
super().__init__(name, model)
self.location = ()
self.direction = model.random.rand() * (2 * np.pi)
self.speed = 2
self.radius = 3
self.moveable = True
self.shared_content = dict()
root = py_trees.composites.Sequence("Sequence")
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
name = type(model.target).__name__
self.shared_content[name] = {model.target}
low = GoTo('1')
low.setup(0, self, name)
mid = Away('2')
mid.setup(0, self)
high = Move('3')
high.setup(0, self)
root.add_children([low, mid, high])
self.behaviour_tree = py_trees.trees.BehaviourTree(root)
def __init__(
self, expname='key', goalspecs='F P_[KE][1,none,==]',
keys=['LO', 'FW', 'KE'], actions=list(range(5)),
seed=None, maxtracelen=40, trainc=False, epoch=80):
env_name = 'MiniGrid-Goals-v0'
env = gym.make(env_name)
if seed is None:
pass
else:
env = ReseedWrapper(env, seeds=[seed])
env = FullyObsWrapper(env)
self.env = env
self.env.max_steps = min(env.max_steps, 200)
# self.env.agent_view_size = 1
self.env.reset()
self.expname = expname
self.goalspecs = goalspecs
self.epoch = epoch
self.maxtracelen = maxtracelen
self.trainc = trainc
self.allkeys = [
'LO', 'FW', 'KE', 'DR',
'BOB', 'BOR', 'BAB', 'BAR',
'LV', 'GO', 'CK',
'CBB', 'CBR', 'CAB', 'CAR',
'DO', 'RM']
self.keys = keys
self.actions = actions
root = goalspec2BT(goalspecs, planner=None, node=CompetentNode)
self.behaviour_tree = BehaviourTree(root)
self.blackboard = Blackboard()
def __init__(self, name, planner, train=True, id=0):
"""Init method for the policy behavior."""
super(GoalNode, self).__init__(name)
self.blackboard = Blackboard()
self.planner = planner
self.train = train
self.blackboard.shared_content = dict()
def __init__(self, name, planner, train=True, id=0):
"""Init method for the policy behavior."""
super(CompetentNode, self).__init__(name)
self.blackboard = Blackboard()
self.planner = planner
self.train = train
self.nodename = name + '_' + str(id)
try:
self.blackboard.shared_content.keys()
except AttributeError:
self.blackboard.shared_content = dict()
self.blackboard.shared_content['curve'] = dict()
try:
# print(self.blackboard.shared_content['curve'], name, str(train))
self.blackboard.shared_content['curve'][self.nodename]
except KeyError:
self.blackboard.shared_content['curve'][self.nodename] = dict()
self.blackboard.shared_content['curve'][self.nodename][str(
True)] = [0, 0, 0]
self.blackboard.shared_content['curve'][self.nodename][str(
False)] = [0, 0, 0]
# print(self.blackboard.shared_content['curve'])
try:
self.blackboard.shared_content['ctdata'][self.nodename] = list()
self.blackboard.shared_content['cidata'][self.nodename] = list()
except KeyError:
self.blackboard.shared_content['ctdata'] = dict()
self.blackboard.shared_content['cidata'] = dict()
self.blackboard.shared_content['ctdata'][self.nodename] = list()
self.blackboard.shared_content['cidata'][self.nodename] = list()
def finish_perception_cb(data):
"""
:type data: FinishPerceptionRequest
:rtype: FinishPerceptionResponse
"""
prefix = 'finish_perception'
print_with_prefix('called', prefix)
lock = Blackboard().lock # type: TimeoutLock
result = FinishPerceptionResponse()
with lock.acquire_timeout(0) as got_lock:
if not got_lock:
Blackboard().finished = True
result.error = FinishPerceptionResponse.SUCCESS
else:
result.error = FinishPerceptionResponse.NO_RUNNING_JOB
print_with_prefix('no running job', prefix)
print_with_prefix('finished', prefix)
return result
def grow_tree(debug=True):
roboserlock_sim = rospy.get_param('~robosherlock_sim')
b = Blackboard()
b.finished = False
b.lock = TimeoutLock()
b.knowrob = KnowRob()
if roboserlock_sim:
b.robosherlock = FakeRoboSherlock(b.knowrob)
else:
b.robosherlock = RoboSherlock(b.knowrob)
finish_perception_srv = rospy.Service('~finish_perception', FinishPerception, finish_perception_cb)
# ----------------------------------------------
shelf_layer_as_name = '~detect_shelf_layers'
shelf_layer1 = Sequence('layer detection 1')
shelf_layer1.add_child(GoalReceived('got req', shelf_layer_as_name, DetectShelfLayersAction))
shelf_layer1.add_child(DetectShelfLayersBehavior('shelf layer detection', shelf_layer_as_name))
# ----------------------------------------------
detect_facings_as_name = '~detect_facings'
detect_facings1 = Sequence('detect facings 1')
detect_facings1.add_child(GoalReceived('got req', detect_facings_as_name, DetectFacingsAction))
detect_facings1.add_child(DetectFacingsBehavior('detect facings', detect_facings_as_name))
# ----------------------------------------------
count_products_as_name = '~count_products'
count_products = Sequence('count products')
count_products.add_child(GoalReceived('got req', count_products_as_name, CountProductsAction))
count_products.add_child(CountProductsBehavior('count products', count_products_as_name))
# ----------------------------------------------
root = Selector(u'root')
root.add_child(QueryBehavior('query'))
root.add_child(shelf_layer1)
root.add_child(detect_facings1)
root.add_child(count_products)
tree = BehaviourTree(root)
if debug:
# TODO create data folder if it does not exist
def post_tick(snapshot_visitor, behaviour_tree):
print(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = py_trees.visitors.SnapshotVisitor()
tree.add_post_tick_handler(functools.partial(post_tick, snapshot_visitor))
tree.visitors.append(snapshot_visitor)
# render_dot_tree(root, name=path_to_data_folder + u'/tree')
tree.setup(30)
return tree
def update(self):
# TODO get result from god map or blackboard
e = self.get_blackboard_exception()
Blackboard().set('exception', None)
result = MoveResult()
result.error_code = self.exception_to_error_code(e)
if self.get_as().is_preempt_requested(
) or not result.error_code == MoveResult.SUCCESS:
self.get_as().send_preempted(result)
else:
self.get_as().send_result(result)
return Status.SUCCESS
def __init__(self, robot, collision_list_size):
"""
:type robot: giskardpy.robot.Robot
"""
self.robot = robot
self.root_T_map = kdl_to_np(self.robot.root_T_map)
self.robot_root = self.robot.get_root()
self.distance_config = Blackboard().god_map.to_symbol(
identifier.lasercollision_avoidance_config + [u'distance'])
self.laser_collisions = ([self._default_collision()] * 15)
self.laser_collisions_simulated = ([self._default_collision()] * 15)
def run_planner(planner, behaviour_tree, env, epoch=10, maxtracelen=10):
child = behaviour_tree.root
# Training setup
child.setup(0, planner, True, epoch)
child.planner.goalspec = child.name
# Training loop
blackboard = Blackboard()
blackboard.shared_content['current'] = dict()
for i in range(epoch):
reset_env(env)
blackboard.shared_content['current']['epoch'] = i
# print(planner.gtable[(1,3)], planner.gtable[(2,3)])
for j in range(maxtracelen):
behaviour_tree.tick()
# print('Train', i,j, behaviour_tree.root.status)
# if behaviour_tree.root.status == Status.SUCCESS:
# break
if blackboard.shared_content['current']['done']:
break
child.planner.trace = dict()
# print('after trace empty', child.planner.trace)
# print(child.name, child.planner.gtable[(1,3)], child.planner.gtable[(2,3)],child.planner.gtable[(3,3)])
# Inference setup
child.train = False
# Inference loop
for i in range(1):
reset_env(env)
blackboard.shared_content['current']['epoch'] = i
# print(planner.gtable[(1,3)], planner.gtable[(2,3)])
for j in range(maxtracelen):
behaviour_tree.tick()
# print('inference', i,j, behaviour_tree.root.status)
if behaviour_tree.root.status == Status.SUCCESS:
break
if blackboard.shared_content['current']['done']:
break
print(env.curr_loc)
def __init__(
self, env, keys, goalspec, max_trace=40,
actions=[0, 1, 2, 3], epoch=10, seed=None, policy=None):
super().__init__(
env, keys, goalspec, dict(), max_trace, actions, epoch, seed)
self.tcount = 0
self.env = env
self.keys = keys
self.goalspec = goalspec
self.epoch = epoch
self.max_trace_len = max_trace
self.actions = actions
self.seed = seed
self.policy = policy
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
def create_blackboard():
"""
Fill with as many different types as we need to get full coverage on
pretty printing blackboard tests.
"""
blackboard = Blackboard()
blackboard.foo = "bar"
blackboard.some_tuple = (1, "bar")
blackboard.foobar = FooBar()
blackboard.nothing = None
return blackboard
def __init__(
self, env, keys, goalspec, gtable=dict(), max_trace=40,
actions=[0, 1, 2, 3], epoch=10, seed=None, allkeys=None,
actionu=0.90, id=None):
super().__init__(
env, keys, goalspec, gtable, max_trace, actions, epoch, seed)
self.tcount = 0
self.door_history = 0
if allkeys is None:
self.allkeys = keys
else:
self.allkeys = allkeys
# Initialize blackboard to store data
self.blackboard = Blackboard()
# Action uncertainty
self.prob = actionu
# Id to reference the blackboard
self.id = self.goalspec if id is None else id
def loop_over_plugins(self):
try:
# self.init_plugins()
while self.is_running() and not rospy.is_shutdown():
for plugin_name, plugin in self._plugins.items():
with self.status_lock:
if not self.is_running():
return
for node in plugin.tick():
status = node.status
if status is not None:
self.set_status(status)
assert self.my_status is not None, u'{} did not return a status'.format(plugin_name)
if not self.is_running():
return
self.looped_once = True
except Exception as e:
traceback.print_exc()
# TODO make 'exception' string a parameter somewhere
Blackboard().set('exception', e)
def __init__(self, name, model):
super().__init__(name, model)
self.location = ()
self.direction = model.random.rand() * (2 * np.pi)
self.speed = 2
self.radius = 3
# self.exchange_time = model.random.randint(2, 4)
# This doesn't help. Maybe only perform genetic operations when
# an agents meet 10% of its total population
# """
self.operation_threshold = 2
self.genome_storage = []
# Define a BTContruct object
self.bt = BTConstruct(None, self)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
self.shared_content = dict()
# Grammatical Evolution part
from ponyge.algorithm.parameters import Parameters
parameter = Parameters()
parameter_list = ['--parameters', 'swarm.txt']
# Comment when different results is desired.
# Else set this for testing purpose
parameter.params['RANDOM_SEED'] = name
# np.random.randint(1, 99999999)
parameter.params['POPULATION_SIZE'] = self.operation_threshold // 2
parameter.set_params(parameter_list)
self.parameter = parameter
individual = initialisation(self.parameter, 1)
individual = evaluate_fitness(individual, self.parameter)
self.individual = individual
self.bt.xmlstring = self.individual[0].phenotype
self.bt.construct()
def __init__(self, name, model):
super().__init__(name, model)
self.location = ()
self.direction = model.random.rand() * (2 * np.pi)
self.speed = 2
self.radius = 3
self.operation_threshold = 2
self.genome_storage = []
# Define a BTContruct object
self.bt = BTConstruct(None, self)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
# Grammatical Evolution part
from ponyge.algorithm.parameters import Parameters
parameter = Parameters()
parameter_list = ['--parameters', 'swarm.txt']
parameter.params['POPULATION_SIZE'] = self.operation_threshold // 2
parameter.params['RANDOM_SEED'] = model.seed
parameter.set_params(parameter_list)
self.parameter = parameter
individual = initialisation(self.parameter, 1)
self.individual = individual
self.bt.xmlstring = self.individual[0].phenotype
self.bt.construct()
self.output = py_trees.display.ascii_tree(self.bt.behaviour_tree.root)
# Location history
self.location_history = set()
self.timestamp = 0
def __init__(self, *a, **kw):
self.robot = None
# self.dictDraw = {'a':self.drawA}
self.dict_planner = {
'detect_cube': self.detect_cube,
'find_cube': self.find_cube,
'carry_cube': self.carry_cube,
'find_charger': self.find_charger,
'move_to_charger': self.move_to_charger,
'drop_cube': self.drop_cube
}
self.blackboard = Blackboard()
# self.blackboard.shared_content = dict()
self.goal = kw['goal']
self.tkeys = kw['tkeys']
normal = kw['normal']
self.states = dict(
zip(self.tkeys, [list() for i in range(len(self.tkeys))]))
if normal:
cozmo.connect(self.runall)
else:
cozmo.connect(self.run)
def get_as(self):
return Blackboard().get(u'~command')
def get_god_map(self):
"""
:rtype: giskardpy.god_map.GodMap
"""
return Blackboard().god_map
def reset_env(robot):
blackboard = Blackboard()
blackboard.shared_content['robot'] = robot
def __init__(self, name):
"""Init method for the MoveAway behavior."""
super(MoveAway, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
def __init__(self, name):
"""Init method for the SendSignal behavior."""
super(CompositePickCue, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
def __init__(self, name):
"""Init method for the Explore behavior."""
super(Explore, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
def __init__(self, name):
"""Init method for the CompositeDrop behavior."""
super(CompositeDropPartial, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()