45#include <visp3/core/vpConfig.h>
47#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
48 (defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON))
53#include <visp3/robot/vpRobotMavsdk.h>
54#include <visp3/sensor/vpMocapQualisys.h>
55#include <visp3/sensor/vpMocapVicon.h>
57using std::chrono::seconds;
58using std::this_thread::sleep_for;
70void quitHandler(
int sig)
73 std::cout << std::endl <<
"TERMINATING AT USER REQUEST" << std::endl << std::endl;
82bool mocap_sdk_loop(std::mutex &lock,
bool qualisys,
bool opt_verbose,
bool opt_all_bodies,
83 std::string &opt_serverAddress, std::string &opt_onlyBody,
84 std::map<std::string, vpHomogeneousMatrix> ¤t_body_poses_enu_M_flu,
bool &mocap_failure,
85 bool &mavlink_failure)
87 std::shared_ptr<vpMocap> mocap;
89#ifdef VISP_HAVE_QUALISYS
90 mocap = std::make_shared<vpMocapQualisys>();
92 std::cout <<
"ERROR : Qualisys not found.";
97 mocap = std::make_shared<vpMocapVicon>();
100 std::cout <<
"ERROR : Vicon not found.";
104 mocap->setVerbose(opt_verbose);
105 mocap->setServerAddress(opt_serverAddress);
106 if (mocap->connect() ==
false) {
108 mocap_failure =
true;
110 std::cout <<
"Mocap connexion failure. Check mocap server IP address" << std::endl;
115 bool internal_mavlink_failure =
false;
116 while (!g_quit && !internal_mavlink_failure) {
117 std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
119 if (opt_onlyBody ==
"") {
120 if (!mocap->getBodiesPose(body_poses_enu_M_flu, opt_all_bodies)) {
125 if (!mocap->getSpecificBodyPose(opt_onlyBody, enu_M_flu)) {
128 body_poses_enu_M_flu[opt_onlyBody] = enu_M_flu;
133 internal_mavlink_failure = mavlink_failure;
134 current_body_poses_enu_M_flu =
135 body_poses_enu_M_flu;
144int top(
const std::string &connection_info, std::map<std::string, vpHomogeneousMatrix> ¤t_body_poses_enu_M_flu,
145 std::mutex &lock,
bool &mocap_failure)
147 std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
148 bool internal_mocap_failure =
false;
149 const double fps = 100;
153 while (!g_quit && !internal_mocap_failure) {
156 body_poses_enu_M_flu = current_body_poses_enu_M_flu;
157 internal_mocap_failure = mocap_failure;
160 for (std::map<std::string, vpHomogeneousMatrix>::iterator it = body_poses_enu_M_flu.begin();
161 it != body_poses_enu_M_flu.end(); ++it) {
162 if (!drone.sendMocapData(it->second)) {
176void usage(
char *argv[],
int error)
178 std::cout <<
"SYNOPSIS" << std::endl
179 <<
" " << argv[0] <<
" [--only-body <name>] [-ob]"
180 <<
" [--mocap-system <qualisys>/<vicon>] [-ms <q>/<v>]"
181 <<
" [--device <device port>] [-d]"
182 <<
" [--server-address <server address>] [-sa]"
183 <<
" [--verbose] [-v]"
184 <<
" [--help] [-h]" << std::endl
186 std::cout <<
"DESCRIPTION" << std::endl
187 <<
"MANDATORY PARAMETERS :" << std::endl
188 <<
" --only-body <name>" << std::endl
189 <<
" Name of the specific body you want to be displayed." << std::endl
191 <<
"OPTIONAL PARAMETERS (DEFAULT VALUES)" << std::endl
192 <<
" --mocap-system, -ms" << std::endl
193 <<
" Specify the name of the mocap system : 'qualisys' / 'q' or 'vicon'/ 'v'." << std::endl
194 <<
" Default: Qualisys mode." << std::endl
196 <<
" --device <device port>, -d" << std::endl
197 <<
" String giving us all the informations necessary for connection." << std::endl
198 <<
" Default: serial:///dev/ttyUSB0 ." << std::endl
199 <<
" UDP example: udp://192.168.30.111:14540 (udp://IP:Port) ." << std::endl
201 <<
" --server-address <address>, -sa" << std::endl
202 <<
" Mocap server address." << std::endl
203 <<
" Default for Qualisys: 192.168.34.42 ." << std::endl
204 <<
" Default for Vicon: 192.168.34.1 ." << std::endl
206 <<
" --verbose, -v" << std::endl
207 <<
" Enable verbose mode." << std::endl
209 <<
" --help, -h" << std::endl
210 <<
" Print this helper message." << std::endl
214 std::cout <<
"Error" << std::endl
216 <<
"Unsupported parameter " << argv[error] << std::endl;
224void parse_commandline(
int argc,
char **argv,
bool &qualisys, std::string &connection_info, std::string &server_address,
225 std::string &only_body,
bool &all_bodies,
bool &verbose)
229 for (
int i = 1; i < argc; i++) {
230 if (std::string(argv[i]) ==
"--only-body" || std::string(argv[i]) ==
"-ob") {
231 only_body = std::string(argv[i + 1]);
233 }
else if (std::string(argv[i]) ==
"--mocap-system" || std::string(argv[i]) ==
"-ms") {
234 std::string mode = std::string(argv[i + 1]);
235 if (mode ==
"qualisys" || mode ==
"q") {
237 }
else if (mode ==
"vicon" || mode ==
"v") {
240 std::cout <<
"ERROR : System not recognized, exiting." << std::endl;
244 }
else if (std::string(argv[i]) ==
"--device" || std::string(argv[i]) ==
"-d") {
245 connection_info = std::string(argv[i + 1]);
247 }
else if (std::string(argv[i]) ==
"--server-address" || std::string(argv[i]) ==
"-sa") {
248 server_address = std::string(argv[i + 1]);
250 }
else if (std::string(argv[i]) ==
"--all-bodies") {
252 }
else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
254 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
269int main(
int argc,
char **argv)
271 std::map<std::string, vpHomogeneousMatrix> current_body_poses_enu_M_flu;
275 std::string opt_connectionInfo =
"/dev/tty.usbmodem1";
277 std::string opt_connectionInfo =
"udp://127.0.0.1:14550";
280 bool opt_qualisys =
false;
281 std::string opt_serverAddress;
282 std::string opt_onlyBody =
"";
283 bool opt_all_bodies =
false;
284 bool opt_verbose =
false;
287 parse_commandline(argc, argv, opt_qualisys, opt_connectionInfo, opt_serverAddress, opt_onlyBody, opt_all_bodies,
290 if (opt_qualisys && opt_serverAddress ==
"") {
291 opt_serverAddress =
"192.168.30.42";
292 }
else if (!opt_qualisys && opt_serverAddress ==
"") {
293 opt_serverAddress =
"192.168.30.1";
296 if (opt_onlyBody ==
"") {
297 std::cout <<
"The parameter --only-body MUST be given in the command line." << std::endl;
303 bool mocap_failure =
false;
304 bool mavlink_failure =
false;
305 std::thread mocap_thread([&lock, &opt_qualisys, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody,
306 ¤t_body_poses_enu_M_flu, &mocap_failure, &mavlink_failure]() {
307 mocap_sdk_loop(lock, opt_qualisys, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody,
308 current_body_poses_enu_M_flu, mocap_failure, mavlink_failure);
311 std::cout <<
"Mocap connexion failure. Check mocap server IP address" << std::endl;
316 std::thread mavlink_thread(
317 [&lock, ¤t_body_poses_enu_M_flu, &opt_connectionInfo, &mocap_failure, &mavlink_failure]() {
319 int result = top(opt_connectionInfo, current_body_poses_enu_M_flu, lock, mocap_failure);
321 }
catch (
int error) {
322 fprintf(stderr,
"mavlink_control threw exception %i \n", error);
324 mavlink_failure =
true;
331 mavlink_thread.join();
343#ifndef VISP_HAVE_MAVSDK
344 std::cout <<
"\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
347#if !(defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON))
348 std::cout <<
"\nThis example requires data from a Qualisys or Vicon mocap system. You should install it, configure "
352#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
354 <<
"\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
Implementation of an homogeneous matrix and operations on such kind of matrices.
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)