Merge branch 'develop'

qcr · Jun 27, 2022 · ca98772 · ca98772
2 parents 9b6e178 + 77ecf49
commit ca98772
Show file tree

Hide file tree

Showing 3 changed files with 81 additions and 39 deletions.
diff --git 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,

diff --git 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)
+
 <p align="center">
   <img src="https://github.com/qcr/ros_trees/wiki/assets/frankie.gif" width="955" />
 </p>
@@ -20,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.
 
@@ -209,5 +218,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`)
diff --git 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,23 +137,25 @@ 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
 
     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\"" %
@@ -167,6 +176,7 @@ def __init__(self,
                  name,
                  topic_name,
                  topic_class,
+                 once_only=False,
                  expiry_time=None,
                  timeout=3.0,
                  save=True,
@@ -175,22 +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,
@@ -201,7 +223,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 +235,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,18 +262,21 @@ 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
 
     def callback(self, *msgs):
         for idx, msg in enumerate(msgs):
             self._cached_data[idx] = msg
         self._cached_time = rospy.get_time()
-