From f974e58b594723b58a851499327c8cb46a6bacc9 Mon Sep 17 00:00:00 2001
From: Julian Keller <ethz@juliankeller.net>
Date: Fri, 20 May 2022 16:40:18 +0200
Subject: [PATCH] Add userdata constants

---
 smach/package.xml                |  2 +-
 smach/src/smach/state_machine.py | 18 ++++++++++++++----
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/smach/package.xml b/smach/package.xml
index 256e233..6679431 100644
--- a/smach/package.xml
+++ b/smach/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>smach</name>
-  <version>2.5.0</version>
+  <version>2.5.99</version>
   <description>
     SMACH is a task-level architecture for rapidly creating complex robot
     behavior. At its core, SMACH is a ROS-independent Python library to build
diff --git a/smach/src/smach/state_machine.py b/smach/src/smach/state_machine.py
index 4989614..2a20263 100644
--- a/smach/src/smach/state_machine.py
+++ b/smach/src/smach/state_machine.py
@@ -62,6 +62,7 @@ def __init__(self, outcomes, input_keys=[], output_keys=[]):
         self._states = {}
         self._transitions = {}
         self._remappings = {}
+        self._constants = {}
 
         # Construction vars
         self._last_added_label = None
@@ -73,7 +74,7 @@ def __init__(self, outcomes, input_keys=[], output_keys=[]):
 
     ### Construction methods
     @staticmethod
-    def add(label, state, transitions=None, remapping=None):
+    def add(label, state, transitions=None, remapping=None, constants=None):
         """Add a state to the opened state machine.
         
         @type label: string
@@ -84,8 +85,11 @@ def add(label, state, transitions=None, remapping=None):
         @param transitions: A dictionary mapping state outcomes to other state
         labels or container outcomes.
 
-        @param remapping: A dictrionary mapping local userdata keys to userdata
+        @param remapping: A dictionary mapping local userdata keys to userdata
         keys in the container.
+
+        @param constants: A dictionary mapping userdata keys in the container
+        to constant values.
         """
         # Get currently opened container
         self = StateMachine._currently_opened_container()
@@ -102,6 +106,9 @@ def add(label, state, transitions=None, remapping=None):
         if remapping is None:
             remapping = {}
 
+        if constants is None:
+            constants = {}
+
         # Add group transitions to this new state, if they exist
         """
         if 'transitions' in smach.Container._context_kwargs:
@@ -133,6 +140,7 @@ def add(label, state, transitions=None, remapping=None):
         self._states[label] = state
         self._transitions[label] = transitions
         self._remappings[label] = remapping
+        self._constants[label] = constants
         smach.logdebug("TRANSITIONS FOR %s: %s" % (label, str(self._transitions[label])))
 
         # Add transition to this state if connected outcome is defined
@@ -146,7 +154,7 @@ def add(label, state, transitions=None, remapping=None):
         return state
 
     @staticmethod
-    def add_auto(label, state, connector_outcomes, transitions=None, remapping=None):
+    def add_auto(label, state, connector_outcomes, transitions=None, remapping=None, constants=None):
         """Add a state to the state machine such that it automatically
         transitions to the next added state.
 
@@ -172,7 +180,7 @@ def add_auto(label, state, connector_outcomes, transitions=None, remapping=None)
         self = StateMachine._currently_opened_container()
 
         # First add this state
-        add_ret = smach.StateMachine.add(label, state, transitions, remapping)
+        add_ret = smach.StateMachine.add(label, state, transitions, remapping, constants)
 
         # Make sure the connector outcomes are valid for this state
         registered_outcomes = state.get_registered_outcomes()
@@ -239,6 +247,8 @@ def _update_once(self):
         # Execute the state
         try:
             self._state_transitioning_lock.release()
+            for k, v in self._constants[self._current_label].items():
+                self.userdata[k] = v
             outcome = self._current_state.execute(
                     smach.Remapper(
                         self.userdata,