Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
48 (defined(VISP_HAVE_QUALISYS) || defined(VISP_HAVE_VICON))
49
50#include <chrono>
51#include <thread>
52
53#include <visp3/robot/vpRobotMavsdk.h>
54#include <visp3/sensor/vpMocapQualisys.h>
55#include <visp3/sensor/vpMocapVicon.h>
56
57using std::chrono::seconds;
58using std::this_thread::sleep_for;
59
60// ------------------------------------------------------------------------------
61// Modifications Qualisys
62// ------------------------------------------------------------------------------
63
64bool g_quit = false;
65
70void quitHandler(int sig)
71{
72 (void)sig;
73 std::cout << std::endl << "TERMINATING AT USER REQUEST" << std::endl << std::endl;
74
75 g_quit = true;
76}
77
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> &current_body_poses_enu_M_flu, bool &mocap_failure,
85 bool &mavlink_failure)
86{
87 std::shared_ptr<vpMocap> mocap;
88 if (qualisys) {
89#ifdef VISP_HAVE_QUALISYS
90 mocap = std::make_shared<vpMocapQualisys>();
91#else
92 std::cout << "ERROR : Qualisys not found.";
93 return false;
94#endif
95 } else {
96#ifdef VISP_HAVE_VICON
97 mocap = std::make_shared<vpMocapVicon>();
98#else
99
100 std::cout << "ERROR : Vicon not found.";
101 return false;
102#endif
103 }
104 mocap->setVerbose(opt_verbose);
105 mocap->setServerAddress(opt_serverAddress);
106 if (mocap->connect() == false) {
107 lock.lock();
108 mocap_failure = true;
109 lock.unlock();
110 std::cout << "Mocap connexion failure. Check mocap server IP address" << std::endl;
111
112 return false;
113 }
114
115 bool internal_mavlink_failure = false;
116 while (!g_quit && !internal_mavlink_failure) {
117 std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
118
119 if (opt_onlyBody == "") {
120 if (!mocap->getBodiesPose(body_poses_enu_M_flu, opt_all_bodies)) {
121 g_quit = true;
122 }
123 } else {
124 vpHomogeneousMatrix enu_M_flu;
125 if (!mocap->getSpecificBodyPose(opt_onlyBody, enu_M_flu)) {
126 g_quit = true;
127 } else {
128 body_poses_enu_M_flu[opt_onlyBody] = enu_M_flu;
129 }
130 }
131
132 lock.lock();
133 internal_mavlink_failure = mavlink_failure;
134 current_body_poses_enu_M_flu =
135 body_poses_enu_M_flu; // Now we send directly the poses in the ENU global reference frame.
136 lock.unlock();
137 }
138 return true;
139}
140
141// ------------------------------------------------------------------------------
142// TOP
143// ------------------------------------------------------------------------------
144int top(const std::string &connection_info, std::map<std::string, vpHomogeneousMatrix> &current_body_poses_enu_M_flu,
145 std::mutex &lock, bool &mocap_failure)
146{
147 std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
148 bool internal_mocap_failure = false;
149 const double fps = 100;
150
151 vpRobotMavsdk drone{connection_info};
152
153 while (!g_quit && !internal_mocap_failure) {
154 double t = vpTime::measureTimeMs();
155 lock.lock();
156 body_poses_enu_M_flu = current_body_poses_enu_M_flu;
157 internal_mocap_failure = mocap_failure;
158 lock.unlock();
159
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)) {
163 return 1;
164 }
165 }
166 vpTime::wait(t, 1000./fps); // Stream MoCap at given framerate
167 }
168
169 return 0;
170}
171
172// ------------------------------------------------------------------------------
173// Usage function
174// ------------------------------------------------------------------------------
175
176void usage(char *argv[], int error)
177{
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
185 << 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
190 << 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
195 << 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
200 << 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
205 << std::endl
206 << " --verbose, -v" << std::endl
207 << " Enable verbose mode." << std::endl
208 << std::endl
209 << " --help, -h" << std::endl
210 << " Print this helper message." << std::endl
211 << std::endl;
212
213 if (error) {
214 std::cout << "Error" << std::endl
215 << " "
216 << "Unsupported parameter " << argv[error] << std::endl;
217 }
218}
219
220// ------------------------------------------------------------------------------
221// Parse Command Line
222// ------------------------------------------------------------------------------
223// throws EXIT_FAILURE if could not open the port
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)
226{
227
228 // Read input arguments
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]);
232 i++;
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") {
236 qualisys = true;
237 } else if (mode == "vicon" || mode == "v") {
238 qualisys = false;
239 } else {
240 std::cout << "ERROR : System not recognized, exiting." << std::endl;
241 throw EXIT_FAILURE;
242 }
243 i++;
244 } else if (std::string(argv[i]) == "--device" || std::string(argv[i]) == "-d") {
245 connection_info = std::string(argv[i + 1]);
246 i++;
247 } else if (std::string(argv[i]) == "--server-address" || std::string(argv[i]) == "-sa") {
248 server_address = std::string(argv[i + 1]);
249 i++;
250 } else if (std::string(argv[i]) == "--all-bodies") {
251 all_bodies = true;
252 } else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
253 verbose = true;
254 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
255 usage(argv, 0);
256 throw EXIT_SUCCESS;
257 } else {
258 usage(argv, i);
259 throw EXIT_FAILURE;
260 }
261 }
262
263 return;
264}
265
266// ------------------------------------------------------------------------------
267// Main
268// ------------------------------------------------------------------------------
269int main(int argc, char **argv)
270{
271 std::map<std::string, vpHomogeneousMatrix> current_body_poses_enu_M_flu;
272
273 // Default input arguments
274#ifdef __APPLE__
275 std::string opt_connectionInfo = "/dev/tty.usbmodem1";
276#else
277 std::string opt_connectionInfo = "udp://127.0.0.1:14550";
278#endif
279
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;
285
286 // User Input
287 parse_commandline(argc, argv, opt_qualisys, opt_connectionInfo, opt_serverAddress, opt_onlyBody, opt_all_bodies,
288 opt_verbose);
289
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";
294 }
295
296 if (opt_onlyBody == "") {
297 std::cout << "The parameter --only-body MUST be given in the command line." << std::endl;
298 return EXIT_FAILURE;
299 }
300
301 // Modifications qualisys ----------------------------------------------------
302 std::mutex lock;
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 &current_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);
309 });
310 if (mocap_failure) {
311 std::cout << "Mocap connexion failure. Check mocap server IP address" << std::endl;
312 return EXIT_FAILURE;
313 }
314
315 // This program uses throw, wrap one big try/catch here
316 std::thread mavlink_thread(
317 [&lock, &current_body_poses_enu_M_flu, &opt_connectionInfo, &mocap_failure, &mavlink_failure]() {
318 try {
319 int result = top(opt_connectionInfo, current_body_poses_enu_M_flu, lock, mocap_failure);
320 return result;
321 } catch (int error) {
322 fprintf(stderr, "mavlink_control threw exception %i \n", error);
323 lock.lock();
324 mavlink_failure = true;
325 lock.unlock();
326 return error;
327 }
328 });
329
330 mocap_thread.join();
331 mavlink_thread.join();
332 if (mocap_failure) {
333 return EXIT_FAILURE;
334 } else {
335 return EXIT_SUCCESS;
336 }
337}
338
339#else
340
341int main()
342{
343#ifndef VISP_HAVE_MAVSDK
344 std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
345 << std::endl;
346#endif
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 "
349 "and rebuid ViSP.\n"
350 << std::endl;
351#endif
352#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
353 std::cout
354 << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
355 "rebuild ViSP.\n"
356 << std::endl;
357#endif
358 return EXIT_SUCCESS;
359}
360
361#endif // #if defined(VISP_HAVE_MAVSDK)
Implementation of an homogeneous matrix and operations on such kind of matrices.
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)