Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
00081
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
00099
00100
00101
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
00110
00111
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)
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
00130 if msg.buttons[self.deadman] <= 0:
00131
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
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()