def main(argv=None): if argv is None: argv = sys.argv parser = optparse.OptionParser() #import socket #useObject = not (socket.gethostname() == 'mcube-002' or socket.gethostname() == 'mcube-003') parser.add_option('-o', '--obj', action="store_true", dest='useObject', help='To use manual object fitting', default=False) parser.add_option('-c', '--cammarker', action="store_true", dest='useCamMarker', help='To use manual camera fitting or not', default=False) parser.add_option('-s', '--shelfmarker', action="store_true", dest='useShelfMarker', help='To use manual shelf fitting or not', default=False) parser.add_option('-a', '--all', action="store_true", dest='useAllMarker', help='To use manual fitting or not', default=False) rospy.init_node("manual_fit_server") global currentExpoPose global currentCrayolaPose global currentWorldcamPose global currentShelfPose global currentTotePose global currentBin0Pose global currentBin1Pose global currentBin2Pose global currentBin3Pose global currentRailsPose global currentBinInsideCollisionPose global currentBinOutsideCollisionPose global br, brWorldcam global nworldcam global vis_pub global opt global pyramid_marker (opt, args) = parser.parse_args() currentExpoPose = Pose() currentCrayolaPose = Pose() currentWorldcamPose = [Pose()] currentShelfPose = Pose() currentTotePose = Pose() currentRailsPose = Pose() currentBin0Pose = Pose() currentBin1Pose = Pose() currentBin2Pose = Pose() currentBin3Pose = Pose() currentBinInsideCollisionPose = Pose() currentBinOutsideCollisionPose = Pose() currentJoint_realsense = Pose() br = TransformBroadcaster() brWorldcam = TransformBroadcaster() global listener listener = tf.TransformListener() rospy.sleep(1) # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("affordance_marker") # on mcube-002 don't use object if opt.useObject or opt.useAllMarker: # create an interactive marker for Expo pose = [ 1.01179456711, -0.596704602242, -0.126571118832, 0.0, -0.707106769085, 0.0, 0.707106769085 ] int_marker = createInteractiveMarker('ExpoEraser', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl', offset=tuple(pose[0:3]), rgba=(0, 1, 0, 1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Crayola pose = [ 1.01179456711, 0.588456273079, 0.0363664329052, 0.0, -0.707106769085, 0.0, 0.707106769085 ] int_marker = createInteractiveMarker('Crayola', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/object_meshes/crayola_64_ct.stl', offset=tuple(pose[0:3]), rgba=(1, 0, 0, 1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # # storage system pose = [] for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']: print 'rospy.get_param(tote_pose/ + term)', rospy.get_param( 'tote_pose/' + term) pose.append(rospy.get_param('tote_pose/' + term)) currentTotePose.position.x = pose[0] currentTotePose.position.y = pose[1] currentTotePose.position.z = pose[2] currentTotePose.orientation.x = pose[3] currentTotePose.orientation.y = pose[4] currentTotePose.orientation.z = pose[5] currentTotePose.orientation.w = pose[6] int_marker = createInteractiveMarker('tote', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/tote/storage_system_compressed.stl', offset=tuple(pose[0:3]), rgba=(0.5, 0.5, 0.5, 1), scale=0.001, orientation=tuple(pose[3:7]), frame_id='/map') int_marker.controls.append(meshmarker) #~ int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.1), frameCallbackShelfTote) # moving rails # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # print 'rospy.get_param(tote_pose/ + term)', term)) #rospy.get_param('tote_pose/' + term) # pose.append(rospy.get_param('tote_pose/' + # pose[1] = pose[1] + 1*(rospy.get_param('bin3_pose/y') - rospy.get_param('bin2_pose/y')) # int_marker = createInteractiveMarker('moving_rails', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/tote/moving_rails_compressed.stl', # offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.5,1), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # #~ int_marker.controls.extend(createMoveControls()) # server.insert(int_marker, processFeedback) # rospy.Timer(rospy.Duration(0.1), frameCallbackShelfTote) # bin0 pose = [] for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']: pose.append(rospy.get_param('bin0_pose/' + term)) int_marker = createInteractiveMarker('bin0', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/bin/bin_large.stl', offset=tuple(pose[0:3]), rgba=(1, 0, 0, 1), scale=0.001, orientation=tuple(pose[3:7]), frame_id='/map') int_marker.controls.append(meshmarker) #int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # bin1 pose = [] for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']: pose.append(rospy.get_param('bin1_pose/' + term)) int_marker = createInteractiveMarker('bin1', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/bin/bin_large.stl', offset=tuple(pose[0:3]), rgba=(1, 0, 0, 1), scale=0.001, orientation=tuple(pose[3:7]), frame_id='/map') int_marker.controls.append(meshmarker) #int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for left bin (tote) # bin2 pose = [] for term in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']: pose.append(rospy.get_param('bin2_pose/' + term)) int_marker = createInteractiveMarker('bin2', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/bin/bin_large.stl', offset=tuple(pose[0:3]), rgba=(1, 0, 0, 1), scale=0.001, orientation=tuple(pose[3:7]), frame_id='/map') int_marker.controls.append(meshmarker) #int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # # # bin3 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin3_pose/' + term)) # #~ pose[1] = rospy.get_param('bin2_pose/y') # int_marker = createInteractiveMarker('bin3', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/bin/bin_small.stl', # offset=tuple(pose[0:3]), rgba=(1,0,0,1), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # #int_marker.controls.extend(createMoveControls()) # server.insert(int_marker, processFeedback) ########### ## boxes ## ########### #~boxes 4-5 # if rospy.get_param('~is_picking', False): # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin4_pose/' + term)) # pose[2] = pose[2] - .07512*1 # int_marker = createInteractiveMarker('box_45', *pose) # meshmarker = createMeshMarker('package://apc_config/models/boxes/box_45.stl', # offset=tuple(pose[0:3]), rgba=(0.3,0.3,0.3,1), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # # #~box 4 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin4_pose/' + term)) # #~ pose[2] = pose[2] + .07512*0 # int_marker = createInteractiveMarker('cardboard_4', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_4.stl', # offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # # #~box 5 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin5_pose/' + term)) # #~ pose[2] = pose[2] + .1037*1 # int_marker = createInteractiveMarker('cardboard_5', *pose) # meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_5.stl', # offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # else: # # #~boxes 6-7 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin6_pose/' + term)) # pose[2]-= rospy.get_param('/bin6/z_flap') # int_marker = createInteractiveMarker('box_67', *pose) # meshmarker = createMeshMarker('package://apc_config/models/boxes/box_67_compressed.stl', # #~ offset=tuple(pose[0:3]), rgba=(0.1,0.1,0.1,1), scale = 0.001, # offset=tuple(pose[0:3]), rgba=(0.3,0.3,0.3,1), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # # #~box 6 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin6_pose/' + term)) # #~ pose[2] = pose[2] + 0.07135 # #~ pose[1] = rospy.get_param('bin2_pose/y') # int_marker = createInteractiveMarker('bin6_cardboard', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_6.stl', # offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # # #~box 7 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin7_pose/' + term)) # #~ pose[1] = rospy.get_param('bin2_pose/y') # int_marker = createInteractiveMarker('cardboard_7', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_7.stl', # offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # # #~box 8 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin8_pose/' + term)) # pose[2]= pose[2] - rospy.get_param('/bin8/z_flap') # int_marker = createInteractiveMarker('bin8', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/boxes/box_8_compressed.stl', # #~ offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.5,1), scale = 0.001, # offset=tuple(pose[0:3]), rgba=(0.3,0.3,0.3,1), scale = 0.001, # #~ offset=tuple(pose[0:3]), rgba=(0.1,0.1,0.1,1), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) # # #~box 8 # pose = [] # for term in ['x','y','z','qx','qy','qz','qw']: # pose.append(rospy.get_param('bin8_pose/' + term)) # int_marker = createInteractiveMarker('bin8_cardboard', *pose) # # meshmarker = createMeshMarker('package://apc_config/models/boxes/cardboard_8.stl', # offset=tuple(pose[0:3]), rgba=(.75,0.5,0,1.), scale = 0.001, # orientation=tuple(pose[3:7]), frame_id='/map') # int_marker.controls.append(meshmarker) # server.insert(int_marker, processFeedback) ############# ## Cameras ## ############# pose = [] pose.append( [1.3216, -.55862, .45089, 0.68986, .678841, -.185574, -.169784]) pose.append( [0.54191, -0.54436, 0.28256, 0.65065, -0.63561, 0.29037, -0.29724]) pose.append( [1.3225, -0.18962, 0.44935, 0.66217, 0.68597, -0.22759, -0.19795]) pose.append( [0.48932, -0.18366, 0.038885, -0.42587, 0.42257, -0.56458, 0.56685]) pose.append( [0.54073, 0.18461, 0.27804, 0.65751, -0.65352, 0.26679, -0.26348]) pose.append( [1.3213, -0.55948, 0.37547, 0.58127, 0.60183, -0.40335, -0.37045]) pose.append( [1.321, 0.57179, 0.36449, 0.60112, 0.58022, -0.38807, -0.38911]) pose.append( [0.49027, 0.54894, 0.032927, -0.41015, 0.39011, -0.58548, 0.58035]) pose.append( [0.48974, -0.55503, 0.042641, -0.42376, 0.42326, -0.567, 0.5655]) pose.append([1.3201, 0.19125, 0.36687, 0.6001, 0.59251, -0.3876, -0.37227]) pose.append( [0.54189, -0.17822, 0.28174, 0.6454, -0.64361, 0.28747, -0.29425]) pose.append( [1.3247, 0.19321, 0.44441, 0.68986, 0.67884, -0.18557, -0.16978]) pose.append( [0.5406, 0.55541, 0.27902, 0.63946, -0.62909, 0.31036, -0.31467]) pose.append( [1.3227, 0.57033, 0.44087, 0.68514, 0.67575, -0.1933, -0.19126]) pose.append( [1.3206, -0.18846, 0.37101, 0.5894, 0.59423, -0.39252, -0.38134]) pose.append( [0.48962, 0.17463, 0.035821, -0.41771, 0.40725, -0.57236, 0.57625]) #~camera 0 for i in range(0, 16): print 'i', i pose_tmp = pose[i] print 'pose_tmp', pose_tmp marker_name = 'realsense_camera' + str(i) print 'marker_name', marker_name int_marker = createInteractiveMarker(marker_name, *pose_tmp) meshmarker = createMeshMarker( 'package://apc_config/models/cameras/realsense_camera_compressed.stl', offset=tuple(pose_tmp[0:3]), rgba=(0.1, 0.1, 0.1, 1), scale=0.001, orientation=tuple(pose_tmp[3:7]), frame_id='/map') int_marker.controls.append(meshmarker) server.insert(int_marker, processFeedback) pose = [] pose.append([1.3216 + .038, 0, 0.45089 + 0.015, 0, 0, 0, 1]) pose.append([1.3216 + .038, 0, 0.36449 + 0.015, 0, 0, 0, 1]) pose.append([0.5406 - .038, 0, 0.27902 + 0.015, 0, 0, 0, 1]) pose.append([0.48962 - .035821, 0, 0.042641 - 0.02, 0, 0, 0, 1]) for i in range(0, 4): pose_temp = pose[i] marker_name = 'camera_bar' + str(i) int_marker = createInteractiveMarker(marker_name, *pose_temp) meshmarker = createMeshMarker( 'package://apc_config/models/cameras/camera_bar.stl', offset=tuple(pose_temp[0:3]), rgba=(0.5, 0.5, 0.5, 1), scale=0.001, orientation=tuple(pose_temp[3:7]), frame_id='/map') int_marker.controls.append(meshmarker) server.insert(int_marker, processFeedback) ################# ## Robot mount ## ################# int_marker = createInteractiveMarker('Robot_mount', 0.02, 0, 0, 0.5, -0.5, -0.5, 0.5) meshmarker = createMeshMarker( 'package://apc_config/models/robot_mount/robot_mount_compressed.stl', offset=(0.02, 0, 0), rgba=(0.1, 0.1, 0.1, 1), orientation=(0.5, -0.5, -0.5, 0.5)) int_marker.controls.append(meshmarker) server.insert(int_marker, processFeedback) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('joint_realsense', *pose_world, frame_id='map') viewMarker = createViewMarker(frame_id='realsense_depth_optical_frame') int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) pyramid_marker = createMeshMarker( 'package://apc_config/models/object_meshes/pyramid.stl', offset=tuple([0, 0, 0]), rgba=(1, 1, 0, 0.1), orientation=tuple([0, 0, 0, 1]), frame_id='realsense_depth_optical_frame', scale=0.002, scales=[2, 2, 1]).markers[0] server.applyChanges() # ros services #startPoseService() s1 = rospy.Service('manualfit_object_pose', GetPose, getPose) rospy.spin()
def main(argv=None): if argv is None: argv = sys.argv parser = optparse.OptionParser() #import socket #useObject = not (socket.gethostname() == 'mcube-002' or socket.gethostname() == 'mcube-003') parser.add_option('-o', '--obj', action="store_true", dest='useObject', help='To use manual object fitting', default=False) parser.add_option('-c', '--cammarker', action="store_true", dest='useCamMarker', help='To use manual camera fitting or not', default=False) parser.add_option('-s', '--shelfmarker', action="store_true", dest='useShelfMarker', help='To use manual shelf fitting or not', default=False) parser.add_option('-a', '--all', action="store_true", dest='useAllMarker', help='To use manual fitting or not', default=False) rospy.init_node("manual_fit_server") global currentExpoPose global currentCrayolaPose global currentKinectPose global currentShelfPose global currentJoint_realsense global currentJoint_realsense_link5 global brKinect global brShelf global nkinect global vis_pub global opt global pyramid_marker (opt, args) = parser.parse_args() currentExpoPose = Pose() currentCrayolaPose = Pose() currentKinectPose = [Pose(),Pose()] currentShelfPose = Pose() currentJoint_realsense = Pose() brKinect = TransformBroadcaster() brShelf = TransformBroadcaster() global listener listener = tf.TransformListener() rospy.sleep(1) # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("affordance_marker") # on mcube-002 don't use object if opt.useObject or opt.useAllMarker: # create an interactive marker for Expo pose = [1.58220779896, 0.296458333731, 1.12021064758, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023] int_marker = createInteractiveMarker('ExpoEraser', *pose) meshmarker = createMeshMarker('package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl', offset=tuple(pose[0:3]), rgba=(0,1,0,1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Crayola pose = [1.63922035694, -0.273455768824, 1.12480783463, -0.0384246744215, 0.697018921375, -0.715074837208, 0.0368278548121] int_marker = createInteractiveMarker('Crayola', *pose) meshmarker = createMeshMarker('package://apc_config/models/object_meshes/crayola_64_ct.stl', offset=tuple(pose[0:3]), rgba=(1,0,0,1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Shelf currentShelfPose.position.x = rospy.get_param('shelf_pose/x') currentShelfPose.position.y = rospy.get_param('shelf_pose/y') currentShelfPose.position.z = rospy.get_param('shelf_pose/z') currentShelfPose.orientation.x = rospy.get_param('shelf_pose/qx') currentShelfPose.orientation.y = rospy.get_param('shelf_pose/qy') currentShelfPose.orientation.z = rospy.get_param('shelf_pose/qz') currentShelfPose.orientation.w = rospy.get_param('shelf_pose/qw') int_marker = createInteractiveMarker('Shelf', currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z, currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w) meshmarker = createMeshMarker('package://apc_config/models/kiva_pod/meshes/pod_lowres.stl', offset=(currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z), rgba=(0,0,1,0.5), orientation=(currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w)) int_marker.controls.append(meshmarker) if opt.useShelfMarker or opt.useAllMarker: int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.01), frameCallbackShelf) # create an interactive marker for Kinects nkinect = 2 # old manual fit kinect pose #pose = [[0.305660784245, -0.883868515491, 1.66722476482, -0.244159132242, 0.827043175697, 0.0950953885913, 0.497335940599], # [0.367319256067, 0.855090618134, 1.78848266602, 0.800817668438, -0.247087046504, 0.528468191624, 0.135500892997]] #pose = [[0.2643078824103202, -0.8475994497881857, 1.5983246933067445, -0.23088847539406132, 0.822983396811164, 0.08995383249481428, 0.511169994763495], #[0.3659496377911322, 0.8087564208783671, 1.779781933883937, 0.8065800933146582, -0.24219685501387836, 0.5258798075015827, 0.11924245508274978]] # pose = [[0.2615747933866079, -0.847265732303331, 1.5924661390467871, -0.22946772410305405, 0.8209085501266424, 0.08173898334984725, 0.5164978476596324], # [0.35734815800402436, 0.809898201970986, 1.7640327819976598, 0.8036871694243676, -0.2417794937981231, 0.5299934028148222, 0.12138698747903716]] pose = [[0.254395043470156, -0.849313121283084, 1.5914400427256015, -0.2499411509851439, 0.8071675924380618, 0.11813937247844475, 0.5215836382491776], [0.3581305540948658, 0.8062610720617674, 1.7711358584054768, 0.8090512113331657, -0.2868798404832606, 0.494998046288383, 0.13458167753046513]] for i in range(nkinect): currentKinectPose[i] = poselist2pose( pose[i] ) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('Kinect%d' % (i+1), *pose[i]) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) #pose = [-0.0246461518109, -0.000698581337929, 0.0109369456768, 0.706081748009, 0.706393122673, -0.0283508431166, 0.04066388309] #pose = [-0.030973277986, 0.0219592899084, -0.0248876661062, 0.702517032623, 0.711110830307, 0.0224311985075, 0.0169796515256] #pose = [-0.0141396149993, 0.0219592899084, -0.0248876661062, 0.70285320282, 0.71131336689, 0.00553341582417, -0.000123182311654] #pose = [-0.0186747914648, 0.00628428290067, -0.0175085197216, 0.702853216106, 0.711313378682, 0.00553342037479, -0.0001231898954] #pose = [-0.0191506993199, 0.0277181266145, -0.0117737966167, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927] pose = [-0.023043141477, 0.0275422775115, -0.0114395739518, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927] pose_world = poseTransform(pose, 'link_5', 'map', listener) currentJoint_realsense = poselist2pose( pose_world ) currentJoint_realsense_link5 = poselist2pose( pose ) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('joint_realsense', *pose_world, frame_id='map') # cubemarker = createCubeMarker(offset=tuple(pose[0:3]), rgba=(1,0,1,0.5), # orientation=tuple(pose[3:7]), # scale=(0.01,0.01,0.01)) #int_marker.controls.append(cubemarker) viewMarker = createViewMarker(frame_id='realsense_depth_optical_frame') #points = [[0.15748027363422223, 0.08267716536239379, 0.2], [-0.15748027363422223, 0.08267716536239379, 0.2], [0.15748027363422223, -0.08267716536239379, 0.2], [-0.15748027363422223, -0.08267716536239379, 0.2] ] # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(1,1,0,0.1), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.001) # int_marker.controls.append(meshmarker) # # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(0,1,0,0.5), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.002) # int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) pyramid_marker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', offset=tuple([0,0,0]), rgba=(1,1,0,0.1), orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.002, scales = [2, 2,1]).markers[0] rospy.Timer(rospy.Duration(0.01), frameCallbackKinect) server.applyChanges() # ros services startPoseService() rospy.spin()
filename = os.environ['APCDATA_BASE']+'/capsen_vision/models/'+obj_id+'/model.pcd' (points, colors) = load_pcd(filename) pose = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw if len(pose) <7: pose = [0,0,0,0,0,0,1] #pdb.set_trace() # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("affordance_marker") int_marker = createInteractiveMarker('Object',*pose) pointmarker = createPointMarker(colors=colors, points=points, offset=tuple(pose[0:3]), orientation=tuple(pose[3:7])) int_marker.controls.append(pointmarker) int_marker.controls.extend(createMoveControls(fixed=True)) #### create a static marker dim = get_obj_dim(obj_id) for i in range(5): vizCubeMarker(size=dim, rgba=(0,1,0,0.3)) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.01), frameCallback) server.applyChanges() # ros services #startPoseService()
global q0 q0 = None listener = tf.TransformListener() rospy.sleep(0.1) currentTargetPose = Pose() br = TransformBroadcaster() currentTargetPoseDirty = False # create an interactive marker server on the topic namespace ik_interactive server = InteractiveMarkerServer("ik_interactive") # create an interactive marker for TargetPose #pose = [1.1756, -0.0080496, 0.79966, 0.96262, -0.039399, -0.25934, 0.06743] pose = [0, 0, 0, 0, 0, 0, 1] currentTargetPose = rosposeTransform(poselist2pose(pose), 'link_6', 'map', listener) pose = pose2list(currentTargetPose) int_marker = createInteractiveMarker('target_pose', *pose, frame_id='map') # cubemarker = createCubeMarker(offset=tuple(pose[0:3]), rgba=(1,0,1,0.5), # orientation=tuple(pose[3:7]), # scale=(0.01,0.01,0.01)) #int_marker.controls.append(cubemarker) int_marker.controls.extend(createMoveControls(fixed=False)) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.1), frameCallback) server.applyChanges() print 'ready' rospy.spin()
brLink6 = TransformBroadcaster() brLink_tag = TransformBroadcaster() # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("urdf_marker") # create an interactive marker for finger_left trans = (0.0658272579312, -0.0897141247988, 0.256312400103) qlink = tf.transformations.quaternion_from_euler(-math.pi/2, math.pi/2, 0) # put in mesh vs joint int_marker = createInteractiveMarker('finger_left', trans[0], trans[1], trans[2], qlink[0], qlink[1], qlink[2], qlink[3], frame_id='finger_left') meshmarker = createMeshMarker('package://apc_config/models/wsg_50/meshes/SpatulaFingersTop.stl', offset=trans, rgba=(0,1,0,1), orientation=(qlink[0], qlink[1], qlink[2], qlink[3]), frame_id='/finger_left', scale=0.001) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) #server.insert(int_marker, processFeedback) # create an interactive marker for finger_right trans = (-0.0974799394608, -0.131322726607, 0.111145801842) qlink = tf.transformations.quaternion_from_euler(-math.pi/2, -math.pi/2, 0) int_marker = createInteractiveMarker('finger_right', trans[0], trans[1], trans[2], qlink[0], qlink[1], qlink[2], qlink[3], frame_id='finger_right') meshmarker = createMeshMarker('package://apc_config/models/wsg_50/meshes/SpatulaFingersBottom.stl', offset=trans, rgba=(0,1,0,1), orientation=(qlink[0], qlink[1], qlink[2], qlink[3]), frame_id='/finger_right', scale=0.001) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for wrist
def main(argv=None): if argv is None: argv = sys.argv parser = optparse.OptionParser() #import socket #useObject = not (socket.gethostname() == 'mcube-002' or socket.gethostname() == 'mcube-003') parser.add_option('-o', '--obj', action="store_true", dest='useObject', help='To use manual object fitting', default=False) parser.add_option('-c', '--cammarker', action="store_true", dest='useCamMarker', help='To use manual camera fitting or not', default=False) parser.add_option('-s', '--shelfmarker', action="store_true", dest='useShelfMarker', help='To use manual shelf fitting or not', default=False) parser.add_option('-a', '--all', action="store_true", dest='useAllMarker', help='To use manual fitting or not', default=False) rospy.init_node("manual_fit_server") global currentExpoPose global currentCrayolaPose global currentKinectPose global currentShelfPose global currentJoint_realsense global currentJoint_realsense_link5 global brKinect global brShelf global nkinect global vis_pub global opt global pyramid_marker (opt, args) = parser.parse_args() currentExpoPose = Pose() currentCrayolaPose = Pose() currentKinectPose = [Pose(), Pose()] currentShelfPose = Pose() currentJoint_realsense = Pose() brKinect = TransformBroadcaster() brShelf = TransformBroadcaster() global listener listener = tf.TransformListener() rospy.sleep(1) # create an interactive marker server on the topic namespace simple_marker server = InteractiveMarkerServer("affordance_marker") # on mcube-002 don't use object if opt.useObject or opt.useAllMarker: # create an interactive marker for Expo pose = [ 1.58220779896, 0.296458333731, 1.12021064758, -0.00197346811183, -0.738883018494, 0.00179956667125, 0.673828423023 ] int_marker = createInteractiveMarker('ExpoEraser', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/object_meshes/expo_dry_erase_board_eraser.stl', offset=tuple(pose[0:3]), rgba=(0, 1, 0, 1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Crayola pose = [ 1.63922035694, -0.273455768824, 1.12480783463, -0.0384246744215, 0.697018921375, -0.715074837208, 0.0368278548121 ] int_marker = createInteractiveMarker('Crayola', *pose) meshmarker = createMeshMarker( 'package://apc_config/models/object_meshes/crayola_64_ct.stl', offset=tuple(pose[0:3]), rgba=(1, 0, 0, 1), orientation=tuple(pose[3:7])) int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) # create an interactive marker for Shelf currentShelfPose.position.x = rospy.get_param('shelf_pose/x') currentShelfPose.position.y = rospy.get_param('shelf_pose/y') currentShelfPose.position.z = rospy.get_param('shelf_pose/z') currentShelfPose.orientation.x = rospy.get_param('shelf_pose/qx') currentShelfPose.orientation.y = rospy.get_param('shelf_pose/qy') currentShelfPose.orientation.z = rospy.get_param('shelf_pose/qz') currentShelfPose.orientation.w = rospy.get_param('shelf_pose/qw') int_marker = createInteractiveMarker( 'Shelf', currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z, currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w) meshmarker = createMeshMarker( 'package://apc_config/models/kiva_pod/meshes/pod_lowres.stl', offset=(currentShelfPose.position.x, currentShelfPose.position.y, currentShelfPose.position.z), rgba=(0, 0, 1, 0.5), orientation=(currentShelfPose.orientation.x, currentShelfPose.orientation.y, currentShelfPose.orientation.z, currentShelfPose.orientation.w)) int_marker.controls.append(meshmarker) if opt.useShelfMarker or opt.useAllMarker: int_marker.controls.extend(createMoveControls()) server.insert(int_marker, processFeedback) rospy.Timer(rospy.Duration(0.01), frameCallbackShelf) # create an interactive marker for Kinects nkinect = 2 # old manual fit kinect pose #pose = [[0.305660784245, -0.883868515491, 1.66722476482, -0.244159132242, 0.827043175697, 0.0950953885913, 0.497335940599], # [0.367319256067, 0.855090618134, 1.78848266602, 0.800817668438, -0.247087046504, 0.528468191624, 0.135500892997]] #pose = [[0.2643078824103202, -0.8475994497881857, 1.5983246933067445, -0.23088847539406132, 0.822983396811164, 0.08995383249481428, 0.511169994763495], #[0.3659496377911322, 0.8087564208783671, 1.779781933883937, 0.8065800933146582, -0.24219685501387836, 0.5258798075015827, 0.11924245508274978]] # pose = [[0.2615747933866079, -0.847265732303331, 1.5924661390467871, -0.22946772410305405, 0.8209085501266424, 0.08173898334984725, 0.5164978476596324], # [0.35734815800402436, 0.809898201970986, 1.7640327819976598, 0.8036871694243676, -0.2417794937981231, 0.5299934028148222, 0.12138698747903716]] pose = [[ 0.254395043470156, -0.849313121283084, 1.5914400427256015, -0.2499411509851439, 0.8071675924380618, 0.11813937247844475, 0.5215836382491776 ], [ 0.3581305540948658, 0.8062610720617674, 1.7711358584054768, 0.8090512113331657, -0.2868798404832606, 0.494998046288383, 0.13458167753046513 ]] for i in range(nkinect): currentKinectPose[i] = poselist2pose(pose[i]) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('Kinect%d' % (i + 1), *pose[i]) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) #pose = [-0.0246461518109, -0.000698581337929, 0.0109369456768, 0.706081748009, 0.706393122673, -0.0283508431166, 0.04066388309] #pose = [-0.030973277986, 0.0219592899084, -0.0248876661062, 0.702517032623, 0.711110830307, 0.0224311985075, 0.0169796515256] #pose = [-0.0141396149993, 0.0219592899084, -0.0248876661062, 0.70285320282, 0.71131336689, 0.00553341582417, -0.000123182311654] #pose = [-0.0186747914648, 0.00628428290067, -0.0175085197216, 0.702853216106, 0.711313378682, 0.00553342037479, -0.0001231898954] #pose = [-0.0191506993199, 0.0277181266145, -0.0117737966167, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927] pose = [ -0.023043141477, 0.0275422775115, -0.0114395739518, 0.70285321606, 0.711313378699, 0.00553342388632, -0.000123194876927 ] pose_world = poseTransform(pose, 'link_5', 'map', listener) currentJoint_realsense = poselist2pose(pose_world) currentJoint_realsense_link5 = poselist2pose(pose) if opt.useCamMarker or opt.useAllMarker: int_marker = createInteractiveMarker('joint_realsense', *pose_world, frame_id='map') # cubemarker = createCubeMarker(offset=tuple(pose[0:3]), rgba=(1,0,1,0.5), # orientation=tuple(pose[3:7]), # scale=(0.01,0.01,0.01)) #int_marker.controls.append(cubemarker) viewMarker = createViewMarker(frame_id='realsense_depth_optical_frame') #points = [[0.15748027363422223, 0.08267716536239379, 0.2], [-0.15748027363422223, 0.08267716536239379, 0.2], [0.15748027363422223, -0.08267716536239379, 0.2], [-0.15748027363422223, -0.08267716536239379, 0.2] ] # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(1,1,0,0.1), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.001) # int_marker.controls.append(meshmarker) # # meshmarker = createMeshMarker('package://apc_config/models/object_meshes/pyramid.stl', # offset=tuple([0,0,0]), rgba=(0,1,0,0.5), # orientation=tuple([0,0,0,1]), frame_id='realsense_depth_optical_frame', scale = 0.002) # int_marker.controls.append(meshmarker) int_marker.controls.extend(createMoveControls(fixed=True)) server.insert(int_marker, processFeedback) vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) pyramid_marker = createMeshMarker( 'package://apc_config/models/object_meshes/pyramid.stl', offset=tuple([0, 0, 0]), rgba=(1, 1, 0, 0.1), orientation=tuple([0, 0, 0, 1]), frame_id='realsense_depth_optical_frame', scale=0.002, scales=[2, 2, 1]).markers[0] rospy.Timer(rospy.Duration(0.01), frameCallbackKinect) server.applyChanges() # ros services startPoseService() rospy.spin()