touchbot/device_spec.py

# Copyright 2015 The Chromium OS Authors. All rights reserved.
# Use of this source code is governed by a BSD-style license that can be
# found in the LICENSE file.

import pickle
import sys

from position import Position


class DeviceSpec:
  """ This is a class to represent the details of a given device
  that the robot will interact with.
  """
  @staticmethod
  def LoadFromDisk(filename):
    device_spec = None
    with open(filename, 'rb') as fo:
      device_spec = pickle.load(fo)
    return device_spec

  def SaveToDisk(self, filename):
    with open(filename, 'wb') as fo:
      pickle.dump(self, fo)

  def __init__(self, tr, tl, bl, br):
    self.tr, self.tl = tr, tl
    self.bl, self.br = bl, br

  def __str__(self):
    s = 'Top Right: %s\n' % str(self.tr)
    s += 'Top Left: %s\n' % str(self.tl)
    s += 'Bottom Left: %s\n' % str(self.bl)
    s += 'Bottom Right: %s\n' % str(self.br)
    return s

  def __InterpolateValue(self, v1, v2, ratio):
    """ Linearly interpolate between v1 and v2 """
    return (1.0 - ratio) * v1 + ratio * v2

  def __InterpolatePosition(self, p1, p2, ratio):
    """ Linearly interpolate all values in two positions """
    return Position(self.__InterpolateValue(p1.x, p2.x, ratio),
            self.__InterpolateValue(p1.y, p2.y, ratio),
            self.__InterpolateValue(p1.z, p2.z, ratio),
            self.__InterpolateValue(p1.roll, p2.roll, ratio),
            self.__InterpolateValue(p1.pitch, p2.pitch, ratio),
            self.__InterpolateValue(p1.yaw, p2.yaw, ratio),
            self.__InterpolateValue(p1.ax_1, p2.ax_1, ratio),
            self.__InterpolateValue(p1.ax_2, p2.ax_2, ratio),
            self.__InterpolateValue(p1.ax_3, p2.ax_3, ratio),
            self.__InterpolateValue(p1.ax_4, p2.ax_4, ratio),
            self.__InterpolateValue(p1.ax_5, p2.ax_5, ratio))

  def __ScaleForMaxDiagonalDistance(self, max_diagonal_distance):
    # If there's no limit to the size, then there's no need to scale
    if not max_diagonal_distance:
      return self.tl, self.tr, self.bl, self.br

    # If the limit on the size is bigger than the device, then we're done.
    width = self.br.x - self.tl.x
    height = self.tl.y - self.br.y
    diagonal_distance = (width ** 2 + height ** 2) ** 0.5
    if diagonal_distance <= max_diagonal_distance:
      return self.tl, self.tr, self.bl, self.br

    # Otherwise we have to scale it down
    shrink_ratio = max_diagonal_distance / diagonal_distance
    center = self.RelativePosToAbsolutePos((0.5, 0.5))
    tl = self.__InterpolatePosition(self.tl, center, shrink_ratio)
    tr = self.__InterpolatePosition(self.tr, center, shrink_ratio)
    bl = self.__InterpolatePosition(self.bl, center, shrink_ratio)
    br = self.__InterpolatePosition(self.br, center, shrink_ratio)
    return tl, tr, bl, br


  def RelativePosToAbsolutePos(self, rel_p, angle=None,
                               max_diagonal_distance=None):
    """ Convert from relative (0.0->1.0 scaled) coordinates to absolute
    values for this device.

    If a max_diagonal_distance is supplied the area will be
    scaled down to a section in the center of the device small
    enough that all points in it are within that distance of each
    other.
    """
    rel_x, rel_y = rel_p
    tl, tr, bl, br = self.__ScaleForMaxDiagonalDistance(max_diagonal_distance)

    ptop = self.__InterpolatePosition(tl, tr, rel_x)
    pbot = self.__InterpolatePosition(bl, br, rel_x)
    abs_p = self.__InterpolatePosition(ptop, pbot, rel_y)
    if angle is not None:
      abs_p.yaw += angle
    return abs_p

  def __Distance(self, pos1, pos2):
    dx = pos1.x - pos2.x
    dy = pos1.y - pos2.y
    return (dx * dx + dy * dy) ** 0.5

  def Height(self):
    heights = [];
    for x in [0.0, 1.0]:
      p1 = self.RelativePosToAbsolutePos((x, 0.0))
      p2 = self.RelativePosToAbsolutePos((x, 1.0))
      heights.append(self.__Distance(p1, p2))
    return (heights[0] + heights[1]) / 2.0

  def Width(self):
    widths = [];
    for y in [0.0, 1.0]:
      p1 = self.RelativePosToAbsolutePos((0.0, y))
      p2 = self.RelativePosToAbsolutePos((1.0, y))
      widths.append(self.__Distance(p1, p2))
    return (widths[0] + widths[1]) / 2.0

if __name__ == '__main__':
  try:
    device = DeviceSpec.LoadFromDisk(sys.argv[1])
  except:
    print 'Usage: %s device_spec.p' % __file__
    sys.exit(1)

  print device_spec.Height(), device_spec.Width()