45 #include <visp3/core/vpConfig.h>
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)
54 #include <visp3/robot/vpRobotMavsdk.h>
55 #include <visp3/sensor/vpMocapQualisys.h>
56 #include <visp3/sensor/vpMocapVicon.h>
58 using std::chrono::seconds;
59 using std::this_thread::sleep_for;
71 void quitHandler(
int sig)
74 std::cout << std::endl <<
"TERMINATING AT USER REQUEST" << std::endl << std::endl;
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> ¤t_body_poses_enu_M_flu,
bool &mocap_failure,
86 bool &mavlink_failure)
88 std::shared_ptr<vpMocap> mocap;
90 #ifdef VISP_HAVE_QUALISYS
91 mocap = std::make_shared<vpMocapQualisys>();
93 std::cout <<
"ERROR : Qualisys not found.";
98 #ifdef VISP_HAVE_VICON
99 mocap = std::make_shared<vpMocapVicon>();
102 std::cout <<
"ERROR : Vicon not found.";
106 mocap->setVerbose(opt_verbose);
107 mocap->setServerAddress(opt_serverAddress);
108 if (mocap->connect() ==
false) {
110 mocap_failure =
true;
112 std::cout <<
"Mocap connexion failure. Check mocap server IP address" << std::endl;
117 bool internal_mavlink_failure =
false;
118 while (!g_quit && !internal_mavlink_failure) {
119 std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
121 if (opt_onlyBody ==
"") {
122 if (!mocap->getBodiesPose(body_poses_enu_M_flu, opt_all_bodies)) {
128 if (!mocap->getSpecificBodyPose(opt_onlyBody, enu_M_flu)) {
132 body_poses_enu_M_flu[opt_onlyBody] = enu_M_flu;
137 internal_mavlink_failure = mavlink_failure;
138 current_body_poses_enu_M_flu =
139 body_poses_enu_M_flu;
148 int top(
const std::string &connection_info, std::map<std::string, vpHomogeneousMatrix> ¤t_body_poses_enu_M_flu,
149 std::mutex &lock,
bool &mocap_failure)
151 std::map<std::string, vpHomogeneousMatrix> body_poses_enu_M_flu;
152 bool internal_mocap_failure =
false;
153 const double fps = 100;
155 vpRobotMavsdk drone { connection_info };
157 while (!g_quit && !internal_mocap_failure) {
160 body_poses_enu_M_flu = current_body_poses_enu_M_flu;
161 internal_mocap_failure = mocap_failure;
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)) {
180 void usage(
char *argv[],
int error)
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
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
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
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
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
210 <<
" --verbose, -v" << std::endl
211 <<
" Enable verbose mode." << std::endl
213 <<
" --help, -h" << std::endl
214 <<
" Print this helper message." << std::endl
218 std::cout <<
"Error" << std::endl
220 <<
"Unsupported parameter " << argv[error] << std::endl;
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)
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]);
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") {
243 else if (mode ==
"vicon" || mode ==
"v") {
247 std::cout <<
"ERROR : System not recognized, exiting." << std::endl;
252 else if (std::string(argv[i]) ==
"--device" || std::string(argv[i]) ==
"-d") {
253 connection_info = std::string(argv[i + 1]);
256 else if (std::string(argv[i]) ==
"--server-address" || std::string(argv[i]) ==
"-sa") {
257 server_address = std::string(argv[i + 1]);
260 else if (std::string(argv[i]) ==
"--all-bodies") {
263 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
266 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
282 int main(
int argc,
char **argv)
284 std::map<std::string, vpHomogeneousMatrix> current_body_poses_enu_M_flu;
288 std::string opt_connectionInfo =
"/dev/tty.usbmodem1";
290 std::string opt_connectionInfo =
"udp://127.0.0.1:14550";
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;
300 parse_commandline(argc, argv, opt_qualisys, opt_connectionInfo, opt_serverAddress, opt_onlyBody, opt_all_bodies,
303 if (opt_qualisys && opt_serverAddress ==
"") {
304 opt_serverAddress =
"192.168.30.42";
306 else if (!opt_qualisys && opt_serverAddress ==
"") {
307 opt_serverAddress =
"192.168.30.1";
310 if (opt_onlyBody ==
"") {
311 std::cout <<
"The parameter --only-body MUST be given in the command line." << std::endl;
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 ¤t_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);
325 std::cout <<
"Mocap connexion failure. Check mocap server IP address" << std::endl;
330 std::thread mavlink_thread(
331 [&lock, ¤t_body_poses_enu_M_flu, &opt_connectionInfo, &mocap_failure, &mavlink_failure]() {
333 int result = top(opt_connectionInfo, current_body_poses_enu_M_flu, lock, mocap_failure);
337 fprintf(stderr,
"mavlink_control threw exception %i \n", error);
339 mavlink_failure =
true;
346 mavlink_thread.join();
359 #ifndef VISP_HAVE_MAVSDK
360 std::cout <<
"\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
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 "
368 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
370 <<
"\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 int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()