Merge pull request #34 from fmessmer/feature/sync_flexbe

Feature/sync flexbe

LICENSE

@@ -1,4 +1,4 @@
-Copyright (c) 2015, DARPA Robotics Challenge Team ViGIR
+Copyright (c) 2015-2023, DARPA Robotics Challenge Team ViGIR
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -25,4 +25,3 @@ SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-

README.md

@@ -9,8 +9,6 @@
 > should use the https://github.com/FlexBE site, which will be the one-stop
 > shop for all things FlexBE going forward.
 
-
-
 FlexBE is a high-level behavior engine coordinating the capabilities of a robot in order to solve complex tasks.
 Behaviors are modeled as hierarchical state machines where states correspond to active actions and transitions describe the reaction to outcomes.
 Main advantage over similar approaches is the good operator integration and extensive user interface.
@@ -20,14 +18,14 @@ The user interface features a runtime control interface as well as a graphical e
 
 Please refer to the FlexBE Homepage ([flexbe.github.io](http://flexbe.github.io)) for further information, tutorials, application examples, and much more.
 
-![FlexBE CI](https://github.com/team-vigir/flexbe_behavior_engine/workflows/FlexBE%20CI/badge.svg)
+![FlexBE CI](https://github.com/FlexBE/flexbe_behavior_engine/workflows/FlexBE%20CI/badge.svg)
 
 ## Installation
 
 Execute the following commands to install FlexBE:
 
     roscd && cd ../src
-    git clone https://github.com/team-vigir/flexbe_behavior_engine.git
+    git clone https://github.com/FlexBE/flexbe_behavior_engine.git
 
 Furthermore, create your own repository for behavior development (contains examples):
 

flexbe_behavior_engine/CHANGELOG.rst

@@ -1,6 +1,14 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_behavior_engine
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
+* [flexbe_core] Merge pull request #153 from omercans/fix/set-current-state-of-cc-to-none-on-forced-exit
+* [flexbe_core] Merge pull request #154 from duwke/patch-1 - Check topic availability before returning last_msg
+* [flexbe_states] Merge pull request #160 from cpswarm/fix_namespace - namespace fix for topic lookup with rostopic in subscriber state
+* [flexbe_core] Merge pull request #163 from LoyVanBeek/fix/nested_sm_userdata - Fix nested state machine userdata
+* [flexbe_onboard] Merge pull request #165 from HannesBachter/feature/get_userdata - get userdata by service
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_behavior_engine/package.xml

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>flexbe_behavior_engine</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
     A meta-package to aggregate all the FlexBE packages
   </description>
 
   <maintainer email="[email protected]">Philipp Schillinger</maintainer>
-  <maintainer email="[email protected]">Alireza Hosseini</maintainer>
+  <maintainer email="[email protected]">David Conner</maintainer>
   <license>BSD</license>
 
   <url>http://ros.org/wiki/flexbe</url>

flexbe_core/CHANGELOG.rst

@@ -1,6 +1,13 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
+* [flexbe_core] Fix handling of boolean behavior args
+* [flexbe_core] Merge pull request #153 from omercans/fix/set-current-state-of-cc-to-none-on-forced-exit
+* [flexbe_core] Merge pull request #154 from duwke/patch-1 - Check topic availability before returning last_msg
+* [flexbe_core] Merge pull request #163 from LoyVanBeek/fix/nested_sm_userdata - Fix nested state machine userdata
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_core/package.xml

@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexbe_core</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
      flexbe_core provides the core components for the FlexBE behavior engine.
   </description>

flexbe_core/src/flexbe_core/proxy/proxy_action_client.py

@@ -100,6 +100,23 @@ def get_result(self, topic):
         @param topic: The topic of interest.
         """
         return ProxyActionClient._result.get(topic)
+
+    def get_goal_status_text(self, topic):
+        """
+        Returns the result status text if any.
+
+        @type topic: string
+        @param topic: The topic of interest.
+        """
+
+        if (not topic in ProxyActionClient._clients):
+            return "Failed to get status text: Invalid action topic = [" + topic +"]"
+
+        try:
+            status = ProxyActionClient._clients[topic].get_goal_status_text()
+            return status
+        except:
+            return "Failed to retrieve goal status text for topic = [" + topic +"]"
 
     def remove_result(self, topic):
         """

flexbe_core/src/flexbe_core/proxy/proxy_subscriber_cached.py

@@ -134,7 +134,9 @@ def has_msg(self, topic):
         @type topic: string
         @param topic: The topic of interest.
         """
-        return ProxySubscriberCached._topics[topic]['last_msg'] is not None
+        if self.is_available(topic):
+            return ProxySubscriberCached._topics[topic]['last_msg'] is not None
+        return False
 
     def has_buffered(self, topic):
         """

flexbe_input/CHANGELOG.rst

@@ -1,6 +1,9 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_input
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_input/package.xml

@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexbe_input</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
      flexbe_input enables to send data to onboard behavior when required.
   </description>

flexbe_mirror/CHANGELOG.rst

@@ -1,6 +1,9 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_mirror
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_mirror/package.xml

@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexbe_mirror</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
      flexbe_mirror implements functionality to remotely mirror an executed behavior.
   </description>

flexbe_msgs/CHANGELOG.rst

@@ -1,6 +1,10 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
+* [flexbe_msgs] Merge pull request #165 from HannesBachter/feature/get_userdata - get userdata by service
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_msgs/package.xml

@@ -1,6 +1,6 @@
 <package format="2">
   <name>flexbe_msgs</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
      flexbe_msgs provides the messages used by FlexBE.
   </description>

flexbe_onboard/CHANGELOG.rst

@@ -1,6 +1,10 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_onboard
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
+* [flexbe_onboard] Merge pull request #165 from HannesBachter/feature/get_userdata - get userdata by service
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_onboard/package.xml

@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexbe_onboard</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
      flexbe_onboard implements the robot-side of the behavior engine from where all behaviors are started.
   </description>

flexbe_onboard/test/test_onboard.py

@@ -14,6 +14,7 @@
 class TestOnboard(unittest.TestCase):
 
     def __init__(self, name):
+        print("Initializing TestOnboard ...")
         super(TestOnboard, self).__init__(name)
         self.sub = ProxySubscriberCached({
             'flexbe/status': BEStatus,
@@ -23,10 +24,13 @@ def __init__(self, name):
         # make sure that behaviors can be imported
         data_folder = os.path.dirname(os.path.realpath(__file__))
         sys.path.insert(0, data_folder)
+        print(f"Using {data_folder} ...")
         # run onboard and add custom test behaviors to onboard lib
+        print(f"Create onboard instance ...")
         self.onboard = FlexbeOnboard()
         self.lib = self.onboard._behavior_lib
         self.lib._add_behavior_manifests(data_folder)
+        print(f"Done adding behavior manifests from {data_folder}")
 
     def assertStatus(self, expected, timeout):
         """ Assert that the expected onboard status is received before the timeout. """
@@ -39,14 +43,17 @@ def assertStatus(self, expected, timeout):
         msg = self.sub.get_last_msg('flexbe/status')
         self.sub.remove_last_msg('flexbe/status')
         self.assertEqual(msg.code, expected)
+        print(f"Successfully received status {msg}")
         return msg
 
     def test_onboard_behaviors(self):
+        print("Creating start_behavior publisher ...")
         behavior_pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=1)
         rospy.sleep(0.5)  # wait for publisher
 
         # wait for the initial status message
         self.assertStatus(BEStatus.READY, 1)
+        print("Behavior engine is ready!")
 
         # send simple behavior request without checksum
         be_id, _ = self.lib.find_behavior("Test Behavior Log")
@@ -55,8 +62,10 @@ def test_onboard_behaviors(self):
         request.autonomy_level = 255
         behavior_pub.publish(request)
         self.assertStatus(BEStatus.ERROR, 2)
+        print(f"Correctly detected checksum issue!")
 
         # send valid simple behavior request
+        print(f"Correctly set checksum and send request ...")
         with open(self.lib.get_sourcecode_filepath(be_id)) as f:
             request.behavior_checksum = zlib.adler32(f.read().encode()) & 0x7fffffff
         self.sub.enable_buffer('flexbe/log')
@@ -67,9 +76,11 @@ def test_onboard_behaviors(self):
         while self.sub.has_buffered('flexbe/log'):
             behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text)
         self.assertIn('Test data', behavior_logs)
+        print(f"Correctly recived update!")
 
         # send valid complex behavior request
         be_id, _ = self.lib.find_behavior("Test Behavior Complex")
+        print(f"Send complex request ...")
         request = BehaviorSelection()
         request.behavior_id = be_id
         request.autonomy_level = 255
@@ -87,15 +98,19 @@ def test_onboard_behaviors(self):
             content = content.replace(replace, by)
         request.behavior_checksum = zlib.adler32(content.encode()) & 0x7fffffff
         behavior_pub.publish(request)
+        print("Wait for status updates ...")
         self.assertStatus(BEStatus.STARTED, 1)
         result = self.assertStatus(BEStatus.FINISHED, 3)
         self.assertEqual(result.args[0], 'finished')
+        print("Successful update, now check logs ...")
+
         behavior_logs = []
         while self.sub.has_buffered('flexbe/log'):
             behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text)
         self.assertIn('value_2', behavior_logs)
 
         # send the same behavior with different parameters
+        print(f"Send complex request with different parameters ...")
         request.arg_keys = ['param', 'invalid']
         request.arg_values = ['value_1', 'should be ignored']
         request.input_keys = []
@@ -108,7 +123,7 @@ def test_onboard_behaviors(self):
         while self.sub.has_buffered('flexbe/log'):
             behavior_logs.append(self.sub.get_from_buffer('flexbe/log').text)
         self.assertIn('value_1', behavior_logs)
-
+        print("Success!")
 
 if __name__ == '__main__':
     rospy.init_node('test_flexbe_onboard')

flexbe_states/CHANGELOG.rst

@@ -1,6 +1,9 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package flexbe_states
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.4.0 (2023-05-18)
+------------------
+* Updates for Melodic and Noetic releases on github.com/FlexBE
 
 1.3.1 (2020-12-11)
 ------------------

flexbe_states/package.xml

@@ -4,7 +4,7 @@
   schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexbe_states</name>
-  <version>1.3.1</version>
+  <version>1.4.0</version>
   <description>
      flexbe_states provides a collection of predefined states.
      Feel free to add new states.

flexbe_states/src/flexbe_states/subscriber_state.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import rospy
 import rostopic
 from flexbe_core import EventState, Logger
 
@@ -53,8 +54,11 @@ def on_enter(self, userdata):
             self._sub.remove_last_msg(self._topic)
 
     def _connect(self):
-        msg_type, msg_topic, _ = rostopic.get_topic_class(self._topic)
-        if msg_topic == self._topic:
+        global_topic = self._topic
+        if global_topic[0] != '/':
+            global_topic = rospy.get_namespace() + global_topic
+        msg_type, msg_topic, _ = rostopic.get_topic_class(global_topic)
+        if msg_topic == global_topic:
             self._sub = ProxySubscriberCached({self._topic: msg_type})
             self._connected = True
             return True

flexbe_states/src/flexbe_states/user_data_state.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+
+from flexbe_core import EventState, Logger
+
+'''
+Created on 14-Feb-2018
+
+@author: David Conner
+'''
+
+class UserDataState(EventState):
+    '''
+    Implements a state that defines user data
+
+    -- data    type        Data for given user data
+
+    #> data                User data
+
+    <= done                Created the user data
+    '''
+
+
+    def __init__(self, data ):
+        '''
+        Constructor
+        '''
+        super(UserDataState, self).__init__( output_keys=["data"], outcomes=["done"])
+
+        self._my_data = data
+        self._return_code = None
+
+    def execute(self, userdata):
+        '''
+        Execute this state
+        '''
+        return self._return_code
+
+
+    def on_enter(self, userdata):
+
+        try:
+          # Add the user data
+          userdata.data = self._my_data
+          self._return_code = 'done'
+        except:
+            raise ValueError('UserDataState %s - invalid data ' % self.name)

flexbe_states/tests/run_tests.launch

@@ -22,6 +22,7 @@
 		$(arg path)/log_state_msg.test
 		$(arg path)/operator_decision_state_suggested.test
 		$(arg path)/subscriber_state_unavailable.test
+		$(arg path)/user_data_state.test
 		$(arg path)/wait_state_short.test
 
 	" />

flexbe_states/tests/user_data_state.test

@@ -0,0 +1,10 @@
+path: flexbe_states.user_data_state
+class: UserDataState
+
+params:
+    data: "Test data"
+
+output:
+    data: "Test data"
+
+outcome: done