def transformObjectsFromCapsenToDesiredFrame(scene, scene_frame_id): global _tflistener global br newscene = copy.deepcopy(scene) for i in range(len(scene.objects)): poselist_capsen_world = poseTransform(pose2list(scene.objects[i].pose), scene_frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf(scene.objects[i].name) # x,y,z,qx,qy,qz,qw poselist_world = transformBack(cap_T_our, poselist_capsen_world) newscene.objects[i].pose = poselist2pose(poselist_world) #pubFrame(br, poselist_world, 'obj_%s' % scene.objects[i].name, 'map') return newscene
def transformObjectsFromCapsenToDesiredFrame(scene, scene_frame_id): global _tflistener global br newscene = copy.deepcopy(scene) for i in range(len(scene.objects)): poselist_capsen_world = poseTransform(pose2list(scene.objects[i].pose), scene_frame_id, 'map', _tflistener) cap_T_our = get_obj_capsentf( scene.objects[i].name) # x,y,z,qx,qy,qz,qw poselist_world = transformBack(cap_T_our, poselist_capsen_world) newscene.objects[i].pose = poselist2pose(poselist_world) #pubFrame(br, poselist_world, 'obj_%s' % scene.objects[i].name, 'map') return newscene
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()
global br, listener 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()
if __name__=="__main__": rospy.init_node("ik_interactive_server") global currentTargetPose, currentTargetPoseDirty global br 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] currentTargetPose = poselist2pose( pose ) 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()
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()