Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
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
50 class vpMocapQualisys::vpMocapQualisysImpl
51 {
52 public:
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, nullptr, 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 
230 private:
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 
251 vpMocapQualisys::vpMocapQualisys() : m_impl(new vpMocapQualisysImpl()) {}
252 
257 
261 void vpMocapQualisys::close() { m_impl->close(); }
262 
268 bool vpMocapQualisys::connect() { return m_impl->connect(); }
269 
277 bool vpMocapQualisys::getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies)
278 {
279  return m_impl->getBodiesPose(bodies_pose, all_bodies);
280 }
281 
288 bool vpMocapQualisys::getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
289 {
290  return m_impl->getSpecificBodyPose(body_name, body_pose);
291 }
298 void vpMocapQualisys::setServerAddress(const std::string &serverAddr) { m_impl->setServerAddress(serverAddr); }
299 
304 void 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
309 void 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)
virtual ~vpMocapQualisys()
bool getBodiesPose(std::map< std::string, vpHomogeneousMatrix > &bodies_pose, bool all_bodies=false)
bool m_verbose
Definition: vpMocap.h:104
std::string m_serverAddr
Definition: vpMocap.h:105
VISP_EXPORT void sleepMs(double t)