Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpForceTorqueAtiNetFTSensor.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <stdint.h>
37 
38 #include <visp3/core/vpConfig.h>
39 #include <visp3/core/vpException.h>
40 #include <visp3/core/vpTime.h>
41 #include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
42 
43 // Make vpForceTorqueAtiNetFTSensor available only if inet_ntop() used to
44 // communicate by UDP with the sensor through vpUDPClient is available; inet_ntop()
45 // is not supported on win XP
46 #ifdef VISP_HAVE_FUNC_INET_NTOP
47 
48 #ifndef DOXYGEN_SHOULD_SKIP_THIS
49 typedef struct response_struct {
50  uint32_t rdt_sequence;
51  uint32_t ft_sequence;
52  uint32_t status;
53  int32_t FTData[6];
54 } RESPONSE;
55 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
56 
64  : vpUDPClient(), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1), m_ft_bias(6, 0),
65  m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
66 {
67 }
68 
74 vpForceTorqueAtiNetFTSensor::vpForceTorqueAtiNetFTSensor(const std::string &hostname, int port)
75  : vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
76  m_ft_bias(6, 0), m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
77 {
78 }
79 
85 {
86  if (!m_is_init) {
87  throw(vpException(vpException::notInitialized, "Cannot start streaming: UDP client is not initialized"));
88  }
89 
91  throw(vpException(vpException::notInitialized, "Streaming is already started"));
92  }
93 
94  // Since UDP packet could be lost, retry startup 10 times before giving up
95  for (unsigned int i = 0; i < 10; ++i) {
96  int len = 8;
97  unsigned char request[8]; // The request data sent to the Net F/T
98  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
99  *(uint16_t *)&request[2] = htons(0x0002); // Start high-speed streaming (see table 10.1 in Net F/T user manual)
100  *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
101 
102  // Send start stream
103  if (send(request, len) != len) {
104  throw(vpException(vpException::notInitialized, "UDP client is not initialized"));
105  }
106  std::cout << "wait: " << i << std::endl;
107 
108  m_is_streaming_started = true;
109  if (waitForNewData()) {
110  return true;
111  }
112  }
113  m_is_streaming_started = false;
114  return false;
115 }
116 
121 {
122  if (!m_is_init) {
123  throw(vpException(vpException::notInitialized, "Cannot stop streaming: UDP client is not initialized"));
124  }
125 
126  if (!m_is_streaming_started) {
127  throw(vpException(vpException::notInitialized, "Cannot stop streaming: streaming was not started"));
128  }
129 
130  int len = 8;
131  unsigned char request[8]; // The request data sent to the Net F/T
132  *(uint16_t *)&request[0] = htons(0x1234); // Standard header
133  *(uint16_t *)&request[2] = htons(0x0000); // Stop streaming (see table 10.1 in Net F/T user manual)
134  *(uint32_t *)&request[4] = htonl(0); // Infinite sample (see section 10.1 in Net F/T user manual
135 
136  // Send start stream
137  if (send(request, len) != len) {
138  throw(vpException(vpException::notInitialized, "Cannot stop streaming"));
139  }
140 
141  m_is_streaming_started = false;
142 }
143 
150 {
152  stopStreaming();
153  }
154 }
155 
163 void vpForceTorqueAtiNetFTSensor::bias(unsigned int n_counts)
164 {
165  if (!m_is_init) {
166  throw(vpException(vpException::notInitialized, "Cannot bias: UDP client is not initialized"));
167  }
168 
169  if (!m_is_streaming_started) {
170  throw(vpException(vpException::notInitialized, "Cannot bias: streaming was not started"));
171  }
172 
173  vpColVector ft_bias_tmp(6, 0);
174  m_ft_bias = 0;
175 
176  if (n_counts == 0) {
178  } else {
179  for (unsigned int i = 0; i < n_counts; i++) {
180  ft_bias_tmp += getForceTorque();
181  waitForNewData();
182  }
183  m_ft_bias = ft_bias_tmp / n_counts;
184  }
185 }
186 
193 
205 {
206  if (!m_is_init) {
207  throw(vpException(vpException::notInitialized, "Cannot get F/T: UDP client is not initialized"));
208  }
209 
210  if (!m_is_streaming_started) {
211  throw(vpException(vpException::notInitialized, "Cannot get F/T: streaming was not started"));
212  }
213 
215  throw(vpException(vpException::notInitialized, "Cannot get F/T: no new data available"));
216  }
217 
218  return m_ft;
219 }
220 
227 {
228  if (!m_is_init) {
229  throw(vpException(vpException::notInitialized, "Cannot wait for new data: UDP client is not initialized"));
230  }
231 
232  if (!m_is_streaming_started) {
233  throw(vpException(vpException::notInitialized, "Cannot wait for new data: streaming was not started"));
234  }
235 
236  double t = vpTime::measureTimeMs();
238  while (vpTime::measureTimeMs() - t < static_cast<double>(timeout)) {
239  unsigned char response[36];
240  RESPONSE resp;
241  if (receive((void *)response, 36)) {
242  resp.rdt_sequence = ntohl(*(uint32_t *)&response[0]);
243  resp.ft_sequence = ntohl(*(uint32_t *)&response[4]);
244  resp.status = ntohl(*(uint32_t *)&response[8]);
245  for (int i = 0; i < 6; i++) {
246  resp.FTData[i] = ntohl(*(int32_t *)&response[12 + i * 4]);
247  }
248  // Output the response data.
249  if (resp.status) {
250  throw(vpException(vpException::notInitialized, "Cannot wait for new data: status 0x%08x is not 0x00000000",
251  resp.status));
252  }
253  double force_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_force);
254  double torque_factor = static_cast<double>(m_scaling_factor) / static_cast<double>(m_counts_per_torque);
255  for (int i = 0; i < 3; i++) {
256  m_ft[i] = resp.FTData[i] * force_factor;
257  }
258  for (int i = 3; i < 6; i++) {
259  m_ft[i] = resp.FTData[i] * torque_factor;
260  }
261  // Consider bias
262  m_ft -= m_ft_bias;
263 
264  m_data_count++;
265  }
266 
268  return true;
269  }
270  vpTime::sleepMs(1);
271  }
272 
273  return false;
274 }
275 
276 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:86
void bias(unsigned int n_counts=50)
bool waitForNewData(unsigned int timeout=50)
virtual ~vpForceTorqueAtiNetFTSensor() vp_override
This class implements a basic (IPv4) User Datagram Protocol (UDP) client.
Definition: vpUDPClient.h:165
int receive(std::string &msg, int timeoutMs=0)
bool m_is_init
Definition: vpUDPClient.h:182
int send(const std::string &msg)
VISP_EXPORT void sleepMs(double t)
VISP_EXPORT double measureTimeMs()