In [1]:

    

import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
sys.path.append('../../rai/rai/ry')
import cv2 as cv
import numpy as np
import libry as ry

print(cv.__version__)


    

    




4.1.0

In [2]:

    

C = ry.Config()
D = C.view()
C.clear()
C.addFile('../../rai-robotModels/baxter/baxter_new.g')
pcl = C.addFrame('pcl', 'head')


    

In [3]:

    

# launch the ros node and baxter operation interface
B = C.operate("marcPythonNode")


    

In [2]:

    

# launch the camera thread, subscribing to the rgb and depth images
cam = ry.Camera("marcPythonNode", "/camera/rgb/image_raw", "/camera/depth/image_rect")


    

In [3]:

    

# launch a second camera!
kinect = ry.Camera("marcPythonNode", "/kinect/rgb/image_rect_color", "/kinect/depth_registered/sw_registered/image_rect")


    

In [4]:

    

## just display, both cameras

while(True):
    rgb = cam.getRgb()
    rgb_kin = cv.cvtColor(kinect.getRgb(), cv.COLOR_BGR2RGB)

    # Display the resulting frame
    cv.imshow('asus', rgb)
    cv.imshow('kinect', rgb_kin)
    
    cv.imshow('asus_depth', 0.5*cam.getDepth())
    cv.imshow('kinect_depth', 0.5*kinect.getDepth())

    if cv.waitKey(1) & 0xFF == ord('q'):
        break


    

In [5]:

    

#the focal length
f = 1./np.tan(0.5*60.8*np.pi/180.)
f = f * 320.
#the relative pose of the camera
pcl.setRelativePose('d(-90 0 0 1) t(-.08 .205 .115) d(26 1 0 0) d(-1 0 1 0) d(6 0 0 1) ')
fxfypxpy = [f, f, 320., 240.]


    

In [11]:

    

#new calibration
pcl.setPosition([-0.0472772, 0.226517, 1.79207 ])
pcl.setQuaternion([0.969594, 0.24362, -0.00590741, 0.0223832])
fxfypxpy = [538.273, 544.277, 307.502, 249.954]


    

In [12]:

    

while(True):
    # sync the model pose with the real robot
    B.sync(C)
    
    # set the "shape" of the pcl frame to be the colored point cloud
    pcl.setPointCloud(cam.getPoints(fxfypxpy), cam.getRgb())
    
    # display rgb and depth
#     cv.imshow('frame',cam.getRgb())
#     cv.imshow('depth',cam.getDepth()/2.) #dividing by 2 to have the gray range in [0,2]
#     if cv.waitKey(1) & 0xFF == ord('q'):
#         break


    

    




---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
<ipython-input-12-ac2c73dd8cf2> in <module>()
      4 
      5     # set the "shape" of the pcl frame to be the colored point cloud
----> 6     pcl.setPointCloud(cam.getPoints(fxfypxpy), cam.getRgb())
      7 
      8     # display rgb and depth

KeyboardInterrupt: 

In [1]:

    

# how to read out the position & rotation for a frame
pcl.setRelativePose('d(-90 0 0 1) t(-.08 .205 .115) d(26 1 0 0) d(-1 0 1 0) d(6 0 0 1) ')
pos = pcl.getPosition()
rot = pcl.getQuaternion()
print('POS=', pos, '\nROT=', rot)


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-1-cb782a95648f> in <module>()
----> 1 pcl.setRelativePose('d(-90 0 0 1) t(-.08 .205 .115) d(26 1 0 0) d(-1 0 1 0) d(6 0 0 1) ')
      2 pos = pcl.getPosition()
      3 rot = pcl.getQuaternion()
      4 print('POS=', pos, '\nROT=', rot)

NameError: name 'pcl' is not defined

In [16]:

    

# how to add a new frame...
obj = C.addFrame('obj')


    

In [20]:

    

#... and set its shape, position, color, etc
obj.setShape(ry.ST.box, [.1,.5,.1,.05])
obj.setPosition([1.,0.5,1.1])
obj.setColor([.1,.1,.1])
obj.info()


    

    
Out[20]:





{'ID': 107,
 'X': [1.0, 0.5, 1.1, 1.0, 0.0, 0.0, 0.0],
 'color': [0.1, 0.1, 0.1],
 'name': 'obj',
 'shape': 'box',
 'size': [0.1, 0.5, 0.1, 0.05]}

In [ ]:

    




    

In [2]:

    

## ONLY FOR WEBCAM!
cap = cv.VideoCapture(0)

while(True):
    # Capture frame-by-frame
    ret, frame = cap.read()

    # Our operations on the frame come here
    gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)

    # Display the resulting frame
    cv.imshow('frame',gray)
    if cv.waitKey(1) & 0xFF == ord('q'):
        break

# When everything done, release the capture
cap.release()
cv.destroyAllWindows()


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-2-a823c24f5a40> in <module>()
      1 ## ONLY FOR WEBCAM!
----> 2 cap = cv.VideoCapture(0)
      3 
      4 while(True):
      5     # Capture frame-by-frame

NameError: name 'cv' is not defined

In [ ]: