Visual Servoing Platform  version 3.2.1 under development (2019-08-19)
vpForceTorqueAtiNetFTSensor.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * ATI Force torque interface.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <stdint.h>
40 
41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpTime.h>
44 #include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
45 
46 typedef struct response_struct {
47  uint32_t rdt_sequence;
48  uint32_t ft_sequence;
49  uint32_t status;
50  int32_t FTData[6];
51 } RESPONSE;
52 
60  : vpUDPClient(), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1), m_ft_bias(6, 0),
61  m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
62 {
63 }
64 
70 vpForceTorqueAtiNetFTSensor::vpForceTorqueAtiNetFTSensor(const std::string &hostname, const int port)
71  : vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
73 {
74 }
75 
81 {
82  if (!m_is_init) {
83  throw(vpException(vpException::notInitialized, "Cannot start streaming: UDP client is not initialized"));
84  }
85 
87  throw(vpException(vpException::notInitialized, "Streaming is already started"));
88  }
89 
90  // Since UDP packet could be lost, retry startup 10 times before giving up
91  for (unsigned int i = 0; i < 10; ++i) {
92  int len = 8;
93  unsigned char request[8]; // The request data sent to the Net F/T
94  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
95  *(uint16_t *)&request[2] = htons(0x0002); // Start high-speed streaming (see table 10.1 in Net F/T user manual)
96  *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
97 
98  // Send start stream
99  if (send(request, len) != len) {
100  throw(vpException(vpException::notInitialized, "UDP client is not initialized"));
101  }
102  std::cout << "wait: " << i << std::endl;
103 
104  m_is_streaming_started = true;
105  if (waitForNewData()) {
106  return true;
107  }
108  }
109  m_is_streaming_started = false;
110  return false;
111 }
112 
117 {
118  if (!m_is_init) {
119  throw(vpException(vpException::notInitialized, "Cannot stop streaming: UDP client is not initialized"));
120  }
121 
122  if (!m_is_streaming_started) {
123  throw(vpException(vpException::notInitialized, "Cannot stop streaming: streaming was not started"));
124  }
125 
126  int len = 8;
127  unsigned char request[8]; // The request data sent to the Net F/T
128  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
129  *(uint16_t *)&request[2] = htons(0x0000); // Stop streaming (see table 10.1 in Net F/T user manual)
130  *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
131 
132  // Send start stream
133  if (send(request, len) != len) {
134  throw(vpException(vpException::notInitialized, "Cannot stop streaming"));
135  }
136 
137  m_is_streaming_started = false;
138 }
139 
146 {
148  stopStreaming();
149  }
150 }
151 
159 void vpForceTorqueAtiNetFTSensor::bias(unsigned int n_counts)
160 {
161  if (!m_is_init) {
162  throw(vpException(vpException::notInitialized, "Cannot bias: UDP client is not initialized"));
163  }
164 
165  if (!m_is_streaming_started) {
166  throw(vpException(vpException::notInitialized, "Cannot bias: streaming was not started"));
167  }
168 
169  vpColVector ft_bias_tmp(6, 0);
170  m_ft_bias = 0;
171 
172  if (n_counts == 0) {
174  } else {
175  for (unsigned int i = 0; i < n_counts; i++) {
176  ft_bias_tmp += getForceTorque();
177  waitForNewData();
178  }
179  m_ft_bias = ft_bias_tmp / n_counts;
180  }
181 }
182 
189 
201 {
202  if (!m_is_init) {
203  throw(vpException(vpException::notInitialized, "Cannot get F/T: UDP client is not initialized"));
204  }
205 
206  if (!m_is_streaming_started) {
207  throw(vpException(vpException::notInitialized, "Cannot get F/T: streaming was not started"));
208  }
209 
211  throw(vpException(vpException::notInitialized, "Cannot get F/T: no new data available"));
212  }
213 
214  return m_ft;
215 }
216 
223 {
224  if (!m_is_init) {
225  throw(vpException(vpException::notInitialized, "Cannot wait for new data: UDP client is not initialized"));
226  }
227 
228  if (!m_is_streaming_started) {
229  throw(vpException(vpException::notInitialized, "Cannot wait for new data: streaming was not started"));
230  }
231 
232  double t = vpTime::measureTimeMs();
234  while (vpTime::measureTimeMs() - t < static_cast<double>(timeout)) {
235  unsigned char response[36];
236  RESPONSE resp;
237  if (receive((void *)response, 36)) {
238  resp.rdt_sequence = ntohl(*(uint32_t *)&response[0]);
239  resp.ft_sequence = ntohl(*(uint32_t *)&response[4]);
240  resp.status = ntohl(*(uint32_t *)&response[8]);
241  for (int i = 0; i < 6; i++) {
242  resp.FTData[i] = ntohl(*(int32_t *)&response[12 + i * 4]);
243  }
244  // Output the response data.
245  if (resp.status) {
246  throw(vpException(vpException::notInitialized, "Cannot wait for new data: status 0x%08x is not 0x00000000",
247  resp.status));
248  }
249  double force_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_force);
250  double torque_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_torque);
251  for (int i = 0; i < 3; i++) {
252  m_ft[i] = resp.FTData[i] * force_factor;
253  }
254  for (int i = 3; i < 6; i++) {
255  m_ft[i] = resp.FTData[i] * torque_factor;
256  }
257  // Consider bias
258  m_ft -= m_ft_bias;
259 
260  m_data_count++;
261  }
262 
264  return true;
265  }
266  vpTime::sleepMs(1);
267  }
268 
269  return false;
270 }
int receive(std::string &msg, const int timeoutMs=0)
error that can be emited by ViSP classes.
Definition: vpException.h:71
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
bool m_is_init
Definition: vpUDPClient.h:186
VISP_EXPORT void sleepMs(double t)
Definition: vpTime.cpp:271
This class implements a basic (IPv4) User Datagram Protocol (UDP) client.
Definition: vpUDPClient.h:168
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
void bias(unsigned int n_counts=50)
bool waitForNewData(unsigned int timeout=50)
int send(const std::string &msg)