tuck_arm.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2015, Fetch Robotics Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #     * Redistributions of source code must retain the above copyright
00010 #       notice, this list of conditions and the following disclaimer.
00011 #     * Redistributions in binary form must reproduce the above copyright
00012 #       notice, this list of conditions and the following disclaimer in the
00013 #       documentation and/or other materials provided with the distribution.
00014 #     * Neither the name of the Fetch Robotics Inc. nor the names of its
00015 #       contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
00022 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00027 # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # Author: Michael Ferguson
00030 
00031 import argparse
00032 import subprocess
00033 import sys
00034 from threading import Thread
00035 
00036 import rospy
00037 from sensor_msgs.msg import Joy
00038 from moveit_msgs.msg import MoveItErrorCodes, PlanningScene
00039 from moveit_python import MoveGroupInterface, PlanningSceneInterface
00040 
00041 class MoveItThread(Thread):
00042 
00043     def __init__(self):
00044         Thread.__init__(self)
00045         self.start()
00046 
00047     def run(self):
00048         self.process = subprocess.Popen(["roslaunch", "fetch_moveit_config", "move_group.launch", "--wait"])
00049         _, _ = self.process.communicate()
00050 
00051     def stop(self):
00052         self.process.send_signal(subprocess.signal.SIGINT)
00053         self.process.wait()
00054 
00055 def is_moveit_running():
00056     output = subprocess.check_output(["rosnode", "info", "move_group"], stderr=subprocess.STDOUT)
00057     if output.find("unknown node") >= 0:
00058         return False
00059     if output.find("Communication with node") >= 0:
00060         return False
00061     return True
00062 
00063 class TuckThread(Thread):
00064 
00065     def __init__(self):
00066         Thread.__init__(self)
00067         self.client = None
00068         self.start()
00069 
00070     def run(self):
00071         move_thread = None
00072         if not is_moveit_running():
00073             rospy.loginfo("starting moveit")
00074             move_thread = MoveItThread()
00075 
00076         rospy.loginfo("Waiting for MoveIt...")
00077         self.client = MoveGroupInterface("arm_with_torso", "base_link")
00078         rospy.loginfo("...connected")
00079 
00080         # Padding does not work (especially for self collisions)
00081         # So we are adding a box above the base of the robot
00082         scene = PlanningSceneInterface("base_link")
00083         scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375)
00084 
00085         joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00086                   "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00087         pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
00088         while not rospy.is_shutdown():
00089             result = self.client.moveToJointPosition(joints,
00090                                                      pose,
00091                                                      0.0,
00092                                                      max_velocity_scaling_factor=0.5)
00093             if result and result.error_code.val == MoveItErrorCodes.SUCCESS:
00094                 scene.removeCollisionObject("keepout")
00095                 if move_thread:
00096                     move_thread.stop()
00097 
00098                 # On success quit
00099                 # Stopping the MoveIt thread works, however, the action client
00100                 # does not get shut down, and future tucks will not work.
00101                 # As a work-around, we die and roslaunch will respawn us.
00102                 rospy.signal_shutdown("done")
00103                 sys.exit(0)
00104                 return
00105 
00106     def stop(self):
00107         if self.client:
00108             self.client.get_move_action().cancel_goal()
00109         # Stopping the MoveIt thread works, however, the action client
00110         # does not get shut down, and future tucks will not work.
00111         # As a work-around, we die and roslaunch will respawn us.
00112         rospy.signal_shutdown("failed")
00113         sys.exit(0)
00114 
00115 class TuckArmTeleop:
00116 
00117     def __init__(self):
00118         self.tuck_button = rospy.get_param("~tuck_button", 6)  # default button is the down button
00119         self.deadman = rospy.get_param("~deadman_button", 10)
00120         self.tucking = False
00121 
00122         self.pressed = False
00123         self.pressed_last = None
00124 
00125         self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_callback)
00126 
00127     def joy_callback(self, msg):
00128         if self.tucking:
00129             # Only run once
00130             if msg.buttons[self.deadman] <= 0:
00131                 # Deadman has been released, don't tuck
00132                 rospy.loginfo("Stopping tuck thread")
00133                 self.tuck_thread.stop()
00134             return
00135         try:
00136             if msg.buttons[self.tuck_button] > 0 and msg.buttons[self.deadman] > 0:
00137                 if not self.pressed:
00138                     self.pressed_last = rospy.Time.now()
00139                     self.pressed = True
00140                 elif self.pressed_last and rospy.Time.now() > self.pressed_last + rospy.Duration(1.0):
00141                     # Tuck the arm
00142                     self.tucking = True
00143                     rospy.loginfo("Starting tuck thread")
00144                     self.tuck_thread = TuckThread()
00145             else:
00146                 self.pressed = False
00147         except KeyError:
00148             rospy.logwarn("tuck_button is out of range")
00149 
00150 if __name__ == "__main__":
00151     parser = argparse.ArgumentParser(description="Tuck the arm, either immediately or as a joystck-controlled server.")
00152     parser.add_argument("--joystick", action="store_true", help="Run as server that tucks on command from joystick.")
00153     args, unknown = parser.parse_known_args()
00154 
00155     rospy.init_node("tuck_arm")
00156     rospy.loginfo("New tuck script running")
00157 
00158     if args.joystick:
00159         t = TuckArmTeleop()
00160         rospy.spin()
00161     else:
00162         rospy.loginfo("Tucking the arm")
00163         TuckThread()


fetch_teleop
Author(s): Michael Ferguson
autogenerated on Sat Apr 27 2019 03:12:18