vpForceTorqueIitSensor.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Wrapper over IIT force-torque sensor.
 *
 * Authors:
 * Fabien Spindler
 * Alexander Oliva
 *
 *****************************************************************************/

#include <visp3/sensor/vpForceTorqueIitSensor.h>

#ifdef VISP_HAVE_FT_IIT_SDK

vpForceTorqueIitSensor::vpForceTorqueIitSensor()
  : m_ftLib(), m_numSensorsInLib(0), m_ft(6, 0), m_ft_filt(6, 0), m_ftSensorsData(), m_acquisitionEnabled(false), m_dataValid(false),
    m_connected(false), m_acquisitionThread(), m_timeCur(), m_timePrev(), m_mutex(), m_warmupMilliseconds(500)
{
  // Get number of connected in library sensors
  m_numSensorsInLib = m_ftLib._getNumberOfConnectedSensors();

  /*
   * Initialize Communication with sensor(-s):
   *
   *      streaming is configured with "storeOption=0",
   *      which means that data will be stored in the library's main thread
   *      and will be given in an output file.
   *
   *      "storeDataFlag" is initialized as "false", so that the
   *      recording will not start with the streaming thread, but
   *      the moment for when the recording will start & stop.
   */
  if (m_ftLib._configureStreaming(false, 0) == 0) {
    // Start the main acquisition thread
    m_ftLib._startStreamingThread();

    m_connected = true;
  }
}

void vpForceTorqueIitSensor::close()
{
  m_ftLib._stopStreamingThread();
  m_acquisitionEnabled = false;
  m_dataValid = false;
}

vpForceTorqueIitSensor::~vpForceTorqueIitSensor() { this->close(); }

void vpForceTorqueIitSensor::join()
{
  if (m_acquisitionThread.joinable()) {
    m_acquisitionThread.join();
  }
}

void vpForceTorqueIitSensor::acquisitionLoop()
{
  m_timePrev = m_timeCur = std::chrono::system_clock::now();

  // Main thread
  auto time_init = std::chrono::system_clock::now();
  while (m_acquisitionEnabled) {

    // Get time passed since the last acquired sample
    m_timeCur = std::chrono::system_clock::now();

    // Calculate delta time
    auto elapsed_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(m_timeCur - m_timePrev).count();

    if (elapsed_milliseconds >= 1) {
      /*
       * Once a new aquisition started,
       * reset loop timers to keep a relatively fixed sampling time
       */
      // Update previous time
      m_timePrev = m_timeCur;

      /*
       * get all connected sensors' data:
       * call to a mutex method allowing to access the ftsensors' data
       * in streaming mode (1 sample / packet / sensor)
       */
      m_ftSensorsData = m_ftLib._getFTSensorsData();

      // Warm up the sensor. At the beginning we experienced that values returned in m_ftSensorsData.ftSensor->ft
      // are completly wrong like the following:
      // 2.237378396e+11  207.3293304  14291.07715 1.479413346e+19  13.26593399  3380.078613
      auto warmup_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(m_timeCur - time_init).count();
      if (warmup_milliseconds > m_warmupMilliseconds) {
        m_dataValid = true;
      } else {
        continue;
      }

      const std::lock_guard<std::mutex> lock(m_mutex);
      for (unsigned int i = 0; i < 6; i++) {
        m_ft[i] = m_ftSensorsData.ftSensor->ft[i];
        m_ft_filt[i] = m_ftSensorsData.ftSensor->filt_ft[i];
      }
    }
  }
}

void vpForceTorqueIitSensor::bias() { m_ftLib._biasAllFTSensorsTCP(); }

bool vpForceTorqueIitSensor::connected(int timeout_ms) const
{
  vpChrono chrono;
  chrono.start();
  while (!m_connected && chrono.getDurationMs() < timeout_ms) {
    vpTime::sleepMs(1);
  }

  return m_connected;
}

vpColVector vpForceTorqueIitSensor::getForceTorque(bool filtered)
{
  const std::lock_guard<std::mutex> lock(m_mutex);
  if (filtered) {
    return m_ft_filt;
  }
  else {
    return m_ft;
  }
}


void vpForceTorqueIitSensor::startStreaming()
{
  m_acquisitionEnabled = true;
  m_acquisitionThread = std::thread([this] { this->acquisitionLoop(); });

  while (!m_dataValid) {
    vpTime::wait(10);
  }
}

void vpForceTorqueIitSensor::stopStreaming()
{
  m_acquisitionEnabled = false;
  if (m_acquisitionThread.joinable()) {
    m_acquisitionThread.join();
  }
}

#else
// Work arround to avoid warning:
// libvisp_sensor.a(vpForceTorqueIitSensor.cpp.o) has no symbols
void dummy_vpForceTorqueIitSensor(){};
#endif