Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
testPixhawkDroneVelocityControl.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Simple example to demonstrate how to control in velocity using mavsdk
32  * a drone equipped with a Pixhawk connected to a Jetson TX2.
33  */
34 
44 #include <iostream>
45 
46 #include <visp3/core/vpConfig.h>
47 
48 // Check if std:c++17 or higher
49 #if defined(VISP_HAVE_MAVSDK) && ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) \
50  && defined(VISP_HAVE_THREADS)
51 
52 #include <thread>
53 #include <visp3/robot/vpRobotMavsdk.h>
54 
55 using std::chrono::seconds;
56 using std::this_thread::sleep_for;
57 
58 void usage(const std::string &bin_name)
59 {
60  std::cerr << "Usage : " << bin_name << " <connection information>\n"
61  << "Connection URL format should be :\n"
62  << " - For TCP : tcp://[server_host][:server_port]\n"
63  << " - For UDP : udp://[bind_host][:bind_port]\n"
64  << " - For Serial : serial:///path/to/serial/dev[:baudrate]\n"
65  << "For example, to connect to the simulator use URL: udp://:14540\n";
66 }
67 
68 int main(int argc, char **argv)
69 {
70 #ifdef ENABLE_VISP_NAMESPACE
71  using namespace VISP_NAMESPACE_NAME;
72 #endif
73  if (argc != 2) {
74  usage(argv[0]);
75  return EXIT_SUCCESS;
76  }
77 
78  auto drone = vpRobotMavsdk(argv[1]);
79 
80  drone.setTakeOffAlt(.5);
81  if (!drone.takeOff()) {
82  std::cout << "Takeoff failed" << std::endl;
83  return EXIT_FAILURE;
84  }
85  vpColVector vel_command { 0.0, 0.0, 0.0, 0.0 };
86 
87  drone.setForwardSpeed(0.3);
88  std::cout << "Set forward speed of 0.3 m/s for 4 sec" << std::endl;
89  sleep_for(seconds(4));
90 
91  drone.setForwardSpeed(-0.3);
92  std::cout << "Set forward speed of -0.3 m/s for 4 sec" << std::endl;
93  sleep_for(seconds(4));
94 
95  drone.stopMoving();
96  std::cout << "Stop moving for 4 sec" << std::endl;
97  sleep_for(seconds(4));
98 
99  std::cout << "Land now..." << std::endl;
100  drone.land();
101 
102  return EXIT_SUCCESS;
103 }
104 
105 #else
106 
107 int main()
108 {
109 #ifndef VISP_HAVE_MAVSDK
110  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
111  << std::endl;
112 #endif
113 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
114  std::cout
115  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
116  "rebuild ViSP.\n"
117  << std::endl;
118 #endif
119  return EXIT_SUCCESS;
120 }
121 
122 #endif // #if defined(VISP_HAVE_MAVSDK)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191