Skip to content

Commit

Permalink
[trees] periodically publish snapshots (#131)
Browse files Browse the repository at this point in the history 
* [trees] periodically publish snapshots
* [trees] topic name simplification, exchange->blackboard
  • Loading branch information
@stonier
stonier committed Nov 27, 2019
1 parent c1d8649 commit 3c47e6e
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 53 deletions.
33 changes: 17 additions & 16 deletions py_trees_ros/blackboard.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,13 +215,13 @@ class Exchange(object):
* streams the (sub)blackboard over a blackboard watcher connection
ROS Services:
* **~/get_blackboard_variables** (:class:`py_trees_msgs.srv.GetBlackboardVariables`)
* **~/get_variables** (:class:`py_trees_msgs.srv.GetBlackboardVariables`)
* list all the blackboard variable names (not values)
* **~/open_blackboard_watcher** (:class:`py_trees_msgs.srv.OpenBlackboardWatcher`)
* **~/open_watcher** (:class:`py_trees_msgs.srv.OpenBlackboardWatcher`)
* request a publisher to stream a part of the blackboard contents
* **~/close_blackboard_watcher** (:class:`py_trees_msgs.srv.CloseBlackboardWatcher`)
* **~/close_watcher** (:class:`py_trees_msgs.srv.CloseBlackboardWatcher`)
* close a previously opened watcher
Expand All @@ -235,7 +235,7 @@ class Exchange(object):
.. seealso::
:class:`~py_trees_ros.trees.BehaviourTree` (in which it is used) and
:class:`py_trees_ros.trees.BehaviourTree` (in which it is used) and
:ref:`py-trees-blackboard-watcher` (working with the watchers).
"""
_counter = 0
Expand Down Expand Up @@ -278,14 +278,15 @@ def setup(self, timeout):
.. seealso:: This class is used as illustrated above in :class:`~py_trees_ros.trees.BehaviourTree`.
"""
self.node = node
for name in ["get_blackboard_variables",
"open_blackboard_watcher",
"close_blackboard_watcher"]:
camel_case_name = ''.join(x.capitalize() for x in name.split('_'))
self.services[name] = self.node.create_service(
srv_type=getattr(py_trees_srvs, camel_case_name),
srv_name='~/exchange/' + name,
callback=getattr(self, "_{}_service".format(name)),
for service_name, service_type in [
("get_variables", py_trees_srvs.GetBlackboardVariables),
("open_watcher", py_trees_srvs.OpenBlackboardWatcher),
("close_watcher", py_trees_srvs.CloseBlackboardWatcher)
]:
self.services[service_name] = self.node.create_service(
srv_type=service_type,
srv_name='~/blackboard/' + service_name,
callback=getattr(self, "_{}_service".format(service_name)),
qos_profile=rclpy.qos.qos_profile_services_default
)

Expand Down Expand Up @@ -340,7 +341,7 @@ def post_tick_handler(self, visited_client_ids: typing.List[uuid.UUID]=None):
if py_trees.blackboard.Blackboard.activity_stream is not None:
py_trees.blackboard.Blackboard.activity_stream.clear()

def _close_blackboard_watcher_service(self, request, response):
def _close_watcher_service(self, request, response):
response.result = False
for view in self.views:
# print(" DJS: close watcher? [%s][%s]" % (view.topic_name, request.topic_name))
Expand All @@ -356,13 +357,13 @@ def _close_blackboard_watcher_service(self, request, response):
py_trees.blackboard.Blackboard.disable_activity_stream()
return response

def _get_blackboard_variables_service(self, unused_request, response):
def _get_variables_service(self, unused_request, response):
response.variables = self._get_nested_keys()
return response

def _open_blackboard_watcher_service(self, request, response):
def _open_watcher_service(self, request, response):
response.topic = rclpy.expand_topic_name.expand_topic_name(
topic_name="~/exchange/_watcher_" + str(Exchange._counter),
topic_name="~/blackboard/_watcher_" + str(Exchange._counter),
node_name=self.node.get_name(),
node_namespace=self.node.get_namespace())
Exchange._counter += 1
Expand Down
125 changes: 88 additions & 37 deletions py_trees_ros/trees.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,27 +58,33 @@ class BehaviourTree(py_trees.trees.BehaviourTree):
Extend the :class:`py_trees.trees.BehaviourTree` class with
a few bells and whistles for ROS:
* ros publishers that serialises a snapshot of the tree for viewing/logging
* a blackboard exchange with introspection and watcher services
ROS Parameters:
* **setup_timeout**: time (s) to wait (default: :data:`math.inf`)
* if :data:`math.inf`, it will block indefinitely
* **snapshot_period**: time (s) between snapshots (default: 2.0s)
* if :data:`math.inf` it will only publish on tree status / graph changes
ROS Publishers:
* **~/snapshots** (:class:`py_trees_msgs.msg.BehaviourTree`)
* **~/snapshots** (:class:`py_trees_interfaces.msg.BehaviourTree`)
* **~/blackboard/close_watcher** (:class:`py_trees_ros_interfaces.srv.CloselackboardWatcher`)
* **~/blackboard/get_variables** (:class:`py_trees_ros_interfaces.srv.GetBlackboardVariables`)
* **~/blackboard/open_watcher** (:class:`py_trees_ros_interfaces.srv.OpenBlackboardWatcher`)
.. seealso::
It also exposes publishers and services from the blackboard exchange
in it's private namespace. Refer to :class:`~py_trees_ros.blackboard.Exchange` for details.
Topics and services are not intended for direct use, but facilitate the operation of the
utilities :ref:`py-trees-tree-watcher` and :ref:`py-trees-blackboard-watcher`.
Args:
root (:class:`~py_trees.behaviour.Behaviour`): root node of the tree
unicode_tree_debug (:obj:`bool`, optional): print to console the visited ascii tree after every tick
root: root node of the tree
unicode_tree_debug: print to console the visited ascii tree after every tick
Raises:
AssertionError: if incoming root variable is not the correct type
"""
def __init__(self,
root,
unicode_tree_debug=False):
root: py_trees.behaviour.Behaviour,
unicode_tree_debug: bool=False):
super(BehaviourTree, self).__init__(root)
if unicode_tree_debug:
self.snapshot_visitor = py_trees.visitors.DisplaySnapshotVisitor()
Expand Down Expand Up @@ -123,6 +129,7 @@ def __init__(self,

def setup(
self,
period: float=2.0,
timeout: float=py_trees.common.Duration.INFINITE,
visitor: py_trees.visitors.VisitorBase=None
):
Expand All @@ -131,13 +138,16 @@ def setup(
Ultimately relays this call down to all the behaviours in the tree.
Args:
period: time (s) between snapshots (use common.Duration.INFINITE to *only* publish on tree status/graph change)
timeout: time (s) to wait (use common.Duration.INFINITE to block indefinitely)
visitor: runnable entities on each node after it's setup
ROS Params:
timeout: time (s) to wait (use common.Duration.INFINITE (math.inf) to block indefinitely)
.. note:
.. note: The timeout parameter takes precedence. If not set, the timeout arg will provide the initial value.
This method declares parameters for the snapshot_period and setup_timeout.
These parameters take precedence over the period and timeout args provided here.
If parameters are not configured at runtime, then the period and timeout args
provided here will inialise the declared parameters.
Raises:
rclpy.exceptions.NotInitializedException: rclpy not yet initialised
Expand All @@ -148,14 +158,35 @@ def setup(
self.node = rclpy.create_node(node_name=default_node_name)
if visitor is None:
visitor = visitors.SetupLogger(node=self.node)
# timeout parameter:
# if not initialised from, e.g. launch, then
# use the arg provided timeout
########################################
# Parameters - snapshot_preiod
########################################
self.node.declare_parameter(
name='setup_timeout_sec',
name='snapshot_period',
value=period if period != py_trees.common.Duration.INFINITE else py_trees.common.Duration.INFINITE.value,
descriptor=rcl_interfaces_msgs.ParameterDescriptor(
name="snapshot_period",
type=rcl_interfaces_msgs.ParameterType.PARAMETER_DOUBLE, # noqa
description="time between snapshots",
additional_constraints="",
read_only=True,
floating_point_range=[rcl_interfaces_msgs.FloatingPointRange(
from_value=0.0,
to_value=py_trees.common.Duration.INFINITE.value)]
)
)
# Get the resulting timeout
self.snapshot_period = self.node.get_parameter("snapshot_period").value
self.last_snapshot_timestamp = time.monotonic()

########################################
# Parameters - setup_timeout
########################################
self.node.declare_parameter(
name='setup_timeout',
value=timeout if timeout != py_trees.common.Duration.INFINITE else py_trees.common.Duration.INFINITE.value,
descriptor=rcl_interfaces_msgs.ParameterDescriptor(
name="setup_timeout_sec",
name="setup_timeout",
type=rcl_interfaces_msgs.ParameterType.PARAMETER_DOUBLE, # noqa
description="timeout for ROS tree setup (node, pubs, subs, ...)",
additional_constraints="",
Expand All @@ -166,23 +197,28 @@ def setup(
)
)
# Get the resulting timeout
setup_timeout_sec = self.node.get_parameter("setup_timeout_sec").value
setup_timeout = self.node.get_parameter("setup_timeout").value
# Ugly workaround to accomodate use of the enum (TODO: rewind this)
# Need to pass the enum for now (instead of just a float) in case
# there are behaviours out in the wild that apply logic around the
# use of the enum
if setup_timeout_sec == py_trees.common.Duration.INFINITE.value:
setup_timeout_sec = py_trees.common.Duration.INFINITE
if setup_timeout == py_trees.common.Duration.INFINITE.value:
setup_timeout = py_trees.common.Duration.INFINITE

########################################
# ROS Comms
########################################
self._setup_publishers()
self.blackboard_exchange = blackboard.Exchange()
self.blackboard_exchange.setup(self.node)
self.post_tick_handlers.append(self._on_change_post_tick_handler)
self.post_tick_handlers.append(self._snapshots_post_tick_handler)

# share the tree's node with it's behaviours
########################################
# Behaviours
########################################
try:
super().setup(
timeout=setup_timeout_sec,
timeout=setup_timeout,
visitor=visitor,
node=self.node
)
Expand Down Expand Up @@ -347,7 +383,7 @@ def _statistics_post_tick_handler(self, tree: py_trees.trees.BehaviourTree):
else:
self.statistics.tick_duration_variance = 0.0

def _on_change_post_tick_handler(self, tree: py_trees.trees.BehaviourTree):
def _snapshots_post_tick_handler(self, tree: py_trees.trees.BehaviourTree):
"""
Post-tick handler that checks for changes in the tree/blackboard as a result
of it's last tick and publish updates on ROS topics.
Expand All @@ -363,15 +399,13 @@ def _on_change_post_tick_handler(self, tree: py_trees.trees.BehaviourTree):
self.node.get_logger().error("the root behaviour failed to return a tip [cause: tree is in an INVALID state]")
return

# if tree state changed, publish
if self.snapshot_visitor.changed:
# serialise the snapshot if tree state/graph changed or it's just been a while...
current_timestamp = time.monotonic()
if self.snapshot_visitor.changed or ((current_timestamp - self.last_snapshot_timestamp) > self.snapshot_period):
self._publish_serialised_tree(changed=self.snapshot_visitor.changed)
# with self.lock:
# if not self._bag_closed:
# # self.bag.write(self.publishers.log_tree.name, self.logging_visitor.tree)
# pass
self.last_snapshot_timestamp = current_timestamp

# check for blackboard watchers, update and publish if necessary, clear activity stream
# every tick publish on watchers, clear activity stream (note: not expensive as watchers by default aren't connected)
self.blackboard_exchange.post_tick_handler(
visited_client_ids=self.snapshot_visitor.visited_blackboard_client_ids # .keys()
)
Expand Down Expand Up @@ -408,7 +442,9 @@ def _publish_serialised_tree(self, changed: bool=False):
)

if py_trees.blackboard.Blackboard.activity_stream is not None:
tree_message.blackboard_activity_stream = py_trees.display.unicode_blackboard_activity_stream()
tree_message.blackboard_activity_stream = py_trees.display.unicode_blackboard_activity_stream(
show_title=False
)
# other
if self.statistics is not None:
tree_message.statistics = self.statistics
Expand Down Expand Up @@ -568,21 +604,34 @@ def deserialise_tree_recursively(msg):
# Streaming
####################
if self.viewing_mode == WatcherMode.STREAM:
console.banner("Tick {}".format(msg.statistics.count))
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(console.green + "-" * 80 + console.reset)
print(colour + "-" * 80 + console.reset)
####################
# Stream Variables
####################
if self.display_blackboard_variables:
print("")
print(console.green + "Blackboard Data" + console.reset)
print(colour + "Blackboard Data" + console.reset)
# could probably re-use the unicode_blackboard by passing a dict to it
# like we've done for the activity stream
indent = " " * 4
Expand All @@ -600,14 +649,15 @@ def deserialise_tree_recursively(msg):
####################
if self.display_blackboard_activity:
print("")
print(colour + "Blackboard Activity Stream" + console.reset)
if msg.blackboard_activity_stream:
print(msg.blackboard_activity_stream)
####################
# Stream Statistics
####################
if self.display_statistics:
print("")
print(console.green + "Statistics" + console.reset)
print(colour + "Statistics" + console.reset)
print(
console.cyan + " Timestamp: " + console.yellow +
"{}".format(
Expand All @@ -634,6 +684,7 @@ def deserialise_tree_recursively(msg):
math.sqrt(msg.statistics.tick_interval_variance)
)
)
print(console.reset)

####################
# Printing
Expand Down

0 comments on commit 3c47e6e

Please sign in to comment.