Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testPixhawkRoverVelocityControl.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 robot = vpRobotMavsdk(argv[1]);
78 
79  if (!robot.setGPSGlobalOrigin(48.117266, -1.6777926, 40.0)) {
80  return EXIT_FAILURE;
81  }
82 
83  std::cout << "Vehicle has flying capability: " << (robot.hasFlyingCapability() ? "yes" : "no") << std::endl;
84  robot.arm();
85 
86  double delta_north = 1.;
87  double delta_east = 0.;
88  double delta_down = 0.;
89  double delta_yaw = 0.;
90 
91  std::cout << "Move 1 meter north" << std::endl;;
92  robot.setPositionRelative(delta_north, delta_east, delta_down, delta_yaw);
93 
94  vpColVector frd_vel { 0.0, 0.0, 0.0, 0.0 };
95  frd_vel[0] = -0.3; // forward vel m/s
96  //frd_vel[3]= vpMath::rad(10.);
97 
98  std::cout << "Go at 0.3m/s backward during 3 sec.\n";
99  robot.setVelocity(frd_vel);
100  vpTime::wait(3000);
101 
102  std::cout << "Go at 0.3m/s forward and rotate 10 deg/s along yaw during 2 sec.\n";
103  frd_vel[0] = 0.3; // forward vel m/s
104  frd_vel[3] = vpMath::rad(10.); // yaw vel 10 deg/s converted in rad/s
105 
106  double t = vpTime::measureTimeMs();
107  do {
108  vpTime::sleepMs(20);
109  robot.setVelocity(frd_vel);
110  } while (vpTime::measureTimeMs() - t < 2000.); //
111 
112  robot.disarm();
113  return EXIT_SUCCESS;
114 }
115 
116 #else
117 
118 int main()
119 {
120 #ifndef VISP_HAVE_MAVSDK
121  std::cout << "\nThis example requires mavsdk library. You should install it, configure and rebuid ViSP.\n"
122  << std::endl;
123 #endif
124 #if !((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
125  std::cout
126  << "\nThis example requires at least cxx17. You should enable cxx17 during ViSP configuration with cmake and "
127  "rebuild ViSP.\n"
128  << std::endl;
129 #endif
130  return EXIT_SUCCESS;
131 }
132 
133 #endif // #if defined(VISP_HAVE_MAVSDK)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
static double rad(double deg)
Definition: vpMath.h:127
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)
VISP_EXPORT double measureTimeMs()