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