Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
45
46#include <mutex>
47#include <signal.h>
48#include <thread>
49
50#include <visp3/core/vpTime.h>
51
52bool g_quit = false;
53
58void 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
66void 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
105void 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 } else {
124 if (!qualisys.getSpecificBodyPose(opt_onlyBody, pose)) {
125 std::cout << "Qualisys error. Check the Qualisys Task Manager" << std::endl;
126 }
127 bodies_pose[opt_onlyBody] = pose;
128 }
129
130 lock.lock();
131 current_bodies_pose = bodies_pose;
132 lock.unlock();
133
135 }
136}
137
138void display_loop(std::mutex &lock, const std::map<std::string, vpHomogeneousMatrix> &current_bodies_pose, bool verbose)
139{
140 std::map<std::string, vpHomogeneousMatrix> bodies_pose;
141
142 while (!g_quit) {
143
144 lock.lock();
145 bodies_pose = current_bodies_pose;
146 lock.unlock();
147 for (std::map<std::string, vpHomogeneousMatrix>::iterator it = bodies_pose.begin(); it != bodies_pose.end(); ++it) {
148 vpRxyzVector rxyz(it->second.getRotationMatrix());
149 std::cout << "Found body: " << it->first << std::endl;
150 if (verbose) {
151 std::cout << " Translation [m]: " << it->second.getTranslationVector().t() << std::endl
152 << " Quaternion: " << vpQuaternionVector(it->second.getRotationMatrix()).t() << std::endl;
153 std::cout << " Roll/pitch/yaw [deg]: ";
154 for (unsigned int i = 0; i < 3; i++) {
155 std::cout << vpMath::deg(rxyz[i]) << " ";
156 }
157 std::cout << std::endl;
158 std::cout << " Transformation Matrix wMb:\n" << it->second << std::endl;
159 }
160 }
161
162 vpTime::sleepMs(200);
163 }
164}
165
166int main(int argc, const char *argv[])
167{
168 bool opt_verbose = false;
169 std::string opt_serverAddress = "192.168.30.42";
170 std::string opt_onlyBody = "";
171 bool opt_all_bodies = false;
172
173 // Map containig all the current poses of the drones
174 std::map<std::string, vpHomogeneousMatrix> current_bodies_pose;
175
176 signal(SIGINT, quitHandler);
177
178 for (int i = 1; i < argc; i++) {
179 if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
180 opt_verbose = true;
181 } else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") {
182 opt_serverAddress = std::string(argv[i + 1]);
183 i++;
184 } else if (std::string(argv[i]) == "--only-body" || std::string(argv[i]) == "-ob") {
185 opt_onlyBody = std::string(argv[i + 1]);
186 i++;
187 } else if (std::string(argv[i]) == "--all-bodies") {
188 opt_all_bodies = true;
189 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
190 usage(argv, 0);
191 return EXIT_SUCCESS;
192 } else {
193 usage(argv, i);
194 return EXIT_FAILURE;
195 }
196 }
197
198 std::mutex lock;
199 std::thread mocap_thread(
200 [&lock, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody, &current_bodies_pose]() {
201 mocap_loop(lock, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody, current_bodies_pose);
202 });
203 std::thread display_thread(
204 [&lock, &current_bodies_pose, &opt_verbose]() { display_loop(lock, current_bodies_pose, opt_verbose); });
205
206 mocap_thread.join();
207 display_thread.join();
208
209 return EXIT_SUCCESS;
210}
211#else
212int main()
213{
214#ifndef VISP_HAVE_QUALISYS
215 std::cout << "Install qualisys_cpp_sdk to be able to test Qualisys Mocap System using ViSP" << std::endl;
216#endif
217#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
218 std::cout << "This test required c++11 or more recent c++ standard." << std::endl;
219#endif
220 return EXIT_SUCCESS;
221}
222#endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double deg(double rad)
Definition vpMath.h:106
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.
VISP_EXPORT void sleepMs(double t)