Merge pull request #276 from tue-robotics/feature/check_ebutton

Feature/check ebutton

robot_skills/src/robot_skills/util/robot_constructor.py

@@ -1,5 +1,10 @@
-def robot_constructor(robot_name):
+import sys
+
+def robot_constructor(robot_name=None):
     """Construct a robot by it's name. Choices are amigo, sergio, mockbot"""
+    if not robot_name:
+        robot_name = sys.argv[1]
+
     if robot_name == "amigo":
         import robot_skills.amigo
         return robot_skills.amigo.Amigo(wait_services=True)

robot_smach_states/src/robot_smach_states/__init__.py

@@ -6,3 +6,4 @@
 from startup import *
 from utility import *
 from world_model import *
+from check_ebutton import *

robot_smach_states/src/robot_smach_states/check_ebutton.py

@@ -0,0 +1,36 @@
+#! /usr/bin/env python
+import rospy
+import smach
+
+from human_interaction import Say
+
+class CheckEButton(smach.State):
+    """Check if the robot's Emergency button is pressed"""
+    def __init__(self, robot=None):
+        smach.State.__init__(self, outcomes=["pressed",
+                                             "released"])
+        self.robot = robot
+
+    def execute(self, userdata=None):
+        if self.robot.ebutton.read_ebutton():
+            return "pressed"
+        else:
+            return "released"
+
+class NotifyEButton(smach.StateMachine):
+    """Alert the operator that the robot's Emergency button is still pressed"""
+
+    def __init__(self, robot):
+        smach.StateMachine.__init__(self, outcomes=["succeeded"])
+        assert hasattr(robot, "ebutton")
+        assert hasattr(robot, "speech")
+
+        with self:
+            smach.StateMachine.add( "CHECK_EBUTTON",
+                                    CheckEButton(robot),
+                                    transitions={   "pressed"   :"SAY_PRESSED",
+                                                    "released"  :"succeeded"})
+
+            smach.StateMachine.add("SAY_PRESSED",
+                                    Say(robot, ["My Emergency button is still pressed", "E-Button still pressed"], block=False),
+                                    transitions={   "spoken":"succeeded"})

robot_smach_states/src/robot_smach_states/startup.py

@@ -8,6 +8,7 @@
 
 import utility
 import human_interaction
+import check_ebutton
 from sensor_msgs.msg import LaserScan
 
 from threading import Event
@@ -24,6 +25,10 @@ def __init__(self, robot, initial_pose, use_entry_points = False, door=True):
         assert hasattr(robot, "speech")
 
         with self:
+            smach.StateMachine.add( "NOTIFY_EBUTTON",
+                                    check_ebutton.NotifyEButton(robot),
+                                    transitions={   "succeeded"     :"INITIALIZE"})
+
             smach.StateMachine.add( "INITIALIZE",
                                     utility.Initialize(robot),
                                     transitions={   "initialized"   :"INSTRUCT_WAIT_FOR_DOOR" if door else "INIT_POSE",