41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpTime.h>
44 #include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
49 #ifdef VISP_HAVE_FUNC_INET_NTOP
51 #ifndef DOXYGEN_SHOULD_SKIP_THIS
52 typedef struct response_struct {
53 uint32_t rdt_sequence;
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)
78 :
vpUDPClient(hostname, port), m_counts_per_force(1000000), m_counts_per_torque(1000000000), m_scaling_factor(1),
79 m_ft_bias(6, 0), m_data_count(0), m_data_count_prev(0), m_ft(6, 0), m_is_streaming_started(false)
98 for (
unsigned int i = 0; i < 10; ++i) {
100 unsigned char request[8];
101 *(uint16_t *)&request[0] = htons(0x1234);
102 *(uint16_t *)&request[2] = htons(0x0002);
103 *(uint32_t *)&request[4] = htonl(0);
106 if (
send(request, len) != len) {
109 std::cout <<
"wait: " << i << std::endl;
134 unsigned char request[8];
135 *(uint16_t *)&request[0] = htons(0x1234);
136 *(uint16_t *)&request[2] = htons(0x0000);
137 *(uint32_t *)&request[4] = htonl(0);
140 if (
send(request, len) != len) {
182 for (
unsigned int i = 0; i < n_counts; i++) {
242 unsigned char response[36];
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]);
258 for (
int i = 0; i < 3; i++) {
259 m_ft[i] = resp.FTData[i] * force_factor;
261 for (
int i = 3; i < 6; i++) {
262 m_ft[i] = resp.FTData[i] * torque_factor;
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
@ notInitialized
Used to indicate that a parameter is not initialized.
vpForceTorqueAtiNetFTSensor()
bool m_is_streaming_started
vpColVector getForceTorque() const
unsigned long m_counts_per_force
unsigned long m_scaling_factor
void bias(unsigned int n_counts=50)
unsigned long m_data_count_prev
unsigned long m_data_count
unsigned long m_counts_per_torque
virtual ~vpForceTorqueAtiNetFTSensor()
bool waitForNewData(unsigned int timeout=50)
This class implements a basic (IPv4) User Datagram Protocol (UDP) client.
int receive(std::string &msg, int timeoutMs=0)
int send(const std::string &msg)
VISP_EXPORT void sleepMs(double t)
VISP_EXPORT double measureTimeMs()