Implement outcome_map disambiguation for Concurrence

According to the documentation:

"""
If the criteria for one outcome is the subset of another outcome,
the container will choose the outcome which has more child outcome
criteria satisfied. If both container outcomes have the same
number of satisfied criteria, the behavior is undefined.
"""

However, this did not seem to be implemented as such, instead it appears
the behavior was undefined regardless of the number of satisfied
criteria. This fixes the outcome_map handling such that a new outcome
from the map is only accepted if it satisfies strictly more child labels
than any previously consider outcome, thus satisfying the documented
behavior.

smach/src/smach/concurrence.py

@@ -281,10 +281,16 @@ def execute(self, parent_ud = smach.UserData()):
 
         # Determine the outcome from the outcome map
         smach.logdebug("SMACH Concurrence determining contained state outcomes.")
+        max_child_outcomes_satisfied = 0
         for (container_outcome, outcomes) in ((k,self._outcome_map[k]) for k in self._outcome_map):
+            child_outcomes_satisfied = len(outcomes)
             if all([self._child_outcomes[label] == outcomes[label] for label in outcomes]):
-                smach.logdebug("Terminating concurrent split with mapped outcome.")
-                outcome = container_outcome
+                if child_outcomes_satisfied > max_child_outcomes_satisfied:
+                    smach.logdebug("Terminating concurrent split with mapped outcome.")
+                    outcome = container_outcome
+                    max_child_outcomes_satisfied = child_outcomes_satisfied
+                else:
+                    smach.logdebug("Skipping mapped outcome '%s' with fewer constraints." % container_outcome)
 
         # Check outcome callback
         if self._outcome_cb: