'''
Quaternion based methods and objects
'''
import numpy as np
from math import sin, cos, asin, atan2, degrees, radians, acos
import ctypes
from auv_python_helpers import load_library
quat_lib = load_library("libquat.so")
quat_t = ctypes.c_double * 4
vect_t = ctypes.c_double * 3
ret_quat = quat_t()
ret_vect = vect_t()
def quat_from_axis_angle(axis, angle):
""" Axis is normalized and angle is in radians in the range [-pi, pi] """
v = axis * sin(angle/2.0)
return Quaternion(q=[cos(angle/2.0), v[0], v[1], v[2]])
class Quaternion(object):
""" A Quaternion """
def __init__(self, q=None, hpr=None, unit=True):
if q is not None:
self.q = np.array(q)
else:
hpr = [radians(ang/2.0) for ang in hpr]
coses = [cos(x) for x in hpr]
sines = [sin(x) for x in hpr]
#!/usr/bin/env python3
import cv2
import shm
import numpy
import time
import ctypes
from auv_python_helpers import load_library
from vision.modules.base import ModuleBase
from vision import options
options = [options.BoolOption('verbose', False)]
_lib_color_balance = load_library('libauv-color-balance.so')
"""
Convert from RGB color space to HSI color space
"""
def conv_rgb_to_hsi(r_channel, g_channel, b_channel):
#start_time = time.time()
i_channel = (r_channel + g_channel + b_channel) / 3
rgb_min_channel = numpy.minimum(r_channel, g_channel)
rgb_min_channel = numpy.minimum(rgb_min_channel, b_channel)
s_channel = 1 - (rgb_min_channel / i_channel)
g_gte_b_channel = g_channel >= b_channel
h_channel1 = numpy.arccos(
(r_channel - (g_channel / 2) - (b_channel / 2)) /
(numpy.sqrt(r_channel**2 + g_channel**2 + b_channel**2 -
(r_channel * g_channel) - (r_channel * b_channel) -
(g_channel * b_channel))))
h_channel2 = (2 * numpy.pi) - h_channel1
import operator
from auv_python_helpers import load_library
# Return values of _lib.read_frame
FRAMEWORK_QUIT = 1
FRAMEWORK_DELETED = 2
class _Frame(Structure):
_fields_ = [('data', POINTER(c_ubyte)),
('last_frame', c_uint32),
('width', c_ssize_t),
('height', c_ssize_t),
('depth', c_ssize_t),
('acq_time', c_uint64)]
_lib_nothread = load_library('libauv-camera-message-framework.so')
_lib_nothread.create_message_framework_from_cstring.argtypes = (c_char_p, c_ssize_t)
_lib_nothread.create_message_framework_from_cstring.restype = c_voidp
_lib_nothread.access_message_framework_from_cstring.argtypes = (c_char_p,)
_lib_nothread.access_message_framework_from_cstring.restype = c_voidp
_lib_nothread.cleanup_message_framework.argtypes = (c_voidp,)
_lib_nothread.cleanup_message_framework.restype = None
_lib_nothread.kill_message_framework.argtypes = (c_voidp,)
_lib_nothread.kill_message_framework.restype = None
_lib_nothread.read_frame.argtypes = (c_voidp, c_voidp)
_lib_nothread.read_frame.restype = c_int32
'''
Quaternion based methods and objects
'''
import numpy as np
from math import sin, cos, asin, atan2, degrees, radians, acos
import ctypes
from auv_python_helpers import load_library
quat_lib = load_library("libquat.so")
quat_t = ctypes.c_double * 4
vect_t = ctypes.c_double * 3
ret_quat = quat_t()
ret_vect = vect_t()
def quat_from_axis_angle(axis, angle):
""" Axis is normalized and angle is in radians in the range [-pi, pi] """
v = axis * sin(angle/2.0)
return Quaternion(q=[cos(angle/2.0), v[0], v[1], v[2]])
class Quaternion(object):
""" A Quaternion """
def __init__(self, q=None, hpr=None, unit=True):
if q is not None:
self.q = np.array(q)
else:
hpr = [radians(ang/2.0) for ang in hpr]
coses = [cos(x) for x in hpr]
sines = [sin(x) for x in hpr]
def __init__(self):
self.lib = load_library("liblcd.so")
self.obj = self.lib.Lcd_new()
import ctypes
from abc import ABCMeta, abstractmethod
from auv_python_helpers import load_library
auv_var_lib = load_library("libshm.so")
class ShmVar(object):
__metaclass__ = ABCMeta
""" Base class for python shared memory variables """
@classmethod
@abstractmethod
def get(cls):
pass
@classmethod
@abstractmethod
def set(cls, value):
pass
from functools import reduce
import operator
from auv_python_helpers import load_library
# Return values of _lib.read_frame
FRAMEWORK_QUIT = 1
FRAMEWORK_DELETED = 2
class _Frame(Structure):
_fields_ = [('data', POINTER(c_ubyte)), ('last_frame', c_uint32),
('width', c_ssize_t), ('height', c_ssize_t),
('depth', c_ssize_t), ('acq_time', c_uint64)]
_lib_nothread = load_library('libauv-camera-message-framework.so')
_lib_nothread.create_message_framework_from_cstring.argtypes = (c_char_p,
c_ssize_t)
_lib_nothread.create_message_framework_from_cstring.restype = c_voidp
_lib_nothread.access_message_framework_from_cstring.argtypes = (c_char_p, )
_lib_nothread.access_message_framework_from_cstring.restype = c_voidp
_lib_nothread.cleanup_message_framework.argtypes = (c_voidp, )
_lib_nothread.cleanup_message_framework.restype = None
_lib_nothread.kill_message_framework.argtypes = (c_voidp, )
_lib_nothread.kill_message_framework.restype = None
_lib_nothread.read_frame.argtypes = (c_voidp, c_voidp)