Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
sendMocapToPixhawk.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  * Example that shows how to send a pose from a motion capture system through masvsdk.
33  *
34 *****************************************************************************/
35 
43 #include <iostream>
44 
45 #include <visp3/core/vpConfig.h>
46 
47 // Check if std:c++17 or higher
48 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \
49  (defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON)) && defined(VISP_HAVE_THREADS)
50 
51 #include <chrono>
52 #include <thread>
53 
54 #include <visp3/robot/vpRobotMavsdk.h>
55 #include <visp3/sensor/vpMocapQualisys.h>
56 #include <visp3/sensor/vpMocapVicon.h>
57 
58 using std::chrono::seconds;
59 using std::this_thread::sleep_for;
60 
61 // ------------------------------------------------------------------------------
62 // Modifications Qualisys
63 // ------------------------------------------------------------------------------
64 
65 bool g_quit = false;
66 
71 void quitHandler(int sig)
72 {
73  (void)sig;
74  std::cout << std::endl << "TERMINATING AT USER REQUEST" << std::endl << std::endl;
75 
76  g_quit = true;
77 }
78 
83 bool mocap_sdk_loop(std::mutex &lock, bool qualisys, bool opt_verbose, bool opt_all_bodies,
84  std::string &opt_serverAddress, std::string &opt_onlyBody,
85  std::map<std::string, vpHomogeneousMatrix> &current_body_poses_enu_M_flu, bool &mocap_failure,
86  bool &mavlink_failure)
87 {
88  std::shared_ptr<vpMocap> mocap;
89  if (qualisys) {
90 #ifdef VISP_HAVE_QUALISYS
91  mocap = std::make_shared<vpMocapQualisys>();
92 #else
93  std::cout << "ERROR : Qualisys not found.";
94  return false;
95 #endif
96  }
97  else {
98 #ifdef VISP_HAVE_VICON
99  mocap = std::make_shared<vpMocapVicon>();
100 #else
101 
102  std::cout << "ERROR : Vicon not found.";
103  return false;
104 #endif
105  }
106  mocap->setVerbose(opt_verbose);
107  mocap->setServerAddress(opt_serverAddress);
108  if (mocap->connect() == false) {
109  lock.lock();
110  mocap_failure = true;
111  lock.unlock();
112  std::cout << "Mocap connexion failure. Check mocap server IP address" << std::endl;
113 
114  return false;
115  }
116 
117  bool internal_mavlink_failure = false;
118  while (!g_quit && !internal_mavlink_failure) {
119  std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
120 
121  if (opt_onlyBody == "") {
122  if (!mocap->getBodiesPose(body_poses_enu_M_flu, opt_all_bodies)) {
123  g_quit = true;
124  }
125  }
126  else {
127  vpHomogeneousMatrix enu_M_flu;
128  if (!mocap->getSpecificBodyPose(opt_onlyBody, enu_M_flu)) {
129  g_quit = true;
130  }
131  else {
132  body_poses_enu_M_flu[opt_onlyBody] = enu_M_flu;
133  }
134  }
135 
136  lock.lock();
137  internal_mavlink_failure = mavlink_failure;
138  current_body_poses_enu_M_flu =
139  body_poses_enu_M_flu; // Now we send directly the poses in the ENU global reference frame.
140  lock.unlock();
141  }
142  return true;
143 }
144 
145 // ------------------------------------------------------------------------------
146 // TOP
147 // ------------------------------------------------------------------------------
148 int top(const std::string &connection_info, std::map<std::string, vpHomogeneousMatrix> &current_body_poses_enu_M_flu,
149  std::mutex &lock, bool &mocap_failure)
150 {
151  std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
152  bool internal_mocap_failure = false;
153  const double fps = 100;
154 
155  vpRobotMavsdk drone { connection_info };
156 
157  while (!g_quit && !internal_mocap_failure) {
158  double t = vpTime::measureTimeMs();
159  lock.lock();
160  body_poses_enu_M_flu = current_body_poses_enu_M_flu;
161  internal_mocap_failure = mocap_failure;
162  lock.unlock();
163 
164  for (std::map<std::string, vpHomogeneousMatrix>::iterator it = body_poses_enu_M_flu.begin();
165  it != body_poses_enu_M_flu.end(); ++it) {
166  if (!drone.sendMocapData(it->second)) {
167  return 1;
168  }
169  }
170  vpTime::wait(t, 1000./fps); // Stream MoCap at given framerate
171  }
172 
173  return 0;
174 }
175 
176 // ------------------------------------------------------------------------------
177 // Usage function
178 // ------------------------------------------------------------------------------
179 
180 void usage(char *argv[], int error)
181 {
182  std::cout << "SYNOPSIS" << std::endl
183  << " " << argv[0] << " [--only-body <name>] [-ob]"
184  << " [--mocap-system <qualisys>/<vicon>] [-ms <q>/<v>]"
185  << " [--device <device port>] [-d]"
186  << " [--server-address <server address>] [-sa]"
187  << " [--verbose] [-v]"
188  << " [--help] [-h]" << std::endl
189  << std::endl;
190  std::cout << "DESCRIPTION" << std::endl
191  << "MANDATORY PARAMETERS :" << std::endl
192  << " --only-body <name>" << std::endl
193  << " Name of the specific body you want to be displayed." << std::endl
194  << std::endl
195  << "OPTIONAL PARAMETERS (DEFAULT VALUES)" << std::endl
196  << " --mocap-system, -ms" << std::endl
197  << " Specify the name of the mocap system : 'qualisys' / 'q' or 'vicon'/ 'v'." << std::endl
198  << " Default: Qualisys mode." << std::endl
199  << std::endl
200  << " --device <device port>, -d" << std::endl
201  << " String giving us all the informations necessary for connection." << std::endl
202  << " Default: serial:///dev/ttyUSB0 ." << std::endl
203  << " UDP example: udp://192.168.30.111:14540 (udp://IP:Port) ." << std::endl
204  << std::endl
205  << " --server-address <address>, -sa" << std::endl
206  << " Mocap server address." << std::endl
207  << " Default for Qualisys: 192.168.34.42 ." << std::endl
208  << " Default for Vicon: 192.168.34.1 ." << std::endl
209  << std::endl
210  << " --verbose, -v" << std::endl
211  << " Enable verbose mode." << std::endl
212  << std::endl
213  << " --help, -h" << std::endl
214  << " Print this helper message." << std::endl
215  << std::endl;
216 
217  if (error) {
218  std::cout << "Error" << std::endl
219  << " "
220  << "Unsupported parameter " << argv[error] << std::endl;
221  }
222 }
223 
224 // ------------------------------------------------------------------------------
225 // Parse Command Line
226 // ------------------------------------------------------------------------------
227 // throws EXIT_FAILURE if could not open the port
228 void parse_commandline(int argc, char **argv, bool &qualisys, std::string &connection_info, std::string &server_address,
229  std::string &only_body, bool &all_bodies, bool &verbose)
230 {
231 
232  // Read input arguments
233  for (int i = 1; i < argc; i++) {
234  if (std::string(argv[i]) == "--only-body" || std::string(argv[i]) == "-ob") {
235  only_body = std::string(argv[i + 1]);
236  i++;
237  }
238  else if (std::string(argv[i]) == "--mocap-system" || std::string(argv[i]) == "-ms") {
239  std::string mode = std::string(argv[i + 1]);
240  if (mode == "qualisys" || mode == "q") {
241  qualisys = true;
242  }
243  else if (mode == "vicon" || mode == "v") {
244  qualisys = false;
245  }
246  else {
247  std::cout << "ERROR : System not recognized, exiting." << std::endl;
248  throw EXIT_FAILURE;
249  }
250  i++;
251  }
252  else if (std::string(argv[i]) == "--device" || std::string(argv[i]) == "-d") {
253  connection_info = std::string(argv[i + 1]);
254  i++;
255  }
256  else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") {
257  server_address = std::string(argv[i + 1]);
258  i++;
259  }
260  else if (std::string(argv[i]) == "--all-bodies") {
261  all_bodies = true;
262  }
263  else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
264  verbose = true;
265  }
266  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
267  usage(argv, 0);
268  throw EXIT_SUCCESS;
269  }
270  else {
271  usage(argv, i);
272  throw EXIT_FAILURE;
273  }
274  }
275 
276  return;
277 }
278 
279 // ------------------------------------------------------------------------------
280 // Main
281 // ------------------------------------------------------------------------------
282 int main(int argc, char **argv)
283 {
284  std::map<std::string, vpHomogeneousMatrix> current_body_poses_enu_M_flu;
285 
286  // Default input arguments
287 #ifdef __APPLE__
288  std::string opt_connectionInfo = "/dev/tty.usbmodem1";
289 #else
290  std::string opt_connectionInfo = "udp://127.0.0.1:14550";
291 #endif
292 
293  bool opt_qualisys = false;
294  std::string opt_serverAddress;
295  std::string opt_onlyBody = "";
296  bool opt_all_bodies = false;
297  bool opt_verbose = false;
298 
299  // User Input
300  parse_commandline(argc, argv, opt_qualisys, opt_connectionInfo, opt_serverAddress, opt_onlyBody, opt_all_bodies,
301  opt_verbose);
302 
303  if (opt_qualisys && opt_serverAddress == "") {
304  opt_serverAddress = "192.168.30.42";
305  }
306  else if (!opt_qualisys && opt_serverAddress == "") {
307  opt_serverAddress = "192.168.30.1";
308  }
309 
310  if (opt_onlyBody == "") {
311  std::cout << "The parameter --only-body MUST be given in the command line." << std::endl;
312  return EXIT_FAILURE;
313  }
314 
315  // Modifications qualisys ----------------------------------------------------
316  std::mutex lock;
317  bool mocap_failure = false;
318  bool mavlink_failure = false;
319  std::thread mocap_thread([&lock, &opt_qualisys, &opt_verbose, &opt_all_bodies, &opt_serverAddress, &opt_onlyBody,
320  &current_body_poses_enu_M_flu, &mocap_failure, &mavlink_failure]() {
321  mocap_sdk_loop(lock, opt_qualisys, opt_verbose, opt_all_bodies, opt_serverAddress, opt_onlyBody,
322  current_body_poses_enu_M_flu, mocap_failure, mavlink_failure);
323  });
324  if (mocap_failure) {
325  std::cout << "Mocap connexion failure. Check mocap server IP address" << std::endl;
326  return EXIT_FAILURE;
327  }
328 
329  // This program uses throw, wrap one big try/catch here
330  std::thread mavlink_thread(
331  [&lock, &current_body_poses_enu_M_flu, &opt_connectionInfo, &mocap_failure, &mavlink_failure]() {
332  try {
333  int result = top(opt_connectionInfo, current_body_poses_enu_M_flu, lock, mocap_failure);
334  return result;
335  }
336  catch (int error) {
337  fprintf(stderr, "mavlink_control threw exception %i \n", error);
338  lock.lock();
339  mavlink_failure = true;
340  lock.unlock();
341  return error;
342  }
343  });
344 
345  mocap_thread.join();
346  mavlink_thread.join();
347  if (mocap_failure) {
348  return EXIT_FAILURE;
349  }
350  else {
351  return EXIT_SUCCESS;
352  }
353 }
354 
355 #else
356 
357 int main()
358 {
359 #ifndef VISP_HAVE_MAVSDK
360  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
361  << std::endl;
362 #endif
363 #if !(defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON))
364  std::cout << "\nThis example requires data from a Qualisys or Vicon mocap system. You should install it, configure "
365  "and rebuid ViSP.\n"
366  << std::endl;
367 #endif
368 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
369  std::cout
370  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
371  "rebuild ViSP.\n"
372  << std::endl;
373 #endif
374  return EXIT_SUCCESS;
375 }
376 
377 #endif // #if defined(VISP_HAVE_MAVSDK)
Implementation of an homogeneous matrix and operations on such kind of matrices.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()