Hello, when I’m running attached code, a system1
drone in potentialField
instead to giving their position as in friendTelemetry->subscribe_position
is enabling failsafe
How to fix it?
code:
#include <future>
#include <iostream>
#include <mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/offboard/offboard.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
using namespace std;
using namespace mavsdk;
using chrono::seconds;
using this_thread::sleep_for;
#define ERROR_CONSOLE_TEXT "\033[31m"
#define NORMAL_CONSOLE_TEXT "\033[0m"
bool friendDroneOnPosition=false;
inline void handleActionError(Action::Result result, const string& message);
inline void handleOffboardError(Offboard::Result result, const string& message);
Telemetry::LandedStateCallback landedStateCallback(shared_ptr<Telemetry>& telemetry, promise<void>& promise);
static int goToStart(System& system1, System& system2, float heading,int chosenAlgorithm);
static int foeProgram(System& system, float heading);
static int potentialField(System& system1, System& system2);
inline void handleActionError(Action::Result result, const string& message){
if(result != Action::Result::Success){
cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT<< endl;
exit(EXIT_FAILURE);
}
};
inline void handleOffboardError(Offboard::Result result, const string& message){
if(result != Offboard::Result::Success){
cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT<< endl;
exit(EXIT_FAILURE);
}
};
static double computeDistance(Telemetry::Position position1, Telemetry::Position position2) {
double horizontalDistanceSquared = pow(position1.longitude_deg-position2.longitude_deg, 2.0) + pow(position1.latitude_deg-position2.latitude_deg, 2.0);
return sqrt(horizontalDistanceSquared + pow(position1.relative_altitude_m-position2.relative_altitude_m, 2.0));
}
static vector<double> computeVector(Telemetry::Position position1, Telemetry::Position position2) {
vector<double> result;
result.push_back(position2.longitude_deg - position1.longitude_deg);
result.push_back(position2.latitude_deg - position1.latitude_deg);
result.push_back(position2.relative_altitude_m - position1.relative_altitude_m);
return result;
}
static int potentialField(System& system1, System& system2){
auto action = make_shared<Action>(system1);
auto offboard = make_shared<Offboard>(system1);
auto friendTelemetry = make_shared<Telemetry>(system1);
auto foeTelemetry = make_shared<Telemetry>(system2);
auto prom = std::make_shared<std::promise<Telemetry::Result>>();
auto future_result = prom->get_future();
const Telemetry::Result set_friend_rate_result = friendTelemetry->set_rate_position(25.0);
if (set_friend_rate_result != Telemetry::Result::Success) {
std::cout << "Setting rate failed:" << set_friend_rate_result << std::endl;
}
const Telemetry::Result set_foe_rate_result = foeTelemetry->set_rate_position(25.0);
if (set_foe_rate_result != Telemetry::Result::Success) {
std::cout << "Setting rate failed:" << set_foe_rate_result << std::endl;
}
Telemetry::Position startPosition = friendTelemetry->position();
Telemetry::Position targetPosition;
targetPosition.longitude_deg = startPosition.longitude_deg;
targetPosition.latitude_deg = startPosition.latitude_deg+(30.0/111000);
Telemetry::Position foePosition = foeTelemetry->position();
Offboard::VelocityBodyYawspeed velocityBodyYawspeed;
velocityBodyYawspeed.down_m_s = 0;
velocityBodyYawspeed.forward_m_s = 0;
velocityBodyYawspeed.right_m_s = 0;
velocityBodyYawspeed.yawspeed_deg_s = 0;
offboard->set_velocity_body(velocityBodyYawspeed);
/*Offboard::Result obResult = offboard->start();
handleOffboardError(obResult,"OFFBOARD FAILED\n");*/
int foeTicks=0;
foeTelemetry->subscribe_position([&foePosition,&foeTicks](Telemetry::Position position){
foePosition = position;
foeTicks++;
});
friendTelemetry->subscribe_position([&friendTelemetry,&targetPosition, //this isn't working
&foePosition,&offboard,&action](Telemetry::Position position){
const double epsilon = -1;
const double mi = 1;
if(computeDistance(position,targetPosition)>=0.1){
double distanceToTarget = computeDistance(position,targetPosition);
vector<double> distanceToTargetVector = computeVector(position,targetPosition),
attractiveForce(3),repulsiveForce(3),forcesVector(3);
attractiveForce[0] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[0]);
attractiveForce[1] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[1]);
attractiveForce[2] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[2]);
double distanceToFoe = computeDistance(position, foePosition);
vector<double> distanceToFoeVector = computeVector(position, foePosition);
repulsiveForce[0] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[0]));
repulsiveForce[1] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[1]));
repulsiveForce[2] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[2]));
forcesVector[0] = attractiveForce[0] + repulsiveForce[0];
forcesVector[1] = attractiveForce[1] + repulsiveForce[1];
forcesVector[2] = attractiveForce[2] + repulsiveForce[2];
Offboard::VelocityBodyYawspeed velocityBodyYawspeed;
velocityBodyYawspeed.forward_m_s=5;
velocityBodyYawspeed.right_m_s = 5/(forcesVector[1]/forcesVector[0]);
velocityBodyYawspeed.down_m_s = 5/(forcesVector[1]/forcesVector[2]);
offboard->set_velocity_body(velocityBodyYawspeed);
} else {
cout<<"We're Arrived"<<endl;
//Offboard::Result obResult = offboard->stop();
//handleOffboardError(obResult,"OFFBOARD FAILED\n");
const Action::Result landResult = action->land();
handleActionError(landResult,"LANDING FAILED\n");
while (friendTelemetry->in_air()) {
cout << "Landing..." << endl;
sleep_for(seconds(1));
}
cout << "Drone 1 Landed!" << endl;
friendTelemetry->subscribe_position(nullptr);
sleep_for(seconds(3));
cout << "Bye! ;)" << endl;
}
}
/*[&action,&targetPosition,&offboard,&foeTelemetry](Telemetry::Position position){
const double epsilon = -1;
const double mi = 1;
if(computeDistance(position,targetPosition)>=0.1){
double distanceToTarget = computeDistance(position,targetPosition);
vector<double> distanceToTargetVector = computeVector(position,targetPosition),
attractiveForce(3),repulsiveForce(3),forcesVector(3);
attractiveForce[0] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[0]);
attractiveForce[1] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[1]);
attractiveForce[2] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[2]);
Telemetry::Position foePosition = foeTelemetry->position();
double distanceToFoe = computeDistance(position, foePosition);
if(distanceToFoe<=5){
vector<double> distanceToFoeVector = computeVector(position, foePosition);
repulsiveForce[0] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[0]));
repulsiveForce[1] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[1]));
repulsiveForce[2] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[2]));
} else {
repulsiveForce[0] = 0;
repulsiveForce[1] = 0;
repulsiveForce[2] = 0;
}
forcesVector[0] = attractiveForce[0] + repulsiveForce[0];
forcesVector[1] = attractiveForce[1] + repulsiveForce[1];
forcesVector[2] = attractiveForce[2] + repulsiveForce[2];
Offboard::VelocityBodyYawspeed velocityBodyYawspeed;
velocityBodyYawspeed.forward_m_s=5;
velocityBodyYawspeed.right_m_s = 5/(forcesVector[1]/forcesVector[0]);
velocityBodyYawspeed.down_m_s = 5/(forcesVector[1]/forcesVector[2]);
offboard->set_velocity_body(velocityBodyYawspeed);
} else {
cout<<"We're Arrived"<<endl;
Offboard::Result obResult = offboard->stop();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
const Action::Result landResult = action->land();
handleActionError(landResult,"LANDING FAILED\n");
}
}*/);
/*while (friendTelemetry->in_air()) {
cout << "Landing..." << endl;
sleep_for(seconds(1));
}
cout << "Drone 1 Landed!" << endl;
friendTelemetry->subscribe_position(nullptr);
sleep_for(seconds(3));
cout << "Bye! ;)" << endl;*/
//cout<<"foe ticks: "<<foeTicks<<endl;
return EXIT_SUCCESS;
}
static int goToStart(System& system1, System& system2, float heading, int chosenAlgorithm){
double sinus,cosinus;
if(heading==0) {
sinus = 0;
cosinus = 1;
} else if (heading == 90) {
sinus = 1;
cosinus = 0;
}
else if (heading == 180) {
sinus = 0;
cosinus = -1;
}
else if (heading == 270) {
sinus = -1;
cosinus = 0;
}
else {
sinus = sin((double)heading / 360 * 2 * M_PI);
cosinus = cos((double)heading / 360 * 2 * M_PI);
}
auto action = make_shared<Action>(system1);
auto offboard = make_shared<Offboard>(system1);
auto telemetry = make_shared<Telemetry>(system1);
const Telemetry::Result setRateResult = telemetry->set_rate_position(1);
if(setRateResult!=Telemetry::Result::Success){
cout << ERROR_CONSOLE_TEXT
<< "Setting rate failed:" << setRateResult
<< NORMAL_CONSOLE_TEXT << endl;
return EXIT_FAILURE;
}
while (!telemetry->health_all_ok()) {
sleep_for(seconds(1));
}
promise<void> inAirPromise;
auto inAirFuture = inAirPromise.get_future();
cout << "Drone 1 arming..."<<endl;
const Action::Result armResult = action->arm();
handleActionError(armResult,"ARM FAILED\n");
cout << "Drone 1 armed"<<endl;
const Action::Result takeOffResult = action->takeoff();
handleActionError(takeOffResult,"TAKEOFF FAILED\n");
telemetry->subscribe_landed_state(landedStateCallback(telemetry,inAirPromise));
inAirFuture.wait();
Offboard::PositionNedYaw point;
point.north_m = 0;
point.east_m = 0;
point.down_m = 0;
point.yaw_deg = 0;
offboard->set_position_ned(point);
Offboard::Result obResult = offboard->start();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
point.north_m = -15*cosinus;
point.east_m = -15*sinus;
point.down_m = -10;
point.yaw_deg = heading;
offboard->set_position_ned(point);
sleep_for(seconds(15));
/*obResult = offboard->stop();
handleOffboardError(obResult,"OFFBOARD FAILED\n");*/
switch(chosenAlgorithm){
case 1:
friendDroneOnPosition=true;
potentialField(system1,system2);
break;
case 2:
cout << "WIP";
break;
case 3:
cout << "WIP";
break;
}
return EXIT_SUCCESS;
}
static int foeProgram(System& system, float heading){
double sinus,cosinus;
if(heading==0) {
sinus = 0;
cosinus = 1;
} else if (heading == 90) {
sinus = 1;
cosinus = 0;
}
else if (heading == 180) {
sinus = 0;
cosinus = -1;
}
else if (heading == 270) {
sinus = -1;
cosinus = 0;
}
else {
sinus = sin((double)heading / 360 * 2 * M_PI);
cosinus = cos((double)heading / 360 * 2 * M_PI);
}
auto action = make_shared<Action>(system);
auto offboard = make_shared<Offboard>(system);
auto telemetry = make_shared<Telemetry>(system);
const Telemetry::Result setRateResult = telemetry->set_rate_position(1);
if(setRateResult!=Telemetry::Result::Success){
cout << ERROR_CONSOLE_TEXT
<< "Setting rate failed:" << setRateResult
<< NORMAL_CONSOLE_TEXT << endl;
return EXIT_FAILURE;
}
while (!telemetry->health_all_ok()) {
sleep_for(seconds(1));
}
promise<void> inAirPromise;
auto inAirFuture = inAirPromise.get_future();
cout << "Drone 2 arming..."<<endl;
const Action::Result armResult = action->arm();
handleActionError(armResult,"ARM FAILED\n");
cout << "Drone 2 armed"<<endl;
const Action::Result takeOffResult = action->takeoff();
handleActionError(takeOffResult,"TAKEOFF FAILED\n");
telemetry->subscribe_landed_state(landedStateCallback(telemetry,inAirPromise));
inAirFuture.wait();
Offboard::PositionNedYaw point;
point.north_m = 0;
point.east_m = 0;
point.down_m = 0;
point.yaw_deg = 0;
offboard->set_position_ned(point);
Offboard::Result obResult = offboard->start();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
point.north_m = -15*cosinus;
point.east_m = -15*sinus;
point.down_m = -10;
point.yaw_deg = heading;
offboard->set_position_ned(point);
//sleep_for(seconds(15));
point.north_m = 15*cosinus;
point.east_m = 15*sinus;
while(!friendDroneOnPosition){
cout<<"Waiting on friend drone..."<<endl;
sleep_for(seconds(1));
}
offboard->set_position_ned(point);
sleep_for(seconds(15));
//obResult = offboard->stop();
//handleOffboardError(obResult,"OFFBOARD FAILED\n");
const Action::Result landResult = action->land();
handleActionError(landResult,"LANDING FAILED\n");
while (telemetry->in_air()) {
cout << "Drone 2 Landing..." << endl;
sleep_for(seconds(1));
}
cout << "Drone 2 Landed!" << endl;
sleep_for(seconds(3));
cout << "Bye! ;)" << endl;
return EXIT_SUCCESS;
}
Telemetry::LandedStateCallback landedStateCallback(shared_ptr<Telemetry>& telemetry, promise<void>& promise) {
return [&promise,&telemetry](Telemetry::LandedState landed){
switch (landed){
case Telemetry::LandedState::OnGround:
std::cout << "On ground" << std::endl;
break;
case Telemetry::LandedState::TakingOff:
std::cout << "Taking off..." << std::endl;
break;
case Telemetry::LandedState::Landing:
std::cout << "Landing..." << std::endl;
break;
case Telemetry::LandedState::InAir:
std::cout << "Taking off has finished." << std::endl;
telemetry->subscribe_landed_state(nullptr);
promise.set_value();
break;
case Telemetry::LandedState::Unknown:
std::cout << "Unknown landed state." << std::endl;
break;
}
};
}
int main() {
int chosenTest = 0, chosenAlgorithm=0;
do {
cout<<"Jaki test chcesz wykonać?\n"<<
"1) Wrogi dron przelatuje równolegle\n"<<
"2) Wrogi dron przelatuje prostopadle\n"<<
"Twój wybór: ";
cin>>chosenTest;
} while (chosenTest!=1 && chosenTest!=2);
do {
cout<<"Jaki algorytm ma być sprawdzony?\n"<<
"1) Pól potencjałów\n"<<
"2) Monte Carlo\n"<<
"3) Podejście od strony szybkości\n"<<
"Twój wybór: ";
cin>>chosenAlgorithm;
} while (chosenAlgorithm!=1 && chosenAlgorithm!=2 && chosenAlgorithm!=3);
float foeHeading;
if(chosenTest==1)
foeHeading=180;
else if(chosenTest==2)
foeHeading=90;
Mavsdk dc;
ConnectionResult connectionResult[2];
connectionResult[0] = dc.add_any_connection("udp://127.0.0.1:14540");
connectionResult[1] = dc.add_any_connection("udp://127.0.0.1:14541");
for (auto & i : connectionResult){
if (i != ConnectionResult::Success) {
cerr << ERROR_CONSOLE_TEXT
<< "Connection failed: " << i
<< NORMAL_CONSOLE_TEXT << endl;
return 1;
}
}
atomic<signed> systemsCount{0};
cout << "Waiting to discover systems..." << endl;
vector<uint64_t> uuids;
dc.register_on_discover([&systemsCount,&uuids](uint64_t uuid) {
cout << "Discovered system with UUID: " << uuid << endl;
uuids.push_back(uuid);
systemsCount++;
});
sleep_for(seconds(5));
if (systemsCount != 2) {
cerr << ERROR_CONSOLE_TEXT << "Not all systems found, exiting." << NORMAL_CONSOLE_TEXT
<< endl;
return 1;
}
vector<thread> threads;
System& system1 = dc.system(uuids[0]);
System& system2 = dc.system(uuids[1]);
thread t1(&goToStart,ref(system1),ref(system2),0,chosenAlgorithm);
thread t2(&foeProgram, ref(system2),foeHeading);
threads.push_back(move(t1));
threads.push_back(move(t2));
for (auto& t : threads){
t.join();
}
return 0;
}