From 3da4e50be1e58177af28a159f67c341e696640be Mon Sep 17 00:00:00 2001 From: Ben Talbot Date: Thu, 25 Nov 2021 09:25:35 +1000 Subject: [PATCH 1/6] Fix formatting in leaves_ros --- src/ros_trees/leaves_ros.py | 86 +++++++++++++++++++++++-------------- 1 file changed, 53 insertions(+), 33 deletions(-) diff --git a/src/ros_trees/leaves_ros.py b/src/ros_trees/leaves_ros.py index 7c47149..0d896f9 100644 --- a/src/ros_trees/leaves_ros.py +++ b/src/ros_trees/leaves_ros.py @@ -15,7 +15,9 @@ class ActionLeaf(Leaf): def __init__(self, name, action_namespace, *args, **kwargs): super(ActionLeaf, self).__init__(name, *args, **kwargs) - self.action_namespace = action_namespace if action_namespace.startswith('/') else '/{}'.format(action_namespace) + self.action_namespace = (action_namespace + if action_namespace.startswith('/') else + '/{}'.format(action_namespace)) self._action_class = None self._action_client = None self.sent_goal = False @@ -26,11 +28,11 @@ def _default_eval_fn(self, value): def _default_load_fn(self, auto_generate=True): if auto_generate: - return dm.auto_generate( - super(ActionLeaf, self)._default_load_fn(), - type(self._action_class().action_goal.goal)) + return dm.auto_generate( + super(ActionLeaf, self)._default_load_fn(), + type(self._action_class().action_goal.goal)) else: - return super(ActionLeaf, self)._default_load_fn() + return super(ActionLeaf, self)._default_load_fn() def _default_result_fn(self): return self._action_client.get_result() @@ -41,14 +43,17 @@ def _extra_initialise(self): def _extra_setup(self, timeout): # Get a client & type for the Action Server try: - self._action_class = roslib.message.get_message_class( - re.sub('Goal$', '', + self._action_class = roslib.message.get_message_class( + re.sub( + 'Goal$', '', rostopic.get_topic_type(self.action_namespace + '/goal')[0])) - self._action_client = actionlib.SimpleActionClient( - self.action_namespace, self._action_class) + self._action_client = actionlib.SimpleActionClient( + self.action_namespace, self._action_class) except: - raise Exception('Failed to set up action client for server: {}'.format(self.action_namespace)) + raise Exception( + 'Failed to set up action client for server: {}'.format( + self.action_namespace)) # Confirm the action client is actually there if not self._action_client.wait_for_server(rospy.Duration(timeout)): @@ -88,14 +93,16 @@ def __init__(self, name, topic_name, topic_class, *args, **kwargs): def _default_load_fn(self, auto_generate=True): if auto_generate: - return dm.auto_generate( - super(PublisherLeaf, self)._default_load_fn(), self.topic_class) + return dm.auto_generate( + super(PublisherLeaf, self)._default_load_fn(), + self.topic_class) else: - return super(PublisherLeaf, self)._default_load_fn() + return super(PublisherLeaf, self)._default_load_fn() def _default_result_fn(self, custom_data=None): try: - self._publisher.publish(custom_data if custom_data is not None else self.loaded_data) + self._publisher.publish( + custom_data if custom_data is not None else self.loaded_data) return True except Exception as e: self.logger.error("%s.result_fn(): %s" % (self.name, e)) @@ -108,15 +115,15 @@ def _extra_setup(self, timeout): # one ROS...) t = rospy.get_time() sub = rospy.Subscriber(self.topic_name, self.topic_class) - + self._publisher = rospy.Publisher(self.topic_name, self.topic_class, queue_size=10) - + while (self._publisher.get_num_connections() == 0 and - (timeout == 0 or rospy.get_time() - t < timeout)): + (timeout == 0 or rospy.get_time() - t < timeout)): rospy.sleep(0.05) - + return self._publisher.get_num_connections() > 0 @@ -130,15 +137,16 @@ def __init__(self, name, service_name, save=True, *args, **kwargs): def _default_load_fn(self, auto_generate=True): if auto_generate: - return dm.auto_generate( - super(ServiceLeaf, self)._default_load_fn(), - self._service_class._request_class) + return dm.auto_generate( + super(ServiceLeaf, self)._default_load_fn(), + self._service_class._request_class) else: - return super(ServiceLeaf, self)._default_load_fn() + return super(ServiceLeaf, self)._default_load_fn() def _default_result_fn(self, custom_data=None): try: - return self._service_proxy(custom_data if custom_data is not None else self.loaded_data) + return self._service_proxy( + custom_data if custom_data is not None else self.loaded_data) except Exception as e: self.logger.error("%s.result_fn(): %s" % (self.name, e)) return None @@ -146,7 +154,8 @@ def _default_result_fn(self, custom_data=None): def _extra_setup(self, timeout): # Confirm the service is actually there try: - rospy.wait_for_service(self.service_name, timeout if timeout else None) + rospy.wait_for_service(self.service_name, + timeout if timeout else None) except rospy.ROSException: self.logger.error( "%s.setup() could not find a Service with name \"%s\"" % @@ -189,7 +198,8 @@ def _default_result_fn(self): rospy.get_time() - t < self.timeout): rospy.sleep(0.1) return (None if self._cached_time is None or - (self.expiry_time and rospy.get_time() - self._cached_time > self.expiry_time) else + (self.expiry_time and + rospy.get_time() - self._cached_time > self.expiry_time) else self._cached_data) def _extra_setup(self, timeout): @@ -201,7 +211,9 @@ def callback(self, msg): self._cached_data = msg self._cached_time = rospy.get_time() + class SyncedSubscriberLeaf(Leaf): + def __init__(self, name, topic_names, @@ -211,10 +223,15 @@ def __init__(self, save=True, *args, **kwargs): - super(SyncedSubscriberLeaf, self).__init__(name, save=save, *args, **kwargs) + super(SyncedSubscriberLeaf, self).__init__(name, + save=save, + *args, + **kwargs) - if not (isinstance(topic_names, list) or isinstance(topic_names, tuple)) or len(topic_names) != len(topic_classes): - raise ValueError('topic_names length does not equal topic_classes length') + if not (isinstance(topic_names, list) or isinstance( + topic_names, tuple)) or len(topic_names) != len(topic_classes): + raise ValueError( + 'topic_names length does not equal topic_classes length') self.topic_names = topic_names self.topic_classes = topic_classes @@ -233,13 +250,17 @@ def _default_result_fn(self): rospy.get_time() - t < self.timeout): rospy.sleep(0.1) return (None if self._cached_time is None or - (self.expiry_time and rospy.get_time() - self._cached_time > self.expiry_time) else + (self.expiry_time and + rospy.get_time() - self._cached_time > self.expiry_time) else self._cached_data) def _extra_setup(self, timeout): - self._subscribers = [ message_filters.Subscriber(topic_name, self.topic_classes[idx]) - for idx, topic_name in enumerate(self.topic_names) ] - self._filter = message_filters.ApproximateTimeSynchronizer(self._subscribers, 10, 0.1, allow_headerless=True) + self._subscribers = [ + message_filters.Subscriber(topic_name, self.topic_classes[idx]) + for idx, topic_name in enumerate(self.topic_names) + ] + self._filter = message_filters.ApproximateTimeSynchronizer( + self._subscribers, 10, 0.1, allow_headerless=True) self._filter.registerCallback(self.callback) return True @@ -247,4 +268,3 @@ def callback(self, *msgs): for idx, msg in enumerate(msgs): self._cached_data[idx] = msg self._cached_time = rospy.get_time() - From d5c12b44a0a56fbd8148339f67802701c27e1ff4 Mon Sep 17 00:00:00 2001 From: Ben Talbot Date: Thu, 25 Nov 2021 09:47:20 +1000 Subject: [PATCH 2/6] Add support for 'once_only' flag to SubscriberLeaf --- src/ros_trees/leaves_ros.py | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/ros_trees/leaves_ros.py b/src/ros_trees/leaves_ros.py index 0d896f9..d5f0b35 100644 --- a/src/ros_trees/leaves_ros.py +++ b/src/ros_trees/leaves_ros.py @@ -176,6 +176,7 @@ def __init__(self, name, topic_name, topic_class, + once_only=False, expiry_time=None, timeout=3.0, save=True, @@ -184,23 +185,34 @@ def __init__(self, super(SubscriberLeaf, self).__init__(name, save=save, *args, **kwargs) self.topic_name = topic_name self.topic_class = topic_class + self.once_only = once_only self.expiry_time = expiry_time self.timeout = timeout self._subscriber = None self._cached_data = None self._cached_time = None + self._last_msg = None def _default_result_fn(self): t = rospy.get_time() + + # While we don't have a valid message, keep waiting while ((self._cached_time is None or (self.expiry_time is not None and rospy.get_time() - self._cached_time > self.expiry_time)) and rospy.get_time() - t < self.timeout): rospy.sleep(0.1) - return (None if self._cached_time is None or - (self.expiry_time and - rospy.get_time() - self._cached_time > self.expiry_time) else - self._cached_data) + + # Return what we found, dumping the cache if we're returning something + # in once_only mode + out = (None if self._cached_time is None or + (self.expiry_time and + rospy.get_time() - self._cached_time > self.expiry_time) else + self._cached_data) + if self.once_only and out: + self._cached_data = None + self._cached_time = None + return out def _extra_setup(self, timeout): self._subscriber = rospy.Subscriber(self.topic_name, self.topic_class, From b5c62bee6cfbfc9c7862e5d92a6434dcf6c8bc3c Mon Sep 17 00:00:00 2001 From: Ben Talbot Date: Thu, 25 Nov 2021 09:48:49 +1000 Subject: [PATCH 3/6] Add documentation for 'once_only' flag --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index ad2bbf3..a4849b1 100644 --- a/README.md +++ b/README.md @@ -209,5 +209,6 @@ The `SubscriberLeaf` class defines four extra parameters: - **`topic_name`**: the name of the topic that the leaf will subscribe to (required) - **`topic_class`**: the type of message that leaf will expect to receive (required) +- **`once_only`**: only new messages are returned from this leaf (no message will be returned twice) - **`expiry_time`**: the time after which a message is deemed to have expired and is deemed to old to be returned by the leaf (default = `None`). No value means all messages will be considered. - **`timeout`**: the max time the leaf will wait before declaring nothing was received (default=`3.0`) From 94446d844bb13d2ca5cb868ae23a8cdcbec1b44f Mon Sep 17 00:00:00 2001 From: Ben Talbot Date: Mon, 27 Jun 2022 11:18:28 +1000 Subject: [PATCH 4/6] Update license Addresses #2. --- LICENSE.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LICENSE.txt b/LICENSE.txt index 9849565..2e00b37 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,4 +1,4 @@ -Copyright (c) 2020, Ben Talbot & Gavin Suddrey, Australian Centre for Robotic Vision +Copyright (c) 2020, Queensland University of Technology (QUT) Centre for Robotics All rights reserved. Redistribution and use in source and binary forms, with or without modification, From d66e02ff9e0249ab0ded69e40aa5cde19aa6a959 Mon Sep 17 00:00:00 2001 From: Ben Talbot Date: Mon, 27 Jun 2022 11:19:02 +1000 Subject: [PATCH 5/6] Add badges to README Closes #2. --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index a4849b1..aa104d6 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,10 @@ # ROS Trees: Behaviour Trees for robotic systems +[![QUT Centre for Robotics Open Source](https://github.com/qcr/qcr.github.io/raw/master/misc/badge.svg)](https://qcr.github.io) +![Primary language](https://img.shields.io/github/languages/top/qcr/ros_trees) +[![License](https://img.shields.io/github/license/qcr/ros_trees)](./LICENSE.txt) +

From 77ecf4919366cb2c0385668082138884d11c88d9 Mon Sep 17 00:00:00 2001 From: Ben Talbot Date: Mon, 27 Jun 2022 15:49:04 +1000 Subject: [PATCH 6/6] Update list of places ROS trees has been used Closes #4. --- README.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index aa104d6..d9640a1 100644 --- a/README.md +++ b/README.md @@ -24,9 +24,14 @@ ROS Trees makes behaviour trees accessible in ROS ecosystems, bringing all the c This package has been used on a number of real world robots, allowing us to combine a range of useful robot capabilities into significant robot behaviours. Featured works include: -- [Holistic Mobile Manipulation](https://jhavl.github.io/holistic/) (shown above) -- [Learning and executing re-usable behaviour trees from natural language instruction](https://arxiv.org/abs/2106.01650) -- Australian Centre for Robotic Vision (ACRV) entry in the [2019 RoboCup@Home competition](https://2019.robocup.org/at-home-league.php) +- [Holistic Mobile Manipulation](https://jhavl.github.io/holistic/) (shown above); +- [a lunar rover demonstration](https://www.brisbanetimes.com.au/national/queensland/qut-rover-gets-its-eyes-for-being-part-of-nasa-moon-mission-20211202-p59e7y.html); +- [a logistic robot prototype for space applications](https://www.qut.edu.au/news?id=177210); +- [an autonomous robotic platform for greenhouses](https://research.qut.edu.au/qcr/Projects/autonomous-robotic-platforms-for-greenhouses/); +- robot manipulators for industrial applications at the [Australian Cobotics Centre](https://www.australiancobotics.org/); +- research results on [applying re-usable behaviour trees generated from natural language instruction](https://arxiv.org/abs/2106.01650); +- multiple demonstrators of robot capabilities shown to QCR labe visitors; and +- the Australian Centre for Robotic Vision (ACRV) entry in the [2019 RoboCup@Home competition](https://2019.robocup.org/at-home-league.php). We are always interested in hearing about where our software is used. If you've used ROS Trees in your work, we'd love to hear more about how it was used.