Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testPixhawkDroneVelocityControl.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  * Simple example to demonstrate how to control in velocity using mavsdk
33  * a drone equipped with a Pixhawk connected to a Jetson TX2.
34  *
35 *****************************************************************************/
36 
46 #include <iostream>
47 
48 #include <visp3/core/vpConfig.h>
49 
50 // Check if std:c++17 or higher
51 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) \
52  && defined(VISP_HAVE_THREADS)
53 
54 #include <thread>
55 #include <visp3/robot/vpRobotMavsdk.h>
56 
57 using std::chrono::seconds;
58 using std::this_thread::sleep_for;
59 
60 void usage(const std::string &bin_name)
61 {
62  std::cerr << "Usage : " << bin_name << " <connection information>\n"
63  << "Connection URL format should be :\n"
64  << " - For TCP : tcp://[server_host][:server_port]\n"
65  << " - For UDP : udp://[bind_host][:bind_port]\n"
66  << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n"
67  << "For example, to connect to the simulator use URL: udp://:14540\n";
68 }
69 
70 int main(int argc, char **argv)
71 {
72  if (argc != 2) {
73  usage(argv[0]);
74  return EXIT_SUCCESS;
75  }
76 
77  auto drone = vpRobotMavsdk(argv[1]);
78 
79  drone.setTakeOffAlt(.5);
80  if (!drone.takeOff()) {
81  std::cout << "Takeoff failed" << std::endl;
82  return EXIT_FAILURE;
83  }
84  vpColVector vel_command { 0.0, 0.0, 0.0, 0.0 };
85 
86  drone.setForwardSpeed(0.3);
87  std::cout << "Set forward speed of 0.3 m/s for 4 sec" << std::endl;
88  sleep_for(seconds(4));
89 
90  drone.setForwardSpeed(-0.3);
91  std::cout << "Set forward speed of -0.3 m/s for 4 sec" << std::endl;
92  sleep_for(seconds(4));
93 
94  drone.stopMoving();
95  std::cout << "Stop moving for 4 sec" << std::endl;
96  sleep_for(seconds(4));
97 
98  std::cout << "Land now..." << std::endl;
99  drone.land();
100 
101  return EXIT_SUCCESS;
102 }
103 
104 #else
105 
106 int main()
107 {
108 #ifndef VISP_HAVE_MAVSDK
109  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
110  << std::endl;
111 #endif
112 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
113  std::cout
114  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
115  "rebuild ViSP.\n"
116  << std::endl;
117 #endif
118  return EXIT_SUCCESS;
119 }
120 
121 #endif // #if defined(VISP_HAVE_MAVSDK)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163