40#include <visp3/sensor/vpMocapQualisys.h>
44#if defined(VISP_HAVE_QUALISYS) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
50#include <visp3/core/vpTime.h>
58void quitHandler(
int sig)
60 std::cout << std::endl <<
"TERMINATING AT USER REQUEST" << std::endl << std::endl;
66void usage(
const char *argv[],
int error)
68 std::cout <<
"SYNOPSIS" << std::endl
69 <<
" " << argv[0] <<
" [--server-address <address>] [-sa]"
70 <<
" [--only-body] [-ob]"
72 <<
" [--verbose] [-v]"
73 <<
" [--help] [-h]" << 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
80 <<
" --only-body <name>" << std::endl
81 <<
" Name of the specific body you want to be displayed." << std::endl
82 <<
" Default: ''" << std::endl
84 <<
" --all-bodies" << std::endl
85 <<
" When used, get all bodies pose including non visible bodies." << std::endl
87 <<
" --verbose, -v" << std::endl
88 <<
" Enable verbose mode." << std::endl
90 <<
" --help, -h" << std::endl
91 <<
" Print this helper message." << 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
99 std::cout <<
"Error" << std::endl
101 <<
"Unsupported parameter " << argv[error] << std::endl;
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> ¤t_bodies_pose)
112 std::cout <<
"Qualisys connection error. Check the Qualisys Task Manager or your IP address." << std::endl;
116 std::map<std::string, vpHomogeneousMatrix> bodies_pose;
118 if (opt_onlyBody ==
"") {
120 std::cout <<
"Qualisys error. Check the Qualisys Task Manager" << std::endl;
125 std::cout <<
"Qualisys error. Check the Qualisys Task Manager" << std::endl;
127 bodies_pose[opt_onlyBody] = pose;
131 current_bodies_pose = bodies_pose;
138void display_loop(std::mutex &lock,
const std::map<std::string, vpHomogeneousMatrix> ¤t_bodies_pose,
bool verbose)
140 std::map<std::string, vpHomogeneousMatrix> bodies_pose;
145 bodies_pose = current_bodies_pose;
147 for (std::map<std::string, vpHomogeneousMatrix>::iterator it = bodies_pose.begin(); it != bodies_pose.end(); ++it) {
149 std::cout <<
"Found body: " << it->first << std::endl;
151 std::cout <<
" Translation [m]: " << it->second.getTranslationVector().t() << std::endl
153 std::cout <<
" Roll/pitch/yaw [deg]: ";
154 for (
unsigned int i = 0; i < 3; i++) {
157 std::cout << std::endl;
158 std::cout <<
" Transformation Matrix wMb:\n" << it->second << std::endl;
166int main(
int argc,
const char *argv[])
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;
174 std::map<std::string, vpHomogeneousMatrix> current_bodies_pose;
176 signal(SIGINT, quitHandler);
178 for (
int i = 1; i < argc; i++) {
179 if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
181 }
else if (std::string(argv[i]) ==
"--server-address" || std::string(argv[i]) ==
"-sa") {
182 opt_serverAddress = std::string(argv[i + 1]);
184 }
else if (std::string(argv[i]) ==
"--only-body" || std::string(argv[i]) ==
"-ob") {
185 opt_onlyBody = std::string(argv[i + 1]);
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") {
199 std::thread mocap_thread(
200 [&lock, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody, ¤t_bodies_pose]() {
201 mocap_loop(lock, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody, current_bodies_pose);
203 std::thread display_thread(
204 [&lock, ¤t_bodies_pose, &opt_verbose]() { display_loop(lock, current_bodies_pose, opt_verbose); });
207 display_thread.join();
214#ifndef VISP_HAVE_QUALISYS
215 std::cout <<
"Install qualisys_cpp_sdk to be able to test Qualisys Mocap System using ViSP" << std::endl;
217#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
218 std::cout <<
"This test required c++11 or more recent c++ standard." << std::endl;
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double deg(double rad)
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.
Implementation of a rotation vector as Euler angle minimal representation.
VISP_EXPORT void sleepMs(double t)