rospy.init_node("find_model_transform")
    br = TransformBroadcaster()
    
    obj_id = sys.argv[1]  # like oreo_mega_stuf
    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)
예제 #2
0
    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()
예제 #3
0
 currentJoint4 = Pose()
 currentJoint5 = Pose()
 currentJoint6 = Pose()
 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) 
예제 #4
0
    rospy.init_node("find_model_transform")
    br = TransformBroadcaster()

    obj_id = sys.argv[1]  # like oreo_mega_stuf
    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)
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()