r2_interactive_sensing.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 numpy as np
00028 
00029 import tf
00030 from interactive_markers.interactive_marker_server import *
00031 from interactive_markers.menu_handler import *
00032 
00033 from sensor_msgs.msg import JointState
00034 from geometry_msgs.msg import Pose
00035 from geometry_msgs.msg import Point
00036 from geometry_msgs.msg import Quaternion
00037 from geometry_msgs.msg import PoseStamped
00038 from geometry_msgs.msg import TransformStamped
00039 
00040 import r2_helper as r2
00041 import kdl_posemath as pm
00042 
00043 
00044 TORAD = math.pi/180.0
00045 TODEG = 1.0/TORAD
00046 
00047 server = None
00048 counter = 0
00049 
00050 marker_data = None
00051 
00052 interactive_marker_list = []
00053 obj_transforms = []
00054 
00055 obj_menu_handler = MenuHandler()
00056 
00057 robot_frame = "r2/robot_reference"
00058 tf_listener = None
00059 tf_broadcaster = None
00060 
00061 need_to_orient = False
00062 
00063 X = np.array([1, 0, 0])
00064 Y = np.array([0, 1, 0])
00065 Z = np.array([0, 0, 1])
00066                         
00067                         
00068 def getInteractiveMarkerID(name) :
00069         for i in range(len(interactive_marker_list)) :
00070                 if name == interactive_marker_list[i].name :
00071                         return i
00072 
00073 def TrotX(theta):
00074         s = math.sin(theta)
00075         c = math.cos(theta)
00076         T = kdl.Frame(kdl.Rotation(1, 0, 0, 0, c, -s, 0, s, c), kdl.Vector(0, 0, 0))
00077         return T
00078 
00079 def TrotY(theta):
00080         s = math.sin(theta)
00081         c = math.cos(theta)
00082         T = kdl.Frame(kdl.Rotation(c, 0, s, 0, 1, 0, -s, 0, c), kdl.Vector(0, 0, 0))
00083         return T
00084 
00085 def TrotZ(theta):
00086         s = math.sin(theta)
00087         c = math.cos(theta)
00088         T = kdl.Frame(kdl.Rotation(c, -s, 0, s, c, 0, 0, 0, 1), kdl.Vector(0, 0, 0))
00089         return T
00090 
00091 def parseObjectPose(idx):
00092         
00093         print "parsing object pose"
00094         now = rospy.Time(0)
00095         pose = PoseStamped()
00096         
00097         frame_id = interactive_marker_list[idx].controls[0].markers[0].header.frame_id
00098         marker = interactive_marker_list[idx].controls[0].markers[0]
00099         scale = (marker.scale.x, marker.scale.y, marker.scale.z)
00100 
00101         print "object[", idx,"]: ", interactive_marker_list[idx].name
00102         print "pose:\n", interactive_marker_list[idx].controls[0].markers[0].pose
00103         print "\tin frame: ", frame_id
00104         print "\tscale: ", scale
00105 
00106         tf_listener.waitForTransform(robot_frame, frame_id, now, rospy.Duration(2.0))
00107     #(trans, rot) = tf_listener.lookupTransform(frame_id, robot_frame, now)
00108 
00109         pose.header = interactive_marker_list[idx].controls[0].markers[0].header 
00110         pose.pose = interactive_marker_list[idx].controls[0].markers[0].pose
00111                 
00112         pose.header.stamp = now
00113         pose = tf_listener.transformPose(robot_frame, pose)
00114         print "transformed pose: "
00115         print pose
00116 
00117         p = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
00118         q = (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w)
00119 
00120         # get object frame
00121         #obj = obj_transforms[idx]
00122         #T = pm.fromTf((obj[0],obj[1]))
00123         #t = pm.toTf(T)
00124         
00125         # get rotational and component parts of frame
00126         #R = T.M
00127         #p = T.p
00128         
00129         if p[1] > 0:
00130                 arm = "right"
00131         else :
00132                 arm = "left"
00133 
00134         print "choosing: ", arm
00135         wide = True
00136         approach= 'top'
00137         if (scale[2] > scale[1]*1.25) and (scale[2] > 0.12) :
00138                 wide = False
00139                 approach = "side"
00140                 print "object tall"
00141         else :
00142                 print "object wide"
00143 
00144         return pose, scale, arm, approach, p, q
00145 
00146 def reachToObject(idx) :
00147 
00148         pose, scale, arm, approach, p, q = parseObjectPose(idx)
00149 
00150         print "p: ", p
00151         print "q: ", q
00152         print "s: ", scale
00153         print "pose: ", pose
00154         print "arm: ", arm
00155         print "approach", approach
00156 
00157         x_off = -0.085
00158         y_off = 0.075
00159         z_off = 0.12
00160         
00161         marker = interactive_marker_list[idx].controls[0].markers[0]
00162         dim = np.array([marker.scale.x, marker.scale.y, marker.scale.z]) 
00163         poseOff = copy.deepcopy(pose)
00164 
00165         if scale[0] > scale[1] :
00166                 max_side_dim = 0
00167         else :
00168                 max_side_dim = 1       
00169         x_reach_off = x_off
00170         z_reach_off = scale[2]+z_off
00171         y_reach_off = scale[max_side_dim]+y_off # should be scaled by side projected width (not just max side width)
00172         
00173         if approach == "side" :
00174                 if arm == "left" :
00175                     q = (kdl.Rotation.RPY(-1.35,0,0)).GetQuaternion()
00176                     y_reach_off *= -1
00177                 else :
00178                     q = (kdl.Rotation.RPY( 1.35,0,0)).GetQuaternion()
00179 
00180         print "y_reach_off: ", y_reach_off
00181         print "z_reach_off: ", z_reach_off
00182 
00183         poseOff.header.stamp = rospy.get_rostime()
00184 
00185         poseOff.pose.position.x = p[0] + x_reach_off
00186         poseOff.pose.position.y = p[1]
00187         poseOff.pose.position.z = p[2]
00188         poseOff.pose.orientation.x = q[0]
00189         poseOff.pose.orientation.y = q[1]
00190         poseOff.pose.orientation.z = q[2]
00191         poseOff.pose.orientation.w = q[3]
00192 
00193         if approach == "top" :
00194                 poseOff.pose.position.z -= z_reach_off
00195         else:   
00196                 poseOff.pose.position.y += y_reach_off
00197 
00198         print "final poseOff: "
00199         print poseOff
00200 
00201         if arm == "left" :
00202                 r2.left_pose_pub.publish(poseOff)
00203         else :
00204                 r2.right_pose_pub.publish(poseOff)
00205 
00206         # wait to finish reaching       
00207         rospy.sleep(4.0)
00208 
00209         # do grasp approach
00210         if approach == "top" :
00211                 graspPose = copy.deepcopy(pose)
00212         else :
00213                 graspPose = copy.deepcopy(pose)
00214                 graspPose.pose.orientation.x = q[0]
00215                 graspPose.pose.orientation.y = q[1]
00216                 graspPose.pose.orientation.z = q[2]
00217                 graspPose.pose.orientation.w = q[3]
00218         
00219         graspPose.header.stamp = rospy.get_rostime()
00220         
00221         if approach == "top" :
00222                 graspPose.pose.position.z -= z_reach_off*0.1
00223         else: 
00224                 graspPose.pose.position.y += y_reach_off*0.0
00225 
00226         if arm == "left" :
00227                 r2.left_pose_pub.publish(graspPose)
00228         else :
00229                 r2.right_pose_pub.publish(graspPose)
00230 
00231         # wait to finish reaching       
00232         rospy.sleep(2.0)
00233 
00234         if arm == "left" :
00235                 r2.left_jnt_pub.publish(r2.leftHandPowerClose)
00236                 rospy.sleep(0.3)
00237                 r2.left_jnt_pub.publish(r2.leftHandPowerCloseThumb)
00238         else :
00239                 r2.right_jnt_pub.publish(r2.rightHandPowerClose)
00240                 rospy.sleep(0.3)
00241                 r2.right_jnt_pub.publish(r2.rightHandPowerCloseThumb)
00242                 
00243         # wait to finish reaching       
00244         rospy.sleep(1.5)
00245 
00246         liftPose = copy.deepcopy(graspPose)
00247         liftPose.pose.position.z -= 0.15        
00248 
00249         if arm == "left" :
00250                 r2.left_pose_pub.publish(liftPose)
00251         else :
00252                 r2.right_pose_pub.publish(liftPose)
00253 
00254 
00255 def handleObjectMenu( feedback ) :
00256         
00257         handle = feedback.menu_entry_id
00258         idx = getInteractiveMarkerID(feedback.marker_name)
00259         print "Pick Me Up call on object: ", feedback.marker_name
00260         
00261         if(handle == 1) : # pick me up
00262                 reachToObject(idx)
00263         elif(handle == 2) : # parse
00264                 parseObjectPose(idx)
00265         
00266 def processFeedback( feedback ) :
00267          
00268     s = "Feedback from marker '" + feedback.marker_name
00269     s += "' / control '" + feedback.control_name + "'"
00270 
00271     mp = ""
00272     if feedback.mouse_point_valid:
00273         mp = " at " + str(feedback.mouse_point.x)
00274         mp += ", " + str(feedback.mouse_point.y)
00275         mp += ", " + str(feedback.mouse_point.z)
00276         mp += " in frame " + feedback.header.frame_id
00277 
00278     if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
00279         rospy.loginfo( s + ": button click" + mp + "." )
00280 
00281     elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
00282         rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." + "marker_name: " + feedback.marker_name  )
00283         handleObjectMenu(feedback)
00284         
00285     #elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
00286     #    poseUpdate(feedback)
00287 
00288     #elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
00289     #    rospy.loginfo( s + ": mouse down" + mp + "." )
00290     
00291     #elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
00292     #    mouseUpdate(feedback)
00293 
00294     server.applyChanges()
00295 
00296 def marker_array_cb(data) :
00297         global marker_data, interactive_marker_list, obj_transforms, need_to_orient
00298         pose = PoseStamped()
00299 
00300         print "in marker array callback"
00301         for m in range(len(interactive_marker_list)) :
00302                 server.erase(interactive_marker_list[m].name)
00303         interactive_marker_list = []
00304         obj_transforms = []
00305         server.applyChanges()
00306 
00307         rospy.sleep(0.25)
00308 
00309         marker_data = data      
00310         print "got new marker array of size: ", len(data.markers)
00311         #print marker_data
00312 
00313         for m in range(len(marker_data.markers)) :
00314                 
00315                 interactive_marker_list.append(InteractiveMarker())
00316                 interactive_marker_list[m].header.frame_id = "r2/asus_frame" 
00317                 interactive_marker_list[m].scale = 1#0.2
00318                 interactive_marker_list[m].name = "obj_"+str(m)
00319                 interactive_marker_list[m].description = "obj_"+str(m)
00320                 control = InteractiveMarkerControl()
00321                 control.interaction_mode = InteractiveMarkerControl.MENU
00322                 
00323                 marker = marker_data.markers[m]
00324                 marker.color.r = 1
00325                 marker.color.g = 0
00326                 marker.color.b = 1
00327                 marker.color.a = 0.75
00328                 control.markers.append( marker )
00329                 interactive_marker_list[m].controls.append(control)
00330                 server.insert(interactive_marker_list[m], processFeedback)
00331                 obj_menu_handler.apply( server, interactive_marker_list[m].name )
00332                 server.applyChanges()
00333 
00334                 pose.header = interactive_marker_list[m].controls[0].markers[0].header 
00335                 pose.pose = interactive_marker_list[m].controls[0].markers[0].pose
00336                 pose = tf_listener.transformPose(robot_frame, pose)
00337                 
00338                 p = (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
00339                 q = (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w)
00340                 obj_transforms.append((p, q, interactive_marker_list[m].name))
00341                 
00342                 need_to_orient = True
00343                 
00344 def SetUpObjectMenu() :
00345         
00346         obj_menu_handler.insert( "Pick Up",  callback=processFeedback )
00347         obj_menu_handler.insert( "Parse Object",  callback=processFeedback )
00348 
00349 if __name__=="__main__":
00350 
00351     rospy.init_node("r2_interactive_sensing")    
00352     server = InteractiveMarkerServer("r2_interactive_sensing")
00353     tf_listener = tf.TransformListener()
00354     tf_broadcaster = tf.TransformBroadcaster()
00355     
00356     need_to_orient = False
00357     
00358     # Subscribe to marker data
00359     rospy.Subscriber("/tabletop_clustering/vis_marker", MarkerArray, marker_array_cb)
00360 
00361     # set up menus
00362     SetUpObjectMenu()
00363 
00364     server.applyChanges()
00365     
00366     print "in r2_interactive_sensing"
00367 #       rate = rospy.Rate(10.0)
00368     while not rospy.is_shutdown():
00369         
00370         if need_to_orient :
00371             need_to_orient = False
00372             print "got objects"
00373             for i in range(len(obj_transforms)) :
00374                 obj = obj_transforms[i]
00375                         
00376                 T = pm.fromTf((obj[0],obj[1]))
00377                 #T = T*TrotX(90*TORAD)*TrotY(270*TORAD)
00378                 t = pm.toTf(T)                                          
00379                 tf_broadcaster.sendTransform(t[0], t[1], rospy.Time.now(), obj[2], robot_frame) 
00380                 q = T.M.GetQuaternion()
00381                 obj_transforms[i] = (t[0], q, interactive_marker_list[i].name)
00382                 rospy.sleep(0.5)
00383 
00384 
00385 


r2_teleop
Author(s): Paul Dinh
autogenerated on Mon Oct 6 2014 02:43:06