def get_pose_rpy(self):
"""Return the current pose as a tuple of location and orientation as [roll, pitch, yaw]"""
if self._keep_pose_state:
orientation_rpy = [math_utils.degrees_to_radians(v) for v in self._orientation_rpy]
return self._location.copy(), orientation_rpy
response = self._request_get([b"location", b"orientation_rpy"])
location = response[b"location"]
orientation_rpy = response[b"orientation_rpy"]
# Convert to radians
orientation_rpy = [math_utils.degrees_to_radians(v) for v in orientation_rpy]
return location, orientation_rpy
def __init__(self,
address="tcp://localhost:22222",
image_scale_factor=1.0,
max_depth_distance=np.finfo(np.float).max,
max_depth_viewing_angle=math_utils.degrees_to_radians(90.),
max_request_trials=3,
request_timeout=0.5,
location_tolerance=1e-3,
orientation_tolerance=1e-3,
connect=True,
connect_wait_time=0.5,
connect_timeout=0.5,
keep_pose_state=False):
super(MeshRendererZMQClient, self).__init__(max_depth_distance, max_depth_viewing_angle)
self._serializer = serialization.MsgPackSerializer()
self._width = None
self._height = None
self._image_scale_factor = image_scale_factor
self._max_request_trials = max_request_trials
self._request_timeout = request_timeout
self._connect_wait_time = connect_wait_time
self._connect_timeout = connect_timeout
self._location_tolerance = location_tolerance
self._orientation_tolerance = orientation_tolerance
self._request_trials = 0
self._conn = zmq_utils.Connection(address, zmq_utils.zmq.REQ)
if keep_pose_state:
self._location = np.array([0, 0, 0], dtype=np.float)
self._orientation_rpy = np.array([0, 0, 0], dtype=np.float)
self._keep_pose_state = keep_pose_state
if connect:
self.connect()
def __init__(self,
max_depth_distance=np.finfo(np.float).max,
max_depth_viewing_angle=math_utils.degrees_to_radians(90.),
image_scale_factor=1.0):
self._max_depth_distance = max_depth_distance
self._max_depth_viewing_dot_prod = np.cos(max_depth_viewing_angle)
self._image_scale_factor = image_scale_factor
self._ray_direction_image = None
def get_orientation_rpy(self):
"""Return the current orientation in radians as [roll, pitch, yaw]"""
if self._keep_pose_state:
orientation_rpy = self._orientation_rpy
else:
response = self._request_get([b"orientation_rpy"])
orientation_rpy = response[b"orientation_rpy"]
# Convert to radians
orientation_rpy = [math_utils.degrees_to_radians(v) for v in orientation_rpy]
roll, pitch, yaw = orientation_rpy
if pitch <= -np.pi:
pitch += 2 * np.pi
elif pitch > np.pi:
pitch -= 2 * np.pi
orientation_rpy = np.array([roll, pitch, yaw])
return orientation_rpy
def handle_request_msg(self, request_dump, raise_exception=False):
try:
request = self._serializer.loads(request_dump)
request_name = request.get(b"_request", None)
response = None
if request_name == b"ping":
logger.info("Received 'ping' request")
response = {b"ping": b"pong"}
elif request_name == b"set":
logger.debug("Received 'set' request")
projection_transform_changed = False
view_transform_changed = False
for name, value in request.items():
if name.startswith(b"_"):
continue
if name == b"width":
# TODO
logger.warn(
"Changing of width and height is not implemented yet"
)
pass
elif name == b"height":
# TODO
logger.warn(
"Changing of width and height is not implemented yet"
)
pass
elif name == b"horz_fov":
horz_fov = value
focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(horz_fov),
self._framebuffer.width)
vert_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(
focal_length, self._framebuffer.height))
logger.info(
"Setting horizontal FOV: {:.4f}, vertical FOV: {:.4f}"
.format(horz_fov, vert_fov))
self._projection_transform.fov = vert_fov
projection_transform_changed = True
elif name == b"location":
world_location = request[b"location"]
self._view_transform.location = -np.dot(
self._world_to_opengl_mat[:3, :3], world_location)
view_transform_changed = True
elif name == b"orientation_rpy":
# TODO: There should also be a minus sign here
self._view_transform.orientation_rpy = request[
b"orientation_rpy"]
view_transform_changed = True
elif name == b"input_enabled":
self._input_enabled = value
elif name == b"window_visible":
self._window_visible_callbacks(value)
elif name == b"window_active":
self._window_active_callbacks(value)
elif name == b"_request":
pass
else:
logger.error(
"Unknown field in 'get' request: {}".format(name))
if projection_transform_changed:
self._projection_transform_callbacks(
self._projection_transform)
if view_transform_changed:
self._view_transform_callbacks(self._view_transform)
response = {}
elif request_name == b"get":
logger.debug("Received 'get' request")
response = {}
for name in request.get(b"names", []):
if name == b"width":
response[b"width"] = self._framebuffer.width
elif name == b"height":
response[b"height"] = self._framebuffer.height
elif name == b"horz_fov":
vert_fov = self._projection_transform.fov
focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(vert_fov),
self._framebuffer.height)
horz_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(
focal_length, self._framebuffer.width))
response[b"horz_fov"] = horz_fov
elif name == b"location":
# Minus sign because we move the world and not the camera
world_location = -np.dot(
self._opengl_to_world_mat[:3, :3],
self._view_transform.location)
response[b"location"] = world_location
elif name == b"orientation_rpy":
# TODO: There should also be a minus sign here
response[
b"orientation_rpy"] = self._view_transform.orientation_rpy
elif request_name == b"render_images":
logger.debug("Received 'render_images' request")
response = {}
requested_images = [
b"rgb_image", b"depth_image", b"normal_image"
]
projection_transform_changed = False
view_transform_changed = False
tmp_projection_transform = self._projection_transform
tmp_view_transform = self._view_transform
for name, value in request.items():
if name == b"location":
world_location = value
self._view_transform.location = -np.dot(
self._world_to_opengl_mat[:3, :3], world_location)
view_transform_changed = True
elif name == b"orientation_rpy":
# TODO: There should also be a minus sign here
self._view_transform.orientation_rpy = value
view_transform_changed = True
elif name == b"use_trackball" and value:
if self._trackball is None:
logger.warn(
"Client asked for trackball viewpoint but no trackball was registered."
)
else:
tmp_view_transform = self._trackball.transform
elif name == b"fov":
tmp_projection_transform = tmp_projection_transform.copy(
)
horz_fov = value
focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(horz_fov),
self._framebuffer.width)
vert_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(
focal_length, self._framebuffer.height))
logger.info(
"Setting horizontal FOV: {:.4f}, vertical FOV: {:.4f}"
.format(horz_fov, vert_fov))
tmp_projection_transform.fov = vert_fov
projection_transform_changed = True
elif name == b"requested_images":
requested_images = value
elif name == b"_request":
pass
else:
logger.error(
"Unknown field in 'render_images' request: {}".
format(name))
if projection_transform_changed:
self._projection_transform_callbacks(
self._projection_transform)
if view_transform_changed:
self._view_transform_callbacks(self._view_transform)
logger.debug("Requested images: {}".format(requested_images))
with self._framebuffer.activate(clear=True):
self._drawer.draw(tmp_projection_transform.matrix,
tmp_view_transform.matrix,
self._framebuffer.width,
self._framebuffer.height)
if b"rgb_image" in requested_images:
rgb_pixels = self._framebuffer.read_rgba_pixels()
rgb_pixels = np.flip(rgb_pixels, 0)
response[b"rgb_image"] = rgb_pixels
if b"depth_image" in requested_images:
depth_pixels = self._framebuffer.read_rgba_pixels(
color_index=1)
depth_pixels = depth_pixels[:, :, 0]
depth_pixels = np.flip(depth_pixels, 0)
response[b"depth_image"] = depth_pixels
if b"normal_image" in requested_images:
normal_pixels = self._framebuffer.read_rgba_pixels(
color_index=2)
normal_pixels = normal_pixels[:, :, :3]
normal_pixels = np.flip(normal_pixels, 0)
response[b"normal_image"] = normal_pixels
else:
logger.warn("WARNING: Invalid request: {}".format(request))
if response is not None:
response_dump = self._serializer.dumps(response)
# logger.debug("Sending response")
return response_dump
except Exception as exc:
logger.exception("Exception when handling request")
if raise_exception:
raise
def run(args):
# Command line arguments
address = args.address
show_window = args.show_window
poll_timeout = args.poll_timeout
width = args.width
height = args.height
mesh_filename = args.mesh_filename
use_msgpack_for_mesh = args.use_msgpack_for_mesh
window_width = args.window_width
if window_width is None:
window_width = width
window_height = args.window_height
if window_height is None:
window_height = height
window_pos_x = args.window_pos_x
window_pos_y = args.window_pos_y
model_scale = args.model_scale
model_rpy = [args.model_roll, args.model_pitch, args.model_yaw]
depth_scale = args.depth_scale
yaw_speed = args.yaw_speed
pitch_speed = args.pitch_speed
# Compute vertical field of view (needed for glm.perspective)
horz_fov = args.horz_fov
focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(horz_fov), width)
vert_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(focal_length, height))
logger.info("Horizontal FOV: {:.4f}, vertical FOV: {:.4f}".format(
horz_fov, vert_fov))
window_focal_length = camera_utils.fov_to_focal_length(
math_utils.degrees_to_radians(horz_fov), window_width)
window_vert_fov = math_utils.radians_to_degrees(
camera_utils.focal_length_to_fov(window_focal_length, window_height))
logger.info("Window vertical FOV: {:.4f}".format(window_vert_fov))
# Fixed parameters
initial_distance = 10 # initial camera distance
# Glumpy initialization
# glumpy.app.use("glfw")
# glumpy.app.use("glfw", 'ES', 3, 3)
glumpy_config = glumpy.app.configuration.get_default()
glumpy_config.double_buffer = True
glumpy_config.samples = 0
glumpy_config.api = "ES"
glumpy_config.depth_size = 32
glumpy_config.major_version = 3
glumpy_config.minor_version = 1
glumpy_config.profile = "core"
# if show_window:
window = Window(width=window_width,
height=window_height,
fov=window_vert_fov,
znear=0.5,
zfar=1000,
config=glumpy_config,
visible=show_window,
title=b"mesh_renderer_zmq")
trackball = Trackball(pitch=0, yaw=0)
window.glumpy_window.attach(trackball)
app = Application()
logger.info("Glumpy backend: {}".format(glumpy.app.__backend__))
# Process initial events
if window_pos_x is not None or window_pos_y is not None:
if window_pos_x is None:
window_pos_x, _ = window.glumpy_window.get_position()
if window_pos_y is None:
_, window_pos_y = window.glumpy_window.get_position()
window.glumpy_window.set_position(window_pos_x, window_pos_y)
# Offscreen surface initialization
framebuffer = Framebuffer(width,
height,
num_color_attachments=3,
color_dtypes=[np.ubyte, np.float32, np.float32])
fb_color_drawer = FramebufferDrawer(framebuffer,
color_index=0,
draw_mode=FramebufferDrawer.BLIT)
fb_depth_drawer = FramebufferDrawer(framebuffer,
color_index=1,
draw_mode=FramebufferDrawer.DRAW_QUAD)
fb_depth_drawer.set_color_scale(depth_scale)
fb_normal_drawer = FramebufferDrawer(framebuffer,
color_index=2,
draw_mode=FramebufferDrawer.DRAW_QUAD)
fb_normal_drawer.set_color_scale(0.5)
fb_normal_drawer.set_color_offset(0.5)
# Offscreen surface initialization
window_framebuffer = Framebuffer(
window_width,
window_height,
num_color_attachments=3,
color_dtypes=[np.ubyte, np.float32, np.float32])
window_fb_color_drawer = FramebufferDrawer(
window_framebuffer, color_index=0, draw_mode=FramebufferDrawer.BLIT)
window_fb_depth_drawer = FramebufferDrawer(
window_framebuffer,
color_index=1,
draw_mode=FramebufferDrawer.DRAW_QUAD)
window_fb_depth_drawer.set_color_scale(depth_scale)
window_fb_normal_drawer = FramebufferDrawer(
window_framebuffer,
color_index=2,
draw_mode=FramebufferDrawer.DRAW_QUAD)
window_fb_normal_drawer.set_color_scale(0.5)
window_fb_normal_drawer.set_color_offset(0.5)
# Add framebuffer draw for normals
window.add_drawer(window_fb_normal_drawer)
# Transformation from world coordinates (z up, x foward, y left) to opengl coordinates (z backward, x right, y up)
world_to_opengl_mat = np.eye(4, dtype=np.float32)
world_to_opengl_mat = glm.xrotate(world_to_opengl_mat, -90)
world_to_opengl_mat = glm.zrotate(world_to_opengl_mat, 90)
opengl_to_world_mat = np.linalg.inv(world_to_opengl_mat)
# Mesh loading
if mesh_filename is not None:
mesh = None
if use_msgpack_for_mesh:
msgpack_mesh_filename = mesh_filename + ".msgpack"
if os.path.isfile(msgpack_mesh_filename):
logger.info("Loading mesh from msgpack file {}".format(
msgpack_mesh_filename))
mesh = SimpleMesh.read_from_msgpack(msgpack_mesh_filename)
if mesh is None:
logger.info("Loading mesh from file {}".format(mesh_filename))
mesh = SimpleMesh.read_from_file(mesh_filename)
if use_msgpack_for_mesh:
logger.info("Saving mesh to msgpack file {}".format(
msgpack_mesh_filename))
mesh.write_to_msgpack(msgpack_mesh_filename)
else:
mesh = CubeMesh()
logger.info("Setting uniform color")
mesh.set_colors_uniform([0.5, 0.5, 0.5, 1])
# logger.info("Computing mesh colors with z-colormap")
# mesh.set_colors_with_coordinate_colormap(min_coord=0, max_coord=10)
# Drawer initialization
# cube_mesh = CubeMesh()
# cube_drawer = MeshDrawer(cube_mesh)
# cube_drawer.depth_scale = 0.1
# cube_drawer.normal_scale = 0.1
# mesh = SimpleMesh.read_from_ply(sys.argv[1])
# mesh.write_to_pickle("mesh.pickle")
# mesh = SimpleMesh.read_from_pickle("mesh2.pickle")
# mesh = Mesh.from_file(filename)
# mesh_drawer = MeshDrawer(mesh)
mesh_drawer = MeshDrawer(mesh)
mesh_drawer.transform.scale = model_scale
mesh_drawer.transform.orientation_rpy = model_rpy
# Assume mesh is in world coordinate system
# mesh_drawer.model = np.matmul(np.transpose(world_to_opengl_mat), mesh_drawer.model)
# mesh_drawer.color_scale = color_scale
# mesh_drawer.normal_scale = normal_scale
# mesh_drawer.depth_scale = depth_scale
# glm.scale(mesh_drawer.model, 0.01)
# cube_drawer = CubeDrawer()
# Server initialization
logger.info("Starting ZMQ server on address {}".format(address))
server_conn = zmq_utils.Connection(address, zmq_utils.zmq.REP)
server_conn.bind()
renderer_service = renderer_zmq_service.RendererZMQService(
framebuffer, mesh_drawer, vert_fov, initial_distance, trackball,
world_to_opengl_mat)
global input_enabled
global override_renderer_service_transform
global window_enabled
input_enabled = args.input_enabled
window_enabled = window.visible
override_renderer_service_transform = False
def window_visible_callback(visible):
if visible:
logger.info("Showing window")
window.show()
else:
logger.info("Hiding window")
window.hide()
def window_active_callback(active):
global window_enabled
if active:
logger.info("Activating window")
else:
logger.info("Deactivating window")
window_enabled = active
renderer_service.window_visible_callbacks.register(window_visible_callback)
renderer_service.window_active_callbacks.register(window_active_callback)
# Some keyboard input handling
@window.glumpy_window.event
def on_key_press(symbol, modifiers):
try:
character = chr(symbol).lower()
except ValueError as err:
character = ""
logger.debug("Key pressed: {}, modifiers: {}, character: {}".format(
symbol, modifiers, character))
if character == 'n':
logger.info("Showing normals")
window.clear_drawers()
window.add_drawer(window_fb_normal_drawer)
elif character == 'c':
logger.info("Showing colors")
window.clear_drawers()
window.add_drawer(window_fb_color_drawer)
elif character == 'd':
logger.info("Showing depth")
window.clear_drawers()
window.add_drawer(window_fb_depth_drawer)
elif character == 'i':
global input_enabled
if input_enabled:
logger.info("Disabling input")
input_enabled = False
else:
logger.info("Enabling input")
input_enabled = True
elif character == 'a':
global window_enabled
if window_enabled:
logger.info("Disabling window")
window_enabled = False
else:
logger.info("Enabling window")
window_enabled = True
elif character == 'o':
global override_renderer_service_transform
if override_renderer_service_transform:
logger.info("Not overriding renderer service transform input")
override_renderer_service_transform = False
else:
logger.info("Overriding renderer service transform input")
override_renderer_service_transform = True
elif character == 'p':
logger.info("Current location: {}".format(trackball.location))
logger.info("Current world location: {}".format(
np.dot(opengl_to_world_mat[:3, :3],
trackball.camera_location)))
logger.info("Current orientation_rpy: {}".format(
trackball.orientation_rpy))
timer = utils.RateTimer(reset_interval=500)
while True:
dt = app.clock.tick()
# Handle network requests
request_dump = server_conn.recv(timeout=poll_timeout * 1000)
if request_dump is not None:
response_dump = renderer_service.handle_request_msg(
request_dump, raise_exception=True)
if response_dump is not None:
server_conn.send(response_dump)
# Perform any drawer processing
mesh_drawer.tick(dt)
# Animate model if desired
if yaw_speed != 0:
mesh_drawer.transform.yaw += yaw_speed * dt
if pitch_speed != 0:
mesh_drawer.transform.pitch += pitch_speed * dt
if override_renderer_service_transform:
renderer_service.view_transform = trackball.transform.copy()
if window.visible and window_enabled:
if input_enabled and renderer_service.input_enabled:
view_transform = trackball.transform
else:
view_transform = renderer_service.view_transform
with window_framebuffer.activate(clear=True):
mesh_drawer.draw(window.projection, view_transform.matrix,
window.width, window.height)
# For debugging
# color_pixels = framebuffer.read_rgba_pixels()
# depth_pixels = framebuffer.read_rgba_pixels(color_index=1)
# import cv2
# cv2.imshow("color", color_pixels)
# cv2.imshow("depth", depth_pixels[:, :, 0])
# cv2.waitKey(50)
window.override_view(trackball.view)
app.process(dt)
timer.update_and_print_rate(print_interval=100)
def get_horizontal_field_of_view(self):
"""Return the horizontal field of view of the camera"""
horz_fov_degrees = self.get_horizontal_field_of_view_degrees()
# Convert to radians
horz_fov = math_utils.degrees_to_radians(horz_fov_degrees)
return horz_fov