Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testMocapQualisys.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  * Test Qualisys Motion Capture System.
33  *
34 *****************************************************************************/
35 
40 #include <visp3/sensor/vpMocapQualisys.h>
41 
42 #include <iostream>
43 
44 #if defined(VISP_HAVE_QUALISYS) && defined(VISP_HAVE_THREADS)
45 
46 #include <mutex>
47 #include <signal.h>
48 #include <thread>
49 
50 #include <visp3/core/vpTime.h>
51 
52 bool g_quit = false;
53 
58 void quitHandler(int sig)
59 {
60  std::cout << std::endl << "TERMINATING AT USER REQUEST" << std::endl << std::endl;
61 
62  g_quit = true;
63  (void)sig;
64 }
65 
66 void usage(const char *argv[], int error)
67 {
68  std::cout << "SYNOPSIS" << std::endl
69  << " " << argv[0] << " [--server-address <address>] [-sa]"
70  << " [--only-body] [-ob]"
71  << " [--all-bodies]"
72  << " [--verbose] [-v]"
73  << " [--help] [-h]" << std::endl
74  << std::endl;
75  std::cout << "DESCRIPTION" << std::endl
76  << " --server-address <address>" << std::endl
77  << " Server address." << std::endl
78  << " Default: 192.168.30.42." << std::endl
79  << std::endl
80  << " --only-body <name>" << std::endl
81  << " Name of the specific body you want to be displayed." << std::endl
82  << " Default: ''" << std::endl
83  << std::endl
84  << " --all-bodies" << std::endl
85  << " When used, get all bodies pose including non visible bodies." << std::endl
86  << std::endl
87  << " --verbose, -v" << std::endl
88  << " Enable verbose mode." << std::endl
89  << std::endl
90  << " --help, -h" << std::endl
91  << " Print this helper message." << std::endl
92  << std::endl;
93  std::cout << "USAGE" << std::endl
94  << " Example to test Qualisys connection:" << std::endl
95  << " " << argv[0] << " --server-address 127.0.0.1 --verbose" << std::endl
96  << std::endl;
97 
98  if (error) {
99  std::cout << "Error" << std::endl
100  << " "
101  << "Unsupported parameter " << argv[error] << std::endl;
102  }
103 }
104 
105 void mocap_loop(std::mutex &lock, bool opt_verbose, bool opt_all_bodies, std::string &opt_serverAddress,
106  std::string &opt_onlyBody, std::map<std::string, vpHomogeneousMatrix> &current_bodies_pose)
107 {
108  vpMocapQualisys qualisys;
109  qualisys.setVerbose(opt_verbose);
110  qualisys.setServerAddress(opt_serverAddress);
111  if (!qualisys.connect()) {
112  std::cout << "Qualisys connection error. Check the Qualisys Task Manager or your IP address." << std::endl;
113  return;
114  }
115  while (!g_quit) {
116  std::map<std::string, vpHomogeneousMatrix> bodies_pose;
117 
118  if (opt_onlyBody == "") {
119  if (!qualisys.getBodiesPose(bodies_pose, opt_all_bodies)) {
120  std::cout << "Qualisys error. Check the Qualisys Task Manager" << std::endl;
121  }
122  }
123  else {
124  vpHomogeneousMatrix pose;
125  if (!qualisys.getSpecificBodyPose(opt_onlyBody, pose)) {
126  std::cout << "Qualisys error. Check the Qualisys Task Manager" << std::endl;
127  }
128  bodies_pose[opt_onlyBody] = pose;
129  }
130 
131  lock.lock();
132  current_bodies_pose = bodies_pose;
133  lock.unlock();
134 
135  vpTime::sleepMs(5);
136  }
137 }
138 
139 void display_loop(std::mutex &lock, const std::map<std::string, vpHomogeneousMatrix> &current_bodies_pose, bool verbose)
140 {
141  std::map<std::string, vpHomogeneousMatrix> bodies_pose;
142 
143  while (!g_quit) {
144 
145  lock.lock();
146  bodies_pose = current_bodies_pose;
147  lock.unlock();
148  for (std::map<std::string, vpHomogeneousMatrix>::iterator it = bodies_pose.begin(); it != bodies_pose.end(); ++it) {
149  vpRxyzVector rxyz(it->second.getRotationMatrix());
150  std::cout << "Found body: " << it->first << std::endl;
151  if (verbose) {
152  std::cout << " Translation [m]: " << it->second.getTranslationVector().t() << std::endl
153  << " Quaternion: " << vpQuaternionVector(it->second.getRotationMatrix()).t() << std::endl;
154  std::cout << " Roll/pitch/yaw [deg]: ";
155  for (unsigned int i = 0; i < 3; i++) {
156  std::cout << vpMath::deg(rxyz[i]) << " ";
157  }
158  std::cout << std::endl;
159  std::cout << " Transformation Matrix wMb:\n" << it->second << std::endl;
160  }
161  }
162 
163  vpTime::sleepMs(200);
164  }
165 }
166 
167 int main(int argc, const char *argv[])
168 {
169  bool opt_verbose = false;
170  std::string opt_serverAddress = "192.168.30.42";
171  std::string opt_onlyBody = "";
172  bool opt_all_bodies = false;
173 
174  // Map containig all the current poses of the drones
175  std::map<std::string, vpHomogeneousMatrix> current_bodies_pose;
176 
177  signal(SIGINT, quitHandler);
178 
179  for (int i = 1; i < argc; i++) {
180  if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
181  opt_verbose = true;
182  }
183  else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") {
184  opt_serverAddress = std::string(argv[i + 1]);
185  i++;
186  }
187  else if (std::string(argv[i]) == "--only-body" || std::string(argv[i]) == "-ob") {
188  opt_onlyBody = std::string(argv[i + 1]);
189  i++;
190  }
191  else if (std::string(argv[i]) == "--all-bodies") {
192  opt_all_bodies = true;
193  }
194  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
195  usage(argv, 0);
196  return EXIT_SUCCESS;
197  }
198  else {
199  usage(argv, i);
200  return EXIT_FAILURE;
201  }
202  }
203 
204  std::mutex lock;
205  std::thread mocap_thread(
206  [&lock, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody, &current_bodies_pose]() {
207  mocap_loop(lock, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody, current_bodies_pose);
208  });
209  std::thread display_thread(
210  [&lock, &current_bodies_pose, &opt_verbose]() { display_loop(lock, current_bodies_pose, opt_verbose); });
211 
212  mocap_thread.join();
213  display_thread.join();
214 
215  return EXIT_SUCCESS;
216 }
217 #else
218 int main()
219 {
220  std::cout << "Install qualisys_cpp_sdk to be able to test Qualisys Mocap System using ViSP" << std::endl;
221 
222  return EXIT_SUCCESS;
223 }
224 #endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double deg(double rad)
Definition: vpMath.h:117
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)
Implementation of a rotation vector as quaternion angle minimal representation.
vpRowVector t() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
VISP_EXPORT void sleepMs(double t)