Merge pull request ZebraDevs#22 from alexhenning/bugfix/stop-tuck-on-…

…release

Tucking now stops if the deadman is released

fetch_teleop/scripts/tuck_arm.py

@@ -30,6 +30,7 @@
 
 import argparse
 import subprocess
+import sys
 from threading import Thread
 
 import rospy
@@ -59,34 +60,57 @@ def is_moveit_running():
         return False
     return True
 
-def tuck():
-    move_thread = None
-    if not is_moveit_running():
-        rospy.loginfo("starting moveit")
-        move_thread = MoveItThread()
-
-    rospy.loginfo("Waiting for MoveIt...")
-    client = MoveGroupInterface("arm_with_torso", "base_link")
-    rospy.loginfo("...connected")
-
-    # Padding does not work (especially for self collisions)
-    # So we are adding a box above the base of the robot
-    scene = PlanningSceneInterface("base_link")
-    scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
-
-    joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
-              "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
-    pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
-    while not rospy.is_shutdown():
-        result = client.moveToJointPosition(joints,
-                                            pose,
-                                            0.0,
-                                            max_velocity_scaling_factor=0.5)
-        if result.error_code.val == MoveItErrorCodes.SUCCESS:
-            scene.removeCollisionObject("keepout")
-            if move_thread:
-                move_thread.stop()
-            return
+class TuckThread(Thread):
+
+    def __init__(self):
+        Thread.__init__(self)
+        self.client = None
+        self.start()
+
+    def run(self):
+        move_thread = None
+        if not is_moveit_running():
+            rospy.loginfo("starting moveit")
+            move_thread = MoveItThread()
+
+        rospy.loginfo("Waiting for MoveIt...")
+        self.client = MoveGroupInterface("arm_with_torso", "base_link")
+        rospy.loginfo("...connected")
+
+        # Padding does not work (especially for self collisions)
+        # So we are adding a box above the base of the robot
+        scene = PlanningSceneInterface("base_link")
+        scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
+
+        joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
+                  "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
+        pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
+        while not rospy.is_shutdown():
+            result = self.client.moveToJointPosition(joints,
+                                                     pose,
+                                                     0.0,
+                                                     max_velocity_scaling_factor=0.5)
+            if result.error_code.val == MoveItErrorCodes.SUCCESS:
+                scene.removeCollisionObject("keepout")
+                if move_thread:
+                    move_thread.stop()
+
+                # On success quit
+                # Stopping the MoveIt thread works, however, the action client
+                # does not get shut down, and future tucks will not work.
+                # As a work-around, we die and roslaunch will respawn us.
+                rospy.signal_shutdown("done")
+                sys.exit(0)
+                return
+
+    def stop(self):
+        if self.client:
+            self.client.get_move_action().cancel_goal()
+        # Stopping the MoveIt thread works, however, the action client
+        # does not get shut down, and future tucks will not work.
+        # As a work-around, we die and roslaunch will respawn us.
+        rospy.signal_shutdown("failed")
+        sys.exit(0)
 
 class TuckArmTeleop:
 
@@ -102,7 +126,11 @@ def __init__(self):
 
     def joy_callback(self, msg):
         if self.tucking:
-            # only run once
+            # Only run once
+            if msg.buttons[self.deadman] <= 0:
+                # Deadman has been released, don't tuck
+                rospy.loginfo("Stopping tuck thread")
+                self.tuck_thread.stop()
             return
         try:
             if msg.buttons[self.tuck_button] > 0 and msg.buttons[self.deadman] > 0:
@@ -112,12 +140,8 @@ def joy_callback(self, msg):
                 elif self.pressed_last and rospy.Time.now() > self.pressed_last + rospy.Duration(1.0):
                     # Tuck the arm
                     self.tucking = True
-                    tuck()
-                    # Stopping the MoveIt thread works, however, the action client
-                    # does not get shut down, and future tucks will not work.
-                    # As a work-around, we die and roslaunch will respawn us.
-                    rospy.signal_shutdown("done")
-                    exit(0)
+                    rospy.loginfo("Starting tuck thread")
+                    self.tuck_thread = TuckThread()
             else:
                 self.pressed = False
         except KeyError:
@@ -129,10 +153,11 @@ def joy_callback(self, msg):
     args, unknown = parser.parse_known_args()
 
     rospy.init_node("tuck_arm")
+    rospy.loginfo("New tuck script running")
 
     if args.joystick:
         t = TuckArmTeleop()
         rospy.spin()
     else:
         rospy.loginfo("Tucking the arm")
-        tuck()
+        TuckThread()