00001
00002
00003 """
00004 Copyright (c) 2012, General Motors, Co.
00005 All rights reserved.
00006
00007 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00008 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00009 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00010 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00011 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00012 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00013 SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS
00014 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00015 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00016 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00017 POSSIBILITY OF SUCH DAMAGE.
00018 """
00019
00020 import roslib;
00021 roslib.load_manifest("r2_teleop")
00022 import rospy
00023 import math
00024 import copy
00025 import PyKDL as kdl
00026
00027 import tf
00028 from interactive_markers.interactive_marker_server import *
00029 from interactive_markers.menu_handler import *
00030
00031 from sensor_msgs.msg import JointState
00032 from geometry_msgs.msg import Pose
00033 from geometry_msgs.msg import Point
00034 from geometry_msgs.msg import Quaternion
00035 from geometry_msgs.msg import PoseStamped
00036 from geometry_msgs.msg import TransformStamped
00037
00038 import kdl_posemath as pm
00039 import r2_helper as r2
00040 import frames
00041 from marker_helper import *
00042
00043 TORAD = math.pi/180.0
00044 TODEG = 1.0/TORAD
00045
00046 server = None
00047 counter = 0
00048
00049 joint_mode = [False, False]
00050 edit_tool = [False, False]
00051 edit_setpoint = [False, False]
00052
00053 gaze_control_mode = False
00054 gaze_tracking_mode = False
00055 gaze_point_tracking_mode = False
00056 gaze_left_tracking_mode = False
00057 gaze_right_tracking_mode = False
00058
00059 power_mode = False
00060
00061 left_arm_menu_handler = MenuHandler()
00062 right_arm_menu_handler = MenuHandler()
00063 waist_menu_handler = MenuHandler()
00064 head_menu_handler = MenuHandler()
00065 gaze_menu_handler = MenuHandler()
00066 backpack_menu_handler = MenuHandler()
00067 left_setpoint_menu_handler = MenuHandler()
00068 right_setpoint_menu_handler = MenuHandler()
00069
00070 tool_offset = [Pose(), Pose()]
00071 setpoint_offset = [Pose(), Pose()]
00072 setpoint_store = [PoseStamped(), PoseStamped()]
00073 joint_data = JointState()
00074
00075 left_arm_cart_marker = InteractiveMarker()
00076 right_arm_cart_marker = InteractiveMarker()
00077 neck_lower_pitch_marker = InteractiveMarker()
00078 neck_roll_marker = InteractiveMarker()
00079 neck_upper_pitch_marker = InteractiveMarker()
00080 left_posture_marker = InteractiveMarker()
00081 right_posture_marker = InteractiveMarker()
00082
00083 left_finger_markers = [InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker()]
00084 right_finger_markers = [InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker(), InteractiveMarker()]
00085
00086 gaze_marker = InteractiveMarker()
00087 backpack_marker = InteractiveMarker()
00088 left_setpoint_marker = InteractiveMarker()
00089 right_setpoint_marker = InteractiveMarker()
00090
00091 gaze_sphere_scale = 0.5
00092
00093 fast_update_rate = 10
00094 slow_update_divider = 10
00095 dur_time = 2.0
00096
00097 tf_listener = None
00098
00099 def resetMarker( feedback, frame_id ):
00100 pose = feedback.pose
00101 pose.position.x = 0
00102 pose.position.y = 0
00103 pose.position.z = 0
00104 pose.orientation.x = 0
00105 pose.orientation.y = 0
00106 pose.orientation.z = 0
00107 pose.orientation.w = 1
00108 server.setPose( frame_id, pose )
00109 server.applyChanges()
00110
00111 def ResetToolOffset(arm) :
00112 global tool_offset
00113 if arm == "left" :
00114 tool_offset[0] = Pose()
00115 tool_offset[0].orientation.w = 1
00116 if arm == "right" :
00117 tool_offset[1] = Pose()
00118 tool_offset[1].orientation.w = 1
00119
00120 def ResetSetpointOffset(arm) :
00121 global setpoint_offset
00122 if arm == "left" :
00123 setpoint_offset[0] = Pose()
00124 setpoint_offset[0].orientation.w = 1
00125 if arm == "right" :
00126 setpoint_offset[1] = Pose()
00127 setpoint_offset[1].orientation.w = 1
00128
00129 def handleLeftArmMenu( feedback ) :
00130 global edit_tool, edit_setpoint
00131 handle = feedback.menu_entry_id
00132
00133 if(handle == 1) :
00134 r2.leftCartReadyPose.header.seq = r2.leftCartReadyPose.header.seq + 1
00135 r2.leftCartReadyPose.header.stamp = rospy.get_rostime()
00136 r2.left_pose_pub.publish(r2.leftCartReadyPose)
00137 r2.leftJointReadyPose.header.seq = r2.leftJointReadyPose.header.seq + 1
00138 r2.leftJointReadyPose.header.stamp = rospy.get_rostime()
00139 r2.left_jnt_pub.publish(r2.leftJointReadyPose)
00140
00141 elif(handle == 2) :
00142 state = left_arm_menu_handler.getCheckState( handle )
00143 if state == MenuHandler.CHECKED:
00144 left_arm_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00145 rospy.loginfo("Setting Cartesian Control Mode for Left Arm")
00146 r2.SetArmToCartMode('left', frames.control_frame_id[0])
00147 joint_mode[0] = False
00148 makeLeftArmControl( )
00149 else:
00150 left_arm_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00151 rospy.loginfo("Setting Joint Control Mode for Left Arm")
00152 r2.SetArmToJointMode('left')
00153 joint_mode[0] = True
00154 removeLeftArmControl()
00155
00156 elif(handle == 3) :
00157 state = left_arm_menu_handler.getCheckState( handle )
00158 if state == MenuHandler.CHECKED:
00159 left_arm_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00160 edit_tool[0] = False
00161 rospy.loginfo("Fixing Left Arm tool offset")
00162 else:
00163 edit_tool[0] = True
00164 left_arm_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00165 rospy.loginfo("Moving Left Arm tool offset ")
00166
00167 elif(handle == 4) :
00168 state = left_arm_menu_handler.getCheckState( handle )
00169 if state == MenuHandler.CHECKED:
00170 left_arm_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00171 rospy.loginfo("Resetting Left Arm Reach Point")
00172 removeLeftSetpointControl()
00173 makeLeftArmControl()
00174 edit_setpoint[0] = False
00175 else:
00176 left_arm_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00177 rospy.loginfo("Setting Left Arm Reach Point ")
00178 removeLeftArmControl()
00179 ResetSetpointOffset("left")
00180 makeLeftSetpointControl()
00181 edit_setpoint[0] = True
00182
00183 elif(handle == 6) :
00184 print "open left hand"
00185 r2.leftHandOpen.header.seq = r2.leftHandOpen.header.seq + 1
00186 r2.leftHandOpen.header.stamp = rospy.get_rostime()
00187 r2.left_jnt_pub.publish(r2.leftHandOpen)
00188 resetMarker(feedback, frames.control_marker_id[0])
00189
00190 elif(handle == 7) :
00191 print "close left hand"
00192 r2.leftHandClose.header.seq = r2.leftHandClose.header.seq + 1
00193 r2.leftHandClose.header.stamp = rospy.get_rostime()
00194 r2.left_jnt_pub.publish(r2.leftHandClose)
00195 resetMarker(feedback, frames.control_marker_id[0])
00196
00197 elif(handle == 8) :
00198 print "Reseting Left Tool Control Frame"
00199 ResetToolOffset("left")
00200 resetMarker(feedback, frames.control_marker_id[0])
00201
00202 left_arm_menu_handler.reApply( server )
00203
00204
00205 def handleRightArmMenu( feedback ) :
00206 global edit_tool, edit_setpoint
00207 handle = feedback.menu_entry_id
00208
00209 if(handle == 1) :
00210 r2.rightCartReadyPose.header.seq = r2.rightCartReadyPose.header.seq + 1
00211 r2.rightCartReadyPose.header.stamp = rospy.get_rostime()
00212 r2.right_pose_pub.publish(r2.rightCartReadyPose)
00213 r2.rightJointReadyPose.header.seq = r2.rightJointReadyPose.header.seq + 1
00214 r2.rightJointReadyPose.header.stamp = rospy.get_rostime()
00215 r2.right_jnt_pub.publish(r2.rightJointReadyPose)
00216
00217 elif(handle == 2) :
00218 state = right_arm_menu_handler.getCheckState( handle )
00219 if state == MenuHandler.CHECKED:
00220 right_arm_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00221 rospy.loginfo("Setting Cartesian Control Mode for Right Arm")
00222 r2.SetArmToCartMode('right', frames.control_frame_id[1])
00223 joint_mode[1] = False
00224 makeRightArmControl( )
00225 else:
00226 right_arm_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00227 rospy.loginfo("Setting Joint Control Mode for Right Arm")
00228 r2.SetArmToJointMode('right')
00229 joint_mode[1] = True
00230 removeRightArmControl()
00231
00232 elif(handle == 3) :
00233 state = right_arm_menu_handler.getCheckState( handle )
00234 if state == MenuHandler.CHECKED:
00235 right_arm_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00236 edit_tool[1] = False
00237 rospy.loginfo("Fixing Right Arm tool offset")
00238 else:
00239 edit_tool[1] = True
00240 right_arm_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00241 rospy.loginfo("Moving Right Arm tool offset ")
00242
00243 elif(handle == 4) :
00244 state = right_arm_menu_handler.getCheckState( handle )
00245 if state == MenuHandler.CHECKED:
00246 right_arm_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00247 rospy.loginfo("Resetting Right Arm Reach Point")
00248 removeRightSetpointControl()
00249 makeRightArmControl()
00250 edit_setpoint[1] = False
00251 else:
00252 right_arm_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00253 rospy.loginfo("Setting Right Arm Reach Point ")
00254 removeRightArmControl()
00255 ResetSetpointOffset("right")
00256 makeRightSetpointControl()
00257 edit_setpoint[1] = True
00258
00259 elif(handle == 6) :
00260 print "open right hand"
00261 r2.rightHandOpen.header.seq = r2.rightHandOpen.header.seq + 1
00262 r2.rightHandOpen.header.stamp = rospy.get_rostime()
00263 r2.right_jnt_pub.publish(r2.rightHandOpen)
00264 resetMarker(feedback, frames.control_marker_id[1])
00265
00266 elif(handle == 7) :
00267 print "close right hand"
00268 r2.rightHandClose.header.seq = r2.rightHandClose.header.seq + 1
00269 r2.rightHandClose.header.stamp = rospy.get_rostime()
00270 r2.right_jnt_pub.publish(r2.rightHandClose)
00271 resetMarker(feedback, frames.control_marker_id[1])
00272
00273 elif(handle == 8) :
00274 print "Reseting Right Tool Control Frame"
00275 ResetToolOffset("right")
00276 resetMarker(feedback, frames.control_marker_id[1])
00277
00278 right_arm_menu_handler.reApply( server )
00279
00280
00281 def handleWaistMenu( feedback ) :
00282 if(feedback.menu_entry_id == 1) :
00283 r2.waistJointReadyPose.header.seq = r2.waistJointReadyPose.header.seq + 1
00284 r2.waistJointReadyPose.header.stamp = rospy.get_rostime()
00285 r2.waist_jnt_pub.publish(r2.waistJointReadyPose)
00286
00287 def handleBackpackMenu( feedback ) :
00288 global power_mode
00289 handle = feedback.menu_entry_id
00290 if(feedback.menu_entry_id == 1) :
00291 state = backpack_menu_handler.getCheckState( handle )
00292 if state == MenuHandler.CHECKED:
00293 backpack_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00294 rospy.loginfo("turning off power")
00295 power_mode = False
00296 else:
00297 backpack_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00298 rospy.loginfo("turning on power")
00299 power_mode = True
00300 r2.SetPower(power_mode)
00301 backpack_menu_handler.reApply( server )
00302
00303 def handleHeadMenu( feedback ) :
00304 handle = feedback.menu_entry_id
00305 if(handle == 1) :
00306 r2.neckJointReadyPose.header.seq = r2.neckJointReadyPose.header.seq + 1
00307 r2.neckJointReadyPose.header.stamp = rospy.get_rostime()
00308 r2.neck_jnt_pub.publish(r2.neckJointReadyPose)
00309 resetMarker(feedback, feedback.marker_name)
00310 elif(handle == 2) :
00311 state = head_menu_handler.getCheckState( handle )
00312 if state == MenuHandler.CHECKED:
00313 head_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00314 rospy.loginfo("turning off head joint control")
00315 removeHeadControl()
00316 else:
00317 head_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00318 rospy.loginfo("turning on head joint control")
00319 makeHeadControl()
00320 head_menu_handler.reApply( server )
00321 elif(feedback.menu_entry_id == 3) :
00322 toggleGazeControl()
00323 elif(feedback.menu_entry_id == 4) :
00324 r2.SegmentTableTop()
00325
00326
00327 def handleGazeMenu( feedback ) :
00328 global gaze_menu_handler
00329 global gaze_tracking_mode, gaze_point_tracking_mode, gaze_left_tracking_mode, gaze_right_tracking_mode
00330 pose = PoseStamped()
00331 handle = feedback.menu_entry_id
00332
00333 if(handle == 1) :
00334 pose.header.seq = pose.header.seq
00335 pose.header.stamp = rospy.get_rostime()
00336 pose.header.frame_id = frames.base_frame_id
00337 pose.pose = feedback.pose
00338 r2.gaze_pose_pub.publish(pose)
00339 gaze_menu_handler.reApply( server )
00340
00341 elif(handle == 3) :
00342 state = gaze_menu_handler.getCheckState( handle )
00343 if state == MenuHandler.CHECKED:
00344 gaze_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00345 rospy.loginfo("turning off gaze point tracking control")
00346 gaze_tracking_mode = False
00347 gaze_point_tracking_mode = False
00348 removeGazeControl()
00349 else:
00350 gaze_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00351 rospy.loginfo("turning on gaze point tracking control")
00352 gaze_menu_handler.setCheckState( handle+1, MenuHandler.UNCHECKED )
00353 gaze_menu_handler.setCheckState( handle+2, MenuHandler.UNCHECKED )
00354 gaze_tracking_mode = True
00355 gaze_point_tracking_mode = True
00356 gaze_left_tracking_mode = False
00357 gaze_right_tracking_mode = False
00358 gaze_menu_handler.reApply( server )
00359
00360 elif(handle == 4) :
00361 state = gaze_menu_handler.getCheckState( handle )
00362 if state == MenuHandler.CHECKED:
00363 gaze_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00364 rospy.loginfo("turning off gaze left tracking control")
00365 gaze_tracking_mode = False
00366 gaze_left_tracking_mode = False
00367 else:
00368 gaze_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00369 gaze_menu_handler.setCheckState( handle-1, MenuHandler.UNCHECKED )
00370 gaze_menu_handler.setCheckState( handle+1, MenuHandler.UNCHECKED )
00371 rospy.loginfo("turning on gaze left tracking control")
00372 gaze_tracking_mode = True
00373 gaze_point_tracking_mode = False
00374 gaze_left_tracking_mode = True
00375 gaze_right_tracking_mode = False
00376 removeGazeControl()
00377 gaze_menu_handler.reApply( server )
00378
00379 elif(handle == 5) :
00380 state = gaze_menu_handler.getCheckState( handle )
00381 if state == MenuHandler.CHECKED:
00382 gaze_menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
00383 rospy.loginfo("turning off gaze right tracking control")
00384 gaze_tracking_mode = False
00385 gaze_right_tracking_mode = False
00386 else:
00387 gaze_menu_handler.setCheckState( handle, MenuHandler.CHECKED )
00388 gaze_menu_handler.setCheckState( handle-1, MenuHandler.UNCHECKED )
00389 gaze_menu_handler.setCheckState( handle-2, MenuHandler.UNCHECKED )
00390 rospy.loginfo("turning on gaze right tracking control")
00391 gaze_tracking_mode = True
00392 gaze_point_tracking_mode = False
00393 gaze_left_tracking_mode = False
00394 gaze_right_tracking_mode = True
00395 removeGazeControl()
00396 gaze_menu_handler.reApply( server )
00397
00398
00399 def handleLeftSetpointMenu( feedback ) :
00400 global edit_setpoint
00401 pose = PoseStamped()
00402 handle = feedback.menu_entry_id
00403 now = rospy.Time(0)
00404 if(handle == 1) :
00405
00406 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[0], now)
00407 pose.pose = pm.toPose(trans, rot)
00408
00409 Fp = pm.fromMsg(pose.pose)
00410 Fs = pm.fromMsg(setpoint_offset[0])
00411
00412
00413 pose.header.seq = 0
00414 pose.header.stamp = now
00415 pose.header.frame_id = frames.base_frame_id
00416 pose.pose = pm.toMsg(Fp * Fs )
00417
00418
00419 r2.left_pose_pub.publish(pose)
00420
00421
00422 removeLeftSetpointControl()
00423 makeLeftArmControl()
00424 left_arm_menu_handler.setCheckState( 4, MenuHandler.UNCHECKED )
00425 left_arm_menu_handler.reApply( server )
00426 edit_setpoint[0] = False
00427
00428 def handleRightSetpointMenu( feedback ) :
00429 global edit_setpoint
00430 pose = PoseStamped()
00431 handle = feedback.menu_entry_id
00432 now = rospy.Time(0)
00433 if(handle == 1) :
00434
00435 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[1], now)
00436 pose.pose = pm.toPose(trans, rot)
00437
00438 Fp = pm.fromMsg(pose.pose)
00439 Fs = pm.fromMsg(setpoint_offset[1])
00440
00441
00442 pose.header.seq = 0
00443 pose.header.stamp = now
00444 pose.header.frame_id = frames.base_frame_id
00445 pose.pose = pm.toMsg(Fp * Fs)
00446
00447
00448 r2.right_pose_pub.publish(pose)
00449
00450
00451 removeRightSetpointControl()
00452 makeRightArmControl()
00453 right_arm_menu_handler.setCheckState( 4, MenuHandler.UNCHECKED )
00454 right_arm_menu_handler.reApply( server )
00455 edit_setpoint[1] = False
00456
00457 def toggleGazeControl() :
00458 global gaze_control_mode, gaze_tracking_mode, gaze_point_tracking_mode, gaze_left_tracking_mode, gaze_right_tracking_mode
00459 gaze_control_mode = not gaze_control_mode
00460 if gaze_control_mode :
00461 makeGazeControl()
00462 gaze_tracking_mode = False
00463 gaze_point_tracking_mode = False
00464 gaze_left_tracking_mode = False
00465 gaze_right_tracking_mode = False
00466 gaze_menu_handler.setCheckState( 3, MenuHandler.UNCHECKED )
00467 gaze_menu_handler.setCheckState( 4, MenuHandler.UNCHECKED )
00468 gaze_menu_handler.setCheckState( 5, MenuHandler.UNCHECKED )
00469 else :
00470 removeGazeControl()
00471 gaze_tracking_mode = False
00472 gaze_point_tracking_mode = False
00473 gaze_left_tracking_mode = False
00474 gaze_right_tracking_mode = False
00475 gaze_menu_handler.setCheckState( 3, MenuHandler.UNCHECKED )
00476 gaze_menu_handler.setCheckState( 4, MenuHandler.UNCHECKED )
00477 gaze_menu_handler.setCheckState( 5, MenuHandler.UNCHECKED )
00478 gaze_menu_handler.reApply( server )
00479
00480 def poseUpdate( feedback ) :
00481 global joint_data, tool_offset, setpoint_offset
00482 pose = PoseStamped()
00483
00484 if feedback.marker_name == frames.gaze_frame_id and gaze_tracking_mode:
00485 pose.header.seq = pose.header.seq
00486 pose.header.stamp = rospy.get_rostime()
00487 pose.header.frame_id = frames.base_frame_id
00488 pose.pose = copy.deepcopy(feedback.pose)
00489 server.setPose( feedback.marker_name, pose.pose )
00490 server.applyChanges()
00491 r2.gaze_pose_pub.publish(pose)
00492
00493
00494 def mouseUpdate( feedback ) :
00495
00496 print "marker_name: ", feedback.marker_name
00497 global joint_data, tool_offset, setpoint_offset
00498
00499 pose = PoseStamped()
00500 stamped = PoseStamped()
00501 now = rospy.get_rostime()
00502
00503 pose.header.seq = 0
00504 pose.header.stamp = now
00505 pose.header.frame_id = feedback.header.frame_id;
00506
00507
00508 wTh = Pose()
00509 wTm = Pose()
00510 jnt_cmd = JointState()
00511 now = rospy.Time(0)
00512
00513 if feedback.marker_name == frames.control_marker_id[0] and edit_tool[0] :
00514 tf_listener.waitForTransform(frames.control_frame_id[0], frames.base_frame_id, now, rospy.Duration(dur_time))
00515 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[0], now)
00516 wTh = pm.toPose(trans, rot)
00517 wTm = feedback.pose
00518 Fwth = pm.fromMsg(wTh)
00519 Fwtm = pm.fromMsg(wTm)
00520 Fhtw = (pm.fromMsg(wTh)).Inverse()
00521 Fhtm = Fhtw*Fwtm
00522 tool_offset[0] = pm.toMsg(Fhtm)
00523 print "Moved left tool frame: ", tool_offset[0]
00524
00525 elif feedback.marker_name == frames.control_marker_id[1] and edit_tool[1] :
00526 tf_listener.waitForTransform(frames.control_frame_id[1], frames.base_frame_id, now, rospy.Duration(dur_time))
00527 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[1], now)
00528 wTh = pm.toPose(trans, rot)
00529 Fwth = pm.fromMsg(wTh)
00530 wTm = feedback.pose
00531 Fwtm = pm.fromMsg(wTm)
00532 Fhtw = (pm.fromMsg(wTh)).Inverse()
00533 Fhtm = Fhtw*Fwtm
00534 tool_offset[1] = pm.toMsg(Fhtm)
00535 print "Moved right tool frame: ", tool_offset[1]
00536
00537 elif feedback.marker_name == frames.control_marker_id[0] :
00538 pose.pose = feedback.pose
00539 tf_listener.waitForTransform(frames.control_frame_id[0], frames.base_frame_id, now, rospy.Duration(dur_time))
00540 ps = tf_listener.transformPose(frames.base_frame_id, pose)
00541 Fp = pm.fromMsg(ps.pose)
00542 Fo = pm.fromMsg(tool_offset[0])
00543
00544 pose.pose = pm.toMsg(Fp * Fo.Inverse())
00545 r2.left_pose_pub.publish(pose)
00546
00547 elif feedback.marker_name == frames.control_marker_id[1] :
00548 pose.pose = feedback.pose
00549 tf_listener.waitForTransform(frames.control_frame_id[1], frames.base_frame_id, now, rospy.Duration(dur_time))
00550 ps = tf_listener.transformPose(frames.base_frame_id, pose)
00551 Fp = pm.fromMsg(ps.pose)
00552 Fo = pm.fromMsg(tool_offset[1])
00553
00554 pose.pose = pm.toMsg(Fp * Fo.Inverse())
00555 r2.right_pose_pub.publish(pose)
00556
00557 elif feedback.marker_name == frames.gaze_frame_id and gaze_tracking_mode:
00558 pose.pose = feedback.pose
00559 r2.gaze_pose_pub.publish(pose)
00560
00561 elif feedback.marker_name == frames.setpoint_marker_id[0] and edit_setpoint[0] :
00562
00563 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[0], now)
00564 wTh = pm.toPose(trans, rot)
00565 Fwth = pm.fromMsg(wTh)
00566 wTs = feedback.pose
00567 Fwts = pm.fromMsg(wTs)
00568 Fhtw = (pm.fromMsg(wTh)*pm.fromMsg(tool_offset[0])).Inverse()
00569 Fhts = Fhtw*Fwts
00570 setpoint_offset[0] = pm.toMsg(Fhts)
00571 print "Moved left setpoint frame: ", setpoint_offset[0]
00572
00573 elif feedback.marker_name == frames.setpoint_marker_id[1] and edit_setpoint[1] :
00574
00575 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[1], now)
00576 wTh = pm.toPose(trans, rot)
00577 Fwth = pm.fromMsg(wTh)
00578 wTs = feedback.pose
00579 Fwts = pm.fromMsg(wTs)
00580 Fhtw = (pm.fromMsg(wTh)*pm.fromMsg(tool_offset[1])).Inverse()
00581 Fhts = Fhtw*Fwts
00582 setpoint_offset[1] = pm.toMsg(Fhts)
00583 print "Moved right setpoint frame: ", setpoint_offset[1]
00584
00585 elif feedback.marker_name == frames.waist_frame_id :
00586 r2.waist_jnt_pub.publish(r2.getJointCommand(joint_data, '/r2/waist/joint0', feedback.pose.orientation.z))
00587 resetMarker(feedback, feedback.marker_name)
00588
00589 elif feedback.marker_name == frames.neck_frame_id[0] :
00590 r2.neck_jnt_pub.publish(r2.getJointCommand(joint_data, '/r2/neck/joint0', -feedback.pose.orientation.y))
00591 resetMarker(feedback, feedback.marker_name)
00592
00593 elif feedback.marker_name == frames.neck_frame_id[1] :
00594 r2.neck_jnt_pub.publish(r2.getJointCommand(joint_data, '/r2/neck/joint1', feedback.pose.orientation.y))
00595 resetMarker(feedback, feedback.marker_name)
00596
00597 elif feedback.marker_name == frames.neck_frame_id[2] :
00598 r2.neck_jnt_pub.publish(r2.getJointCommand(joint_data, '/r2/neck/joint2', feedback.pose.orientation.y))
00599 resetMarker(feedback, feedback.marker_name)
00600
00601 elif feedback.marker_name == frames.posture_frame_id[0] :
00602 r2.left_jnt_pub.publish(r2.getJointCommand(joint_data, '/r2/left_arm/joint1', feedback.pose.orientation.z))
00603 resetMarker(feedback, feedback.marker_name)
00604
00605 elif feedback.marker_name == frames.posture_frame_id[1] :
00606 r2.right_jnt_pub.publish(r2.getJointCommand(joint_data, '/r2/right_arm/joint1', feedback.pose.orientation.z))
00607 resetMarker(feedback, feedback.marker_name)
00608
00609 for i in range(len(r2.leftHandNames)) :
00610 if feedback.marker_name == frames.finger_frame_id[0][i] :
00611 r2.left_jnt_pub.publish(r2.getJointCommand(joint_data, r2.leftHandNames[i], feedback.pose.orientation.z))
00612 resetMarker(feedback, feedback.marker_name)
00613
00614 for i in range(len(r2.rightHandNames)) :
00615 if feedback.marker_name == frames.finger_frame_id[1][i] :
00616 r2.right_jnt_pub.publish(r2.getJointCommand(joint_data, r2.rightHandNames[i], feedback.pose.orientation.z))
00617 resetMarker(feedback, feedback.marker_name)
00618
00619 def processFeedback( feedback ) :
00620
00621 s = "Feedback from marker '" + feedback.marker_name
00622 s += "' / control '" + feedback.control_name + "'"
00623
00624 mp = ""
00625 if feedback.mouse_point_valid:
00626 mp = " at " + str(feedback.mouse_point.x)
00627 mp += ", " + str(feedback.mouse_point.y)
00628 mp += ", " + str(feedback.mouse_point.z)
00629 mp += " in frame " + feedback.header.frame_id
00630
00631 if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
00632 rospy.loginfo( s + ": button click" + mp + "." )
00633
00634 elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
00635 rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." + "marker_name: " + feedback.marker_name )
00636 if feedback.marker_name == frames.menu_frame_id[0]:
00637 handleLeftArmMenu(feedback)
00638 elif feedback.marker_name == frames.menu_frame_id[1] :
00639 handleRightArmMenu(feedback)
00640 elif feedback.marker_name == frames.waist_frame_id :
00641 handleWaistMenu(feedback)
00642 elif feedback.marker_name == frames.head_frame_id :
00643 handleHeadMenu(feedback)
00644 elif feedback.marker_name == frames.gaze_frame_id :
00645 handleGazeMenu(feedback)
00646 elif feedback.marker_name == frames.backpack_frame_id :
00647 handleBackpackMenu(feedback)
00648 elif feedback.marker_name == frames.setpoint_marker_id[0]:
00649 handleLeftSetpointMenu(feedback)
00650 elif feedback.marker_name == frames.setpoint_marker_id[1]:
00651 handleRightSetpointMenu(feedback)
00652
00653 elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
00654 poseUpdate(feedback)
00655
00656
00657
00658
00659 elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
00660 mouseUpdate(feedback)
00661
00662 server.applyChanges()
00663
00664
00665
00666
00667
00668 def makeLeftArmControl( ):
00669 global left_arm_cart_marker
00670 left_arm_cart_marker.controls.append(makeXTransControl())
00671 left_arm_cart_marker.controls.append(makeYTransControl())
00672 left_arm_cart_marker.controls.append(makeZTransControl())
00673 left_arm_cart_marker.controls.append(makeXRotControl())
00674 left_arm_cart_marker.controls.append(makeYRotControl())
00675 left_arm_cart_marker.controls.append(makeZRotControl())
00676 server.insert(left_arm_cart_marker, processFeedback)
00677
00678 def makeRightArmControl( ):
00679 global right_arm_cart_marker
00680 right_arm_cart_marker.controls.append(makeXTransControl())
00681 right_arm_cart_marker.controls.append(makeYTransControl())
00682 right_arm_cart_marker.controls.append(makeZTransControl())
00683 right_arm_cart_marker.controls.append(makeXRotControl())
00684 right_arm_cart_marker.controls.append(makeYRotControl())
00685 right_arm_cart_marker.controls.append(makeZRotControl())
00686 server.insert(right_arm_cart_marker, processFeedback)
00687
00688 def removeLeftArmControl() :
00689 global left_arm_cart_marker
00690 left_arm_cart_marker.controls = []
00691 server.erase(left_arm_cart_marker.name)
00692
00693 def removeRightArmControl() :
00694 global right_arm_cart_marker
00695 right_arm_cart_marker.controls = []
00696 server.erase(right_arm_cart_marker.name)
00697
00698 def makeLeftSetpointControl( ) :
00699 global left_setpoint_marker
00700 left_setpoint_marker.controls.append(makeXTransControl())
00701 left_setpoint_marker.controls.append(makeYTransControl())
00702 left_setpoint_marker.controls.append(makeZTransControl())
00703 left_setpoint_marker.controls.append(makeXRotControl())
00704 left_setpoint_marker.controls.append(makeYRotControl())
00705 left_setpoint_marker.controls.append(makeZRotControl())
00706 r2.makeLeftHandSetpointMarker(left_setpoint_marker)
00707 server.insert(left_setpoint_marker, processFeedback)
00708 left_setpoint_menu_handler.apply( server, left_setpoint_marker.name )
00709
00710 def makeRightSetpointControl( ) :
00711 global right_setpoint_marker
00712 right_setpoint_marker.controls.append(makeXTransControl())
00713 right_setpoint_marker.controls.append(makeYTransControl())
00714 right_setpoint_marker.controls.append(makeZTransControl())
00715 right_setpoint_marker.controls.append(makeXRotControl())
00716 right_setpoint_marker.controls.append(makeYRotControl())
00717 right_setpoint_marker.controls.append(makeZRotControl())
00718 r2.makeRightHandSetpointMarker(right_setpoint_marker)
00719 server.insert(right_setpoint_marker, processFeedback)
00720 right_setpoint_menu_handler.apply( server, right_setpoint_marker.name )
00721
00722 def removeLeftSetpointControl() :
00723 global left_setpoint_marker
00724 left_setpoint_marker.controls = []
00725 server.erase(left_setpoint_marker.name)
00726
00727 def removeRightSetpointControl() :
00728 global right_setpoint_marker
00729 right_setpoint_marker.controls = []
00730 server.erase(right_setpoint_marker.name)
00731
00732 def makeArmMenu( frame_id, mesh_name, p, menu_handler ):
00733
00734 int_marker = InteractiveMarker()
00735 int_marker.header.frame_id = frame_id
00736 int_marker.scale = 0.2
00737 int_marker.name = frame_id
00738
00739 control = InteractiveMarkerControl()
00740 control.interaction_mode = InteractiveMarkerControl.MENU
00741 marker = makeMesh( int_marker, mesh_name, p, 1.02 )
00742 control.markers.append( marker )
00743 int_marker.controls.append(control)
00744
00745 server.insert(int_marker, processFeedback)
00746 menu_handler.apply( server, int_marker.name )
00747
00748 def makeGazeControl( ):
00749
00750 global gaze_sphere_scale
00751 global gaze_marker
00752
00753 makeSphereControl(gaze_marker, gaze_sphere_scale)
00754
00755 gaze_marker.controls.append(makeXTransControl())
00756 gaze_marker.controls.append(makeYTransControl())
00757 gaze_marker.controls.append(makeZTransControl())
00758 gaze_marker.controls.append(makeXRotControl())
00759 gaze_marker.controls.append(makeYRotControl())
00760 gaze_marker.controls.append(makeZRotControl())
00761
00762 control = InteractiveMarkerControl()
00763 control.interaction_mode = InteractiveMarkerControl.MENU
00764 marker = makeSphere( gaze_marker, gaze_sphere_scale*1.01 )
00765 control.markers.append( marker )
00766 gaze_marker.controls.append(control)
00767
00768 server.insert(gaze_marker, processFeedback)
00769 gaze_menu_handler.apply( server, gaze_marker.name )
00770
00771
00772 def removeGazeControl() :
00773 global gaze_marker
00774 gaze_marker.controls = []
00775 server.erase(gaze_marker.name)
00776
00777
00778 def makeWaistMarker( ):
00779
00780 int_marker = InteractiveMarker()
00781 frame_id = frames.waist_frame_id
00782 int_marker.header.frame_id = frame_id
00783 int_marker.scale = 0.4
00784 int_marker.name = frame_id
00785
00786 int_marker.controls.append(makeYRotControl())
00787
00788 control = InteractiveMarkerControl()
00789 control.interaction_mode = InteractiveMarkerControl.MENU
00790 marker = makeMesh( int_marker, r2.body_mesh, r2.waist_mesh_pose, 1.02 )
00791 control.markers.append( marker )
00792 int_marker.controls.append(control)
00793
00794 server.insert(int_marker, processFeedback)
00795 waist_menu_handler.apply( server, int_marker.name )
00796
00797
00798 def makeHeadControl( ) :
00799
00800 global neck_lower_pitch_marker, neck_roll_marker, neck_upper_pitch_marker
00801
00802 lower_pitch_control = makeZRotControl()
00803 roll_control = makeZRotControl()
00804 upper_pitch_control = makeZRotControl()
00805
00806 neck_lower_pitch_marker.controls.append(lower_pitch_control)
00807 neck_roll_marker.controls.append(roll_control)
00808 neck_upper_pitch_marker.controls.append(upper_pitch_control)
00809
00810 server.insert(neck_lower_pitch_marker, processFeedback)
00811 server.insert(neck_roll_marker, processFeedback)
00812 server.insert(neck_upper_pitch_marker, processFeedback)
00813
00814
00815 def makeHeadMenu( ) :
00816 global neck_roll_marker
00817 head_menu_control = InteractiveMarkerControl()
00818 head_menu_control.interaction_mode = InteractiveMarkerControl.MENU
00819 marker = makeMesh( neck_roll_marker, r2.head_mesh, r2.head_mesh_pose, 1.02 )
00820 head_menu_control.markers.append( marker )
00821 neck_roll_marker.controls.append(head_menu_control)
00822 server.insert(neck_roll_marker, processFeedback)
00823 head_menu_handler.apply( server, neck_roll_marker.name )
00824
00825 def makeBackpackMenu( ) :
00826 global backpack_marker
00827 backpack_menu_control = InteractiveMarkerControl()
00828 backpack_menu_control.interaction_mode = InteractiveMarkerControl.MENU
00829 marker = makeMesh( backpack_marker, r2.backpack_mesh, r2.backpack_mesh_pose, 1.02 )
00830 backpack_menu_control.markers.append( marker )
00831 backpack_marker.controls.append(backpack_menu_control)
00832 server.insert(backpack_marker, processFeedback)
00833 backpack_menu_handler.apply( server, backpack_marker.name )
00834
00835 def removeHeadControl() :
00836 global neck_lower_pitch_marker, neck_roll_marker, neck_upper_pitch_marker
00837 neck_roll_marker.controls = []
00838 server.erase(neck_lower_pitch_marker.name)
00839 server.erase(neck_roll_marker.name)
00840 server.erase(neck_upper_pitch_marker.name)
00841 makeHeadMenu()
00842
00843 def makePostureMarkers():
00844 left_posture_marker.name = frames.posture_frame_id[0]
00845 right_posture_marker.name = frames.posture_frame_id[1]
00846 left_posture_control = makeYRotControl()
00847 right_posture_control = makeYRotControl()
00848 left_posture_marker.controls.append(left_posture_control)
00849 right_posture_marker.controls.append(right_posture_control)
00850 server.insert(right_posture_marker, processFeedback)
00851 server.insert(left_posture_marker, processFeedback)
00852
00853 def makeFingerMarkers():
00854 for i in range(len(left_finger_markers)) :
00855 left_finger_markers[i].name = frames.finger_frame_id[0][i]
00856 right_finger_markers[i].name = frames.finger_frame_id[1][i]
00857 left_finger_markers[i].controls.append(makeYRotControl())
00858 right_finger_markers[i].controls.append(makeYRotControl())
00859 server.insert(left_finger_markers[i], processFeedback)
00860 server.insert(right_finger_markers[i], processFeedback)
00861
00862 def joint_state_cb(data) :
00863 global joint_data
00864 joint_data = data
00865
00866 def SetUpMenus() :
00867
00868 left_arm_menu_handler.insert( "Go To ReadyPose", callback=processFeedback )
00869 left_arm_menu_handler.setCheckState( left_arm_menu_handler.insert( "Joint Mode", callback=processFeedback ), MenuHandler.UNCHECKED )
00870 left_arm_menu_handler.setCheckState( left_arm_menu_handler.insert( "Edit Control Frame", callback=processFeedback ), MenuHandler.UNCHECKED )
00871 left_arm_menu_handler.setCheckState( left_arm_menu_handler.insert( "Set Reach Point", callback=processFeedback ), MenuHandler.UNCHECKED )
00872 left_arm_sub_menu_handle = left_arm_menu_handler.insert( "Grasp Controls" )
00873 left_arm_menu_handler.insert( "Open Hand", parent=left_arm_sub_menu_handle, callback=processFeedback )
00874 left_arm_menu_handler.insert( "Close Hand", parent=left_arm_sub_menu_handle, callback=processFeedback )
00875 left_arm_menu_handler.insert( "Reset Control Frame", callback=processFeedback )
00876
00877 right_arm_menu_handler.insert( "Go To ReadyPose", callback=processFeedback )
00878 right_arm_menu_handler.setCheckState( right_arm_menu_handler.insert( "Joint Mode", callback=processFeedback ), MenuHandler.UNCHECKED )
00879 right_arm_menu_handler.setCheckState( right_arm_menu_handler.insert( "Edit Control Frame", callback=processFeedback ), MenuHandler.UNCHECKED )
00880 right_arm_menu_handler.setCheckState( right_arm_menu_handler.insert( "Set Reach Point", callback=processFeedback ), MenuHandler.UNCHECKED )
00881 right_arm_sub_menu_handle = right_arm_menu_handler.insert( "Grasp Controls" )
00882 right_arm_menu_handler.insert( "Open Hand", parent=right_arm_sub_menu_handle, callback=processFeedback )
00883 right_arm_menu_handler.insert( "Close Hand", parent=right_arm_sub_menu_handle, callback=processFeedback )
00884 right_arm_menu_handler.insert( "Reset Control Frame", callback=processFeedback )
00885
00886 waist_menu_handler.insert( "Go To ReadyPose", callback=processFeedback )
00887
00888 head_menu_handler.insert( "Go To ReadyPose", callback=processFeedback )
00889 head_menu_handler.setCheckState( head_menu_handler.insert( "Joint Control", callback=processFeedback ), MenuHandler.UNCHECKED )
00890 head_menu_handler.insert( "Toggle Gaze Control", callback=processFeedback )
00891 head_menu_handler.insert( "Segment Table Top", callback=processFeedback )
00892
00893 gaze_menu_handler.insert( "Look At This Point", callback=processFeedback )
00894 gaze_tracking_sub_menu_handle = gaze_menu_handler.insert( "Tracking Mode", callback=processFeedback )
00895 gaze_menu_handler.setCheckState( gaze_menu_handler.insert( "Free Floating", parent=gaze_tracking_sub_menu_handle, callback=processFeedback ), MenuHandler.UNCHECKED )
00896 gaze_menu_handler.setCheckState( gaze_menu_handler.insert( "Left Tool", parent=gaze_tracking_sub_menu_handle, callback=processFeedback ), MenuHandler.UNCHECKED )
00897 gaze_menu_handler.setCheckState( gaze_menu_handler.insert( "Right Tool", parent=gaze_tracking_sub_menu_handle, callback=processFeedback ), MenuHandler.UNCHECKED )
00898
00899 backpack_menu_handler.setCheckState( backpack_menu_handler.insert( "Power", callback=processFeedback ), MenuHandler.UNCHECKED )
00900
00901 left_setpoint_menu_handler.insert( "Move to Point", callback=processFeedback )
00902 right_setpoint_menu_handler.insert( "Move to Point", callback=processFeedback )
00903
00904
00905 def SetUpMarkers() :
00906
00907 global left_finger_markers, right_finger_markers
00908
00909 left_arm_cart_marker.header.frame_id = frames.base_frame_id
00910 left_arm_cart_marker.name = frames.control_marker_id[0]
00911 left_arm_cart_marker.scale = 0.2
00912
00913 right_arm_cart_marker.header.frame_id = frames.base_frame_id
00914 right_arm_cart_marker.name = frames.control_marker_id[1]
00915 right_arm_cart_marker.scale = 0.2
00916
00917 left_setpoint_marker.header.frame_id = frames.base_frame_id
00918 left_setpoint_marker.name = frames.setpoint_marker_id[0]
00919 left_setpoint_marker.scale = 0.2
00920
00921 right_setpoint_marker.header.frame_id = frames.base_frame_id
00922 right_setpoint_marker.name = frames.setpoint_marker_id[1]
00923 right_setpoint_marker.scale = 0.2
00924
00925 left_posture_marker.header.frame_id = frames.posture_frame_id[0]
00926 right_posture_marker.header.frame_id = frames.posture_frame_id[1]
00927 left_posture_marker.scale = 0.25
00928 right_posture_marker.scale = 0.25
00929
00930 neck_lower_pitch_marker.header.frame_id = frames.neck_frame_id[0]
00931 neck_lower_pitch_marker.name = frames.neck_frame_id[0]
00932 neck_lower_pitch_marker.scale = 0.4
00933
00934 neck_roll_marker.header.frame_id = frames.neck_frame_id[1]
00935 neck_roll_marker.name = frames.neck_frame_id[1]
00936 neck_roll_marker.scale = 0.4
00937
00938 neck_upper_pitch_marker.header.frame_id = frames.neck_frame_id[2]
00939 neck_upper_pitch_marker.name = frames.neck_frame_id[2]
00940 neck_upper_pitch_marker.scale = 0.4
00941
00942 gaze_marker.header.frame_id = frames.base_frame_id
00943 gaze_marker.pose.position.x = -1.5
00944 gaze_marker.pose.position.z = 0
00945 gaze_marker.scale = 0.2
00946 gaze_marker.name = 'gaze_control_link'
00947
00948 backpack_marker.header.frame_id = frames.backpack_frame_id
00949 backpack_marker.name = frames.backpack_frame_id
00950 backpack_marker.scale = 0.4
00951
00952 for i in range(len(left_finger_markers)) :
00953 left_finger_markers[i].header.frame_id = frames.finger_frame_id[0][i]
00954 right_finger_markers[i].header.frame_id = frames.finger_frame_id[1][i]
00955 left_finger_markers[i].scale = .03
00956 right_finger_markers[i].scale = .03
00957
00958 def slowUpdate( ) :
00959
00960 now = rospy.get_rostime()
00961 if gaze_left_tracking_mode :
00962 tf_listener.waitForTransform(frames.control_frame_id[0],frames.base_frame_id, now, rospy.Duration(5.0))
00963
00964 elif gaze_right_tracking_mode :
00965 tf_listener.waitForTransform(frames.control_frame_id[1],frames.base_frame_id, now, rospy.Duration(5.0))
00966
00967 tf_listener.waitForTransform(frames.control_frame_id[0],frames.base_frame_id, rospy.Time(0), rospy.Duration(5.0))
00968
00969
00970 tf_listener.waitForTransform(frames.control_frame_id[1],frames.base_frame_id, rospy.Time(0), rospy.Duration(5.0))
00971
00972
00973 def fastUpdate( ) :
00974
00975 global setpoint_store
00976
00977 pose = PoseStamped()
00978 pose.header.seq = 0
00979 pose.header.stamp = rospy.Time(0)
00980 pose.header.frame_id = frames.base_frame_id
00981
00982 now = rospy.Time(0)
00983 if gaze_left_tracking_mode :
00984 tf_listener.waitForTransform(frames.control_frame_id[0],frames.base_frame_id, now, rospy.Duration(dur_time))
00985 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[0], now)
00986 pose.pose = pm.toPose(trans, rot)
00987 r2.gaze_pose_pub.publish(pose)
00988
00989 elif gaze_right_tracking_mode :
00990 tf_listener.waitForTransform(frames.control_frame_id[1],frames.base_frame_id, now, rospy.Duration(dur_time))
00991 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[1], now)
00992 pose.pose = pm.toPose(trans, rot)
00993 r2.gaze_pose_pub.publish(pose)
00994
00995 tf_listener.waitForTransform(frames.control_frame_id[0],frames.base_frame_id, now, rospy.Duration(dur_time))
00996 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[0], now)
00997 pose.pose = pm.toPose(trans, rot)
00998
00999
01000 if not edit_setpoint[0] :
01001 Fp = pm.fromMsg(pose.pose)
01002 Ft = pm.fromMsg(tool_offset[0])
01003 pose.pose = pm.toMsg(Fp*Ft)
01004 pose.header.frame_id = frames.base_frame_id
01005 server.setPose( left_arm_cart_marker.name, pose.pose )
01006
01007
01008 else :
01009 Fp = pm.fromMsg(pose.pose)
01010 Fs = pm.fromMsg(setpoint_offset[0])
01011 Ft = pm.fromMsg(tool_offset[0])
01012 pose.pose = pm.toMsg(Fp*Ft*Fs)
01013 pose.header.frame_id = frames.base_frame_id
01014 server.setPose( left_setpoint_marker.name, pose.pose )
01015
01016
01017
01018 tf_listener.waitForTransform(frames.control_frame_id[1],frames.base_frame_id, now, rospy.Duration(dur_time))
01019 (trans, rot) = tf_listener.lookupTransform(frames.base_frame_id, frames.control_frame_id[1], now)
01020 pose.pose = pm.toPose(trans, rot)
01021
01022
01023 if not edit_setpoint[1] :
01024 Fp = pm.fromMsg(pose.pose)
01025 Ft = pm.fromMsg(tool_offset[1])
01026 pose.pose = pm.toMsg(Fp*Ft)
01027 pose.header.frame_id = frames.base_frame_id
01028 server.setPose( right_arm_cart_marker.name, pose.pose )
01029
01030
01031 else :
01032 Fp = pm.fromMsg(pose.pose)
01033 Fs = pm.fromMsg(setpoint_offset[1])
01034 Ft = pm.fromMsg(tool_offset[1])
01035 pose.pose = pm.toMsg(Fp*Ft*Fs)
01036 pose.header.frame_id = frames.base_frame_id
01037 server.setPose( right_setpoint_marker.name, pose.pose )
01038
01039 server.applyChanges()
01040
01041
01042 if __name__=="__main__":
01043
01044 rospy.init_node("r2_interactive_control")
01045 server = InteractiveMarkerServer("r2_interactive_control")
01046 tf_listener = tf.TransformListener()
01047
01048
01049 r2.SetArmsToCartMode(frames.control_frame_id[0], frames.control_frame_id[1])
01050
01051
01052 rospy.Subscriber("/joint_states", JointState, joint_state_cb)
01053
01054
01055 SetUpMenus()
01056
01057
01058 r2.SetUpMeshData()
01059
01060
01061 SetUpMarkers()
01062
01063
01064 ResetToolOffset("left")
01065 ResetToolOffset("right")
01066
01067
01068 makeLeftArmControl( )
01069 makeRightArmControl( )
01070
01071 makeWaistMarker( )
01072 makePostureMarkers()
01073 makeFingerMarkers()
01074
01075
01076 makeArmMenu( frames.menu_frame_id[0], r2.left_palm_mesh, r2.left_palm_mesh_pose, left_arm_menu_handler)
01077 makeArmMenu( frames.menu_frame_id[1], r2.right_palm_mesh, r2.right_palm_mesh_pose, right_arm_menu_handler )
01078 makeHeadMenu( )
01079 makeBackpackMenu( )
01080
01081 server.applyChanges()
01082
01083
01084 r = rospy.Rate(fast_update_rate)
01085 c = 0
01086 while not rospy.is_shutdown():
01087 if(c % slow_update_divider == 0) : slowUpdate()
01088 c = c+1
01089 fastUpdate()
01090 r.sleep()
01091 rospy.spin()
01092