Exemple #1
0
def vizBlock(vizpub, pose, mesh, frame_id, rgba=(0.5,0.5,0.5,1), marker_id=2):
    # prepare block visualization
    meshmarker = createMeshMarker(mesh, 
                 offset=tuple(pose[0:3]), rgba=rgba,
                 orientation=tuple(pose[3:7]), frame_id=frame_id, marker_id = marker_id)
    meshmarker.ns = frame_id
    vizpub.publish(meshmarker)
Exemple #2
0
def vizBlock(pose, mesh, frame_id):
    global vizpub
    meshmarker = createMeshMarker(mesh,
                                  offset=tuple(pose[0:3]),
                                  rgba=(0.5, 0.5, 0.6, 1),
                                  orientation=tuple(pose[3:7]),
                                  frame_id=frame_id)
    vizpub.publish(meshmarker)
Exemple #3
0
def vizBlock(pose):
    # prepare block visualization
    global vizpub
    meshmarker = createMeshMarker('package://pnpush_config/models/object_meshes/SteelBlock.stl', 
                              offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.5,1),
                              orientation=tuple(pose[3:7]), frame_id='vicon/SteelBlock/SteelBlock')
    vizpub.publish(meshmarker)
    rospy.sleep(0.05)
Exemple #4
0
def vizBlock(pose):
    # prepare block visualization
    global vizpub
    meshmarker = createMeshMarker('package://pnpush_config/models/object_meshes/SteelBlock.stl', 
                              offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.5,1),
                              orientation=tuple(pose[3:7]), frame_id='vicon/SteelBlock/SteelBlock')
    vizpub.publish(meshmarker)
    rospy.sleep(0.05)
def vizBlock(pose, mesh, frame_id):
    """block visualization"""
    meshmarker = createMeshMarker(mesh,
                                  offset=tuple(pose[0:3]),
                                  rgba=(0.5, 0.5, 0.5, 1),
                                  orientation=tuple(pose[3:7]),
                                  frame_id=frame_id)
    meshmarker.id = 200
    vizpub.publish(meshmarker)
def vizBlock(pose, mesh, frame_id):
    # prepare block visualization
    global vizpub
    meshmarker = createMeshMarker(mesh,
                                  offset=tuple(pose[0:3]),
                                  rgba=(0.5, 0.5, 0.5, 1),
                                  orientation=tuple(pose[3:7]),
                                  frame_id=frame_id,
                                  marker_id=2)
    vizpub.publish(meshmarker)
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 vizBlock(pose, mesh, frame_id):
    global vizpub
    meshmarker = createMeshMarker(mesh, 
                 offset=tuple(pose[0:3]), rgba=(0.5,0.5,0.6,1),
                 orientation=tuple(pose[3:7]), frame_id=frame_id)
    vizpub.publish(meshmarker)
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()
Exemple #10
0
 currentJoint_tag = Pose()
 brLink4 = TransformBroadcaster()
 brLink5 = TransformBroadcaster()
 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())
Exemple #11
0
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()