def receive_incoming_job(self, msg: std_msgs.Empty):
"""
Incoming job callback.
Args:
msg: incoming goal message
Raises:
Exception: be ready to catch if any of the behaviours raise an exception
"""
if self.busy():
self.node.get_logger().warning(
"rejecting new job, last job is still active")
else:
scan_subtree = tutorial_create_scan_subtree()
try:
py_trees.trees.setup(root=scan_subtree, node=self.node)
except Exception as e:
console.logerror(
console.red +
"failed to setup the scan subtree, aborting [{}]".format(
str(e)) + console.reset)
sys.exit(1)
self.insert_subtree(scan_subtree, self.priorities.id, 1)
self.node.get_logger().info("inserted job subtree")
def tutorial_main():
"""
Entry point for the demo script.
"""
rclpy.init(args=None)
tree = DynamicApplicationTree()
try:
tree.setup(timeout=15)
except py_trees_ros.exceptions.TimedOutError as e:
console.logerror(
console.red +
"failed to setup the tree, aborting [{}]".format(str(e)) +
console.reset)
tree.shutdown()
rclpy.shutdown()
sys.exit(1)
except KeyboardInterrupt:
# not a warning, nor error, usually a user-initiated shutdown
console.logerror("tree setup interrupted")
tree.shutdown()
rclpy.shutdown()
sys.exit(1)
tree.tick_tock(period_ms=1000.0)
try:
rclpy.spin(tree.node)
except KeyboardInterrupt:
pass
tree.shutdown()
rclpy.shutdown()
def main():
"""
Entry point for the latched echo script.
"""
parser = command_line_argument_parser()
args = parser.parse_args()
rclpy.init() # picks up sys.argv automagically internally
node = rclpy.create_node("latched_echo" + "_" + str(os.getpid()))
# timeout_reached = False
#
# def timer_callback():
# nonlocal timeout_reached
# timeout_reached = True
# timer = node.create_timer(0.5, timer_callback)
# while not timeout_reached:
# rclpy.spin_once(node)
# node.destroy_timer(timer)
result = 0
try:
unused_subscriber = create_subscription(node, args.latched, args.topic_name, args.message_type, echo)
rclpy.spin(node)
except RuntimeError as e:
console.logerror(str(e))
node.destroy_node()
result = 1
except KeyboardInterrupt:
pass
node.destroy_node()
sys.exit(result)
def main():
"""
Entry point for the demo script.
"""
rospy.init_node("tree")
splintered_reality = SplinteredReality()
rospy.on_shutdown(splintered_reality.shutdown)
if not splintered_reality.setup():
console.logerror("failed to setup the tree, aborting.")
sys.exit(1)
splintered_reality.tick_tock()
def main():
"""
Entry point for the main script.
"""
rospy.init_node("tree")
root = create_root()
behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
rospy.on_shutdown(functools.partial(shutdown, behaviour_tree))
if not behaviour_tree.setup(timeout=15):
console.logerror("failed to setup the tree, aborting.")
sys.exit(1)
behaviour_tree.tick_tock(500)
def main():
"""
Entry point.
"""
args = command_line_argument_parser().parse_args()
args.enum_level = py_trees.common.string_to_visibility_level(args.level)
(module_or_class_name, method_name) = args.method.rsplit(".", 1)
class_name = None
try:
module_itself = importlib.import_module(module_or_class_name)
module_name = module_or_class_name
except ImportError:
# maybe it's a class?
(module_name, class_name) = module_or_class_name.rsplit(".", 1)
try:
module_itself = importlib.import_module(module_name)
except ImportError:
console.logerror(
"Could not import module [{0}]".format(module_or_class_name))
sys.exit(1)
if class_name is not None:
class_type = getattr(module_itself, class_name)
# first guess - it's a static method
method_itself = getattr(class_type, method_name)
try:
root = method_itself(**(args.kwargs))
except TypeError: # oops, it's an instance method
try:
method_itself = getattr(class_type(), method_name)
except TypeError:
console.logerror(
"Can only instantiate class methods if the class __init__ has no non-default arguments"
)
sys.exit(1)
root = method_itself(**(args.kwargs))
else:
method_itself = getattr(module_itself, method_name)
root = method_itself(**(args.kwargs))
# TODO figure out how to insert keyword arguments
py_trees.display.render_dot_tree(
root,
args.enum_level,
args.name,
with_blackboard_variables=args.with_blackboard_variables,
with_qualified_names=args.verbose)
def main():
"""
Entry point for the demo script.
"""
rclpy.init(args=None)
root = create_root()
behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
# TODO: rospy.on_shutdown(functools.partial(shutdown, behaviour_tree))
if not behaviour_tree.setup(timeout=15):
console.logerror("failed to setup the tree, aborting.")
sys.exit(1)
try:
rclpy.spin(behaviour_tree.node)
# TODO: behaviour_tree.tick_tock(500)
except KeyboardInterrupt:
pass
def main():
"""
Entry point.
"""
args = command_line_argument_parser().parse_args()
args.enum_level = py_trees.common.string_to_visibility_level(args.level)
(module_name, method_name) = args.method.rsplit(".", 1)
try:
module_itself = importlib.import_module(module_name)
except ImportError:
console.logerror("Could not import module [{0}]".format(module_name))
sys.exit(1)
method_itself = getattr(module_itself, method_name)
# TODO figure out how to insert keyword arguments
root = method_itself(**(args.kwargs))
py_trees.display.render_dot_tree(root, args.enum_level, args.name)
def main(args=None):
rclpy.init(args=args)
robot = Robot()
root = create_tree(robot)
tree = py_trees_ros.trees.BehaviourTree(root=root,
unicode_tree_debug=False)
try:
tree.setup(timeout=15.0)
except py_trees_ros.exceptions.TimedOutError as e:
console.logerror(
console.red +
"failed to setup the tree, aborting [{}]".format(str(e)) +
console.reset)
tree.shutdown()
rclpy.shutdown()
sys.exit(1)
except KeyboardInterrupt:
# not a warning, nor error, usually a user-initiated shutdown
console.logerror("tree setup interrupted")
tree.shutdown()
rclpy.shutdown()
sys.exit(1)
tree.tick_tock(period_ms=100)
executor = SingleThreadedExecutor()
executor.add_node(robot)
executor.add_node(tree.node)
try:
executor.spin()
except KeyboardInterrupt:
pass
executor.shutdown()
robot.destroy_node()
tree.node.destroy_node()
rclpy.shutdown()
def main():
"""
Entry point for the demo script.
"""
args = command_line_argument_parser().parse_args()
args.enum_level = py_trees.common.string_to_visibility_level(args.level)
print(description())
py_trees.logging.level = py_trees.logging.Level.DEBUG
root = create_tree(args.level)
py_trees.display.render_dot_tree(root, args.enum_level)
if py_trees.utilities.which("xdot"):
try:
subprocess.call(["xdot", "demo_dot_graphs_%s.dot" % args.level])
except KeyboardInterrupt:
pass
else:
print("")
console.logerror(
"No xdot viewer found, skipping display [hint: sudo apt install xdot]"
)
print("")
def callback_snapshot(self, msg):
"""
Formats the string message coming in.
Args
msg (:class:`py_trees_ros_interfaces.msg.BehaviourTree`):serialised snapshot
"""
####################
# Processing
####################
self.snapshot_visitor.previously_visited = self.snapshot_visitor.visited
self.snapshot_visitor.visited = {}
serialised_behaviours = {}
root_id = None
for serialised_behaviour in msg.behaviours:
if serialised_behaviour.parent_id == unique_identifier_msgs.UUID():
root_id = conversions.msg_to_uuid4(serialised_behaviour.own_id)
serialised_behaviours[conversions.msg_to_uuid4(
serialised_behaviour.own_id)] = serialised_behaviour
def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
# invasive hack to revert the dummy child we added in msg_to_behaviour
if isinstance(behaviour, py_trees.decorators.Decorator):
behaviour.children = [child]
behaviour.decorated = behaviour.children[0]
else:
behaviour.children.append(child)
child.parent = behaviour
# set the current child so tip() works properly everywhere
if behaviour.children:
if msg.current_child_id != unique_identifier_msgs.UUID():
current_child_id = conversions.msg_to_uuid4(
msg.current_child_id)
for index, child in enumerate(behaviour.children):
if child.id == current_child_id:
# somewhat ugly not having a consistent api here
if isinstance(behaviour,
py_trees.composites.Selector):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Sequence):
behaviour.current_index = index
# else Parallel, nothing to do since it infers
# the current child from children's status on the fly
break
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
# we didn't set the tip in any behaviour, but nothing depends
# on that right now
root = deserialise_tree_recursively(serialised_behaviours[root_id])
####################
# Streaming
####################
if self.mode == WatcherMode.SNAPSHOTS:
if msg.changed:
colour = console.green
else:
colour = console.bold
####################
# Banner
####################
title = "Tick {}".format(msg.statistics.count)
print(colour + "\n" + 80 * "*" + console.reset)
print(colour + "* " + console.bold + title.center(80) +
console.reset)
print(colour + 80 * "*" + "\n" + console.reset)
####################
# Tree Snapshot
####################
print(
py_trees.display.unicode_tree(
root=root,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
print(colour + "-" * 80 + console.reset)
####################
# Stream Variables
####################
if self.parameters.blackboard_data:
print("")
print(colour + "Blackboard Data" + console.reset)
indent = " " * 4
if not msg.blackboard_on_visited_path:
print(console.cyan + indent + "-" + console.reset + " : " +
console.yellow + "-" + console.reset)
else:
# could probably re-use the unicode_blackboard by passing a dict to it
# like we've done for the activity stream
max_length = 0
for variable in msg.blackboard_on_visited_path:
max_length = len(variable.key) if len(
variable.key) > max_length else max_length
for variable in msg.blackboard_on_visited_path:
print(console.cyan + indent +
'{0: <{1}}'.format(variable.key, max_length +
1) + console.reset + ": " +
console.yellow + '{0}'.format(variable.value) +
console.reset)
####################
# Stream Activity
####################
if self.parameters.blackboard_activity:
print("")
print(colour + "Blackboard Activity Stream" + console.reset)
if msg.blackboard_activity:
print(
py_trees.display.unicode_blackboard_activity_stream(
msg.blackboard_activity,
indent=0,
show_title=False))
else:
indent = " " * 4
print(console.cyan + indent + "-" + console.reset + " : " +
console.yellow + "-" + console.reset)
####################
# Stream Statistics
####################
if self.statistics:
print("")
print(colour + "Statistics" + console.reset)
print(console.cyan + " Timestamp: " + console.yellow +
"{}".format(
conversions.rclpy_time_to_float(
rclpy.time.Time.from_msg(msg.statistics.stamp))))
print(console.cyan + " Duration : " + console.yellow +
"{:.3f}/{:.3f}/{:.3f} (ms) [time/avg/stddev]".format(
msg.statistics.tick_duration *
1000, msg.statistics.tick_duration_average * 1000,
math.sqrt(msg.statistics.tick_duration_variance) *
1000))
print(console.cyan + " Interval : " + console.yellow +
"{:.3f}/{:.3f}/{:.3f} (s) [time/avg/stddev]".format(
msg.statistics.tick_interval,
msg.statistics.tick_interval_average,
math.sqrt(msg.statistics.tick_interval_variance)))
print(console.reset)
####################
# Dot Graph
####################
elif self.mode == WatcherMode.DOT_GRAPH and not self.rendered:
self.rendered = True
directory_name = tempfile.mkdtemp()
py_trees.display.render_dot_tree(
root=root,
target_directory=directory_name,
with_blackboard_variables=self.parameters.blackboard_data)
xdot_program = py_trees.utilities.which('xdot')
if not xdot_program:
print("")
console.logerror(
"No xdot viewer found [hint: sudo apt install xdot]")
print("")
print(py_trees.display.dot_tree(root=root).to_string())
self.done = True
self.xdot_process = None
return
filename = py_trees.utilities.get_valid_filename(
root.name) + '.dot'
if xdot_program:
try:
self.xdot_process = subprocess.Popen(
[xdot_program,
os.path.join(directory_name, filename)])
except KeyboardInterrupt:
pass
self.done = True
##############################################################################
import argparse
import importlib
import py_trees.console as console
import rospy
import sys
##############################################################################
# Main
##############################################################################
if __name__ == '__main__':
# parser = argparse.ArgumentParser(description='Start of the rex-tree')
# parser.add_argument('name', action='store', nargs='?',
# default='rex_tree',
# choices=['rex_tree'],
# help='name of the tutorial to start')
# command_line_args = rospy.myargv(argv=sys.argv)[1:]
# args = parser.parse_args(command_line_args)
module_name = "sa_rex.main_script"
try:
module_itself = importlib.import_module(module_name)
except ImportError:
console.logerror(
"Could not import this module [{0}]".format("main_script"))
sys.exit(1)
main_itself = getattr(module_itself, "main")
main_itself()
[wait_elevation_down,
elevation_down_mention1,
elevation_down_10cm_action,
done_scene,
]
)
root.add_children([gripper_open_cmd, intro, find_target, arm_control, grasp_object, finish_demo, put_object, elevation_up, elevation_down])
# root.add_children([scene1, scene3, scene4, scene5, scene6, scene7])
return root
def shutdown(behaviour_tree):
behaviour_tree.interrupt()
if __name__ == '__main__':
rospy.init_node('app_introduction', anonymous=False)
#py_trees.logging.level = py_trees.logging.Level.DEBUG
print("starting..")
root = create_root()
behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
rospy.on_shutdown(functools.partial(shutdown, behaviour_tree))
if not behaviour_tree.setup(timeout=15):
console.logerror("failed to setup the tree, aborting.")
sys.exit(1)
print('initialize...')
behaviour_tree.tick_tock(500)
rospy.spin()
def callback_snapshot(self, msg):
"""
Formats the string message coming in.
Args
msg (:class:`py_trees_ros_interfaces.msg.BehaviourTree`):serialised snapshot
"""
####################
# Processing
####################
self.snapshot_visitor.previously_visited = self.snapshot_visitor.visited
self.snapshot_visitor.visited = {}
serialised_behaviours = {}
root_id = None
for serialised_behaviour in msg.behaviours:
if serialised_behaviour.parent_id == unique_identifier_msgs.UUID():
root_id = conversions.msg_to_uuid4(serialised_behaviour.own_id)
serialised_behaviours[conversions.msg_to_uuid4(
serialised_behaviour.own_id)] = serialised_behaviour
def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
child.parent = behaviour
behaviour.children.append(child)
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
# we didn't set the tip in any behaviour, but nothing depends
# on that right now
root = deserialise_tree_recursively(serialised_behaviours[root_id])
####################
# Streaming
####################
if self.viewing_mode == WatcherMode.ASCII_SNAPSHOT:
console.banner("Tick {}".format(msg.statistics.count))
print(
py_trees.display.ascii_tree(
root=root,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
print(console.green + "-" * 80 + "\n" + console.reset)
print("Timestamp: {}".format(
conversions.rclpy_time_to_float(
rclpy.time.Time.from_msg(msg.statistics.stamp))))
print("Duration : {:.3f}/{:.3f}/{:.3f} (ms) [time/avg/stddev]".
format(
msg.statistics.tick_duration * 1000,
msg.statistics.tick_duration_average * 1000,
math.sqrt(msg.statistics.tick_duration_variance) * 1000))
print(
"Interval : {:.3f}/{:.3f}/{:.3f} (s) [time/avg/stddev]".format(
msg.statistics.tick_interval,
msg.statistics.tick_interval_average,
math.sqrt(msg.statistics.tick_interval_variance)))
####################
# Printing
####################
elif self.viewing_mode == WatcherMode.ASCII_TREE:
print("")
print(
py_trees.display.ascii_tree(
root=root,
show_status=True,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
self.done = True
####################
# Dot Graph
####################
elif self.viewing_mode == WatcherMode.DOT_TREE and not self.rendered:
self.rendered = True
directory_name = tempfile.mkdtemp()
py_trees.display.render_dot_tree(root=root,
target_directory=directory_name)
xdot_program = py_trees.utilities.which('xdot')
if not xdot_program:
console.logerror(
"No xdot viewer found, skipping display [hint: sudo apt install xdot]"
)
print(py_trees.display.dot_graph(root=root).to_string())
self.done = True
self.xdot_process = 1
return
filename = py_trees.utilities.get_valid_filename(
root.name) + '.dot'
if xdot_program:
try:
self.xdot_process = subprocess.Popen(
[xdot_program,
os.path.join(directory_name, filename)])
except KeyboardInterrupt:
pass
self.done = True
def callback_snapshot(self, msg):
"""
Formats the string message coming in.
Args
msg (:class:`py_trees_ros_interfaces.msg.BehaviourTree`):serialised snapshot
"""
####################
# Processing
####################
self.snapshot_visitor.previously_visited = self.snapshot_visitor.visited
self.snapshot_visitor.visited = {}
serialised_behaviours = {}
root_id = None
for serialised_behaviour in msg.behaviours:
if serialised_behaviour.parent_id == unique_identifier_msgs.UUID():
root_id = conversions.msg_to_uuid4(serialised_behaviour.own_id)
serialised_behaviours[conversions.msg_to_uuid4(
serialised_behaviour.own_id)] = serialised_behaviour
def deserialise_tree_recursively(msg):
behaviour = conversions.msg_to_behaviour(msg)
for serialised_child_id in msg.child_ids:
child_id = conversions.msg_to_uuid4(serialised_child_id)
child = deserialise_tree_recursively(
serialised_behaviours[child_id])
# invasive hack to revert the dummy child we added in msg_to_behaviour
if isinstance(behaviour, py_trees.decorators.Decorator):
behaviour.children = [child]
behaviour.decorated = behaviour.children[0]
else:
behaviour.children.append(child)
child.parent = behaviour
# set the current child so tip() works properly everywhere
if behaviour.children:
if msg.current_child_id != unique_identifier_msgs.UUID():
current_child_id = conversions.msg_to_uuid4(
msg.current_child_id)
for index, child in enumerate(behaviour.children):
if child.id == current_child_id:
# somewhat ugly not having a consistent api here
if isinstance(behaviour,
py_trees.composites.Selector):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Chooser):
behaviour.current_child = child
elif isinstance(behaviour,
py_trees.composites.Sequence):
behaviour.current_index = index
# else Parallel, nothing to do since it infers
# the current child from children's status on the fly
break
if msg.is_active:
self.snapshot_visitor.visited[behaviour.id] = behaviour.status
return behaviour
# we didn't set the tip in any behaviour, but nothing depends
# on that right now
root = deserialise_tree_recursively(serialised_behaviours[root_id])
####################
# Streaming
####################
if self.viewing_mode == WatcherMode.STREAM:
console.banner("Tick {}".format(msg.statistics.count))
print(
py_trees.display.unicode_tree(
root=root,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
print(console.green + "-" * 80 + "\n" + console.reset)
print("Timestamp: {}".format(
conversions.rclpy_time_to_float(
rclpy.time.Time.from_msg(msg.statistics.stamp))))
print("Duration : {:.3f}/{:.3f}/{:.3f} (ms) [time/avg/stddev]".
format(
msg.statistics.tick_duration * 1000,
msg.statistics.tick_duration_average * 1000,
math.sqrt(msg.statistics.tick_duration_variance) * 1000))
print(
"Interval : {:.3f}/{:.3f}/{:.3f} (s) [time/avg/stddev]".format(
msg.statistics.tick_interval,
msg.statistics.tick_interval_average,
math.sqrt(msg.statistics.tick_interval_variance)))
####################
# Printing
####################
elif self.viewing_mode == WatcherMode.SNAPSHOT:
print("")
print(
py_trees.display.unicode_tree(
root=root,
show_status=True,
visited=self.snapshot_visitor.visited,
previously_visited=self.snapshot_visitor.previously_visited
))
self.done = True
####################
# Dot Graph
####################
elif self.viewing_mode == WatcherMode.DOT_GRAPH and not self.rendered:
self.rendered = True
directory_name = tempfile.mkdtemp()
py_trees.display.render_dot_tree(root=root,
target_directory=directory_name)
xdot_program = py_trees.utilities.which('xdot')
if not xdot_program:
print("")
console.logerror(
"No xdot viewer found [hint: sudo apt install xdot]")
print("")
print(py_trees.display.dot_tree(root=root).to_string())
self.done = True
self.xdot_process = None
return
filename = py_trees.utilities.get_valid_filename(
root.name) + '.dot'
if xdot_program:
try:
self.xdot_process = subprocess.Popen(
[xdot_program,
os.path.join(directory_name, filename)])
except KeyboardInterrupt:
pass
self.done = True
