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
00032
00033
00034
00035
00036
00037
00038
00039
00040 import xml.dom.minidom
00041 from operator import add
00042 import sys
00043 import threading
00044 from moveit_ros_planning_interface._moveit_robot_interface import RobotInterface
00045
00046 import rospy
00047 import roslib
00048 import numpy
00049 import time
00050 import tf
00051 from std_msgs.msg import Empty, String
00052 from sensor_msgs.msg import Joy
00053 from geometry_msgs.msg import PoseStamped
00054 from visualization_msgs.msg import InteractiveMarkerInit
00055
00056 def signedSquare(val):
00057 if val > 0:
00058 sign = 1
00059 else:
00060 sign = -1
00061 return val * val * sign
00062
00063
00064
00065
00066 class JoyStatus():
00067 def __init__(self):
00068 self.center = False
00069 self.select = False
00070 self.start = False
00071 self.L3 = False
00072 self.R3 = False
00073 self.square = False
00074 self.up = False
00075 self.down = False
00076 self.left = False
00077 self.right = False
00078 self.triangle = False
00079 self.cross = False
00080 self.circle = False
00081 self.L1 = False
00082 self.R1 = False
00083 self.L2 = False
00084 self.R2 = False
00085 self.left_analog_x = 0.0
00086 self.left_analog_y = 0.0
00087 self.right_analog_x = 0.0
00088 self.right_analog_y = 0.0
00089
00090 class XBoxStatus(JoyStatus):
00091 def __init__(self, msg):
00092 JoyStatus.__init__(self)
00093 if msg.buttons[8] == 1:
00094 self.center = True
00095 else:
00096 self.center = False
00097 if msg.buttons[6] == 1:
00098 self.select = True
00099 else:
00100 self.select = False
00101 if msg.buttons[7] == 1:
00102 self.start = True
00103 else:
00104 self.start = False
00105 if msg.buttons[9] == 1:
00106 self.L3 = True
00107 else:
00108 self.L3 = False
00109 if msg.buttons[10] == 1:
00110 self.R3 = True
00111 else:
00112 self.R3 = False
00113 if msg.buttons[2] == 1:
00114 self.square = True
00115 else:
00116 self.square = False
00117 if msg.buttons[1] == 1:
00118 self.circle = True
00119 else:
00120 self.circle = False
00121 if msg.axes[7] > 0.1:
00122 self.up = True
00123 else:
00124 self.up = False
00125 if msg.axes[7] < -0.1:
00126 self.down = True
00127 else:
00128 self.down = False
00129 if msg.axes[6] > 0.1:
00130 self.left = True
00131 else:
00132 self.left = False
00133 if msg.axes[6] < -0.1:
00134 self.right = True
00135 else:
00136 self.right = False
00137 if msg.buttons[3] == 1:
00138 self.triangle = True
00139 else:
00140 self.triangle = False
00141 if msg.buttons[0] == 1:
00142 self.cross = True
00143 else:
00144 self.cross = False
00145 if msg.buttons[4] == 1:
00146 self.L1 = True
00147 else:
00148 self.L1 = False
00149 if msg.buttons[5] == 1:
00150 self.R1 = True
00151 else:
00152 self.R1 = False
00153 if msg.axes[2] < -0.5:
00154 self.L2 = True
00155 else:
00156 self.L2 = False
00157 if msg.axes[5] < -0.5:
00158 self.R2 = True
00159 else:
00160 self.R2 = False
00161 self.left_analog_x = msg.axes[0]
00162 self.left_analog_y = msg.axes[1]
00163 self.right_analog_x = msg.axes[3]
00164 self.right_analog_y = msg.axes[4]
00165 self.orig_msg = msg
00166
00167 class PS3Status(JoyStatus):
00168 def __init__(self, msg):
00169 JoyStatus.__init__(self)
00170
00171 if msg.buttons[16] == 1:
00172 self.center = True
00173 else:
00174 self.center = False
00175 if msg.buttons[0] == 1:
00176 self.select = True
00177 else:
00178 self.select = False
00179 if msg.buttons[3] == 1:
00180 self.start = True
00181 else:
00182 self.start = False
00183 if msg.buttons[1] == 1:
00184 self.L3 = True
00185 else:
00186 self.L3 = False
00187 if msg.buttons[2] == 1:
00188 self.R3 = True
00189 else:
00190 self.R3 = False
00191 if msg.axes[15] < 0:
00192 self.square = True
00193 else:
00194 self.square = False
00195 if msg.axes[4] < 0:
00196 self.up = True
00197 else:
00198 self.up = False
00199 if msg.axes[6] < 0:
00200 self.down = True
00201 else:
00202 self.down = False
00203 if msg.axes[7] < 0:
00204 self.left = True
00205 else:
00206 self.left = False
00207 if msg.axes[5] < 0:
00208 self.right = True
00209 else:
00210 self.right = False
00211 if msg.axes[12] < 0:
00212 self.triangle = True
00213 else:
00214 self.triangle = False
00215 if msg.axes[14] < 0:
00216 self.cross = True
00217 else:
00218 self.cross = False
00219 if msg.axes[13] < 0:
00220 self.circle = True
00221 else:
00222 self.circle = False
00223 if msg.axes[10] < 0:
00224 self.L1 = True
00225 else:
00226 self.L1 = False
00227 if msg.axes[11] < 0:
00228 self.R1 = True
00229 else:
00230 self.R1 = False
00231 if msg.axes[8] < 0:
00232 self.L2 = True
00233 else:
00234 self.L2 = False
00235 if msg.axes[9] < 0:
00236 self.R2 = True
00237 else:
00238 self.R2 = False
00239 self.left_analog_x = msg.axes[0]
00240 self.left_analog_y = msg.axes[1]
00241 self.right_analog_x = msg.axes[2]
00242 self.right_analog_y = msg.axes[3]
00243 self.orig_msg = msg
00244
00245 class PS3WiredStatus(JoyStatus):
00246 def __init__(self, msg):
00247 JoyStatus.__init__(self)
00248
00249 if msg.buttons[16] == 1:
00250 self.center = True
00251 else:
00252 self.center = False
00253 if msg.buttons[0] == 1:
00254 self.select = True
00255 else:
00256 self.select = False
00257 if msg.buttons[3] == 1:
00258 self.start = True
00259 else:
00260 self.start = False
00261 if msg.buttons[1] == 1:
00262 self.L3 = True
00263 else:
00264 self.L3 = False
00265 if msg.buttons[2] == 1:
00266 self.R3 = True
00267 else:
00268 self.R3 = False
00269 if msg.buttons[15] == 1:
00270 self.square = True
00271 else:
00272 self.square = False
00273 if msg.buttons[4] == 1:
00274 self.up = True
00275 else:
00276 self.up = False
00277 if msg.buttons[6] == 1:
00278 self.down = True
00279 else:
00280 self.down = False
00281 if msg.buttons[7] == 1:
00282 self.left = True
00283 else:
00284 self.left = False
00285 if msg.buttons[5] == 1:
00286 self.right = True
00287 else:
00288 self.right = False
00289 if msg.buttons[12] == 1:
00290 self.triangle = True
00291 else:
00292 self.triangle = False
00293 if msg.buttons[14] == 1:
00294 self.cross = True
00295 else:
00296 self.cross = False
00297 if msg.buttons[13] == 1:
00298 self.circle = True
00299 else:
00300 self.circle = False
00301 if msg.buttons[10] == 1:
00302 self.L1 = True
00303 else:
00304 self.L1 = False
00305 if msg.buttons[11] == 1:
00306 self.R1 = True
00307 else:
00308 self.R1 = False
00309 if msg.buttons[8] == 1:
00310 self.L2 = True
00311 else:
00312 self.L2 = False
00313 if msg.buttons[9] == 1:
00314 self.R2 = True
00315 else:
00316 self.R2 = False
00317 self.left_analog_x = msg.axes[0]
00318 self.left_analog_y = msg.axes[1]
00319 self.right_analog_x = msg.axes[2]
00320 self.right_analog_y = msg.axes[3]
00321 self.orig_msg = msg
00322
00323 class StatusHistory():
00324 def __init__(self, max_length=10):
00325 self.max_length = max_length
00326 self.buffer = []
00327 def add(self, status):
00328 self.buffer.append(status)
00329 if len(self.buffer) > self.max_length:
00330 self.buffer = self.buffer[1:self.max_length+1]
00331 def all(self, proc):
00332 for status in self.buffer:
00333 if not proc(status):
00334 return False
00335 return True
00336 def latest(self):
00337 if len(self.buffer) > 0:
00338 return self.buffer[-1]
00339 else:
00340 return None
00341 def length(self):
00342 return len(self.buffer)
00343 def new(self, status, attr):
00344 if len(self.buffer) == 0:
00345 return getattr(status, attr)
00346 else:
00347 return getattr(status, attr) and not getattr(self.latest(), attr)
00348
00349 class MoveitJoy:
00350 def parseSRDF(self):
00351 ri = RobotInterface("/robot_description")
00352 planning_groups = {}
00353 for g in ri.get_group_names():
00354 self.planning_groups_tips[g] = ri.get_group_joint_tips(g)
00355 planning_groups[g] = ["/rviz/moveit/move_marker/goal_" + l
00356 for l in self.planning_groups_tips[g]]
00357 for name in planning_groups.keys():
00358 if len(planning_groups[name]) == 0:
00359 del planning_groups[name]
00360 else:
00361 print name, planning_groups[name]
00362 self.planning_groups = planning_groups
00363 self.planning_groups_keys = planning_groups.keys()
00364 self.frame_id = ri.get_planning_frame()
00365 def __init__(self):
00366 self.initial_poses = {}
00367 self.planning_groups_tips = {}
00368 self.tf_listener = tf.TransformListener()
00369 self.marker_lock = threading.Lock()
00370 self.prev_time = rospy.Time.now()
00371 self.counter = 0
00372 self.history = StatusHistory(max_length=10)
00373 self.pre_pose = PoseStamped()
00374 self.pre_pose.pose.orientation.w = 1
00375 self.current_planning_group_index = 0
00376 self.current_eef_index = 0
00377 self.initialize_poses = False
00378 self.initialized = False
00379 self.parseSRDF()
00380 self.plan_group_pub = rospy.Publisher('/rviz/moveit/select_planning_group', String, queue_size=5)
00381 self.updatePlanningGroup(0)
00382 self.updatePoseTopic(0, False)
00383 self.joy_pose_pub = rospy.Publisher("/joy_pose", PoseStamped, queue_size=1)
00384 self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty, queue_size=5)
00385 self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty, queue_size=5)
00386 self.update_start_state_pub = rospy.Publisher("/rviz/moveit/update_start_state", Empty, queue_size=5)
00387 self.update_goal_state_pub = rospy.Publisher("/rviz/moveit/update_goal_state", Empty, queue_size=5)
00388 self.interactive_marker_sub = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full",
00389 InteractiveMarkerInit,
00390 self.markerCB, queue_size=1)
00391 self.sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)
00392 def updatePlanningGroup(self, next_index):
00393 if next_index >= len(self.planning_groups_keys):
00394 self.current_planning_group_index = 0
00395 elif next_index < 0:
00396 self.current_planning_group_index = len(self.planning_groups_keys) - 1
00397 else:
00398 self.current_planning_group_index = next_index
00399 next_planning_group = None
00400 try:
00401 next_planning_group = self.planning_groups_keys[self.current_planning_group_index]
00402 except IndexError:
00403 msg = 'Check if you started movegroups. Exiting.'
00404 rospy.logfatal(msg)
00405 raise rospy.ROSInitException(msg)
00406 rospy.loginfo("Changed planning group to " + next_planning_group)
00407 self.plan_group_pub.publish(next_planning_group)
00408 def updatePoseTopic(self, next_index, wait=True):
00409 planning_group = self.planning_groups_keys[self.current_planning_group_index]
00410 topics = self.planning_groups[planning_group]
00411 if next_index >= len(topics):
00412 self.current_eef_index = 0
00413 elif next_index < 0:
00414 self.current_eef_index = len(topics) - 1
00415 else:
00416 self.current_eef_index = next_index
00417 next_topic = topics[self.current_eef_index]
00418
00419 rospy.loginfo("Changed controlled end effector to " + self.planning_groups_tips[planning_group][self.current_eef_index])
00420 self.pose_pub = rospy.Publisher(next_topic, PoseStamped, queue_size=5)
00421 if wait:
00422 self.waitForInitialPose(next_topic)
00423 self.current_pose_topic = next_topic
00424 def markerCB(self, msg):
00425 try:
00426 self.marker_lock.acquire()
00427 if not self.initialize_poses:
00428 return
00429 self.initial_poses = {}
00430 for marker in msg.markers:
00431 if marker.name.startswith("EE:goal_"):
00432
00433 if marker.header.frame_id != self.frame_id:
00434 ps = PoseStamped(header=marker.header, pose=marker.pose)
00435 try:
00436 transformed_pose = self.tf_listener.transformPose(self.frame_id, ps)
00437 self.initial_poses[marker.name[3:]] = transformed_pose.pose
00438 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e):
00439 rospy.logerr("tf error when resolving tf: %s" % e)
00440 else:
00441 self.initial_poses[marker.name[3:]] = marker.pose
00442 finally:
00443 self.marker_lock.release()
00444 def waitForInitialPose(self, next_topic, timeout=None):
00445 counter = 0
00446 while not rospy.is_shutdown():
00447 counter = counter + 1
00448 if timeout and counter >= timeout:
00449 return False
00450 try:
00451 self.marker_lock.acquire()
00452 self.initialize_poses = True
00453 topic_suffix = next_topic.split("/")[-1]
00454 if self.initial_poses.has_key(topic_suffix):
00455 self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix])
00456 self.initialize_poses = False
00457 return True
00458 else:
00459 rospy.logdebug(self.initial_poses.keys())
00460 rospy.loginfo("Waiting for pose topic of '%s' to be initialized",
00461 topic_suffix)
00462 rospy.sleep(1)
00463 finally:
00464 self.marker_lock.release()
00465 def joyCB(self, msg):
00466 if len(msg.axes) == 27 and len(msg.buttons) == 19:
00467 status = PS3WiredStatus(msg)
00468 elif len(msg.axes) == 8 and len(msg.buttons) == 11:
00469 status = XBoxStatus(msg)
00470 elif len(msg.axes) == 20 and len(msg.buttons) == 17:
00471 status = PS3Status(msg)
00472 else:
00473 raise Exception("Unknown joystick")
00474 self.run(status)
00475 self.history.add(status)
00476 def computePoseFromJoy(self, pre_pose, status):
00477 new_pose = PoseStamped()
00478 new_pose.header.frame_id = self.frame_id
00479 new_pose.header.stamp = rospy.Time(0.0)
00480
00481 dist = status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x
00482 scale = 200.0
00483 x_diff = signedSquare(status.left_analog_y) / scale
00484 y_diff = signedSquare(status.left_analog_x) / scale
00485
00486 if status.L2:
00487 z_diff = 0.005
00488 elif status.R2:
00489 z_diff = -0.005
00490 else:
00491 z_diff = 0.0
00492 if self.history.all(lambda s: s.L2) or self.history.all(lambda s: s.R2):
00493 z_scale = 4.0
00494 else:
00495 z_scale = 2.0
00496 local_move = numpy.array((x_diff, y_diff,
00497 z_diff * z_scale,
00498 1.0))
00499 q = numpy.array((pre_pose.pose.orientation.x,
00500 pre_pose.pose.orientation.y,
00501 pre_pose.pose.orientation.z,
00502 pre_pose.pose.orientation.w))
00503 xyz_move = numpy.dot(tf.transformations.quaternion_matrix(q),
00504 local_move)
00505 new_pose.pose.position.x = pre_pose.pose.position.x + xyz_move[0]
00506 new_pose.pose.position.y = pre_pose.pose.position.y + xyz_move[1]
00507 new_pose.pose.position.z = pre_pose.pose.position.z + xyz_move[2]
00508 roll = 0.0
00509 pitch = 0.0
00510 yaw = 0.0
00511 DTHETA = 0.005
00512 if status.L1:
00513 if self.history.all(lambda s: s.L1):
00514 yaw = yaw + DTHETA * 2
00515 else:
00516 yaw = yaw + DTHETA
00517 elif status.R1:
00518 if self.history.all(lambda s: s.R1):
00519 yaw = yaw - DTHETA * 2
00520 else:
00521 yaw = yaw - DTHETA
00522 if status.up:
00523 if self.history.all(lambda s: s.up):
00524 pitch = pitch + DTHETA * 2
00525 else:
00526 pitch = pitch + DTHETA
00527 elif status.down:
00528 if self.history.all(lambda s: s.down):
00529 pitch = pitch - DTHETA * 2
00530 else:
00531 pitch = pitch - DTHETA
00532 if status.right:
00533 if self.history.all(lambda s: s.right):
00534 roll = roll + DTHETA * 2
00535 else:
00536 roll = roll + DTHETA
00537 elif status.left:
00538 if self.history.all(lambda s: s.left):
00539 roll = roll - DTHETA * 2
00540 else:
00541 roll = roll - DTHETA
00542 diff_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
00543 new_q = tf.transformations.quaternion_multiply(q, diff_q)
00544 new_pose.pose.orientation.x = new_q[0]
00545 new_pose.pose.orientation.y = new_q[1]
00546 new_pose.pose.orientation.z = new_q[2]
00547 new_pose.pose.orientation.w = new_q[3]
00548 return new_pose
00549 def run(self, status):
00550 if not self.initialized:
00551
00552 while True:
00553 self.updatePlanningGroup(self.current_planning_group_index)
00554 planning_group = self.planning_groups_keys[self.current_planning_group_index]
00555 topics = self.planning_groups[planning_group]
00556 next_topic = topics[self.current_eef_index]
00557 if not self.waitForInitialPose(next_topic, timeout=3):
00558 rospy.logwarn("Unable to initialize planning group " + planning_group + ". Trying different group.")
00559 rospy.logwarn("Is 'Allow External Comm.' enabled in Rviz? Is the 'Query Goal State' robot enabled?")
00560 else:
00561 rospy.loginfo("Initialized planning group")
00562 self.initialized = True
00563 self.updatePoseTopic(self.current_eef_index)
00564 return
00565
00566 self.current_planning_group_index += 1
00567 if self.current_planning_group_index >= len(self.planning_groups_keys):
00568 self.current_planning_group_index = 0
00569 if self.history.new(status, "select"):
00570 self.updatePlanningGroup(self.current_planning_group_index + 1)
00571 self.current_eef_index = 0
00572 self.updatePoseTopic(self.current_eef_index)
00573 return
00574 elif self.history.new(status, "start"):
00575 self.updatePlanningGroup(self.current_planning_group_index - 1)
00576 self.current_eef_index = 0
00577 self.updatePoseTopic(self.current_eef_index)
00578 return
00579 elif self.history.new(status, "triangle"):
00580 self.updatePoseTopic(self.current_eef_index + 1)
00581 return
00582 elif self.history.new(status, "cross"):
00583 self.updatePoseTopic(self.current_eef_index - 1)
00584 return
00585 elif self.history.new(status, "square"):
00586 rospy.loginfo("Plan")
00587 self.plan_pub.publish(Empty())
00588 return
00589 elif self.history.new(status, "circle"):
00590 rospy.loginfo("Execute")
00591 self.execute_pub.publish(Empty())
00592 return
00593 self.marker_lock.acquire()
00594 pre_pose = self.pre_pose
00595 new_pose = self.computePoseFromJoy(pre_pose, status)
00596 now = rospy.Time.from_sec(time.time())
00597
00598 if (now - self.prev_time).to_sec() > 1 / 30.0:
00599
00600 self.pose_pub.publish(new_pose)
00601 self.joy_pose_pub.publish(new_pose)
00602 self.prev_time = now
00603
00604 self.counter = self.counter + 1
00605 if self.counter % 10:
00606 self.update_start_state_pub.publish(Empty())
00607 self.pre_pose = new_pose
00608 self.marker_lock.release()
00609
00610 self.marker_lock.acquire()
00611 self.initial_poses[self.current_pose_topic.split("/")[-1]] = new_pose.pose
00612 self.marker_lock.release()