class Camera(object):
def __init__(self, topic='/camera', view=np.eye(4), projection=np.eye(4)):
self.im_pub = Image_Publisher(topic=topic)
self.view = view
self.projection = projection
self.fbo = FrameBuffer
def set_projection(self, projection):
'''Projection matrix'''
self.projection = projection
def set_view(self, view):
'''View matrix'''
self.view = view
def draw(self):
self.im_pub.publish(frame)
class Camera(object):
def __init__(self, topic='/camera', view=np.eye(4), projection=np.eye(4)):
self.im_pub = Image_Publisher(topic=topic)
self.view = view
self.projection = projection
self.fbo = FrameBuffer
def set_projection(self, projection):
'''Projection matrix'''
self.projection = projection
def set_view(self, view):
'''View matrix'''
self.view = view
def draw(self):
self.im_pub.publish(frame)
class Canvas(app.Canvas):
def __init__(self):
app.Canvas.__init__(self, title='Framebuffer post-processing',
keys='interactive', size=(640, 640))
rospy.init_node('simulated_arm_view')
self.img_pub = Image_Publisher('/sim/arm_camera/image_rect', encoding='8UC1')
self.listener = tf.TransformListener()
def on_initialize(self, event):
# Build cube data
# --------------------------------------
texcoord = [(0, 0), (0, 1), (1, 0), (1, 1)]
vertices = [(-1, -1, 0), (-1, +1, 0), (+1, -1, 0), (+1, +1, 0)]
vertices = VertexBuffer(vertices)
#self.listener.waitForTransform("/robot", "/wrist_joint", rospy.Time(), rospy.Duration(4))
"""while not rospy.is_shutdown():
try:
now = rospy.Time.now()
self.listener.waitForTransform("/wrist_joint", "/robot", rospy.Time(), rospy.Duration(4))
(trans,rot) = listener.lookupTransform("/wrist_joint", "/robot", rospy.Time(), rospy.Duration(4))
"""
#pos, rot = self.listener.lookupTransform("/wrist_joint", "/robot", rospy.Time(0))
#print list(pos)
camera_pitch = 0.0 # Degrees
self.rotate = [camera_pitch, 0, 0]
#self.translate = list(pos)
self.translate = [0, 0, -5]
# Build program
# --------------------------------------
view = np.eye(4, dtype=np.float32)
model = np.eye(4, dtype=np.float32)
scale(model, 10, 10, 10)
self.phi = 0
self.cube = Program(cube_vertex, cube_fragment)
self.cube['position'] = vertices
# 4640 x 2256
imtex = cv2.imread(os.path.join(img_path, 'rubixFront.jpg'))
self.cube['texcoord'] = texcoord
self.cube["texture"] = np.uint8(np.clip(imtex + np.random.randint(-60, 20, size=imtex.shape), 0, 255)) + 5
self.cube["texture"].interpolation = 'linear'
self.cube['model'] = model
self.cube['view'] = view
color = Texture2D((640, 640, 3), interpolation='linear')
self.framebuffer = FrameBuffer(color, RenderBuffer((640, 640)))
self.quad = Program(quad_vertex, quad_fragment)
self.quad['texcoord'] = [(0, 0), (0, 1), (1, 0), (1, 1)]
self.quad['position'] = [(-1, -1), (-1, +1), (+1, -1), (+1, +1)]
self.quad['texture'] = color
self.objects = [self.cube]
# OpenGL and Timer initalization
# --------------------------------------
set_state(clear_color=(.3, .3, .35, 1), depth_test=True)
self.timer = app.Timer('auto', connect=self.on_timer, start=True)
self.pub_timer = app.Timer(0.1, connect=self.send_ros_img, start=True)
self._set_projection(self.size)
def send_ros_img(self, event):
# Read frame buffer, get rid of alpha channel
img = self.framebuffer.read()[:, :, 2]
self.img_pub.publish(img)
def on_draw(self, event):
# Image rect
with self.framebuffer:
set_clear_color('gray')
clear(color=True, depth=True)
set_state(depth_test=True)
self.cube.draw('triangle_strip')
set_clear_color('pink')
clear(color=True)
set_state(depth_test=True)
self.quad.draw('triangle_strip')
def on_resize(self, event):
self._set_projection(event.size)
def _set_projection(self, size):
width, height = self.size
set_viewport(0, 0, width, height)
projection = perspective(100.0, width / float(height), 1.0, 20.0)
self.cube['projection'] = projection
def on_timer(self, event):
model = np.eye(4, dtype=np.float32)
#scale(model, 1, 1, 1)
self.cube['model'] = model
self.view = np.eye(4)
xrotate(self.view, self.rotate[0])
yrotate(self.view, self.rotate[1])
zrotate(self.view, self.rotate[2])
translate(self.view, *self.translate)
self.listener.waitForTransform("/robot", "/wrist_joint", rospy.Time(), rospy.Duration(4))
pos, rot = self.listener.lookupTransform("/robot", "/wrist_joint", rospy.Time(0))
print list(pos)
self.translate[0] = -list(pos)[0] * 10
self.translate[1] = -list(pos)[1] * 10
self.translate[2] = -5
self.cube['view'] = self.view
self.update()
def on_key_press(self, event):
"""Controls -
a(A) - move left
d(D) - move right
w(W) - move up
s(S) - move down"""
if(event.text.lower() == 'p'):
print(self.view)
elif(event.text.lower() == 'd'):
self.translate[0] += -0.1
elif(event.text.lower() == 'a'):
self.translate[0] += 0.1
elif(event.text.lower() == 'w'):
self.translate[1] += -0.1
elif(event.text.lower() == 's'):
self.translate[1] += 0.1
elif(event.text.lower() == 'q'):
self.rotate[2] += -1
elif(event.text.lower() == 'e'):
self.rotate[2] += 1
elif(event.text.lower() == 'z'):
self.translate[2] += -0.1
elif(event.text.lower() == 'x'):
self.translate[2] += 0.1
class Test_Transform(object):
def __init__(self):
rospy.init_node('test_transform')
height = 0.085 # meters
angle = -0.4
# view_points = Transform.get_view_points(angle, height)
''' Using our dict of known calibrations'''
self.im_sub = Image_Subscriber('/robot/base_camera/image_rect', self.image_callback, queue_size=1)
self.view_pub = Image_Publisher('/robot/base_camera/down_view', encoding="8UC1", queue_size=1)
self.view = Transform.views['20_half_max']
self.transformer = Transform(view=self.view, sq_size=10)
self.im_num = 0
def update(self):
top = cv2.getTrackbarPos('top_dist', 'image') - 40
bot = cv2.getTrackbarPos('bot_dist', 'image') - 40
# self.view = Transform.views['20_half_max']
self.view = {
'view_points': np.float32([
(440 - bot, 213),
(390 - top, 165),
(210 + top, 165),
(170 + bot, 213),
]),
'frame': {
'x': (95, 557),
'y': (0, 484),
}
}
self.transformer = Transform(view=self.view, sq_size=30)
def image_callback(self, rected):
# self.update()
# for point in self.view_['view_points']
rected = rected[:, :, 2]
# bounds = self.view['bounds']
xformed = self.transformer.transform_image(rected, (1000, 1000))
if DEBUG:
cv2.imshow("rected", rected)
cv2.imshow("xformed", xformed)
# xform_cropped = xformed[310-(280 - 135):310, 135:280]
# xform_cropped = xformed[378 - (296 - 156):378, 156:296]
xform_cropped = xformed[194:345, 100:290]
# cv2.imshow("xform_cropped", xform_cropped)
key_press = cv2.waitKey(1)
if key_press & 0xFF == ord('q'):
exit()
elif key_press & 0xFF == ord('f'):
pyplot.imshow(xformed)
pyplot.show()
elif key_press & 0xFF == ord('p'):
cv2.imwrite('rected{}.png'.format(self.im_num), rected)
cv2.imwrite('xformed{}.png'.format(self.im_num), xformed)
self.im_num += 1
self.view_pub.publish(xformed)