Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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#if defined(VISP_HAVE_MAVSDK) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
51
52#include <thread>
53#include <visp3/robot/vpRobotMavsdk.h>
54
55using std::chrono::seconds;
56using std::this_thread::sleep_for;
57
58void 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
68int main(int argc, char **argv)
69{
70 if (argc != 2) {
71 usage(argv[0]);
72 return EXIT_SUCCESS;
73 }
74
75 auto robot = vpRobotMavsdk(argv[1]);
76
77 if (! robot.setGPSGlobalOrigin(48.117266, -1.6777926, 40.0))
78 {
79 return EXIT_FAILURE;
80 }
81
82 std::cout << "Vehicle has flying capability: " << (robot.hasFlyingCapability() ? "yes" : "no") << std::endl;
83 robot.arm();
84
85 double delta_north = 1.;
86 double delta_east = 0.;
87 double delta_down = 0.;
88 double delta_yaw = 0.;
89
90 std::cout << "Move 1 meter north" << std::endl;;
91 robot.setPositionRelative(delta_north, delta_east, delta_down, delta_yaw);
92
93 vpColVector frd_vel{0.0, 0.0, 0.0, 0.0};
94 frd_vel[0]= -0.3; // forward vel m/s
95 //frd_vel[3]= vpMath::rad(10.);
96
97 std::cout << "Go at 0.3m/s backward during 3 sec.\n";
98 robot.setVelocity(frd_vel);
99 vpTime::wait(3000);
100
101 std::cout << "Go at 0.3m/s forward and rotate 10 deg/s along yaw during 2 sec.\n";
102 frd_vel[0]= 0.3; // forward vel m/s
103 frd_vel[3]= vpMath::rad(10.); // yaw vel 10 deg/s converted in rad/s
104
105 double t = vpTime::measureTimeMs();
106 do {
107 vpTime::sleepMs(20);
108 robot.setVelocity(frd_vel);
109 }
110 while(vpTime::measureTimeMs() - t < 2000.); //
111
112 robot.disarm();
113 return EXIT_SUCCESS;
114}
115
116#else
117
118int 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 !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
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.
static double rad(double deg)
Definition vpMath.h:116
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)
VISP_EXPORT double measureTimeMs()