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)
class UntilNode(Behaviour):
"""Until Sub-tree.
Until sub-tree is eqivalent to the Until LTL operator.
"""
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 setup(self, timeout):
"""Have defined the setup method.
This method defines the other objects required for the
behavior. Agent is the actor in the environment,
item is the name of the item we are trying to find in the
environment and timeout defines the execution time for the
behavior.
"""
pass
def initialise(self):
"""Everytime initialization. Not required for now."""
pass
def update(self):
"""Just call the tick method for the sequence.
This will execute the primitive behaviors defined in the sequence
"""
self.bt.tick()
return self.bt.root.status
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
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 grow_tree():
action_server_name = u'giskardpy/command'
god_map = initialize_god_map()
# ----------------------------------------------
wait_for_goal = Sequence(u'wait for goal')
wait_for_goal.add_child(TFPlugin(u'tf'))
wait_for_goal.add_child(ConfigurationPlugin(u'js'))
wait_for_goal.add_child(WorldUpdatePlugin(u'pybullet updater'))
wait_for_goal.add_child(GoalReceived(u'has goal', action_server_name, MoveAction))
wait_for_goal.add_child(ConfigurationPlugin(u'js'))
# ----------------------------------------------
planning_3 = PluginBehavior(u'planning III', sleep=0)
planning_3.add_plugin(CollisionChecker(u'coll'))
planning_3.add_plugin(ControllerPlugin(u'controller'))
planning_3.add_plugin(KinSimPlugin(u'kin sim'))
planning_3.add_plugin(LogTrajPlugin(u'log'))
planning_3.add_plugin(GoalReachedPlugin(u'goal reached'))
planning_3.add_plugin(WiggleCancel(u'wiggle'))
planning_3.add_plugin(TimePlugin(u'time'))
# ----------------------------------------------
publish_result = failure_is_success(Selector)(u'monitor execution')
publish_result.add_child(GoalCanceled(u'goal canceled', action_server_name))
publish_result.add_child(SendTrajectory(u'send traj'))
# ----------------------------------------------
# ----------------------------------------------
planning_2 = failure_is_success(Selector)(u'planning II')
planning_2.add_child(GoalCanceled(u'goal canceled', action_server_name))
# planning.add_child(CollisionCancel(u'in collision', collision_time_threshold))
if god_map.safe_get_data(identifier.marker_visualization):
planning_2.add_child(success_is_failure(VisualizationBehavior)(u'visualization'))
if god_map.safe_get_data(identifier.enable_collision_marker):
planning_2.add_child(success_is_failure(CPIMarker)(u'cpi marker'))
planning_2.add_child(planning_3)
# ----------------------------------------------
move_robot = failure_is_success(Sequence)(u'move robot')
move_robot.add_child(IF(u'execute?', identifier.execute))
move_robot.add_child(publish_result)
# ----------------------------------------------
# ----------------------------------------------
planning_1 = success_is_failure(Sequence)(u'planning I')
planning_1.add_child(GoalToConstraints(u'update constraints', action_server_name))
planning_1.add_child(planning_2)
# ----------------------------------------------
# ----------------------------------------------
process_move_goal = failure_is_success(Selector)(u'process move goal')
process_move_goal.add_child(planning_1)
process_move_goal.add_child(SetCmd(u'set move goal', action_server_name))
# ----------------------------------------------
# ----------------------------------------------
root = Sequence(u'root')
root.add_child(wait_for_goal)
root.add_child(CleanUp(u'cleanup'))
root.add_child(process_move_goal)
if god_map.safe_get_data(identifier.plot_trajectory):
root.add_child(PlotTrajectory(u'plot trajectory'))
root.add_child(move_robot)
root.add_child(SendResult(u'send result', action_server_name, MoveAction))
tree = BehaviourTree(root)
if god_map.safe_get_data(identifier.debug):
def post_tick(snapshot_visitor, behaviour_tree):
logging.logdebug(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = py_trees_ros.visitors.SnapshotVisitor()
tree.add_post_tick_handler(functools.partial(post_tick, snapshot_visitor))
tree.visitors.append(snapshot_visitor)
path = god_map.safe_get_data(identifier.data_folder) + u'tree'
create_path(path)
render_dot_tree(root, name=path)
tree.setup(30)
return tree
def grow_tree():
action_server_name = u'~command'
god_map = initialize_god_map()
# ----------------------------------------------
wait_for_goal = Sequence(u'wait for goal')
wait_for_goal.add_child(TFPlugin(u'tf'))
wait_for_goal.add_child(ConfigurationPlugin(u'js1'))
wait_for_goal.add_child(WorldUpdatePlugin(u'pybullet updater'))
wait_for_goal.add_child(GoalReceived(u'has goal', action_server_name, MoveAction))
wait_for_goal.add_child(ConfigurationPlugin(u'js2'))
# ----------------------------------------------
planning_3 = PluginBehavior(u'planning III', sleep=0)
planning_3.add_plugin(CollisionChecker(u'coll'))
# if god_map.safe_get_data(identifier.enable_collision_marker):
# planning_3.add_plugin(success_is_running(CPIMarker)(u'cpi marker'))
planning_3.add_plugin(ControllerPlugin(u'controller'))
planning_3.add_plugin(KinSimPlugin(u'kin sim'))
planning_3.add_plugin(LogTrajPlugin(u'log'))
planning_3.add_plugin(WiggleCancel(u'wiggle'))
planning_3.add_plugin(LoopDetector(u'loop detector'))
planning_3.add_plugin(GoalReachedPlugin(u'goal reached'))
planning_3.add_plugin(TimePlugin(u'time'))
# planning_3.add_plugin(MaxTrajLength(u'traj length check'))
# ----------------------------------------------
publish_result = failure_is_success(Selector)(u'monitor execution')
publish_result.add_child(GoalCanceled(u'goal canceled', action_server_name))
publish_result.add_child(SendTrajectory(u'send traj'))
# ----------------------------------------------
# ----------------------------------------------
planning_2 = failure_is_success(Selector)(u'planning II')
planning_2.add_child(GoalCanceled(u'goal canceled', action_server_name))
if god_map.get_data(identifier.enable_VisualizationBehavior):
planning_2.add_child(success_is_failure(VisualizationBehavior)(u'visualization'))
if god_map.get_data(identifier.enable_WorldVisualizationBehavior):
planning_2.add_child(success_is_failure(WorldVisualizationBehavior)(u'world_visualization'))
if god_map.get_data(identifier.enable_CPIMarker):
planning_2.add_child(success_is_failure(CollisionMarker)(u'cpi marker'))
planning_2.add_child(planning_3)
# ----------------------------------------------
move_robot = failure_is_success(Sequence)(u'move robot')
move_robot.add_child(IF(u'execute?', identifier.execute))
move_robot.add_child(publish_result)
# ----------------------------------------------
# ----------------------------------------------
planning_1 = Sequence(u'planning I')
planning_1.add_child(GoalToConstraints(u'update constraints', action_server_name))
planning_1.add_child(planning_2)
planning_1.add_child(running_is_success(TimePlugin)(u'time for zero velocity'))
planning_1.add_child(AppendZeroVelocity(u'append zero velocity'))
planning_1.add_child(running_is_success(LogTrajPlugin)(u'log zero velocity'))
# planning_1.add_child(running_is_success(TimePlugin)(u'time for zero velocity'))
# planning_1.add_child(AppendZeroVelocity(u'append zero velocity'))
# planning_1.add_child(running_is_success(LogTrajPlugin)(u'log zero velocity'))
if god_map.get_data(identifier.enable_VisualizationBehavior):
planning_1.add_child(VisualizationBehavior(u'visualization', ensure_publish=True))
if god_map.get_data(identifier.enable_WorldVisualizationBehavior):
planning_1.add_child(WorldVisualizationBehavior(u'world_visualization', ensure_publish=True))
if god_map.get_data(identifier.enable_CPIMarker):
planning_1.add_child(CollisionMarker(u'cpi marker'))
# ----------------------------------------------
post_processing = failure_is_success(Sequence)(u'post planning')
# post_processing.add_child(WiggleCancel(u'final wiggle detection', final_detection=True))
if god_map.get_data(identifier.enable_PlotTrajectory):
post_processing.add_child(PlotTrajectory(u'plot trajectory', order=3))
post_processing.add_child(PostProcessing(u'evaluate result'))
# post_processing.add_child(PostProcessing(u'check reachability'))
# ----------------------------------------------
planning = success_is_failure(Sequence)(u'planning')
planning.add_child(IF(u'goal_set?', identifier.next_move_goal))
planning.add_child(planning_1)
planning.add_child(post_processing)
process_move_goal = failure_is_success(Selector)(u'process move goal')
process_move_goal.add_child(planning)
# process_move_goal.add_child(planning_1)
# process_move_goal.add_child(post_processing)
process_move_goal.add_child(SetCmd(u'set move goal', action_server_name))
# ----------------------------------------------
#
# post_processing = failure_is_success(Sequence)(u'post processing')
# post_processing.add_child(PostProcessing(u'post_processing'))
# ----------------------------------------------
# ----------------------------------------------
root = Sequence(u'root')
root.add_child(wait_for_goal)
root.add_child(CleanUp(u'cleanup'))
root.add_child(process_move_goal)
root.add_child(move_robot)
root.add_child(SendResult(u'send result', action_server_name, MoveAction))
tree = BehaviourTree(root)
if god_map.get_data(identifier.debug):
def post_tick(snapshot_visitor, behaviour_tree):
logging.logdebug(u'\n' + py_trees.display.ascii_tree(behaviour_tree.root,
snapshot_information=snapshot_visitor))
snapshot_visitor = py_trees_ros.visitors.SnapshotVisitor()
tree.add_post_tick_handler(functools.partial(post_tick, snapshot_visitor))
tree.visitors.append(snapshot_visitor)
path = god_map.get_data(identifier.data_folder) + u'tree'
create_path(path)
render_dot_tree(root, name=path)
tree.setup(30)
tree_m = TreeManager(tree)
god_map.set_data(identifier.tree_manager, tree_m)
return tree