vpForceTorqueAtiNetFTSensor.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:
 * ATI Force torque interface.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <stdint.h>

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpTime.h>
#include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>

// Make vpForceTorqueAtiNetFTSensor available only if inet_ntop() used to
// communicate by UDP with the sensor through vpUDPClient is available; inet_ntop()
// is not supported on win XP
#ifdef VISP_HAVE_FUNC_INET_NTOP

typedef struct response_struct {
  uint32_t rdt_sequence;
  uint32_t ft_sequence;
  uint32_t status;
  int32_t FTData[6];
} RESPONSE;

vpForceTorqueAtiNetFTSensor::vpForceTorqueAtiNetFTSensor()
  : vpUDPClient(), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1), m_ft_bias(6, 0),
    m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
{
}

vpForceTorqueAtiNetFTSensor::vpForceTorqueAtiNetFTSensor(const std::string &hostname, int port)
  : vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
    m_ft_bias(6, 0), m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
{
}

bool vpForceTorqueAtiNetFTSensor::startStreaming()
{
  if (!m_is_init) {
    throw(vpException(vpException::notInitialized, "Cannot start streaming: UDP client is not initialized"));
  }

  if (m_is_streaming_started) {
    throw(vpException(vpException::notInitialized, "Streaming is already started"));
  }

  // Since UDP packet could be lost, retry startup 10 times before giving up
  for (unsigned int i = 0; i < 10; ++i) {
    int len = 8;
    unsigned char request[8];                 // The request data sent to the Net F/T
    *(uint16_t *)&request[0] = htons(0x1234); // Standard header
    *(uint16_t *)&request[2] = htons(0x0002); // Start high-speed streaming (see table 10.1 in Net F/T user manual)
    *(uint32_t *)&request[4] = htonl(0);      // Infinite sample (see section 10.1 in Net F/T user manual

    // Send start stream
    if (send(request, len) != len) {
      throw(vpException(vpException::notInitialized, "UDP client is not initialized"));
    }
    std::cout << "wait: " << i << std::endl;

    m_is_streaming_started = true;
    if (waitForNewData()) {
      return true;
    }
  }
  m_is_streaming_started = false;
  return false;
}

void vpForceTorqueAtiNetFTSensor::stopStreaming()
{
  if (!m_is_init) {
    throw(vpException(vpException::notInitialized, "Cannot stop streaming: UDP client is not initialized"));
  }

  if (!m_is_streaming_started) {
    throw(vpException(vpException::notInitialized, "Cannot stop streaming: streaming was not started"));
  }

  int len = 8;
  unsigned char request[8];                 // The request data sent to the Net F/T
  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
  *(uint16_t *)&request[2] = htons(0x0000); // Stop streaming (see table 10.1 in Net F/T user manual)
  *(uint32_t *)&request[4] = htonl(0);      // Infinite sample (see section 10.1 in Net F/T user manual

  // Send start stream
  if (send(request, len) != len) {
    throw(vpException(vpException::notInitialized, "Cannot stop streaming"));
  }

  m_is_streaming_started = false;
}

vpForceTorqueAtiNetFTSensor::~vpForceTorqueAtiNetFTSensor()
{
  if (m_is_streaming_started) {
    stopStreaming();
  }
}

void vpForceTorqueAtiNetFTSensor::bias(unsigned int n_counts)
{
  if (!m_is_init) {
    throw(vpException(vpException::notInitialized, "Cannot bias: UDP client is not initialized"));
  }

  if (!m_is_streaming_started) {
    throw(vpException(vpException::notInitialized, "Cannot bias: streaming was not started"));
  }

  vpColVector ft_bias_tmp(6, 0);
  m_ft_bias = 0;

  if (n_counts == 0) {
    m_ft_bias = getForceTorque();
  } else {
    for (unsigned int i = 0; i < n_counts; i++) {
      ft_bias_tmp += getForceTorque();
      waitForNewData();
    }
    m_ft_bias = ft_bias_tmp / n_counts;
  }
}

void vpForceTorqueAtiNetFTSensor::unbias() { m_ft_bias = 0; }

vpColVector vpForceTorqueAtiNetFTSensor::getForceTorque() const
{
  if (!m_is_init) {
    throw(vpException(vpException::notInitialized, "Cannot get F/T: UDP client is not initialized"));
  }

  if (!m_is_streaming_started) {
    throw(vpException(vpException::notInitialized, "Cannot get F/T: streaming was not started"));
  }

  if (m_data_count_prev == m_data_count) {
    throw(vpException(vpException::notInitialized, "Cannot get F/T: no new data available"));
  }

  return m_ft;
}

bool vpForceTorqueAtiNetFTSensor::waitForNewData(unsigned int timeout)
{
  if (!m_is_init) {
    throw(vpException(vpException::notInitialized, "Cannot wait for new data: UDP client is not initialized"));
  }

  if (!m_is_streaming_started) {
    throw(vpException(vpException::notInitialized, "Cannot wait for new data: streaming was not started"));
  }

  double t = vpTime::measureTimeMs();
  m_data_count_prev = m_data_count;
  while (vpTime::measureTimeMs() - t < static_cast<double>(timeout)) {
    unsigned char response[36];
    RESPONSE resp;
    if (receive((void *)response, 36)) {
      resp.rdt_sequence = ntohl(*(uint32_t *)&response[0]);
      resp.ft_sequence = ntohl(*(uint32_t *)&response[4]);
      resp.status = ntohl(*(uint32_t *)&response[8]);
      for (int i = 0; i < 6; i++) {
        resp.FTData[i] = ntohl(*(int32_t *)&response[12 + i * 4]);
      }
      // Output the response data.
      if (resp.status) {
        throw(vpException(vpException::notInitialized, "Cannot wait for new data: status 0x%08x is not 0x00000000",
                          resp.status));
      }
      double force_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_force);
      double torque_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_torque);
      for (int i = 0; i < 3; i++) {
        m_ft[i] = resp.FTData[i] * force_factor;
      }
      for (int i = 3; i < 6; i++) {
        m_ft[i] = resp.FTData[i] * torque_factor;
      }
      // Consider bias
      m_ft -= m_ft_bias;

      m_data_count++;
    }

    if (m_data_count != m_data_count_prev) {
      return true;
    }
    vpTime::sleepMs(1);
  }

  return false;
}

#endif