Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
servoBebop2.cpp

example showing how to do visual servoing of Parrot Bebop 2 drone.

WARNING: this program does no sensing or avoiding of obstacles, the drone WILL collide with any objects in the way! Make sure the drone has about 3-4 meters of free space around it before starting the program.

This program makes the drone detect and follow an AprilTag from the 36h11 family.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2019 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See http://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Example that shows how to do visual servoing with Parrot Bebop 2 drone in ViSP.
*
* Authors:
* Fabien Spindler
* Gatien Gaumerais
*
*****************************************************************************/
#include <iostream>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentGravityCenterNormalized.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpTime.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpRobotBebop2.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
#include <visp3/visual_features/vpFeatureVanishingPoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#ifdef VISP_HAVE_PUGIXML
#include <visp3/core/vpXmlParserCamera.h>
#endif
#if !defined(VISP_HAVE_ARSDK)
int main()
{
std::cout << "\nThis example requires Parrot ARSDK3 library. You should install it.\n" << std::endl;
return EXIT_SUCCESS;
}
#elif !defined(VISP_HAVE_FFMPEG)
int main()
{
std::cout << "\nThis example requires ffmpeg library. You should install it.\n" << std::endl;
return EXIT_SUCCESS;
}
#else
bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
{
return (p1.second.get_v() < p2.second.get_v());
};
int main(int argc, char **argv)
{
try {
std::string ip_address = "192.168.42.1";
std::string opt_cam_parameters;
bool opt_has_cam_parameters = false;
double tagSize = -1;
double opt_distance_to_tag = -1;
bool opt_has_distance_to_tag = false;
int stream_res = 0; // Default 480p resolution
bool verbose = false;
if (argc >= 3 && std::string(argv[1]) == "--tag_size") {
tagSize = std::atof(argv[2]); // Tag size option is required
if (tagSize <= 0) {
std::cout << "Error : invalid tag size." << std::endl << "See " << argv[0] << " --help" << std::endl;
return 0;
}
for (int i = 3; i < argc; i++) {
if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
ip_address = std::string(argv[i + 1]);
i++;
} else if (std::string(argv[i]) == "--distance_to_tag" && i + 1 < argc) {
opt_distance_to_tag = std::atof(argv[i + 1]);
if (opt_distance_to_tag <= 0) {
std::cout << "Error : invalid distance to tag." << std::endl << "See " << argv[0] << " --help" << std::endl;
return 0;
}
opt_has_distance_to_tag = true;
i++;
} else if (std::string(argv[i]) == "--intrinsic") {
#ifdef VISP_HAVE_PUGIXML
opt_cam_parameters = std::string(argv[i + 1]);
opt_has_cam_parameters = true;
i++;
#else
std::cout << "PUGIXML is required for custom camera parameters input." << std::endl;
return 0;
#endif
} else if (std::string(argv[i]) == "--hd_stream") {
stream_res = 1;
} else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
verbose = true;
} else {
std::cout << "Error : unknown parameter " << argv[i] << std::endl
<< "See " << argv[0] << " --help" << std::endl;
return 0;
}
}
} else if (argc >= 2 && (std::string(argv[1]) == "--help" || std::string(argv[1]) == "-h")) {
std::cout << "\nUsage:\n"
<< " " << argv[0]
<< " [--tag_size <size>] [--ip <drone ip>] [--distance_to_tag <distance>] [--intrinsic <xml file>] "
<< "[--hd_stream] [--verbose] [-v] [--help] [-h]\n"
<< std::endl
<< "Description:\n"
<< " --tag_size <size>\n"
<< " The size of the tag to detect in meters, required.\n\n"
<< " --ip <drone ip>\n"
<< " Ip address of the drone to which you want to connect (default : 192.168.42.1).\n\n"
<< " --distance_to_tag <distance>\n"
<< " The desired distance to the tag in meters (default: 1 meter).\n\n"
<< " --intrinsic <xml file>\n"
<< " XML file containing computed intrinsic camera parameters (default: empty).\n\n"
<< " --hd_stream\n"
<< " Enables HD 720p streaming instead of default 480p.\n"
<< " Allows to increase range and accuracy of the tag detection,\n"
<< " but increases latency and computation time.\n"
<< " Caution: camera calibration settings are different for the two resolutions.\n"
<< " Make sure that if you pass custom intrinsic camera parameters,\n"
<< " they were obtained with the correct resolution.\n\n"
<< " --verbose, -v\n"
<< " Enables verbose (drone information messages and velocity commands\n"
<< " are then displayed).\n\n"
<< " --help, -h\n"
<< " Print help message.\n"
<< std::endl;
return 0;
} else {
std::cout << "Error : tag size parameter required." << std::endl << "See " << argv[0] << " --help" << std::endl;
return 0;
}
std::cout
<< "\nWARNING: \n - This program does no sensing or avoiding of "
"obstacles, \n"
"the drone WILL collide with any objects in the way! Make sure "
"the \n"
"drone has approximately 3 meters of free space on all sides.\n"
" - The drone uses a downward-facing camera for horizontal speed estimation,\n make sure the drone flies "
"above a non-uniform flooring,\n or its movement will be inacurate and dangerous !\n"
<< std::endl;
verbose, true, ip_address); // Create the drone with desired verbose level, settings reset, and corresponding IP
if (drone.isRunning()) {
drone.setVideoResolution(stream_res); // Set video resolution to 480p (default) or 720p
drone.setStreamingMode(0); // Set lowest latency stream mode
drone.setVideoStabilisationMode(0); // Disable video stabilisation
drone.doFlatTrim(); // Flat trim calibration
drone.startStreaming(); // Start streaming and decoding video data
drone.setExposure(1.5f); // Set exposure to max so that the aprilTag detection is more efficient
drone.setCameraOrientation(-5., 0.,
true); // Set camera to look slightly down so that the drone is slightly above the tag
drone.takeOff(true); // Take off
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV display;
#endif
int orig_displayX = 100;
int orig_displayY = 100;
display.init(I, orig_displayX, orig_displayY, "DRONE VIEW");
vpPlot plotter(1, 700, 700, orig_displayX + static_cast<int>(I.getWidth()) + 20, orig_displayY,
"Visual servoing tasks");
unsigned int iter = 0;
vpDetectorAprilTag detector(tagFamily); // The detector used to detect Apritags
detector.setAprilTagQuadDecimate(4.0);
detector.setAprilTagNbThreads(4);
detector.setDisplayTag(true);
if (opt_has_cam_parameters) {
if (p.parse(cam, opt_cam_parameters, "Camera", projModel, I.getWidth(), I.getHeight()) !=
std::cout << "Cannot find parameters in XML file " << opt_cam_parameters << std::endl;
if (drone.getVideoHeight() == 720) { // 720p streaming
cam.initPersProjWithoutDistortion(785.6412585, 785.3322447, 637.9049857, 359.7524531);
} else { // 480p streaming
cam.initPersProjWithoutDistortion(531.9213063, 520.8495788, 429.133986, 240.9464457);
}
}
} else {
std::cout << "Setting default camera parameters ... " << std::endl;
if (drone.getVideoHeight() == 720) { // 720p streaming
cam.initPersProjWithoutDistortion(785.6412585, 785.3322447, 637.9049857, 359.7524531);
} else { // 480p streaming
cam.initPersProjWithoutDistortion(531.9213063, 520.8495788, 429.133986, 240.9464457);
}
}
vpServo task; // Visual servoing task
// double lambda = 0.5;
vpAdaptiveGain lambda = vpAdaptiveGain(1.5, 0.7, 30);
task.setLambda(lambda);
/*
In the following section, camera 1 refers to camera coordonates system of the drone, but without taking camera
pan and camera tilt into account.
Those values are taken into consideration in Camera 2.
E is the effective coordinate system of the drone, the one in which we need to convert every velocity command.
We can easily compute homogeneous matrix between camera 1 and camera 2, and between camera 1
and effective coordonate system E of the drone.
Using those matrices, we can in the end obtain the matrix between c2 and E
*/
vpRotationMatrix c1Rc2(c1_rxyz_c2); // Rotation between camera 1 and 2
vpHomogeneousMatrix c1Mc2(vpTranslationVector(), c1Rc2); // Homogeneous matrix between c1 and c2
vpRotationMatrix c1Re{0, 1, 0, 0, 0, 1, 1, 0, 0}; // Rotation between camera 1 and E
vpTranslationVector c1te(0, 0, -0.09); // Translation between camera 1 and E
vpHomogeneousMatrix c1Me(c1te, c1Re); // Homogeneous matrix between c1 and E
vpHomogeneousMatrix c2Me = c1Mc2.inverse() * c1Me; // Homogeneous matrix between c2 and E
task.set_cVe(cVe);
vpMatrix eJe(6, 4, 0);
eJe[0][0] = 1;
eJe[1][1] = 1;
eJe[2][2] = 1;
eJe[5][3] = 1;
// double Z_d = 1.; // Desired distance to the target
double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.);
// Define the desired polygon corresponding the the AprilTag CLOCKWISE
double X[4] = {tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2.};
double Y[4] = {tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2.};
std::vector<vpPoint> vec_P, vec_P_d;
for (int i = 0; i < 4; i++) {
vpPoint P_d(X[i], Y[i], 0);
vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);
P_d.track(cdMo); //
vec_P_d.push_back(P_d);
}
vpMomentObject m_obj(3), m_obj_d(3);
vpMomentDatabase mdb, mdb_d;
vpMomentBasic mb_d; // Here only to get the desired area m00
vpMomentCentered mc, mc_d;
vpMomentAreaNormalized man(0, Z_d), man_d(0, Z_d); // Declare normalized area updated below with m00
vpMomentGravityCenterNormalized mgn, mgn_d; // Declare normalized gravity center
// Desired moments
m_obj_d.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
m_obj_d.fromVector(vec_P_d); // Initialize the object with the points coordinates
mb_d.linkTo(mdb_d); // Add basic moments to database
mg_d.linkTo(mdb_d); // Add gravity center to database
mc_d.linkTo(mdb_d); // Add centered moments to database
man_d.linkTo(mdb_d); // Add area normalized to database
mgn_d.linkTo(mdb_d); // Add gravity center normalized to database
mdb_d.updateAll(m_obj_d); // All of the moments must be updated, not just an_d
mg_d.compute(); // Compute gravity center moment
mc_d.compute(); // Compute centered moments AFTER gravity center
double area = 0;
area = mb_d.get(2, 0) + mb_d.get(0, 2);
else
area = mb_d.get(0, 0);
// Update moment with the desired area
man_d.setDesiredArea(area);
man_d.compute(); // Compute area normalized moment AFTER centered moments
mgn_d.compute(); // Compute gravity center normalized moment AFTER area normalized moment
// Desired plane
double A = 0.0;
double B = 0.0;
double C = 1.0 / Z_d;
// Construct area normalized features
vpFeatureMomentGravityCenterNormalized s_mgn(mdb, A, B, C), s_mgn_d(mdb_d, A, B, C);
vpFeatureMomentAreaNormalized s_man(mdb, A, B, C), s_man_d(mdb_d, A, B, C);
// Add the features
task.addFeature(s_mgn, s_mgn_d);
task.addFeature(s_man, s_man_d);
plotter.initGraph(0, 4);
plotter.setLegend(0, 0, "Xn"); // Distance from center on X axis feature
plotter.setLegend(0, 1, "Yn"); // Distance from center on Y axis feature
plotter.setLegend(0, 2, "an"); // Tag area feature
plotter.setLegend(0, 3, "atan(1/rho)"); // Vanishing point feature
// Update desired gravity center normalized feature
s_mgn_d.update(A, B, C);
// Update desired area normalized feature
s_man_d.update(A, B, C);
// Update desired vanishing point feature for the horizontal line
s_vp_d.setAtanOneOverRho(0);
s_vp_d.setAlpha(0);
bool runLoop = true;
bool vec_ip_has_been_sorted = false;
std::vector<std::pair<size_t, vpImagePoint> > vec_ip_sorted;
//** Visual servoing loop **//
while (drone.isRunning() && drone.isStreaming() && runLoop) {
double startTime = vpTime::measureTimeMs();
std::vector<vpHomogeneousMatrix> cMo_vec;
detector.detect(I, tagSize, cam, cMo_vec); // Detect AprilTags in current image
double t = vpTime::measureTimeMs() - startTime;
{
std::stringstream ss;
ss << "Detection time: " << t << " ms";
vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
}
if (detector.getNbObjects() == 1) {
// Update current points used to compute the moments
std::vector<vpImagePoint> vec_ip = detector.getPolygon(0);
vec_P.clear();
for (size_t i = 0; i < vec_ip.size(); i++) { // size = 4
double x = 0, y = 0;
vpPixelMeterConversion::convertPoint(cam, vec_ip[i], x, y);
P.set_x(x);
P.set_y(y);
vec_P.push_back(P);
}
// Current moments
m_obj.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
m_obj.fromVector(vec_P); // Initialize the object with the points coordinates
mg.linkTo(mdb); // Add gravity center to database
mc.linkTo(mdb); // Add centered moments to database
man.linkTo(mdb); // Add area normalized to database
mgn.linkTo(mdb); // Add gravity center normalized to database
mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d
mg.compute(); // Compute gravity center moment
mc.compute(); // Compute centered moments AFTER gravity center
man.setDesiredArea(area); // Desired area was init at 0 (unknow at contruction), need to be updated here
man.compute(); // Compute area normalized moment AFTER centered moment
mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment
s_mgn.update(A, B, C);
s_mgn.compute_interaction();
s_man.update(A, B, C);
s_man.compute_interaction();
/* Sort points from their height in the image, and keep original indexes.
This is done once, in order to be independent from the orientation of the tag
when detecting vanishing points. */
if (!vec_ip_has_been_sorted) {
for (size_t i = 0; i < vec_ip.size(); i++) {
// Add the points and their corresponding index
std::pair<size_t, vpImagePoint> index_pair = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
vec_ip_sorted.push_back(index_pair);
}
// Sort the points and indexes from the v value of the points
std::sort(vec_ip_sorted.begin(), vec_ip_sorted.end(), compareImagePoint);
vec_ip_has_been_sorted = true;
}
// Use the two highest points for the first line, and the two others for the second line.
vpFeatureBuilder::create(s_vp, cam, vec_ip[vec_ip_sorted[0].first], vec_ip[vec_ip_sorted[1].first],
vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first],
task.set_cVe(cVe);
task.set_eJe(eJe);
// Compute the control law. Velocities are computed in the mobile robot reference frame
// Sending the control law to the drone
if (verbose) {
std::cout << "ve: " << ve.t() << std::endl;
}
drone.setVelocity(ve, 1.0);
for (size_t i = 0; i < 4; i++) {
vpDisplay::displayCross(I, vec_ip[i], 15, vpColor::red, 1);
std::stringstream ss;
ss << i;
vpDisplay::displayText(I, vec_ip[i] + vpImagePoint(15, 15), ss.str(), vpColor::green);
}
// Display visual features
vpDisplay::displayPolygon(I, vec_ip, vpColor::green, 3); // Current polygon used to compure an moment
3); // Current polygon used to compute a moment
vpDisplay::displayLine(I, 0, static_cast<int>(cam.get_u0()), static_cast<int>(I.getHeight()) - 1,
static_cast<int>(cam.get_u0()), vpColor::red,
3); // Vertical line as desired x position
vpDisplay::displayLine(I, static_cast<int>(cam.get_v0()), 0, static_cast<int>(cam.get_v0()),
static_cast<int>(I.getWidth()) - 1, vpColor::red,
3); // Horizontal line as desired y position
// Display lines corresponding to the vanishing point for the horizontal lines
vpDisplay::displayLine(I, vec_ip[vec_ip_sorted[0].first], vec_ip[vec_ip_sorted[1].first], vpColor::red, 1,
false);
vpDisplay::displayLine(I, vec_ip[vec_ip_sorted[2].first], vec_ip[vec_ip_sorted[3].first], vpColor::red, 1,
false);
} else {
std::stringstream sserr;
sserr << "Failed to detect an Apriltag, or detected multiple ones";
vpDisplay::displayText(I, 120, 20, sserr.str(), vpColor::red);
drone.stopMoving(); // In this case, we stop the drone
}
vpDisplay::displayText(I, 10, 10, "Click to exit", vpColor::red);
if (vpDisplay::getClick(I, false)) {
drone.land();
runLoop = false;
}
plotter.plot(0, iter, task.getError());
double totalTime = vpTime::measureTimeMs() - startTime;
std::stringstream sstime;
sstime << "Total time: " << totalTime << " ms";
vpDisplay::displayText(I, 80, 20, sstime.str(), vpColor::red);
iter++;
vpTime::wait(startTime, 40.0); // We wait a total of 40 milliseconds
}
return EXIT_SUCCESS;
} else {
std::cout << "ERROR : failed to setup drone control." << std::endl;
return EXIT_FAILURE;
}
} catch (const vpException &e) {
std::cout << "Caught an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#endif // #elif !defined(VISP_HAVE_FFMPEG)