Visual Servoing Platform  version 3.2.1 under development (2019-12-07)
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 // Make vpForceTorqueAtiNetFTSensor available only if inet_ntop() used to
47 // communicate by UDP with the sensor through vpUDPClient is available; inet_ntop()
48 // is not supported on win XP
49 #ifdef VISP_HAVE_FUNC_INET_NTOP
50 
51 typedef struct response_struct {
52  uint32_t rdt_sequence;
53  uint32_t ft_sequence;
54  uint32_t status;
55  int32_t FTData[6];
56 } RESPONSE;
57 
65  : vpUDPClient(), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1), m_ft_bias(6, 0),
66  m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
67 {
68 }
69 
75 vpForceTorqueAtiNetFTSensor::vpForceTorqueAtiNetFTSensor(const std::string &hostname, const int port)
76  : vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
78 {
79 }
80 
86 {
87  if (!m_is_init) {
88  throw(vpException(vpException::notInitialized, "Cannot start streaming: UDP client is not initialized"));
89  }
90 
92  throw(vpException(vpException::notInitialized, "Streaming is already started"));
93  }
94 
95  // Since UDP packet could be lost, retry startup 10 times before giving up
96  for (unsigned int i = 0; i < 10; ++i) {
97  int len = 8;
98  unsigned char request[8]; // The request data sent to the Net F/T
99  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
100  *(uint16_t *)&request[2] = htons(0x0002); // Start high-speed streaming (see table 10.1 in Net F/T user manual)
101  *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
102 
103  // Send start stream
104  if (send(request, len) != len) {
105  throw(vpException(vpException::notInitialized, "UDP client is not initialized"));
106  }
107  std::cout << "wait: " << i << std::endl;
108 
109  m_is_streaming_started = true;
110  if (waitForNewData()) {
111  return true;
112  }
113  }
114  m_is_streaming_started = false;
115  return false;
116 }
117 
122 {
123  if (!m_is_init) {
124  throw(vpException(vpException::notInitialized, "Cannot stop streaming: UDP client is not initialized"));
125  }
126 
127  if (!m_is_streaming_started) {
128  throw(vpException(vpException::notInitialized, "Cannot stop streaming: streaming was not started"));
129  }
130 
131  int len = 8;
132  unsigned char request[8]; // The request data sent to the Net F/T
133  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
134  *(uint16_t *)&request[2] = htons(0x0000); // Stop streaming (see table 10.1 in Net F/T user manual)
135  *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
136 
137  // Send start stream
138  if (send(request, len) != len) {
139  throw(vpException(vpException::notInitialized, "Cannot stop streaming"));
140  }
141 
142  m_is_streaming_started = false;
143 }
144 
151 {
153  stopStreaming();
154  }
155 }
156 
164 void vpForceTorqueAtiNetFTSensor::bias(unsigned int n_counts)
165 {
166  if (!m_is_init) {
167  throw(vpException(vpException::notInitialized, "Cannot bias: UDP client is not initialized"));
168  }
169 
170  if (!m_is_streaming_started) {
171  throw(vpException(vpException::notInitialized, "Cannot bias: streaming was not started"));
172  }
173 
174  vpColVector ft_bias_tmp(6, 0);
175  m_ft_bias = 0;
176 
177  if (n_counts == 0) {
179  } else {
180  for (unsigned int i = 0; i < n_counts; i++) {
181  ft_bias_tmp += getForceTorque();
182  waitForNewData();
183  }
184  m_ft_bias = ft_bias_tmp / n_counts;
185  }
186 }
187 
194 
206 {
207  if (!m_is_init) {
208  throw(vpException(vpException::notInitialized, "Cannot get F/T: UDP client is not initialized"));
209  }
210 
211  if (!m_is_streaming_started) {
212  throw(vpException(vpException::notInitialized, "Cannot get F/T: streaming was not started"));
213  }
214 
216  throw(vpException(vpException::notInitialized, "Cannot get F/T: no new data available"));
217  }
218 
219  return m_ft;
220 }
221 
228 {
229  if (!m_is_init) {
230  throw(vpException(vpException::notInitialized, "Cannot wait for new data: UDP client is not initialized"));
231  }
232 
233  if (!m_is_streaming_started) {
234  throw(vpException(vpException::notInitialized, "Cannot wait for new data: streaming was not started"));
235  }
236 
237  double t = vpTime::measureTimeMs();
239  while (vpTime::measureTimeMs() - t < static_cast<double>(timeout)) {
240  unsigned char response[36];
241  RESPONSE resp;
242  if (receive((void *)response, 36)) {
243  resp.rdt_sequence = ntohl(*(uint32_t *)&response[0]);
244  resp.ft_sequence = ntohl(*(uint32_t *)&response[4]);
245  resp.status = ntohl(*(uint32_t *)&response[8]);
246  for (int i = 0; i < 6; i++) {
247  resp.FTData[i] = ntohl(*(int32_t *)&response[12 + i * 4]);
248  }
249  // Output the response data.
250  if (resp.status) {
251  throw(vpException(vpException::notInitialized, "Cannot wait for new data: status 0x%08x is not 0x00000000",
252  resp.status));
253  }
254  double force_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_force);
255  double torque_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_torque);
256  for (int i = 0; i < 3; i++) {
257  m_ft[i] = resp.FTData[i] * force_factor;
258  }
259  for (int i = 3; i < 6; i++) {
260  m_ft[i] = resp.FTData[i] * torque_factor;
261  }
262  // Consider bias
263  m_ft -= m_ft_bias;
264 
265  m_data_count++;
266  }
267 
269  return true;
270  }
271  vpTime::sleepMs(1);
272  }
273 
274  return false;
275 }
276 
277 #endif
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)