<commit_before>#include <libmemcached/memcached.h>

#include <string>
#include <vector>

class Memcached
{
public:

  Memcached()
    :
      memc(),
      result()
  {
    memcached_create(&memc);
  }

  Memcached(memcached_st *clone)
    :
      memc(),
      result()
  {
    memcached_clone(&memc, clone);
  }

  ~Memcached()
  {
    memcached_free(&memc);
  }

  bool fetch(std::string &key,
             std::string &ret_val,
             size_t *key_length,
             size_t *value_length,
             uint32_t *flags,
             memcached_return *rc)
  {
    char ret_key[MEMCACHED_MAX_KEY];
    char *value= memcached_fetch(&memc, ret_key, key_length,
                                 value_length, flags, rc);
    if (value)
    {
      ret_val.assign(value);
      key.assign(ret_key);
      return true;
    }
    return false;
  }

  std::string get(const std::string &key, size_t *value_length)
  {
    uint32_t flags;
    memcached_return rc;
    std::string ret_val;

    char *value= memcached_get(&memc, key.c_str(), key.length(),
                               value_length, &flags, &rc);
    if (value)
    {
      ret_val.assign(value);
    }
    return ret_val;
  }

  std::string get_by_key(const std::string &master_key,
                         const std::string &key,
                         size_t *value_length)
  {
    uint32_t flags;
    memcached_return rc;
    std::string ret_val;

    char *value= memcached_get_by_key(&memc, master_key.c_str(), master_key.length(),
                                      key.c_str(), key.length(),
                                      value_length, &flags, &rc);
    if (value)
    {
      ret_val.assign(value);
    }
    return ret_val;
  }

  bool mget(std::vector<std::string> &keys)
  {
    std::vector<const char *> real_keys;
    std::vector<size_t> key_len;
    /*
     * Construct an array which will contain the length
     * of each of the strings in the input vector. Also, to
     * interface with the memcached C API, we need to convert
     * the vector of std::string's to a vector of char *.
     */
    real_keys.reserve(keys.size());
    key_len.reserve(keys.size());

    std::vector<std::string>::iterator it= keys.begin();

    while (it != keys.end())
    {
      real_keys.push_back(const_cast<char *>((*it).c_str()));
      key_len.push_back((*it).length());
      ++it;
    }

    /*
     * If the std::vector of keys is empty then we cannot
     * call memcached_mget as we will get undefined behavior.
     */
    if (!real_keys.empty())
    {
      memcached_return rc= memcached_mget(&memc, &real_keys[0], &key_len[0],
                                          real_keys.size());
      return (rc == MEMCACHED_SUCCESS);
    }

    return false;
  }

  bool set(const std::string &key,
           const std::string &value,
           time_t expiration,
           uint32_t flags)
  {
    memcached_return rc= memcached_set(&memc,
                                       key.c_str(), key.length(),
                                       value.c_str(), value.length(),
                                       expiration, flags);
    return (rc == MEMCACHED_SUCCESS || rc == MEMCACHED_BUFFERED);
  }

  bool set_all(std::vector<std::string> &keys,
               std::vector<std::string> &values,
               time_t expiration,
               uint32_t flags)
  {
    if (keys.size() != values.size())
    {
      return false;
    }
    bool retval= true;
    std::vector<std::string>::iterator key_it= keys.begin();
    std::vector<std::string>::iterator val_it= values.begin();
    while (key_it != keys.end())
    {
      retval= set((*key_it), (*val_it), expiration, flags);
      if (retval == false)
      {
        return retval;
      }
      ++key_it;
      ++val_it;
    }
    return retval;
  }

  bool set_by_key(const std::string &master_key,
                  const std::string &key,
                  const std::string &value,
                  time_t expiration,
                  uint32_t flags)
  {
    memcached_return rc= memcached_set_by_key(&memc, master_key.c_str(),
                                              master_key.length(),
                                              key.c_str(), key.length(),
                                              value.c_str(), value.length(),
                                              expiration,
                                              flags);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool increment(const std::string &key, unsigned int offset, uint64_t *value)
  {
    memcached_return rc= memcached_increment(&memc, key.c_str(), key.length(),
                                             offset, value);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool decrement(const std::string &key, unsigned int offset, uint64_t *value)
  {
    memcached_return rc= memcached_decrement(&memc, key.c_str(),
                                             key.length(),
                                             offset, value);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool add(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_add(&memc, key.c_str(), key.length(),
                                       value.c_str(), value.length(), 0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool add_by_key(const std::string &master_key,
                  const std::string &key,
                  const std::string &value)
  {
    memcached_return rc= memcached_add_by_key(&memc,
                                              master_key.c_str(),
                                              master_key.length(),
                                              key.c_str(),
                                              key.length(),
                                              value.c_str(),
                                              value.length(),
                                              0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool replace(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_replace(&memc, key.c_str(), key.length(),
                                           value.c_str(), value.length(),
                                           0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool replace_by_key(const std::string &master_key,
                      const std::string &key,
                      const std::string &value)
  {
    memcached_return rc= memcached_replace_by_key(&memc,
                                                  master_key.c_str(),
                                                  master_key.length(),
                                                  key.c_str(),
                                                  key.length(),
                                                  value.c_str(),
                                                  value.length(),
                                                  0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool prepend(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_prepend(&memc, key.c_str(), key.length(),
                                           value.c_str(), value.length(), 0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool prepend_by_key(const std::string &master_key,
                      const std::string &key,
                      const std::string &value)
  {
    memcached_return rc= memcached_prepend_by_key(&memc,
                                                  master_key.c_str(),
                                                  master_key.length(),
                                                  key.c_str(),
                                                  key.length(),
                                                  value.c_str(),
                                                  value.length(),
                                                  0,
                                                  0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool append(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_append(&memc,
                                          key.c_str(),
                                          key.length(),
                                          value.c_str(),
                                          value.length(),
                                          0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool append_by_key(const std::string &master_key,
                     const std::string &key,
                     const std::string &value)
  {
    memcached_return rc= memcached_append_by_key(&memc,
                                                 master_key.c_str(),
                                                 master_key.length(),
                                                 key.c_str(),
                                                 key.length(),
                                                 value.c_str(),
                                                 value.length(),
                                                 0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool cas(const std::string &key,
           const std::string &value,
           uint64_t cas_arg)
  {
    memcached_return rc= memcached_cas(&memc, key.c_str(), key.length(),
                                       value.c_str(), value.length(),
                                       0, 0, cas_arg);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool cas_by_key(const std::string &master_key,
                  const std::string &key,
                  const std::string &value,
                  uint64_t cas_arg)
  {
    memcached_return rc= memcached_cas_by_key(&memc,
                                              master_key.c_str(),
                                              master_key.length(),
                                              key.c_str(),
                                              key.length(),
                                              value.c_str(),
                                              value.length(),
                                              0, 0, cas_arg);
    return (rc == MEMCACHED_SUCCESS);
  }

  // using 'remove' vs. 'delete' since 'delete' is a keyword
  bool remove(const std::string &key)
  {
    memcached_return rc= memcached_delete(&memc, key.c_str(), key.length(), 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool delete_by_key(const std::string &master_key,
                     const std::string &key)
  {
    memcached_return rc= memcached_delete_by_key(&memc,
                                                 master_key.c_str(),
                                                 master_key.length(),
                                                 key.c_str(),
                                                 key.length(),
                                                 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool flush(time_t expiration)
  {
    memcached_return rc= memcached_flush(&memc, expiration);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool fetch_execute(memcached_execute_function *callback,
                     void *context,
                     unsigned int num_of_callbacks)
  {
    memcached_return rc= memcached_fetch_execute(&memc,
                                                 callback,
                                                 context,
                                                 num_of_callbacks);
    return (rc == MEMCACHED_SUCCESS);
  }

  const std::string lib_version() const
  {
    const char *ver= memcached_lib_version();
    const std::string version(ver);
    return version;
  }

private:
  memcached_st memc;
  memcached_result_st result;
};
<commit_msg>Updated C++ interface to have include guards. Also modified the naming convention for the functions in the C++ interface. We should use camel case for the C++ interface.<commit_after>/*
 * Summary: C++ interface for memcached server
 *
 * Copy: See Copyright for the status of this software.
 *
 * Authors: Padraig O'Sullivan, Patrick Galbraith
 */

#ifndef LIBMEMCACHEDPP_H
#define LIBMEMCACHEDPP_H

#include <libmemcached/memcached.h>

#include <string>
#include <vector>

class Memcached
{
public:

  Memcached()
    :
      memc(),
      result()
  {
    memcached_create(&memc);
  }

  Memcached(memcached_st *clone)
    :
      memc(),
      result()
  {
    memcached_clone(&memc, clone);
  }

  ~Memcached()
  {
    memcached_free(&memc);
  }

  bool fetch(std::string &key,
             std::string &ret_val,
             size_t *key_length,
             size_t *value_length,
             uint32_t *flags,
             memcached_return *rc)
  {
    char ret_key[MEMCACHED_MAX_KEY];
    char *value= memcached_fetch(&memc, ret_key, key_length,
                                 value_length, flags, rc);
    if (value)
    {
      ret_val.assign(value);
      key.assign(ret_key);
      return true;
    }
    return false;
  }

  std::string get(const std::string &key, size_t *value_length)
  {
    uint32_t flags;
    memcached_return rc;
    std::string ret_val;

    char *value= memcached_get(&memc, key.c_str(), key.length(),
                               value_length, &flags, &rc);
    if (value)
    {
      ret_val.assign(value);
    }
    return ret_val;
  }

  std::string getByKey(const std::string &master_key,
                       const std::string &key,
                       size_t *value_length)
  {
    uint32_t flags;
    memcached_return rc;
    std::string ret_val;

    char *value= memcached_get_by_key(&memc,
                                      master_key.c_str(), master_key.length(),
                                      key.c_str(), key.length(),
                                      value_length, &flags, &rc);
    if (value)
    {
      ret_val.assign(value);
    }
    return ret_val;
  }

  bool mget(std::vector<std::string> &keys)
  {
    std::vector<const char *> real_keys;
    std::vector<size_t> key_len;
    /*
     * Construct an array which will contain the length
     * of each of the strings in the input vector. Also, to
     * interface with the memcached C API, we need to convert
     * the vector of std::string's to a vector of char *.
     */
    real_keys.reserve(keys.size());
    key_len.reserve(keys.size());

    std::vector<std::string>::iterator it= keys.begin();

    while (it != keys.end())
    {
      real_keys.push_back(const_cast<char *>((*it).c_str()));
      key_len.push_back((*it).length());
      ++it;
    }

    /*
     * If the std::vector of keys is empty then we cannot
     * call memcached_mget as we will get undefined behavior.
     */
    if (!real_keys.empty())
    {
      memcached_return rc= memcached_mget(&memc, &real_keys[0], &key_len[0],
                                          real_keys.size());
      return (rc == MEMCACHED_SUCCESS);
    }

    return false;
  }

  bool set(const std::string &key,
           const std::string &value,
           time_t expiration,
           uint32_t flags)
  {
    memcached_return rc= memcached_set(&memc,
                                       key.c_str(), key.length(),
                                       value.c_str(), value.length(),
                                       expiration, flags);
    return (rc == MEMCACHED_SUCCESS || rc == MEMCACHED_BUFFERED);
  }

  bool setAll(std::vector<std::string> &keys,
              std::vector<std::string> &values,
              time_t expiration,
              uint32_t flags)
  {
    if (keys.size() != values.size())
    {
      return false;
    }
    bool retval= true;
    std::vector<std::string>::iterator key_it= keys.begin();
    std::vector<std::string>::iterator val_it= values.begin();
    while (key_it != keys.end())
    {
      retval= set((*key_it), (*val_it), expiration, flags);
      if (retval == false)
      {
        return retval;
      }
      ++key_it;
      ++val_it;
    }
    return retval;
  }

  bool setByKey(const std::string &master_key,
                const std::string &key,
                const std::string &value,
                time_t expiration,
                uint32_t flags)
  {
    memcached_return rc= memcached_set_by_key(&memc, master_key.c_str(),
                                              master_key.length(),
                                              key.c_str(), key.length(),
                                              value.c_str(), value.length(),
                                              expiration,
                                              flags);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool increment(const std::string &key, unsigned int offset, uint64_t *value)
  {
    memcached_return rc= memcached_increment(&memc, key.c_str(), key.length(),
                                             offset, value);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool decrement(const std::string &key, unsigned int offset, uint64_t *value)
  {
    memcached_return rc= memcached_decrement(&memc, key.c_str(),
                                             key.length(),
                                             offset, value);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool add(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_add(&memc, key.c_str(), key.length(),
                                       value.c_str(), value.length(), 0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool addByKey(const std::string &master_key,
                const std::string &key,
                const std::string &value)
  {
    memcached_return rc= memcached_add_by_key(&memc,
                                              master_key.c_str(),
                                              master_key.length(),
                                              key.c_str(),
                                              key.length(),
                                              value.c_str(),
                                              value.length(),
                                              0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool replace(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_replace(&memc, key.c_str(), key.length(),
                                           value.c_str(), value.length(),
                                           0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool replaceByKey(const std::string &master_key,
                    const std::string &key,
                    const std::string &value)
  {
    memcached_return rc= memcached_replace_by_key(&memc,
                                                  master_key.c_str(),
                                                  master_key.length(),
                                                  key.c_str(),
                                                  key.length(),
                                                  value.c_str(),
                                                  value.length(),
                                                  0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool prepend(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_prepend(&memc, key.c_str(), key.length(),
                                           value.c_str(), value.length(), 0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool prependByKey(const std::string &master_key,
                    const std::string &key,
                    const std::string &value)
  {
    memcached_return rc= memcached_prepend_by_key(&memc,
                                                  master_key.c_str(),
                                                  master_key.length(),
                                                  key.c_str(),
                                                  key.length(),
                                                  value.c_str(),
                                                  value.length(),
                                                  0,
                                                  0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool append(const std::string &key, const std::string &value)
  {
    memcached_return rc= memcached_append(&memc,
                                          key.c_str(),
                                          key.length(),
                                          value.c_str(),
                                          value.length(),
                                          0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool appendByKey(const std::string &master_key,
                   const std::string &key,
                   const std::string &value)
  {
    memcached_return rc= memcached_append_by_key(&memc,
                                                 master_key.c_str(),
                                                 master_key.length(),
                                                 key.c_str(),
                                                 key.length(),
                                                 value.c_str(),
                                                 value.length(),
                                                 0, 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool cas(const std::string &key,
           const std::string &value,
           uint64_t cas_arg)
  {
    memcached_return rc= memcached_cas(&memc, key.c_str(), key.length(),
                                       value.c_str(), value.length(),
                                       0, 0, cas_arg);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool casByKey(const std::string &master_key,
                const std::string &key,
                const std::string &value,
                uint64_t cas_arg)
  {
    memcached_return rc= memcached_cas_by_key(&memc,
                                              master_key.c_str(),
                                              master_key.length(),
                                              key.c_str(),
                                              key.length(),
                                              value.c_str(),
                                              value.length(),
                                              0, 0, cas_arg);
    return (rc == MEMCACHED_SUCCESS);
  }

  // using 'remove' vs. 'delete' since 'delete' is a keyword
  bool remove(const std::string &key)
  {
    memcached_return rc= memcached_delete(&memc, key.c_str(), key.length(), 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool removeByKey(const std::string &master_key,
                   const std::string &key)
  {
    memcached_return rc= memcached_delete_by_key(&memc,
                                                 master_key.c_str(),
                                                 master_key.length(),
                                                 key.c_str(),
                                                 key.length(),
                                                 0);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool flush(time_t expiration)
  {
    memcached_return rc= memcached_flush(&memc, expiration);
    return (rc == MEMCACHED_SUCCESS);
  }

  bool fetchExecute(memcached_execute_function *callback,
                    void *context,
                    unsigned int num_of_callbacks)
  {
    memcached_return rc= memcached_fetch_execute(&memc,
                                                 callback,
                                                 context,
                                                 num_of_callbacks);
    return (rc == MEMCACHED_SUCCESS);
  }

  const std::string libVersion() const
  {
    const char *ver= memcached_lib_version();
    const std::string version(ver);
    return version;
  }

private:
  memcached_st memc;
  memcached_result_st result;
};

#endif /* LIBMEMCACHEDPP_H */
<|endoftext|>


<commit_before>#include "Accel.h"

#define LOG_OUT 1
#include <FHT.h>

#define MOVING_AVERAGE_INTERVALS 20
#define ACCEL_THRESHOLD 1
#define COMPASS_AVERAGE_INTERVALS 10
#define ACCELEROMETER_CALIBRATE 0
#define MAG_CALIBRATE_X 0.7
#define MAG_CALIBRATE_Y -0.5
#define MAG_INVERT_Z -1.0

Accel::Accel(uint32_t intervalMS) {
    _intervalMS = intervalMS;
}

bool Accel::begin() {
    // Try to initialise and warn if we couldn't detect the chip
    if (!_lsm.begin()) {
      Serial.println("LSM fail");
      while (1);
    }
    // _lsm.setupAccel(_lsm.LSM9DS0_ACCELRANGE_4G);
    // _lsm.setupMag(_lsm.LSM9DS0_MAGGAIN_2GAUSS);
    _lsm.setupGyro(_lsm.LSM9DS0_GYROSCALE_500DPS);

    return true;
}

bool Accel::Update() {
    int newMillis = millis();
    if ( newMillis - _lastUpdateMS > _intervalMS + 10) {
        Serial.println("E: accel time");
    }
    if (newMillis - _lastUpdateMS < _intervalMS) {
        return false;
    }
    int elaspedMillis = newMillis - _lastUpdateMS;
    _lastUpdateMS = newMillis;
    _count++;

    sensors_event_t accelEvent;
    sensors_event_t magEvent;
    sensors_event_t gyroEvent;
    //sensors_event_t tempEvent;
    _lsm.getEvent(&accelEvent, &magEvent, &gyroEvent, NULL);

    float magx = magEvent.magnetic.x + MAG_CALIBRATE_X;
    float magy = magEvent.magnetic.y + MAG_CALIBRATE_Y;
    float magz = magEvent.magnetic.z * MAG_INVERT_Z; // For some reason z points the opposite of north.

    // Gyro is in degrees per second, so we change to rads and mult by dt to get rotation.
    float degPerSec = Quaternion(gyroEvent.gyro.x, gyroEvent.gyro.y, gyroEvent.gyro.z).norm();
    float degPerSecToRads = PI/180.0 * elaspedMillis / 1000.0;
    float gyrox = gyroEvent.gyro.x * degPerSecToRads;
    float gyroy = gyroEvent.gyro.y * degPerSecToRads;
    float gyroz = gyroEvent.gyro.z * degPerSecToRads;

    // Rotate by the gyro.
    Quaternion gyroRotation = Quaternion().from_euler_rotation(gyrox, gyroy, gyroz);
    _q *= gyroRotation;
    _q.normalize();

    //if (_count % 10 == 0) {
    //Serial.print("_q W: "); Serial.print(_q.a);
    //Serial.print(" X: "); Serial.print(_q.b);
    //Serial.print(" Y: "); Serial.print(_q.c);
    //Serial.print(" Z: "); Serial.print(_q.d);
    //Serial.print(" deg: "); Serial.print(2.0*acos(_q.a)*180/PI);
    //Serial.print(" norm: "); Serial.println(_q.norm());
    //}

    Quaternion gravity(accelEvent.acceleration.x, accelEvent.acceleration.y, accelEvent.acceleration.z);

    // Get the size of the vector before normalizing it.
    float currentAccel = gravity.norm() - SENSORS_GRAVITY_EARTH + ACCELEROMETER_CALIBRATE;
    float currentAbsAccel = abs(currentAccel);
    _avgAbsAccel = (_avgAbsAccel * (MOVING_AVERAGE_INTERVALS-1) + currentAbsAccel)/MOVING_AVERAGE_INTERVALS;
    if (elaspedMillis >= FHT_INTERVAL_MS) {
        computeFht(currentAccel);
    }

    Quaternion expected_gravity = _q.conj().rotate(Quaternion(0, 0, 1));

    // Ignore gravity if it isn't around G.  We only want to update based on the accelrometer if we aren't bouncing.
    // We also want to ignore gravity if the gyro is moving a lot.
    // TODO: carrino: make these defines at the top
    if (degPerSec < 90 && currentAbsAccel < 1) {
        gravity.normalize();
        Quaternion toRotateG = gravity.rotation_between_vectors(expected_gravity);

        // We want to subtract gravity from the magnetic reading.
        // mag readings point into the earth quite a bit, but we don't really care about that.
        // We just want to use mag for rotation around the gravity axis.
        // https://en.wikipedia.org/wiki/Earth%27s_magnetic_field#Inclination
        Quaternion expected_north = _q.conj().rotate(Quaternion(1, 0, 0));
        expected_north += expected_gravity * (-expected_gravity.dot_product(expected_north));
        expected_north.normalize();

        Quaternion mag(magx, magy, magz);
        mag += expected_gravity * (-expected_gravity.dot_product(mag));
        mag.normalize();

        Quaternion toRotateMag = mag.rotation_between_vectors(expected_north);

        _q = _q * toRotateG.fractional(0.1);
        _q = _q * toRotateMag.fractional(0.05);

        // Serial.print("magRot W: "); Serial.print(toRotateMag.a);
        // Serial.print(" X: "); Serial.print(toRotateMag.b);
        // Serial.print(" Y: "); Serial.print(toRotateMag.c);
        // Serial.print(" Z: "); Serial.print(toRotateMag.d);
        // Serial.print(" deg: "); Serial.print(2.0*acos(toRotateMag.a)*180/PI);
        // Serial.print(" norm: "); Serial.println(toRotateMag.norm());

        //if (_count % 10 == 0) {
        //Serial.print("gyroRot W: "); Serial.print(gyroRotation.a);
        //Serial.print(" X: "); Serial.print(gyroRotation.b);
        //Serial.print(" Y: "); Serial.print(gyroRotation.c);
        //Serial.print(" Z: "); Serial.print(gyroRotation.d);
        //Serial.print(" deg: "); Serial.print(2.0*acos(gyroRotation.a)*180/PI);
        //Serial.print(" norm: "); Serial.println(gyroRotation.norm());

        //Serial.print("G W: "); Serial.print(gravity.a);
        //Serial.print(" X: "); Serial.print(gravity.b);
        //Serial.print(" Y: "); Serial.print(gravity.c);
        //Serial.print(" Z: "); Serial.print(gravity.d);
        //Serial.print(" norm: "); Serial.println(gravity.norm());

        //Serial.print("expectG W: "); Serial.print(expected_gravity.a);
        //Serial.print(" X: "); Serial.print(expected_gravity.b);
        //Serial.print(" Y: "); Serial.print(expected_gravity.c);
        //Serial.print(" Z: "); Serial.print(expected_gravity.d);
        //Serial.print(" norm: "); Serial.println(expected_gravity.norm());

        // Serial.print("MagNorth W: "); Serial.print(mag.a);
        // Serial.print(" X: "); Serial.print(mag.b);
        // Serial.print(" Y: "); Serial.print(mag.c);
        // Serial.print(" Z: "); Serial.print(mag.d);
        // Serial.print(" norm: "); Serial.println(mag.norm());

        // Serial.print("expectNorth W: "); Serial.print(expected_north.a);
        // Serial.print(" X: "); Serial.print(expected_north.b);
        // Serial.print(" Y: "); Serial.print(expected_north.c);
        // Serial.print(" Z: "); Serial.print(expected_north.d);
        // Serial.print(" norm: "); Serial.println(expected_north.norm());

        //Serial.print("toRotG W: "); Serial.print(toRotateG.a);
        //Serial.print(" X: "); Serial.print(toRotateG.b);
        //Serial.print(" Y: "); Serial.print(toRotateG.c);
        //Serial.print(" Z: "); Serial.print(toRotateG.d);
        //Serial.print(" deg: "); Serial.print(2.0*acos(toRotateG.a)*180/PI);
        //Serial.print(" norm: "); Serial.println(toRotateG.norm());

        //expected_gravity = (_q * toRotateG).conj().rotate(Quaternion(0, 0, 1));

        //toRotateG = gravity.rotation_between_vectors(expected_gravity);
        //Serial.print("toRotG2 W: "); Serial.print(toRotateG.a);
        //Serial.print(" X: "); Serial.print(toRotateG.b);
        //Serial.print(" Y: "); Serial.print(toRotateG.c);
        //Serial.print(" Z: "); Serial.print(toRotateG.d);
        //Serial.print(" deg: "); Serial.print(2.0*acos(toRotateG.a)*180/PI);
        //Serial.print(" norm: "); Serial.println(toRotateG.norm());

        //}

    }

    return true;
}

float normalize_rads(float angle_rad) {
    if (angle_rad < 0) {
        angle_rad += 2 * PI;
    } else if (angle_rad >= 2*PI) {
        angle_rad -= 2 * PI;
    }
    return angle_rad;
}

void Accel::computeFht(float lastValue) {
    lastValue *= 4000;
    lastValue =  max(-32767, min(32767, lastValue));
    for (int i = 0; i < FHT_N - 1; i++) {
        _old_fht[i] = _old_fht[i + 1];
        fht_input[i] = _old_fht[i];
    }
    fht_input[FHT_N - 1] = (int) lastValue;
    _old_fht[FHT_N - 1] = (int) lastValue;
    fht_window();
    fht_reorder();
    fht_run();
    fht_mag_log();

    //Serial.write(255); // send a start byte
    //Serial.write(fht_log_out, FHT_N/2); // send out the data

    int max = 0;
    int maxIndex = 0;
    for (int i = 0; i < FHT_N/2; i++) {
        uint8_t val = fht_log_out[i];
        if (val > max) {
            max = val;
            maxIndex = i;
        }
        //Serial.print("index: "); Serial.print(i);
        //Serial.print(" log: "); Serial.print(fht_log_out[i]);
        //Serial.print(" val: "); Serial.print(fht_input[i]);
        //Serial.print(" val2: "); Serial.println(fht_input[FHT_N - 1 - i]);
    }
    int realPlusImg = fht_input[maxIndex];
    int realMinusImg = fht_input[FHT_N - 1 - maxIndex];
    //Serial.print("maxIndex: "); Serial.print(maxIndex);
    //Serial.print(" realPlusImg: "); Serial.print(realPlusImg);
    //Serial.print(" realMinusImg: "); Serial.println(realMinusImg);

    //float phase = atan2((fht_input[k]  - fht_input[negK]), (fht_input[k] + fht_input[negK]));
    float phase = atan2(realPlusImg - realMinusImg, realPlusImg + realMinusImg);
    // Serial.print("Phase: "); Serial.println(phase);
    if (maxIndex == _old_max_index && maxIndex > 1) {
        float _phaseDiff = normalize_rads(phase - _old_phase);

        // Only update the rate if we are in the same fht bucket.
        _phaseRateAverage = (((_phaseRateAverage * 9) + _phaseDiff ) / 10);
    }
    _old_phase = phase;
    _old_max_index = maxIndex;

    _phase_avg += _phaseRateAverage;
    if (phase + PI < _phase_avg) {
        phase += 2*PI;
    } else if (phase - PI > _phase_avg) {
        phase -= 2*PI;
    }

    _phase_avg = (_phase_avg * 19 + phase) / 20;
    _phase_avg = normalize_rads(_phase_avg + PI) - PI;

    // Serial.print("Phase Rate Avg: "); Serial.println(_phaseRateAverage);
    // Serial.print("Phase Avg: "); Serial.println(_phase_avg);
    //for (int i = 0; i < FHT_N; i++) {
    //    fht_input[i] = (int) old_fht[0];
    //}
}

float Accel::getPhasePercentage() {
    return (_phase_avg + PI)/(2*PI);
}

bool Accel::isDancing() {
    return  _avgAbsAccel > ACCEL_THRESHOLD;
}

float Accel::getPhaseRatePercentage() {
    return _phaseRateAverage/(2*PI);
}
<commit_msg>clear interrupts for fht<commit_after>#include "Accel.h"

#define LOG_OUT 1
#include <FHT.h>

#define MOVING_AVERAGE_INTERVALS 20
#define ACCEL_THRESHOLD 1
#define COMPASS_AVERAGE_INTERVALS 10
#define ACCELEROMETER_CALIBRATE 0
#define MAG_CALIBRATE_X 0.7
#define MAG_CALIBRATE_Y -0.5
#define MAG_INVERT_Z -1.0

Accel::Accel(uint32_t intervalMS) {
    _intervalMS = intervalMS;
}

bool Accel::begin() {
    // Try to initialise and warn if we couldn't detect the chip
    if (!_lsm.begin()) {
      Serial.println("LSM fail");
      while (1);
    }
    // _lsm.setupAccel(_lsm.LSM9DS0_ACCELRANGE_4G);
    // _lsm.setupMag(_lsm.LSM9DS0_MAGGAIN_2GAUSS);
    _lsm.setupGyro(_lsm.LSM9DS0_GYROSCALE_500DPS);

    return true;
}

bool Accel::Update() {
    int newMillis = millis();
    if ( newMillis - _lastUpdateMS > _intervalMS + 10) {
        Serial.println("E: accel time");
    }
    if (newMillis - _lastUpdateMS < _intervalMS) {
        return false;
    }
    int elaspedMillis = newMillis - _lastUpdateMS;
    _lastUpdateMS = newMillis;
    _count++;

    sensors_event_t accelEvent;
    sensors_event_t magEvent;
    sensors_event_t gyroEvent;
    //sensors_event_t tempEvent;
    _lsm.getEvent(&accelEvent, &magEvent, &gyroEvent, NULL);

    float magx = magEvent.magnetic.x + MAG_CALIBRATE_X;
    float magy = magEvent.magnetic.y + MAG_CALIBRATE_Y;
    float magz = magEvent.magnetic.z * MAG_INVERT_Z; // For some reason z points the opposite of north.

    // Gyro is in degrees per second, so we change to rads and mult by dt to get rotation.
    float degPerSec = Quaternion(gyroEvent.gyro.x, gyroEvent.gyro.y, gyroEvent.gyro.z).norm();
    float degPerSecToRads = PI/180.0 * elaspedMillis / 1000.0;
    float gyrox = gyroEvent.gyro.x * degPerSecToRads;
    float gyroy = gyroEvent.gyro.y * degPerSecToRads;
    float gyroz = gyroEvent.gyro.z * degPerSecToRads;

    // Rotate by the gyro.
    Quaternion gyroRotation = Quaternion().from_euler_rotation(gyrox, gyroy, gyroz);
    _q *= gyroRotation;
    _q.normalize();

    //if (_count % 10 == 0) {
    //Serial.print("_q W: "); Serial.print(_q.a);
    //Serial.print(" X: "); Serial.print(_q.b);
    //Serial.print(" Y: "); Serial.print(_q.c);
    //Serial.print(" Z: "); Serial.print(_q.d);
    //Serial.print(" deg: "); Serial.print(2.0*acos(_q.a)*180/PI);
    //Serial.print(" norm: "); Serial.println(_q.norm());
    //}

    Quaternion gravity(accelEvent.acceleration.x, accelEvent.acceleration.y, accelEvent.acceleration.z);

    // Get the size of the vector before normalizing it.
    float currentAccel = gravity.norm() - SENSORS_GRAVITY_EARTH + ACCELEROMETER_CALIBRATE;
    float currentAbsAccel = abs(currentAccel);
    _avgAbsAccel = (_avgAbsAccel * (MOVING_AVERAGE_INTERVALS-1) + currentAbsAccel)/MOVING_AVERAGE_INTERVALS;
    if (elaspedMillis >= FHT_INTERVAL_MS) {
        // clear interrupts when we are doing FHT
        cli();
        computeFht(currentAccel);
        sei();
    }

    Quaternion expected_gravity = _q.conj().rotate(Quaternion(0, 0, 1));

    // Ignore gravity if it isn't around G.  We only want to update based on the accelrometer if we aren't bouncing.
    // We also want to ignore gravity if the gyro is moving a lot.
    // TODO: carrino: make these defines at the top
    if (degPerSec < 90 && currentAbsAccel < 1) {
        gravity.normalize();
        Quaternion toRotateG = gravity.rotation_between_vectors(expected_gravity);

        // We want to subtract gravity from the magnetic reading.
        // mag readings point into the earth quite a bit, but we don't really care about that.
        // We just want to use mag for rotation around the gravity axis.
        // https://en.wikipedia.org/wiki/Earth%27s_magnetic_field#Inclination
        Quaternion expected_north = _q.conj().rotate(Quaternion(1, 0, 0));
        expected_north += expected_gravity * (-expected_gravity.dot_product(expected_north));
        expected_north.normalize();

        Quaternion mag(magx, magy, magz);
        mag += expected_gravity * (-expected_gravity.dot_product(mag));
        mag.normalize();

        Quaternion toRotateMag = mag.rotation_between_vectors(expected_north);

        _q = _q * toRotateG.fractional(0.1);
        _q = _q * toRotateMag.fractional(0.05);

        // Serial.print("magRot W: "); Serial.print(toRotateMag.a);
        // Serial.print(" X: "); Serial.print(toRotateMag.b);
        // Serial.print(" Y: "); Serial.print(toRotateMag.c);
        // Serial.print(" Z: "); Serial.print(toRotateMag.d);
        // Serial.print(" deg: "); Serial.print(2.0*acos(toRotateMag.a)*180/PI);
        // Serial.print(" norm: "); Serial.println(toRotateMag.norm());

        //if (_count % 10 == 0) {
        //Serial.print("gyroRot W: "); Serial.print(gyroRotation.a);
        //Serial.print(" X: "); Serial.print(gyroRotation.b);
        //Serial.print(" Y: "); Serial.print(gyroRotation.c);
        //Serial.print(" Z: "); Serial.print(gyroRotation.d);
        //Serial.print(" deg: "); Serial.print(2.0*acos(gyroRotation.a)*180/PI);
        //Serial.print(" norm: "); Serial.println(gyroRotation.norm());

        //Serial.print("G W: "); Serial.print(gravity.a);
        //Serial.print(" X: "); Serial.print(gravity.b);
        //Serial.print(" Y: "); Serial.print(gravity.c);
        //Serial.print(" Z: "); Serial.print(gravity.d);
        //Serial.print(" norm: "); Serial.println(gravity.norm());

        //Serial.print("expectG W: "); Serial.print(expected_gravity.a);
        //Serial.print(" X: "); Serial.print(expected_gravity.b);
        //Serial.print(" Y: "); Serial.print(expected_gravity.c);
        //Serial.print(" Z: "); Serial.print(expected_gravity.d);
        //Serial.print(" norm: "); Serial.println(expected_gravity.norm());

        // Serial.print("MagNorth W: "); Serial.print(mag.a);
        // Serial.print(" X: "); Serial.print(mag.b);
        // Serial.print(" Y: "); Serial.print(mag.c);
        // Serial.print(" Z: "); Serial.print(mag.d);
        // Serial.print(" norm: "); Serial.println(mag.norm());

        // Serial.print("expectNorth W: "); Serial.print(expected_north.a);
        // Serial.print(" X: "); Serial.print(expected_north.b);
        // Serial.print(" Y: "); Serial.print(expected_north.c);
        // Serial.print(" Z: "); Serial.print(expected_north.d);
        // Serial.print(" norm: "); Serial.println(expected_north.norm());

        //Serial.print("toRotG W: "); Serial.print(toRotateG.a);
        //Serial.print(" X: "); Serial.print(toRotateG.b);
        //Serial.print(" Y: "); Serial.print(toRotateG.c);
        //Serial.print(" Z: "); Serial.print(toRotateG.d);
        //Serial.print(" deg: "); Serial.print(2.0*acos(toRotateG.a)*180/PI);
        //Serial.print(" norm: "); Serial.println(toRotateG.norm());

        //expected_gravity = (_q * toRotateG).conj().rotate(Quaternion(0, 0, 1));

        //toRotateG = gravity.rotation_between_vectors(expected_gravity);
        //Serial.print("toRotG2 W: "); Serial.print(toRotateG.a);
        //Serial.print(" X: "); Serial.print(toRotateG.b);
        //Serial.print(" Y: "); Serial.print(toRotateG.c);
        //Serial.print(" Z: "); Serial.print(toRotateG.d);
        //Serial.print(" deg: "); Serial.print(2.0*acos(toRotateG.a)*180/PI);
        //Serial.print(" norm: "); Serial.println(toRotateG.norm());

        //}

    }

    return true;
}

float normalize_rads(float angle_rad) {
    if (angle_rad < 0) {
        angle_rad += 2 * PI;
    } else if (angle_rad >= 2*PI) {
        angle_rad -= 2 * PI;
    }
    return angle_rad;
}

void Accel::computeFht(float lastValue) {
    lastValue *= 4000;
    lastValue =  max(-32767, min(32767, lastValue));
    for (int i = 0; i < FHT_N - 1; i++) {
        _old_fht[i] = _old_fht[i + 1];
        fht_input[i] = _old_fht[i];
    }
    fht_input[FHT_N - 1] = (int) lastValue;
    _old_fht[FHT_N - 1] = (int) lastValue;
    fht_window();
    fht_reorder();
    fht_run();
    fht_mag_log();

    //Serial.write(255); // send a start byte
    //Serial.write(fht_log_out, FHT_N/2); // send out the data

    int max = 0;
    int maxIndex = 0;
    for (int i = 0; i < FHT_N/2; i++) {
        uint8_t val = fht_log_out[i];
        if (val > max) {
            max = val;
            maxIndex = i;
        }
        //Serial.print("index: "); Serial.print(i);
        //Serial.print(" log: "); Serial.print(fht_log_out[i]);
        //Serial.print(" val: "); Serial.print(fht_input[i]);
        //Serial.print(" val2: "); Serial.println(fht_input[FHT_N - 1 - i]);
    }
    int realPlusImg = fht_input[maxIndex];
    int realMinusImg = fht_input[FHT_N - 1 - maxIndex];
    //Serial.print("maxIndex: "); Serial.print(maxIndex);
    //Serial.print(" realPlusImg: "); Serial.print(realPlusImg);
    //Serial.print(" realMinusImg: "); Serial.println(realMinusImg);

    //float phase = atan2((fht_input[k]  - fht_input[negK]), (fht_input[k] + fht_input[negK]));
    float phase = atan2(realPlusImg - realMinusImg, realPlusImg + realMinusImg);
    // Serial.print("Phase: "); Serial.println(phase);
    if (maxIndex == _old_max_index && maxIndex > 1) {
        float _phaseDiff = normalize_rads(phase - _old_phase);

        // Only update the rate if we are in the same fht bucket.
        _phaseRateAverage = (((_phaseRateAverage * 9) + _phaseDiff ) / 10);
    }
    _old_phase = phase;
    _old_max_index = maxIndex;

    _phase_avg += _phaseRateAverage;
    if (phase + PI < _phase_avg) {
        phase += 2*PI;
    } else if (phase - PI > _phase_avg) {
        phase -= 2*PI;
    }

    _phase_avg = (_phase_avg * 19 + phase) / 20;
    _phase_avg = normalize_rads(_phase_avg + PI) - PI;

    // Serial.print("Phase Rate Avg: "); Serial.println(_phaseRateAverage);
    // Serial.print("Phase Avg: "); Serial.println(_phase_avg);
    //for (int i = 0; i < FHT_N; i++) {
    //    fht_input[i] = (int) old_fht[0];
    //}
}

float Accel::getPhasePercentage() {
    return (_phase_avg + PI)/(2*PI);
}

bool Accel::isDancing() {
    return  _avgAbsAccel > ACCEL_THRESHOLD;
}

float Accel::getPhaseRatePercentage() {
    return _phaseRateAverage/(2*PI);
}
<|endoftext|>


<commit_before>#include "plane_calibration/plane_calibration_nodelet.hpp"

#include <Eigen/Dense>
#include <ecl/geometry/angle.hpp>

#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>
#include <tf2_msgs/TFMessage.h>
#include <image_geometry/pinhole_camera_model.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_broadcaster.h>

#include "plane_calibration/image_msg_eigen_converter.hpp"
#include "plane_calibration/plane_to_depth_image.hpp"

namespace plane_calibration
{

PlaneCalibrationNodelet::PlaneCalibrationNodelet()
{
  debug_ = false;
  max_noise_ = 0.0;
  plane_calibration_ = std::make_shared<PlaneCalibration>();
}

void PlaneCalibrationNodelet::onInit()
{
  ROS_INFO("[PlaneCalibrationNodelet]: Initializing");

  reconfigure_server_ = std::shared_ptr<dynamic_reconfigure::Server<PlaneCalibrationConfig> >(
      new dynamic_reconfigure::Server<PlaneCalibrationConfig>());
  dynamic_reconfigure::Server<PlaneCalibrationConfig>::CallbackType reconfigure_cb = boost::bind(
      &PlaneCalibrationNodelet::reconfigureCB, this, _1, _2);
  reconfigure_server_->setCallback(reconfigure_cb);

  ros::NodeHandle node_handle = this->getPrivateNodeHandle();
  depth_visualizer_ = std::make_shared<DepthVisualizer>(node_handle);

  pub_candidate_points_ = node_handle.advertise<sensor_msgs::Image>("candidates", 1);
  pub_plane_points_ = node_handle.advertise<sensor_msgs::Image>("plane_points", 1);
  pub_transform_ = node_handle.advertise<tf2_msgs::TFMessage>("adjusted_tf", 1);

  sub_camera_info_ = node_handle.subscribe<sensor_msgs::CameraInfo>("camera_info", 1,
                                                                    &PlaneCalibrationNodelet::cameraInfoCB, this);
  sub_depth_image_ = node_handle.subscribe<sensor_msgs::Image>("input_depth_image", 1,
                                                               &PlaneCalibrationNodelet::depthImageCB, this);

  ROS_INFO("[PlaneCalibrationNodelet]: Initialization finished");
}

void PlaneCalibrationNodelet::reconfigureCB(PlaneCalibrationConfig &config, uint32_t level)
{
  if (debug_ != config.debug)
  {
    debug_ = config.debug;
    std::string debug_status = debug_ ? "enabled" : "disabled";
    ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Debug " << debug_status);
  }

  x_offset_ = config.x;
  y_offset_ = config.y;
  z_offset_ = config.z;

  px_offset_ = ecl::degrees_to_radians(config.px_degree);
  py_offset_ = ecl::degrees_to_radians(config.py_degree);
  pz_offset_ = ecl::degrees_to_radians(config.pz_degree);
  max_noise_ = config.max_noise;

  max_deviation_ = ecl::degrees_to_radians(config.max_deviation_degrees);

  Eigen::Vector3d ground_plane_offset = ground_plane_offset_;
  if (config.use_manual_ground_transform)
  {
    ground_plane_offset = Eigen::Vector3d(x_offset_, y_offset_, z_offset_);
  }

  PlaneCalibration::Parameters parameters;
  parameters.ground_plane_offset_ = ground_plane_offset;
  parameters.max_deviation_ = ecl::degrees_to_radians(config.max_deviation_degrees);
  plane_calibration_->updateParameters(parameters);
}

void PlaneCalibrationNodelet::cameraInfoCB(const sensor_msgs::CameraInfoConstPtr& camera_info_msg)
{
  image_geometry::PinholeCameraModel pinhole_camera_model;
  pinhole_camera_model.fromCameraInfo(camera_info_msg);

  depth_visualizer_->setCameraModel(pinhole_camera_model);

  camera_model_.update(pinhole_camera_model.cx(), pinhole_camera_model.cy(), pinhole_camera_model.fx(),
                       pinhole_camera_model.fy(), camera_info_msg->width, camera_info_msg->height);
  plane_calibration_->updateParameters(camera_model_);
}

void PlaneCalibrationNodelet::depthImageCB(const sensor_msgs::ImageConstPtr& depth_image_msg)
{
  Eigen::MatrixXf depth_matrix;

  bool converted_successfully = ImageMsgEigenConverter::convert(depth_image_msg, depth_matrix);
  if (!converted_successfully)
  {
    ROS_ERROR_STREAM("[PlaneCalibrationNodelet]: Conversion from image msg to Eigen matrix failed");
    return;
  }

  plane_calibration_->updateMaxDeviationPlanesIfNeeded();

  if (debug_)
  {
    publishMaxDeviationPlanes();
  }

  magic_estimator_ = std::make_shared<MagicMultiplierEstimation>(camera_model_, plane_calibration_, z_offset_,
                                                                 max_deviation_, max_deviation_);

  testCalibration();
//  test();
}

void PlaneCalibrationNodelet::publishMaxDeviationPlanes()
{
  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;

  transformStamped.header.stamp = ros::Time::now();
  std::string frame_id = "camera_link";
  transformStamped.header.frame_id = frame_id;

  PlaneCalibration::PlanesWithTransforms planes = plane_calibration_->getDeviationPlanes();

  for (int i = 0; i < planes.size(); ++i)
  {
    Eigen::MatrixXf plane_image_matrix = planes[i].plane;
    Eigen::Affine3d transform = planes[i].transform;
    std::string index_string = std::to_string(i);

    depth_visualizer_->publishImage("debug/images/max_deviation_" + index_string, plane_image_matrix, frame_id);
    depth_visualizer_->publishCloud("debug/clouds/max_deviation_" + index_string, plane_image_matrix, frame_id);

    transformStamped.child_frame_id = "max_deviation_plane_" + index_string;
    tf::transformEigenToMsg(transform, transformStamped.transform);
    transform_broadcaster.sendTransform(transformStamped);
  }
}

void PlaneCalibrationNodelet::testCalibration()
{
  //construct plane we want to fit
  std::string frame_id = "camera_link";
  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(px_offset_, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(py_offset_, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(pz_offset_, Eigen::Vector3d::UnitZ());

  Eigen::Affine3d transform = Eigen::Translation3d(Eigen::Vector3d(x_offset_, y_offset_, z_offset_)) * rotation;
  Eigen::MatrixXf plane_image_matrix = PlaneToDepthImage::convert(transform, camera_model_.getParameters());

  //get initial estimate
  bool calculate_errors = false;
  MagicMultiplierEstimation::Result magic_result = magic_estimator_->estimate(calculate_errors);

  Eigen::AngleAxisd estimation = plane_calibration_->estimateRotation(plane_image_matrix, magic_result.multiplier_x,
                                                                      magic_result.multiplier_y, 20);

  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = frame_id;

  transformStamped.child_frame_id = "test_plane";
  tf::transformEigenToMsg(transform, transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  transformStamped.child_frame_id = "result";
  tf::transformEigenToMsg(Eigen::Translation3d(Eigen::Vector3d(x_offset_, y_offset_, z_offset_)) * estimation,
                          transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  std::cout << "actual: " << std::endl << rotation << std::endl;
  std::cout << "estimated: " << std::endl << estimation.matrix() << std::endl;
}

void PlaneCalibrationNodelet::test()
{
  std::string frame_id = "camera_link";
  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(px_offset_, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(py_offset_, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(pz_offset_, Eigen::Vector3d::UnitZ());

  Eigen::Affine3d transform = Eigen::Translation3d(Eigen::Vector3d(x_offset_, y_offset_, z_offset_)) * rotation;
  Eigen::MatrixXf plane_image_matrix = PlaneToDepthImage::convert(transform, camera_model_.getParameters());
  Eigen::MatrixXf noise = Eigen::MatrixXf::Random(plane_image_matrix.rows(), plane_image_matrix.cols());
  Eigen::MatrixXf random_plane_image = plane_image_matrix + max_noise_ * noise;

  depth_visualizer_->publishCloud("debug/test", random_plane_image, frame_id);

  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = frame_id;
  transformStamped.child_frame_id = "test_plane";
  tf::transformEigenToMsg(transform, transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  auto distances = plane_calibration_->getDistancesToMaxDeviations(random_plane_image);

  depth_visualizer_->publishDouble("debug/x_positive", distances[0]);
  depth_visualizer_->publishDouble("debug/y_positive", distances[1]);
  depth_visualizer_->publishDouble("debug/x_negative", distances[2]);
  depth_visualizer_->publishDouble("debug/y_negative", distances[3]);

  auto distances_diffs = plane_calibration_->getXYDistanceDiff(random_plane_image);
  depth_visualizer_->publishDouble("debug/x_diff", distances_diffs.first);
  depth_visualizer_->publishDouble("debug/y_diff", distances_diffs.second);

  bool calculate_errors = false;
  MagicMultiplierEstimation::Result magic_result = magic_estimator_->estimate(calculate_errors);

  double px_estimation = magic_result.multiplier_x * distances_diffs.first;
  double py_estimation = magic_result.multiplier_y * distances_diffs.second;
  depth_visualizer_->publishDouble("debug/px_degree_estimated", ecl::radians_to_degrees(px_estimation));
  depth_visualizer_->publishDouble("debug/py_degree_estimated", ecl::radians_to_degrees(py_estimation));

  double px_estimation_abs_error = px_estimation - px_offset_;
  double py_estimation_abs_error = py_estimation - py_offset_;

  depth_visualizer_->publishDouble("debug/px_estimation_abs_error_degree",
                                   ecl::radians_to_degrees(px_estimation_abs_error));
  depth_visualizer_->publishDouble("debug/py_estimation_abs_error_degree",
                                   ecl::radians_to_degrees(py_estimation_abs_error));

  double px_estimation_error = px_offset_ == 0.0 ? 0.0 : px_estimation_abs_error / px_offset_;
  double py_estimation_error = py_offset_ == 0.0 ? 0.0 : py_estimation_abs_error / py_offset_;

  depth_visualizer_->publishDouble("debug/px_estimation_error", px_estimation_error);
  depth_visualizer_->publishDouble("debug/py_estimation_error", py_estimation_error);
}

} /* end namespace */

PLUGINLIB_EXPORT_CLASS(plane_calibration::PlaneCalibrationNodelet, nodelet::Nodelet)
<commit_msg>Uncomment test stuff<commit_after>#include "plane_calibration/plane_calibration_nodelet.hpp"

#include <Eigen/Dense>
#include <ecl/geometry/angle.hpp>

#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>
#include <tf2_msgs/TFMessage.h>
#include <image_geometry/pinhole_camera_model.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_broadcaster.h>

#include "plane_calibration/image_msg_eigen_converter.hpp"
#include "plane_calibration/plane_to_depth_image.hpp"

namespace plane_calibration
{

PlaneCalibrationNodelet::PlaneCalibrationNodelet()
{
  debug_ = false;
  max_noise_ = 0.0;
  plane_calibration_ = std::make_shared<PlaneCalibration>();
}

void PlaneCalibrationNodelet::onInit()
{
  ROS_INFO("[PlaneCalibrationNodelet]: Initializing");

  reconfigure_server_ = std::shared_ptr<dynamic_reconfigure::Server<PlaneCalibrationConfig> >(
      new dynamic_reconfigure::Server<PlaneCalibrationConfig>());
  dynamic_reconfigure::Server<PlaneCalibrationConfig>::CallbackType reconfigure_cb = boost::bind(
      &PlaneCalibrationNodelet::reconfigureCB, this, _1, _2);
  reconfigure_server_->setCallback(reconfigure_cb);

  ros::NodeHandle node_handle = this->getPrivateNodeHandle();
  depth_visualizer_ = std::make_shared<DepthVisualizer>(node_handle);

  pub_candidate_points_ = node_handle.advertise<sensor_msgs::Image>("candidates", 1);
  pub_plane_points_ = node_handle.advertise<sensor_msgs::Image>("plane_points", 1);
  pub_transform_ = node_handle.advertise<tf2_msgs::TFMessage>("adjusted_tf", 1);

  sub_camera_info_ = node_handle.subscribe<sensor_msgs::CameraInfo>("camera_info", 1,
                                                                    &PlaneCalibrationNodelet::cameraInfoCB, this);
  sub_depth_image_ = node_handle.subscribe<sensor_msgs::Image>("input_depth_image", 1,
                                                               &PlaneCalibrationNodelet::depthImageCB, this);

  ROS_INFO("[PlaneCalibrationNodelet]: Initialization finished");
}

void PlaneCalibrationNodelet::reconfigureCB(PlaneCalibrationConfig &config, uint32_t level)
{
  if (debug_ != config.debug)
  {
    debug_ = config.debug;
    std::string debug_status = debug_ ? "enabled" : "disabled";
    ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Debug " << debug_status);
  }

  x_offset_ = config.x;
  y_offset_ = config.y;
  z_offset_ = config.z;

  px_offset_ = ecl::degrees_to_radians(config.px_degree);
  py_offset_ = ecl::degrees_to_radians(config.py_degree);
  pz_offset_ = ecl::degrees_to_radians(config.pz_degree);
  max_noise_ = config.max_noise;

  max_deviation_ = ecl::degrees_to_radians(config.max_deviation_degrees);

  Eigen::Vector3d ground_plane_offset = ground_plane_offset_;
  if (config.use_manual_ground_transform)
  {
    ground_plane_offset = Eigen::Vector3d(x_offset_, y_offset_, z_offset_);
  }

  PlaneCalibration::Parameters parameters;
  parameters.ground_plane_offset_ = ground_plane_offset;
  parameters.max_deviation_ = ecl::degrees_to_radians(config.max_deviation_degrees);
  plane_calibration_->updateParameters(parameters);
}

void PlaneCalibrationNodelet::cameraInfoCB(const sensor_msgs::CameraInfoConstPtr& camera_info_msg)
{
  image_geometry::PinholeCameraModel pinhole_camera_model;
  pinhole_camera_model.fromCameraInfo(camera_info_msg);

  depth_visualizer_->setCameraModel(pinhole_camera_model);

  camera_model_.update(pinhole_camera_model.cx(), pinhole_camera_model.cy(), pinhole_camera_model.fx(),
                       pinhole_camera_model.fy(), camera_info_msg->width, camera_info_msg->height);
  plane_calibration_->updateParameters(camera_model_);
}

void PlaneCalibrationNodelet::depthImageCB(const sensor_msgs::ImageConstPtr& depth_image_msg)
{
  Eigen::MatrixXf depth_matrix;

  bool converted_successfully = ImageMsgEigenConverter::convert(depth_image_msg, depth_matrix);
  if (!converted_successfully)
  {
    ROS_ERROR_STREAM("[PlaneCalibrationNodelet]: Conversion from image msg to Eigen matrix failed");
    return;
  }

  plane_calibration_->updateMaxDeviationPlanesIfNeeded();

  if (debug_)
  {
    publishMaxDeviationPlanes();
  }

  magic_estimator_ = std::make_shared<MagicMultiplierEstimation>(camera_model_, plane_calibration_, z_offset_,
                                                                 max_deviation_, max_deviation_);

//  testCalibration();
//  test();
}

void PlaneCalibrationNodelet::publishMaxDeviationPlanes()
{
  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;

  transformStamped.header.stamp = ros::Time::now();
  std::string frame_id = "camera_link";
  transformStamped.header.frame_id = frame_id;

  PlaneCalibration::PlanesWithTransforms planes = plane_calibration_->getDeviationPlanes();

  for (int i = 0; i < planes.size(); ++i)
  {
    Eigen::MatrixXf plane_image_matrix = planes[i].plane;
    Eigen::Affine3d transform = planes[i].transform;
    std::string index_string = std::to_string(i);

    depth_visualizer_->publishImage("debug/images/max_deviation_" + index_string, plane_image_matrix, frame_id);
    depth_visualizer_->publishCloud("debug/clouds/max_deviation_" + index_string, plane_image_matrix, frame_id);

    transformStamped.child_frame_id = "max_deviation_plane_" + index_string;
    tf::transformEigenToMsg(transform, transformStamped.transform);
    transform_broadcaster.sendTransform(transformStamped);
  }
}

void PlaneCalibrationNodelet::testCalibration()
{
  //construct plane we want to fit
  std::string frame_id = "camera_link";
  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(px_offset_, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(py_offset_, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(pz_offset_, Eigen::Vector3d::UnitZ());

  Eigen::Affine3d transform = Eigen::Translation3d(Eigen::Vector3d(x_offset_, y_offset_, z_offset_)) * rotation;
  Eigen::MatrixXf plane_image_matrix = PlaneToDepthImage::convert(transform, camera_model_.getParameters());

  //get initial estimate
  bool calculate_errors = false;
  MagicMultiplierEstimation::Result magic_result = magic_estimator_->estimate(calculate_errors);

  int iterations = 10;
  double step_size = 0.5;
  Eigen::AngleAxisd estimation = plane_calibration_->estimateRotation(plane_image_matrix, magic_result.multiplier_x,
                                                                      magic_result.multiplier_y, iterations, step_size);

  std::pair<double, double> one_shot_result = plane_calibration_->estimateAngles(plane_image_matrix,
                                                                                magic_result.multiplier_x,
                                                                                magic_result.multiplier_y);

  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = frame_id;

  transformStamped.child_frame_id = "test_plane";
  tf::transformEigenToMsg(transform, transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  transformStamped.child_frame_id = "result";
  tf::transformEigenToMsg(Eigen::Translation3d(Eigen::Vector3d(x_offset_, y_offset_, z_offset_)) * estimation,
                          transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  std::cout << "actual: " << std::endl << rotation << std::endl;
  std::cout << "estimated: " << std::endl << estimation.matrix() << std::endl;
}

void PlaneCalibrationNodelet::test()
{
  std::string frame_id = "camera_link";
  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(px_offset_, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(py_offset_, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(pz_offset_, Eigen::Vector3d::UnitZ());

  Eigen::Affine3d transform = Eigen::Translation3d(Eigen::Vector3d(x_offset_, y_offset_, z_offset_)) * rotation;
  Eigen::MatrixXf plane_image_matrix = PlaneToDepthImage::convert(transform, camera_model_.getParameters());
  Eigen::MatrixXf noise = Eigen::MatrixXf::Random(plane_image_matrix.rows(), plane_image_matrix.cols());
  Eigen::MatrixXf random_plane_image = plane_image_matrix + max_noise_ * noise;

  depth_visualizer_->publishCloud("debug/test", random_plane_image, frame_id);

  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = frame_id;
  transformStamped.child_frame_id = "test_plane";
  tf::transformEigenToMsg(transform, transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  auto distances = plane_calibration_->getDistancesToMaxDeviations(random_plane_image);

  depth_visualizer_->publishDouble("debug/x_positive", distances[0]);
  depth_visualizer_->publishDouble("debug/y_positive", distances[1]);
  depth_visualizer_->publishDouble("debug/x_negative", distances[2]);
  depth_visualizer_->publishDouble("debug/y_negative", distances[3]);

  auto distances_diffs = plane_calibration_->getXYDistanceDiff(random_plane_image);
  depth_visualizer_->publishDouble("debug/x_diff", distances_diffs.first);
  depth_visualizer_->publishDouble("debug/y_diff", distances_diffs.second);

  bool calculate_errors = false;
  MagicMultiplierEstimation::Result magic_result = magic_estimator_->estimate(calculate_errors);

  double px_estimation = magic_result.multiplier_x * distances_diffs.first;
  double py_estimation = magic_result.multiplier_y * distances_diffs.second;
  depth_visualizer_->publishDouble("debug/px_degree_estimated", ecl::radians_to_degrees(px_estimation));
  depth_visualizer_->publishDouble("debug/py_degree_estimated", ecl::radians_to_degrees(py_estimation));

  double px_estimation_abs_error = px_estimation - px_offset_;
  double py_estimation_abs_error = py_estimation - py_offset_;

  depth_visualizer_->publishDouble("debug/px_estimation_abs_error_degree",
                                   ecl::radians_to_degrees(px_estimation_abs_error));
  depth_visualizer_->publishDouble("debug/py_estimation_abs_error_degree",
                                   ecl::radians_to_degrees(py_estimation_abs_error));

  double px_estimation_error = px_offset_ == 0.0 ? 0.0 : px_estimation_abs_error / px_offset_;
  double py_estimation_error = py_offset_ == 0.0 ? 0.0 : py_estimation_abs_error / py_offset_;

  depth_visualizer_->publishDouble("debug/px_estimation_error", px_estimation_error);
  depth_visualizer_->publishDouble("debug/py_estimation_error", py_estimation_error);
}

} /* end namespace */

PLUGINLIB_EXPORT_CLASS(plane_calibration::PlaneCalibrationNodelet, nodelet::Nodelet)
<|endoftext|>


<commit_before>/*
 * Copyright 2021 Google LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "fcp/client/engine/tflite_utils.h"

#include "fcp/base/monitoring.h"
#include "tensorflow/lite/string_util.h"

namespace fcp {
namespace client {
namespace engine {
namespace {
// Returns whether the TfLiteTensor is a resource or variant tensor.
bool IsResourceOrVariant(const TfLiteTensor* tensor) {
  return tensor->type == kTfLiteResource || tensor->type == kTfLiteVariant;
}

// Returns the TF C API Data type that corresponds to the given TfLiteType.
tensorflow::DataType GetTensorFlowDataType(TfLiteType type) {
  switch (type) {
    case kTfLiteNoType:
      return tensorflow::DataType::DT_FLOAT;
    case kTfLiteFloat32:
      return tensorflow::DataType::DT_FLOAT;
    case kTfLiteFloat16:
      return tensorflow::DataType::DT_HALF;
    case kTfLiteFloat64:
      return tensorflow::DataType::DT_DOUBLE;
    case kTfLiteInt16:
      return tensorflow::DataType::DT_INT16;
    case kTfLiteInt32:
      return tensorflow::DataType::DT_INT32;
    case kTfLiteUInt32:
      return tensorflow::DataType::DT_UINT32;
    case kTfLiteUInt8:
      return tensorflow::DataType::DT_UINT8;
    case kTfLiteInt8:
      return tensorflow::DataType::DT_INT8;
    case kTfLiteInt64:
      return tensorflow::DataType::DT_INT64;
    case kTfLiteUInt64:
      return tensorflow::DataType::DT_UINT64;
    case kTfLiteComplex64:
      return tensorflow::DataType::DT_COMPLEX64;
    case kTfLiteComplex128:
      return tensorflow::DataType::DT_COMPLEX128;
    case kTfLiteString:
      return tensorflow::DataType::DT_STRING;
    case kTfLiteBool:
      return tensorflow::DataType::DT_BOOL;
    case kTfLiteResource:
      return tensorflow::DataType::DT_RESOURCE;
    case kTfLiteVariant:
      return tensorflow::DataType::DT_VARIANT;
  }
}

}  // anonymous namespace
absl::StatusOr<tensorflow::Tensor> CreateTfTensorFromTfLiteTensor(
    const TfLiteTensor* tflite_tensor) {
  // If the tflite tensor is a resource or variant tensor, then we can assume
  // that it's underlying data is a pointer to a TF tensor and just return a
  // copy of that Tensor (which will share the underlying data with the original
  // tensor, and increase its refcount).
  if (IsResourceOrVariant(tflite_tensor)) {
    return absl::InvalidArgumentError(
        "Resource and Variant tensors are not supported as output tensor.");
  }

  // If the tflite tensor does not point to an already existing Tensorflow
  // tensor though, we have to make a copy of it because TfLite tensor and TF
  // tensor might have different memory alignment, and there is no guarantee
  // that the TfLite tensor will outlive the TF tensor.
  tensorflow::TensorShape shape;
  int num_dims = tflite_tensor->dims->size;
  for (int i = 0; i < num_dims; ++i) {
    shape.AddDim(tflite_tensor->dims->data[i]);
  }

  tensorflow::Tensor tf_tensor(GetTensorFlowDataType(tflite_tensor->type),
                               shape);
  if (tf_tensor.dtype() == tensorflow::DataType::DT_STRING) {
    if (tf_tensor.data()) {
      tensorflow::tstring* p =
          static_cast<tensorflow::tstring*>(tf_tensor.data());
      for (int i = 0; i < tflite::GetStringCount(tflite_tensor); ++p, ++i) {
        auto ref = tflite::GetString(tflite_tensor, i);
        p->assign(ref.str, ref.len);
      }
    }
  } else {
    FCP_CHECK(tf_tensor.tensor_data().size() == tflite_tensor->bytes);

    if (tflite_tensor->data.raw) {
      std::memcpy(tf_tensor.data(), tflite_tensor->data.raw,
                  tflite_tensor->bytes);
    }
  }

  return tf_tensor;
}

}  // namespace engine
}  // namespace client
}  // namespace fcp
<commit_msg>Internal change<commit_after>/*
 * Copyright 2021 Google LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "fcp/client/engine/tflite_utils.h"

#include "fcp/base/monitoring.h"
#include "tensorflow/lite/string_util.h"

namespace fcp {
namespace client {
namespace engine {
namespace {
// Returns whether the TfLiteTensor is a resource or variant tensor.
bool IsResourceOrVariant(const TfLiteTensor* tensor) {
  return tensor->type == kTfLiteResource || tensor->type == kTfLiteVariant;
}

// Returns the TF C API Data type that corresponds to the given TfLiteType.
tensorflow::DataType GetTensorFlowDataType(TfLiteType type) {
  switch (type) {
    case kTfLiteNoType:
      return tensorflow::DataType::DT_FLOAT;
    case kTfLiteFloat32:
      return tensorflow::DataType::DT_FLOAT;
    case kTfLiteFloat16:
      return tensorflow::DataType::DT_HALF;
    case kTfLiteFloat64:
      return tensorflow::DataType::DT_DOUBLE;
    case kTfLiteInt16:
      return tensorflow::DataType::DT_INT16;
    case kTfLiteInt32:
      return tensorflow::DataType::DT_INT32;
    case kTfLiteUInt32:
      return tensorflow::DataType::DT_UINT32;
    case kTfLiteUInt8:
      return tensorflow::DataType::DT_UINT8;
    case kTfLiteInt8:
      return tensorflow::DataType::DT_INT8;
    case kTfLiteInt64:
      return tensorflow::DataType::DT_INT64;
    case kTfLiteUInt64:
      return tensorflow::DataType::DT_UINT64;
    case kTfLiteComplex64:
      return tensorflow::DataType::DT_COMPLEX64;
    case kTfLiteComplex128:
      return tensorflow::DataType::DT_COMPLEX128;
    case kTfLiteString:
      return tensorflow::DataType::DT_STRING;
    case kTfLiteBool:
      return tensorflow::DataType::DT_BOOL;
    case kTfLiteResource:
      return tensorflow::DataType::DT_RESOURCE;
    case kTfLiteVariant:
      return tensorflow::DataType::DT_VARIANT;
    default:
      return tensorflow::DataType::DT_INVALID;
  }
}

}  // anonymous namespace
absl::StatusOr<tensorflow::Tensor> CreateTfTensorFromTfLiteTensor(
    const TfLiteTensor* tflite_tensor) {
  // If the tflite tensor is a resource or variant tensor, then we can assume
  // that it's underlying data is a pointer to a TF tensor and just return a
  // copy of that Tensor (which will share the underlying data with the original
  // tensor, and increase its refcount).
  if (IsResourceOrVariant(tflite_tensor)) {
    return absl::InvalidArgumentError(
        "Resource and Variant tensors are not supported as output tensor.");
  }

  // If the tflite tensor does not point to an already existing Tensorflow
  // tensor though, we have to make a copy of it because TfLite tensor and TF
  // tensor might have different memory alignment, and there is no guarantee
  // that the TfLite tensor will outlive the TF tensor.
  tensorflow::TensorShape shape;
  int num_dims = tflite_tensor->dims->size;
  for (int i = 0; i < num_dims; ++i) {
    shape.AddDim(tflite_tensor->dims->data[i]);
  }

  tensorflow::Tensor tf_tensor(GetTensorFlowDataType(tflite_tensor->type),
                               shape);
  if (tf_tensor.dtype() == tensorflow::DataType::DT_STRING) {
    if (tf_tensor.data()) {
      tensorflow::tstring* p =
          static_cast<tensorflow::tstring*>(tf_tensor.data());
      for (int i = 0; i < tflite::GetStringCount(tflite_tensor); ++p, ++i) {
        auto ref = tflite::GetString(tflite_tensor, i);
        p->assign(ref.str, ref.len);
      }
    }
  } else {
    FCP_CHECK(tf_tensor.tensor_data().size() == tflite_tensor->bytes);

    if (tflite_tensor->data.raw) {
      std::memcpy(tf_tensor.data(), tflite_tensor->data.raw,
                  tflite_tensor->bytes);
    }
  }

  return tf_tensor;
}

}  // namespace engine
}  // namespace client
}  // namespace fcp
<|endoftext|>


<commit_before>//////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2004-2019 musikcube team
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice,
//      this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the author nor the names of other contributors may
//      be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
//////////////////////////////////////////////////////////////////////////////

#include "SndioOut.h"

#include <core/sdk/constants.h>
#include <core/sdk/IPreferences.h>
#include <math.h>
#include <limits.h>
#include <iostream>
#include <unistd.h>
#include <poll.h>

#define BUFFER_COUNT 16
#define ERROR(str) std::cerr << "SndioOut Error: " << str << "\n";
#define INFO(str) std::cerr << "SndioOut Info: " << str << "\n";
#define LOCK() std::unique_lock<std::mutex> lock(this->mutex);
#define WAIT() this->threadEvent.wait(lock);
#define NOTIFY() this->threadEvent.notify_all();

#define STOP() \
  if(started) { \
    if (!sio_stop(handle)) { \
      INFO("failed to stop sndio") \
      quit = true; \
      continue; \
    } \
    else { \
      INFO("stopped handle") \
      started = false; \
   } \
  } \
  else { \
    INFO("already stopped") \
  }

#define START() \
  if(!started) { \
    if (!sio_start(handle)) { \
      INFO("failed to start sndio") \
      quit = true; \
      continue; \
    } \
    else { \
      INFO("started handle") \
      started = true; \
   } \
  } \
  else { \
    INFO("handle already started") \
  }

#define PREF_DEVICE_ID "device_id"

using namespace musik::core::sdk;

static char* deviceId = nullptr;

extern "C" void SetPreferences(musik::core::sdk::IPreferences* prefs) {
    size_t len = prefs->GetString(PREF_DEVICE_ID, nullptr, 0, "");
    if (len > 1) {
        delete[] deviceId;
        deviceId = new char[len];
        prefs->GetString(PREF_DEVICE_ID, deviceId, len, "");
        prefs->Save();
        INFO("setting deviceId to " + std::string(deviceId));
    }
}

static bool waitForDevice(sio_hdl* hdl) {
    INFO("waiting for device")
    int nfds, revents;
    struct pollfd pfds[1];
    do {
        nfds = sio_pollfd(hdl, pfds, POLLOUT);
        if (nfds > 0) {
            if (poll(pfds, nfds, -1) < 0) {
                INFO("poll failed");
                return false;
            }
        }
        revents = sio_revents(hdl, pfds);
        if (revents & POLLHUP) {
            INFO("device disappeared");
            return false;
        }
    } while (!(revents & POLLOUT));
    INFO("done waiting for device")
    return true;
}

SndioOut::SndioOut() {
    INFO("---------- sndout.ctor ----------")
    this->volume = 1.0f;
    this->state = StateStopped;
    this->latency = 0.0;
    this->writeThread.reset(new std::thread(
        std::bind(&SndioOut::WriteLoop, this)));
}

SndioOut::~SndioOut() {
    this->PushCommand(Command::Quit);

    INFO("joining thread")
    this->writeThread->join();
    INFO("thread finished")
}

void SndioOut::Release() {
    delete this;
}

void SndioOut::PushCommand(Command command) {
    INFO("PushCommand.start")
    {
        LOCK()
        commands.push_back(command);
    }
    NOTIFY()
    INFO("PushCommand.end")
}

void SndioOut::Pause() {
    this->PushCommand(Command::Pause);
}

void SndioOut::Resume() {
    this->PushCommand(Command::Resume);
}

void SndioOut::SetVolume(double volume) {
    this->volume = volume;
    this->PushCommand(Command::SetVolume);
}

double SndioOut::GetVolume() {
    return this->volume;
}

void SndioOut::Stop() {
    this->PushCommand(Command::Stop);
}

void SndioOut::Drain() {
    this->PushCommand(Command::Drain);
}

IDeviceList* SndioOut::GetDeviceList() {
    return nullptr;
}

bool SndioOut::SetDefaultDevice(const char* deviceId) {
    return false;
}

IDevice* SndioOut::GetDefaultDevice() {
    return nullptr;
}

int SndioOut::Play(IBuffer *buffer, IBufferProvider *provider) {
    std::this_thread::yield();

    if (this->state != StatePlaying) {
        return OutputInvalidState;
    }

    {
        LOCK()
        if (this->CountBuffersWithProvider(provider) >= BUFFER_COUNT) {
            return OutputBufferFull;
        }
        this->buffers.push_back(BufferContext{provider, buffer});
    }

    NOTIFY()
    return OutputBufferWritten;
}

void SndioOut::WriteLoop() {
    bool started = false;
    std::list<BufferContext> toNotify;
    sio_hdl* handle = nullptr;
    sio_par pars = { 0 };
    short* pcm = nullptr;
    long pcmSamples = 0;
    float ditherState = 0.0;
    bool quit = false;
    BufferContext next {nullptr, nullptr};

    /* open the output device. we only do this once */
    const char* device = (::deviceId && strlen(::deviceId)) ? deviceId : nullptr;
    handle = sio_open(device, SIO_PLAY, 0);
    if (handle == nullptr) {
        INFO("failed to init device")
        quit = true;
    }
    else {
        if (!sio_start(handle)) {
            INFO("device opened, but couldn't start")
            quit = true;
        }
        else {
            started = true;
            INFO("device handle initialized")
        }
    }

    while (!quit) {
        /* drain any old buffers (we do this outside of the critical section */
        if (toNotify.size()) {
            INFO("cleaning up dead buffers")
            for (auto& it : toNotify) {
                it.provider->OnBufferProcessed(it.buffer);
            }
            toNotify.clear();
        }

        {
            /* we wait until we have commands to process or samples to play */
            LOCK()
            while (!quit && !this->commands.size() &&
                   (this->state != StatePlaying || !this->buffers.size()))
            {
                INFO("waiting")
                WAIT()
                INFO("done waiting")
            }

            if (quit) {
                continue;
            }

            /* process commands */
            for (auto command : this->commands) {
                switch (command) {
                    case Command::Pause: {
                        INFO("command.pause")
                        STOP()
                        this->state = StatePaused;
                    } break;
                    case Command::Resume: {
                        INFO("command.resume")
                        START()
                        this->state = StatePlaying;
                    } break;
                    case Command::Stop: {
                        INFO("command.stop")
                        STOP()
                        std::swap(toNotify, this->buffers);
                        this->state = StateStopped;
                    } break;
                    case Command::SetVolume: {
                        INFO("command.setvolume")
                        if (handle) {
                            sio_setvol(handle, lround(this->volume * SIO_MAXVOL));
                        }
                    } break;
                    case Command::Drain: {
                        INFO("command.drain")
                        STOP()
                        START()
                    } break;
                    case Command::Quit: {
                        quit = true;
                        continue;
                    } break;
                }
            }

            this->commands.clear();

            /* play the next buffer, if it exists */
            if (started &&
                this->state == StatePlaying &&
                this->buffers.size())
            {
                next = this->buffers.front();
                this->buffers.pop_front();
            }
            else {
                INFO(std::string("state=") + std::to_string((int) this->state));
                INFO(std::string("count=") + std::to_string((int) this->buffers.size()));
                continue;
            }
        }

        auto buffer = next.buffer;

        if (buffer) {
            /* ensure our output params are setup properly based on this
            buffer's params */
            if (pars.pchan != buffer->Channels() ||
                pars.rate != buffer->SampleRate())
            {
                INFO("updating output params")

                STOP()

                sio_initpar(&pars);
                pars.pchan = buffer->Channels();
                pars.rate = buffer->SampleRate();
                pars.sig = 1;
                pars.le = SIO_LE_NATIVE;
                pars.bits = 16;
                pars.appbufsz = (pars.rate * 250) / 1000;

                if (!sio_setpar(handle, &pars)) {
                    INFO("sio_setpar() failed");
                    quit = true;
                    continue;
                }

                if (!sio_getpar(handle, &pars)) {
                    INFO("sio_getpar() failed");
                    quit = true;
                    continue;
                }

                START()

                this->latency = (double) pars.bufsz / pars.pchan / pars.rate;
            }

            /* allocate PCM buffer, if needed. most of the time we'll be
            able to re-use the previously allocated one. */
            long samples = buffer->Samples();
            if (!pcm || samples > pcmSamples) {
                delete[] pcm;
                pcm = new short[samples];
                pcmSamples = samples;
            }

            /* convert to 16-bit PCM */
            float* src = buffer->BufferPointer();
            short* dst = pcm;
            for (long i = 0; i < samples; i++) {
                float sample = *src;
                if (sample > 1.0f) sample = 1.0f;
                if (sample < -1.0f) sample = -1.0f;
                sample *= SHRT_MAX;

                /* triangle (high pass) dither, based on Audacity's
                implementation */
                float r = (rand() / (float) RAND_MAX - 0.5f);
                sample = sample + r - ditherState;
                ditherState = r;

                *dst = sample;
                ++dst; ++src;
            }

            /* write the entire output buffer. this may require multiple passes;
            that's ok, just loop until we're done */
            char* data = (char*) pcm;
            size_t dataLength = samples * sizeof(short);
            size_t totalWritten = 0;

            {
//              LOCK()
//              INFO("start write")
                while (totalWritten < dataLength) {
                    size_t remaining = dataLength - totalWritten;
                    size_t written = sio_write(handle, data, remaining);
                    if (written == 0) { INFO("failed to write!") break; }
//                  else { std::cerr << "wrote " << written << " of " << remaining << " bytes\n"; }
                    totalWritten += written;
                    data += written;
                }
//              INFO("end write")
            }

            next.provider->OnBufferProcessed(buffer);
        }
    }

    /* done, free remaining buffers and close the handle */
    {
        LOCK()
        std::swap(toNotify, this->buffers);
        this->buffers.clear();
    }

    if (handle) {
        INFO("closing")
        sio_close(handle);
        handle = nullptr;
    }

    for (auto& it : toNotify) {
    INFO("cleaning up dead buffer")
        it.provider->OnBufferProcessed(it.buffer);
    }

    delete[] pcm;
}

double SndioOut::Latency() {
    return this->latency;
}

size_t SndioOut::CountBuffersWithProvider(IBufferProvider* provider) {
    size_t count = 0;
    for (auto& it : this->buffers) {
        if (it.provider == provider) {
            ++count;
        }
    }
    return count;
}

<commit_msg>Removed extraneous logging.<commit_after>//////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2004-2019 musikcube team
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice,
//      this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the author nor the names of other contributors may
//      be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
//////////////////////////////////////////////////////////////////////////////

#include "SndioOut.h"

#include <core/sdk/constants.h>
#include <core/sdk/IPreferences.h>
#include <math.h>
#include <limits.h>
#include <iostream>
#include <unistd.h>
#include <poll.h>

#define BUFFER_COUNT 16
#define ERROR(str) std::cerr << "SndioOut Error: " << str << "\n";
#define INFO(str) std::cerr << "SndioOut Info: " << str << "\n";
#define LOCK() std::unique_lock<std::mutex> lock(this->mutex);
#define WAIT() this->threadEvent.wait(lock);
#define NOTIFY() this->threadEvent.notify_all();

#define STOP() \
  if(started) { \
    if (!sio_stop(handle)) { \
      INFO("failed to stop sndio") \
      quit = true; \
      continue; \
    } \
    else { \
      INFO("stopped handle") \
      started = false; \
   } \
  } \
  else { \
    INFO("already stopped") \
  }

#define START() \
  if(!started) { \
    if (!sio_start(handle)) { \
      INFO("failed to start sndio") \
      quit = true; \
      continue; \
    } \
    else { \
      INFO("started handle") \
      started = true; \
   } \
  } \
  else { \
    INFO("handle already started") \
  }

#define PREF_DEVICE_ID "device_id"

using namespace musik::core::sdk;

static char* deviceId = nullptr;

extern "C" void SetPreferences(musik::core::sdk::IPreferences* prefs) {
    size_t len = prefs->GetString(PREF_DEVICE_ID, nullptr, 0, "");
    if (len > 1) {
        delete[] deviceId;
        deviceId = new char[len];
        prefs->GetString(PREF_DEVICE_ID, deviceId, len, "");
        prefs->Save();
        INFO("setting deviceId to " + std::string(deviceId));
    }
}

static bool waitForDevice(sio_hdl* hdl) {
    INFO("waiting for device")
    int nfds, revents;
    struct pollfd pfds[1];
    do {
        nfds = sio_pollfd(hdl, pfds, POLLOUT);
        if (nfds > 0) {
            if (poll(pfds, nfds, -1) < 0) {
                INFO("poll failed");
                return false;
            }
        }
        revents = sio_revents(hdl, pfds);
        if (revents & POLLHUP) {
            INFO("device disappeared");
            return false;
        }
    } while (!(revents & POLLOUT));
    INFO("done waiting for device")
    return true;
}

SndioOut::SndioOut() {
    INFO("---------- sndout.ctor ----------")
    this->volume = 1.0f;
    this->state = StateStopped;
    this->latency = 0.0;
    this->writeThread.reset(new std::thread(
        std::bind(&SndioOut::WriteLoop, this)));
}

SndioOut::~SndioOut() {
    this->PushCommand(Command::Quit);
    INFO("joining thread")
    this->writeThread->join();
    INFO("thread finished")
}

void SndioOut::Release() {
    delete this;
}

void SndioOut::PushCommand(Command command) {
    //INFO("PushCommand.start")
    {
        LOCK()
        commands.push_back(command);
    }
    NOTIFY()
    //INFO("PushCommand.end")
}

void SndioOut::Pause() {
    this->PushCommand(Command::Pause);
}

void SndioOut::Resume() {
    this->PushCommand(Command::Resume);
}

void SndioOut::SetVolume(double volume) {
    this->volume = volume;
    this->PushCommand(Command::SetVolume);
}

double SndioOut::GetVolume() {
    return this->volume;
}

void SndioOut::Stop() {
    this->PushCommand(Command::Stop);
}

void SndioOut::Drain() {
    this->PushCommand(Command::Drain);
}

IDeviceList* SndioOut::GetDeviceList() {
    return nullptr;
}

bool SndioOut::SetDefaultDevice(const char* deviceId) {
    return false;
}

IDevice* SndioOut::GetDefaultDevice() {
    return nullptr;
}

int SndioOut::Play(IBuffer *buffer, IBufferProvider *provider) {
    std::this_thread::yield();

    if (this->state != StatePlaying) {
        return OutputInvalidState;
    }

    {
        LOCK()
        if (this->CountBuffersWithProvider(provider) >= BUFFER_COUNT) {
            return OutputBufferFull;
        }
        this->buffers.push_back(BufferContext{provider, buffer});
    }

    NOTIFY()
    return OutputBufferWritten;
}

void SndioOut::WriteLoop() {
    bool started = false;
    std::list<BufferContext> toNotify;
    sio_hdl* handle = nullptr;
    sio_par pars = { 0 };
    short* pcm = nullptr;
    long pcmSamples = 0;
    float ditherState = 0.0;
    bool quit = false;
    BufferContext next {nullptr, nullptr};

    /* open the output device. we only do this once */
    const char* device = (::deviceId && strlen(::deviceId)) ? deviceId : nullptr;
    handle = sio_open(device, SIO_PLAY, 0);
    if (handle == nullptr) {
        INFO("failed to init device")
        quit = true;
    }
    else {
        if (!sio_start(handle)) {
            INFO("device opened, but couldn't start")
            quit = true;
        }
        else {
            started = true;
            INFO("device handle initialized")
        }
    }

    while (!quit) {
        /* drain any old buffers (we do this outside of the critical section */
        if (toNotify.size()) {
            INFO("cleaning up dead buffers")
            for (auto& it : toNotify) {
                it.provider->OnBufferProcessed(it.buffer);
            }
            toNotify.clear();
        }

        {
            /* we wait until we have commands to process or samples to play */
            LOCK()
            while (!quit && !this->commands.size() &&
                   (this->state != StatePlaying || !this->buffers.size()))
            {
                //INFO("waiting")
                WAIT()
                //INFO("done waiting")
            }

            if (quit) {
                continue;
            }

            /* process commands */
            for (auto command : this->commands) {
                switch (command) {
                    case Command::Pause: {
                        INFO("command.pause")
                        STOP()
                        this->state = StatePaused;
                    } break;
                    case Command::Resume: {
                        INFO("command.resume")
                        START()
                        this->state = StatePlaying;
                    } break;
                    case Command::Stop: {
                        INFO("command.stop")
                        STOP()
                        std::swap(toNotify, this->buffers);
                        this->state = StateStopped;
                    } break;
                    case Command::SetVolume: {
                        INFO("command.setvolume")
                        if (handle) {
                            sio_setvol(handle, lround(this->volume * SIO_MAXVOL));
                        }
                    } break;
                    case Command::Drain: {
                        INFO("command.drain")
                        STOP()
                        START()
                    } break;
                    case Command::Quit: {
                        quit = true;
                        continue;
                    } break;
                }
            }

            this->commands.clear();

            /* play the next buffer, if it exists */
            if (started &&
                this->state == StatePlaying &&
                this->buffers.size())
            {
                next = this->buffers.front();
                this->buffers.pop_front();
            }
            else {
                //INFO(std::string("state=") + std::to_string((int) this->state));
                //INFO(std::string("count=") + std::to_string((int) this->buffers.size()));
                continue;
            }
        }

        auto buffer = next.buffer;

        if (buffer) {
            /* ensure our output params are setup properly based on this
            buffer's params */
            if (pars.pchan != buffer->Channels() ||
                pars.rate != buffer->SampleRate())
            {
                INFO("updating output params")

                STOP()

                sio_initpar(&pars);
                pars.pchan = buffer->Channels();
                pars.rate = buffer->SampleRate();
                pars.sig = 1;
                pars.le = SIO_LE_NATIVE;
                pars.bits = 16;
                pars.appbufsz = (pars.rate * 250) / 1000;

                if (!sio_setpar(handle, &pars)) {
                    INFO("sio_setpar() failed");
                    quit = true;
                    continue;
                }

                if (!sio_getpar(handle, &pars)) {
                    INFO("sio_getpar() failed");
                    quit = true;
                    continue;
                }

                START()

                this->latency = (double) pars.bufsz / pars.pchan / pars.rate;
            }

            /* allocate PCM buffer, if needed. most of the time we'll be
            able to re-use the previously allocated one. */
            long samples = buffer->Samples();
            if (!pcm || samples > pcmSamples) {
                delete[] pcm;
                pcm = new short[samples];
                pcmSamples = samples;
            }

            /* convert to 16-bit PCM */
            float* src = buffer->BufferPointer();
            short* dst = pcm;
            for (long i = 0; i < samples; i++) {
                float sample = *src;
                if (sample > 1.0f) sample = 1.0f;
                if (sample < -1.0f) sample = -1.0f;
                sample *= SHRT_MAX;

                /* triangle (high pass) dither, based on Audacity's
                implementation */
                float r = (rand() / (float) RAND_MAX - 0.5f);
                sample = sample + r - ditherState;
                ditherState = r;

                *dst = sample;
                ++dst; ++src;
            }

            /* write the entire output buffer. this may require multiple passes;
            that's ok, just loop until we're done */
            char* data = (char*) pcm;
            size_t dataLength = samples * sizeof(short);
            size_t totalWritten = 0;

            {
//              LOCK()
//              INFO("start write")
                while (totalWritten < dataLength) {
                    size_t remaining = dataLength - totalWritten;
                    size_t written = sio_write(handle, data, remaining);
                    if (written == 0) { INFO("failed to write!") break; }
//                  else { std::cerr << "wrote " << written << " of " << remaining << " bytes\n"; }
                    totalWritten += written;
                    data += written;
                }
//              INFO("end write")
            }

            next.provider->OnBufferProcessed(buffer);
        }
    }

    /* done, free remaining buffers and close the handle */
    {
        LOCK()
        std::swap(toNotify, this->buffers);
        this->buffers.clear();
    }

    if (handle) {
        INFO("closing")
        sio_close(handle);
        handle = nullptr;
    }

    for (auto& it : toNotify) {
    INFO("cleaning up dead buffer")
        it.provider->OnBufferProcessed(it.buffer);
    }

    delete[] pcm;
}

double SndioOut::Latency() {
    return this->latency;
}

size_t SndioOut::CountBuffersWithProvider(IBufferProvider* provider) {
    size_t count = 0;
    for (auto& it : this->buffers) {
        if (it.provider == provider) {
            ++count;
        }
    }
    return count;
}

<|endoftext|>