Merge pull request #850 from tue-robotics/fix/startChallengeRobust

a slightly hacky fix for StartChallengeRobust
tue-robotics · Jun 6, 2019 · 830678c · 830678c
2 parents 40436b1 + 52a8bd5
commit 830678c
Showing 1 changed file with 9 additions and 2 deletions.
diff --git a/robot_smach_states/src/robot_smach_states/startup.py b/robot_smach_states/src/robot_smach_states/startup.py
@@ -43,9 +43,16 @@ def __init__(self, robot, initial_pose, use_entry_points=False, door=True):
 
             smach.StateMachine.add("INITIALIZE",
                                    utility.Initialize(robot),
-                                   transitions={"initialized": "INSTRUCT_WAIT_FOR_DOOR" if door else "INIT_POSE",
+                                   transitions={"initialized": "INSTRUCT_WAIT_FOR_DOOR" if door else "WAIT_FOR_COMPONENTS",
                                                 "abort": "Aborted"})
 
+            # ToDo: HACK
+            # We wait a while before starting the challenge, otherwise some software is not initialised yet.
+            smach.StateMachine.add("WAIT_FOR_COMPONENTS",
+                                   utility.WaitTime(robot, 5),
+                                   transitions={"waited": "INIT_POSE",
+                                                "preempted": "Aborted"})
+
             smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
                                    human_interaction.Say(robot, ["Hi there, I will now wait until the door is opened",
                                                                  "I'm waiting for the door"], block=False),
@@ -68,7 +75,7 @@ def __init__(self, robot, initial_pose, use_entry_points=False, door=True):
                                    transitions={"spoken": "INIT_POSE"})
 
             # Initial pose is set after opening door, otherwise snapmap will fail if door is still closed and initial
-            # pose is set, since it is thinks the robot is standing in front of a wall if door is closed and 
+            # pose is set, since it is thinks the robot is standing in front of a wall if door is closed and
             # localization can(/will) be messed up.
             smach.StateMachine.add('INIT_POSE',
                                    utility.SetInitialPose(robot, initial_pose),