Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMocapQualisys.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 * Motion capture using Qualisys device.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_QUALISYS
39
40#include <cmath>
41#include <iostream>
42
43#include <visp3/core/vpTime.h>
44#include <visp3/sensor/vpMocapQualisys.h>
45
46#include <qualisys_cpp_sdk/RTPacket.h>
47#include <qualisys_cpp_sdk/RTProtocol.h>
48
49#ifndef DOXYGEN_SHOULD_SKIP_THIS
50class vpMocapQualisys::vpMocapQualisysImpl
51{
52public:
53 vpMocapQualisysImpl()
54 : m_rtProtocol(), m_basePort(22222), m_udpPort(6734), m_majorVersion(1), m_minorVersion(19), m_bigEndian(false),
55 m_dataAvailable(false), m_streamFrames(false), m_verbose(false), m_serverAddr()
56 {
57 }
58
59 virtual ~vpMocapQualisysImpl() { close(); }
60
61 void close()
62 {
63 m_rtProtocol.StopCapture();
64 m_rtProtocol.Disconnect();
65 }
66
67 bool connect()
68 {
69 int n_attempt = 2;
70 for (auto i = 0; i < n_attempt; i++) {
71 if (!m_rtProtocol.Connected()) {
72 if (!m_rtProtocol.Connect(m_serverAddr.c_str(), m_basePort, &m_udpPort, m_majorVersion, m_minorVersion,
73 m_bigEndian)) {
74 std::cout << "Qualisys connection error: " << m_rtProtocol.GetErrorString() << std::endl;
75
76 vpTime::sleepMs(1000);
77 }
78 } else {
79 if (m_verbose) {
80 std::cout << "Qualisys connected" << std::endl;
81 }
82 return verifyDataStreamed();
83 }
84 }
85
86 std::cout << "Qualisys connection timeout" << std::endl;
87
88 return false;
89 }
90
91 bool verifyDataStreamed()
92 {
93 bool readSettingsOK = false;
94
95 for (auto i = 0; i < 6; i++) {
96 if (!m_dataAvailable) {
97 if (!m_rtProtocol.Read6DOFSettings(m_dataAvailable)) {
98 if (m_verbose) {
99 std::cout << "Reading 6DOF settings error: " << m_rtProtocol.GetErrorString() << std::endl;
100 }
101
102 vpTime::sleepMs(1000);
103 }
104 } else {
105 if (m_verbose && !readSettingsOK) {
106 std::cout << "Reading 6DOF settings succeded." << std::endl;
107 }
108 readSettingsOK = true;
109 }
110 }
111
112 if (!readSettingsOK) {
113 if (m_verbose) {
114 std::cout << "Reading 6DOF settings timeout: " << std::endl;
115 }
116 return false;
117 } else {
118 for (auto i = 0; i < 6; i++) {
119 if (!m_streamFrames) {
120 if (!m_rtProtocol.StreamFrames(CRTProtocol::RateAllFrames, 0, m_udpPort, NULL, CRTProtocol::cComponent6d)) {
121 if (m_verbose) {
122 std::cout << "Streaming frames error: " << m_rtProtocol.GetErrorString() << std::endl;
123 }
124
125 vpTime::sleepMs(1000);
126 }
127 m_streamFrames = true;
128 } else {
129 if (m_verbose) {
130 std::cout << "Starting to stream 6DOF data" << std::endl;
131 }
132 return true;
133 }
134 }
135
136 std::cout << "Streaming frames timeout: " << std::endl;
137
138 return false;
139 }
140 }
141
142 bool getBodyPose(int iBody, std::string &name, vpHomogeneousMatrix &M, CRTPacket *rtPacket)
143 {
144 float fX, fY, fZ;
145 float rotationMatrix[9];
146
147 if (rtPacket->Get6DOFBody(iBody, fX, fY, fZ, rotationMatrix)) {
148 const char *pTmpStr = m_rtProtocol.Get6DOFBodyName(iBody);
149 if (pTmpStr) {
150 name = std::string(pTmpStr);
151 } else {
152 if (m_verbose) {
153 std::cout << "Unknown body" << std::endl;
154 }
155 return false;
156 }
157
158 M[0][3] = fX / 1000.;
159 M[1][3] = fY / 1000.;
160 M[2][3] = fZ / 1000.;
161 M[3][3] = 1.;
162 unsigned int k = 0;
163 for (unsigned int j = 0; j < 3; j++) {
164 for (unsigned int i = 0; i < 3; i++) {
165 M[i][j] = rotationMatrix[k++];
166 }
167 }
168
169 return true;
170 } else {
171 return false;
172 }
173 }
174
175 bool getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies)
176 {
177 CRTPacket::EPacketType packetType;
178
179 if (m_rtProtocol.Receive(packetType, true) == CNetwork::ResponseType::success) {
180 if (packetType == CRTPacket::PacketData) {
181 CRTPacket *rtPacket = m_rtProtocol.GetRTPacket();
182 for (unsigned int iBody = 0; iBody < rtPacket->Get6DOFBodyCount(); iBody++) {
183 std::string bodyName;
184
185 vpHomogeneousMatrix bodyPose;
186 if (!getBodyPose(iBody, bodyName, bodyPose, rtPacket)) {
187 std::cout << "Error : Could not get pose from body n°" << iBody << std::endl;
188
189 return false;
190 }
191 if (all_bodies) {
192 bodies_pose[bodyName] = bodyPose;
193 } else if (bodyPose.isValid()) {
194 bodies_pose[bodyName] = bodyPose;
195 }
196 }
197 return true;
198 }
199 }
200 return false;
201 }
202
203 bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
204 {
205 std::map<std::string, vpHomogeneousMatrix> bodies_pose;
206 if (getBodiesPose(bodies_pose, true)) {
207 if (bodies_pose.find(body_name) != bodies_pose.end()) {
208 body_pose = bodies_pose[body_name];
209 if (m_verbose) {
210 std::cout << "I found bodyName" << body_name << std::endl;
211 }
212 return true;
213 } else {
214 std::cout << "The body " << body_name << " was not found in Qualisys. Please check the name you typed."
215 << std::endl;
216
217 return false;
218 }
219 } else {
220 std::cout << "Error : could not process data from Qualisys" << std::endl;
221
222 return false;
223 }
224 }
225
226 void setServerAddress(const std::string &serverAddr) { m_serverAddr = serverAddr; }
227
228 void setVerbose(bool verbose) { m_verbose = verbose; }
229
230private:
231 CRTProtocol m_rtProtocol;
232 unsigned short m_basePort;
233 unsigned short m_udpPort;
234 int m_majorVersion;
235 int m_minorVersion;
236 bool m_bigEndian;
237 bool m_dataAvailable;
238 bool m_streamFrames;
239 bool m_verbose;
240 std::string m_serverAddr;
241};
242#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
243
244/*
245 **********************************************************************************************
246 */
247
251vpMocapQualisys::vpMocapQualisys() : m_impl(new vpMocapQualisysImpl()) {}
252
257
261void vpMocapQualisys::close() { m_impl->close(); }
262
268bool vpMocapQualisys::connect() { return m_impl->connect(); }
269
277bool vpMocapQualisys::getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies)
278{
279 return m_impl->getBodiesPose(bodies_pose, all_bodies);
280}
281
288bool vpMocapQualisys::getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
289{
290 return m_impl->getSpecificBodyPose(body_name, body_pose);
291}
298void vpMocapQualisys::setServerAddress(const std::string &serverAddr) { m_impl->setServerAddress(serverAddr); }
299
304void vpMocapQualisys::setVerbose(bool verbose) { m_impl->setVerbose(verbose); }
305
306#else
307// Work around to avoid warning:
308// libvisp_sensor.a(vpMocapQualisys.cpp.o) has no symbols
309void dummy_vpMocapQualisys(){};
310#endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setServerAddress(const std::string &serverAddr)
bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
void setVerbose(bool verbose)
bool getBodiesPose(std::map< std::string, vpHomogeneousMatrix > &bodies_pose, bool all_bodies=false)
VISP_EXPORT void sleepMs(double t)