Merge pull request #165 from HannesBachter/feature/get_userdata

get userdata by service
team-vigir · Apr 28, 2023 · f7b9154 · f7b9154
2 parents d02963e + e889e34
commit f7b9154
Show file tree

Hide file tree

Showing 4 changed files with 122 additions and 56 deletions.
diff --git a/flexbe_msgs/CMakeLists.txt b/flexbe_msgs/CMakeLists.txt
@@ -1,55 +1,61 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(flexbe_msgs)
-
-find_package(catkin REQUIRED COMPONENTS message_generation actionlib_msgs actionlib)
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-#catkin_python_setup()
-
-# generate msg and srv
-add_message_files(FILES 
-  BEStatus.msg
-  BehaviorLog.msg
-  BehaviorModification.msg
-  BehaviorRequest.msg
-  BehaviorSelection.msg
-  BehaviorSync.msg
-  CommandFeedback.msg
-  Container.msg
-  ContainerStructure.msg
-  OutcomeCondition.msg
-  OutcomeRequest.msg
-  StateInstantiation.msg
-  SynthesisErrorCodes.msg
-  SynthesisRequest.msg
-  UICommand.msg
-)
-add_action_files(FILES 
-  BehaviorInput.action
-  BehaviorExecution.action
-  BehaviorSynthesis.action
-)
-#add_action_files(FILES ...)
-# opt arg DEPENDENCIES ...
-generate_messages(DEPENDENCIES actionlib_msgs)
-
-# specify catkin-specific information
-# INCLUDE_DIRS - The exported include paths (i.e. cflags) for the package
-# LIBRARIES - The exported libraries from the project
-# CATKIN_DEPENDS - Other catkin projects that this project depends on
-# DEPENDS - Non-catkin CMake projects that this project depends on
-# CFG_EXTRAS - Additional configuration options 
-catkin_package(
-#    INCLUDE_DIRS src
-#    LIBRARIES ${PROJECT_NAME}
-    CATKIN_DEPENDS message_runtime actionlib_msgs)
-
-# use add_library() or add_executable() as required
-#add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS})
-
-# install executables
-#install(PROGRAMS bin/hello
-#        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
+cmake_minimum_required(VERSION 2.8.3)
+project(flexbe_msgs)
+
+find_package(catkin REQUIRED COMPONENTS message_generation actionlib_msgs actionlib)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+#catkin_python_setup()
+
+# generate msg and srv
+add_message_files(FILES 
+  BEStatus.msg
+  BehaviorLog.msg
+  BehaviorModification.msg
+  BehaviorRequest.msg
+  BehaviorSelection.msg
+  BehaviorSync.msg
+  CommandFeedback.msg
+  Container.msg
+  ContainerStructure.msg
+  OutcomeCondition.msg
+  OutcomeRequest.msg
+  StateInstantiation.msg
+  SynthesisErrorCodes.msg
+  SynthesisRequest.msg
+  UICommand.msg
+  UserdataInfo.msg
+)
+add_action_files(FILES 
+  BehaviorInput.action
+  BehaviorExecution.action
+  BehaviorSynthesis.action
+)
+
+add_service_files(FILES
+  GetUserdata.srv
+)
+
+#add_action_files(FILES ...)
+# opt arg DEPENDENCIES ...
+generate_messages(DEPENDENCIES actionlib_msgs)
+
+# specify catkin-specific information
+# INCLUDE_DIRS - The exported include paths (i.e. cflags) for the package
+# LIBRARIES - The exported libraries from the project
+# CATKIN_DEPENDS - Other catkin projects that this project depends on
+# DEPENDS - Non-catkin CMake projects that this project depends on
+# CFG_EXTRAS - Additional configuration options 
+catkin_package(
+#    INCLUDE_DIRS src
+#    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS message_runtime actionlib_msgs)
+
+# use add_library() or add_executable() as required
+#add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS})
+
+# install executables
+#install(PROGRAMS bin/hello
+#        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
diff --git a/flexbe_msgs/srv/GetUserdata.srv b/flexbe_msgs/srv/GetUserdata.srv
@@ -0,0 +1,6 @@
+# specify key to filter userdata, leave empty to get all userdata
+string userdata_key
+---
+bool success
+string message
+flexbe_msgs/UserdataInfo[] userdata
diff --git a/flexbe_onboard/package.xml b/flexbe_onboard/package.xml
@@ -24,5 +24,6 @@
   <exec_depend>flexbe_msgs</exec_depend>
   <exec_depend>flexbe_states</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
 
 </package>
diff --git a/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/src/flexbe_onboard/flexbe_onboard.py
@@ -12,8 +12,10 @@
 
 from flexbe_core import Logger, BehaviorLibrary
 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
+from flexbe_core.core.state_machine import StateMachine
 
-from flexbe_msgs.msg import BehaviorSelection, BEStatus, CommandFeedback
+from flexbe_msgs.msg import BehaviorSelection, BEStatus, CommandFeedback, UserdataInfo
+from flexbe_msgs.srv import GetUserdata, GetUserdataRequest, GetUserdataResponse
 from std_msgs.msg import Empty
 
 
@@ -53,6 +55,8 @@ def __init__(self):
         self._sub = ProxySubscriberCached()
         self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback)
 
+        self._userdata_service = rospy.Service('flexbe/get_userdata', GetUserdata, self._userdata_callback)
+
         rospy.sleep(0.5)  # wait for publishers etc to really be set up
         self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
         rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
@@ -159,6 +163,39 @@ def _behavior_execution(self, msg):
             self._running = False
             self.be = None
 
+
+    def _userdata_callback(self, request):
+        response = GetUserdataResponse()
+        userdata = []
+        if self.be and self.be._state_machine:
+            # get userdata from top-level behavior
+            if self.be._state_machine._userdata:
+                for key, data in self.be._state_machine._userdata._data.items():
+                    # add userdata if fits to the requested key (get all userdata for empty string)
+                    if (request.userdata_key == "" or request.userdata_key == key):
+                        userdata.append(UserdataInfo(state=self.be._state_machine._name,
+                                                    key=str(key),
+                                                    type=type(data).__name__,
+                                                    data=str(data)))
+            # get userdata from sub-behaviors
+            userdata = self._get_userdata_from_whole_sm(self.be._state_machine, userdata, request.userdata_key, str(self.be._state_machine._name) + "/")
+
+            if (len(userdata) > 0):
+                # also print in terminal (better readable for complex message types)
+                printable_userdata = ""
+                for ud in userdata:
+                    printable_userdata += "\t{}:{}\n".format(ud.key, ud.data)
+                Logger.loginfo("Current userdata: \n{}".format(printable_userdata))
+                response.success = True
+            else:
+                response.success = False
+            response.message = "Found {} occurences of '{}'".format(len(userdata), request.userdata_key)
+            response.userdata = userdata
+        else:
+            response.success = False
+            response.message = "no state_machine running"
+        return response
+
     # ==================================== #
     # Preparation of new behavior requests #
     # ------------------------------------ #
@@ -346,6 +383,22 @@ def _convert_dict(self, o):
         else:
             return o
 
+    def _get_userdata_from_whole_sm(self, state_machine, userdata, userdata_key, path):
+        # iterate recursively through all subbehaviors
+        for subbehavior in state_machine._states:
+            # check if userdata available
+            if isinstance(subbehavior, StateMachine):
+                if subbehavior._userdata:
+                    for key, data in subbehavior._userdata._data.items():
+                        # add userdata if fits to the requested key (get all userdata for empty string)
+                        if (userdata_key == "" or userdata_key == key):
+                            userdata.append(UserdataInfo(state=path + subbehavior.name + "/",
+                                                        key=str(key),
+                                                        type=type(data).__name__,
+                                                        data=str(data)))
+                self._get_userdata_from_whole_sm(subbehavior, userdata, userdata_key, path + subbehavior.name + "/")
+        return userdata
+
     class _attr_dict(dict):
         __getattr__ = dict.__getitem__