Hey, I’m using FollowMe plugin to making one drone follow another and I don’t know how to stream drone position to TargetLocation; I know in Java i can use Flowables, but how to do that in C++?
1 Like
You can either loop and poll the position with position() or set it when you get a callback with the new position using subscribe_position().
There is also an example here: https://github.com/mavlink/MAVSDK/tree/develop/examples/follow_me
Are you trying to follow yourself with the drone? 
nah, I’m try to make one drone follow another; now after I’m starting FollowMe plugin, it stucks on
[09:27:59|Debug] Waiting for the system confirmation of the new configuration.. (follow_me_impl.cpp:93)
followMe plugin config:
followMeConfig.follow_distance_m = 5.0f;
followMeConfig.responsiveness = 0.2f;
followMeConfig.follow_direction = FollowMe::Config::FollowDirection::Behind;
Edit: this should work, I think.
Does the integration test work for you?
You can run it with:
cmake --build build/default -j8 && build/default/src/integration_tests/integration_tests_runner --gtest_filter="SitlTest.FollowMe*"
Tests failed…
Note: Google Test filter = SitlTest.FollowMe*
[==========] Running 2 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 2 tests from SitlTest
[ RUN ] SitlTest.FollowMeOneLocation
[12:24:43|Debug] Model chosen: 'iris' (integration_test_helper.h:59)
SITL model: iris
Not autostarting SITL (use AUTOSTART_SITL=1)
[12:24:43|Info ] MAVSDK version: 0.26.0-19-g10812a40 (mavsdk_impl.cpp:26)
[12:24:45|Debug] New: System ID: 0 Comp ID: 0 (mavsdk_impl.cpp:380)
/home/kris/MAVSDK/src/integration_tests/follow_me.cpp:34: Failure
Value of: system.has_autopilot()
Actual: false
Expected: true
[ FAILED ] SitlTest.FollowMeOneLocation (3012 ms)
[ RUN ] SitlTest.FollowMeMultiLocationWithConfig
[12:24:46|Debug] Model chosen: 'iris' (integration_test_helper.h:59)
SITL model: iris
Not autostarting SITL (use AUTOSTART_SITL=1)
[12:24:46|Info ] MAVSDK version: 0.26.0-19-g10812a40 (mavsdk_impl.cpp:26)
/home/kris/MAVSDK/src/integration_tests/follow_me.cpp:108: Failure
Value of: poll_condition_with_timeout( [&mavsdk]() { return mavsdk.is_connected(); }, std::chrono::seconds(10))
Actual: false
Expected: true
[ FAILED ] SitlTest.FollowMeMultiLocationWithConfig (12015 ms)
[----------] 2 tests from SitlTest (15027 ms total)
[----------] Global test environment tear-down
[==========] 2 tests from 1 test suite ran. (15027 ms total)
[ PASSED ] 0 tests.
[ FAILED ] 2 tests, listed below:
[ FAILED ] SitlTest.FollowMeOneLocation
[ FAILED ] SitlTest.FollowMeMultiLocationWithConfig
2 FAILED TESTS
So nothing is connecting. How are you connecting to th drone?
I just simulate them in Gazebo 
EDIT: I launched 1 drone in Gazebo, and tested it
Note: Google Test filter = SitlTest.FollowMe*
[==========] Running 2 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 2 tests from SitlTest
[ RUN ] SitlTest.FollowMeOneLocation
[06:50:56|Debug] Model chosen: 'iris' (integration_test_helper.h:59)
SITL model: iris
Not autostarting SITL (use AUTOSTART_SITL=1)
[06:50:56|Info ] MAVSDK version: 0.26.0-19-g10812a40 (mavsdk_impl.cpp:26)
[06:50:56|Info ] New system on: 127.0.0.1:14580 (udp_connection.cpp:256)
[06:50:56|Debug] New: System ID: 1 Comp ID: 1 (mavsdk_impl.cpp:380)
[06:50:56|Debug] Component Autopilot (1) added. (system_impl.cpp:352)
[06:50:56|Debug] Discovered 1 component(s) (UUID: 5283920058631409231) (system_impl.cpp:525)
waiting for system to be ready
[06:50:59|Debug] MAVLink: info: ARMED by Arm/Disarm component command (system_impl.cpp:257)
[06:50:59|Debug] MAVLink: info: Using minimum takeoff altitude: 2.50 m (system_impl.cpp:257)
[FlightMode: Takeoff] Vehicle is at Lat: nan deg, Lon: nan deg.
[06:51:00|Debug] MAVLink: info: Takeoff detected (system_impl.cpp:257)
[FlightMode: Takeoff] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Takeoff] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Hold] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Hold] Vehicle is at Lat: nan deg, Lon: nan deg.
Current FollowMe configuration of the system
---------------------------
Min Height: 8m
Distance: 8m
Responsiveness: 0.5
Following from: Behind
---------------------------
[FlightMode: Follow Me] Vehicle is at Lat: nan deg, Lon: nan deg.
We're waiting (for 5s) to see the drone moving target location set.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Hold] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Hold] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[06:51:12|Debug] MAVLink: info: Landing at current position (system_impl.cpp:257)
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[06:51:24|Debug] MAVLink: info: Landing detected (system_impl.cpp:257)
waiting for system to disarm
[06:51:24|Debug] MAVLink: info: DISARMED by Auto disarm initiated (system_impl.cpp:257)
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54564 deg.
[ OK ] SitlTest.FollowMeOneLocation (30112 ms)
[ RUN ] SitlTest.FollowMeMultiLocationWithConfig
[06:51:26|Debug] Model chosen: 'iris' (integration_test_helper.h:59)
SITL model: iris
Not autostarting SITL (use AUTOSTART_SITL=1)
[06:51:26|Info ] MAVSDK version: 0.26.0-19-g10812a40 (mavsdk_impl.cpp:26)
[06:51:26|Info ] New system on: 127.0.0.1:14580 (udp_connection.cpp:256)
[06:51:26|Debug] New: System ID: 1 Comp ID: 1 (mavsdk_impl.cpp:380)
[06:51:26|Debug] Component Autopilot (1) added. (system_impl.cpp:352)
[06:51:26|Debug] Discovered 1 component(s) (UUID: 5283920058631409231) (system_impl.cpp:525)
Waiting for system to be ready
Waiting for system to be ready
[06:51:29|Debug] MAVLink: info: ARMED by Arm/Disarm component command (system_impl.cpp:257)
[06:51:29|Debug] MAVLink: info: [logger] ./log/2020-06-08/04_51_29.ulg (system_impl.cpp:257)
[06:51:29|Debug] MAVLink: info: Using minimum takeoff altitude: 2.50 m (system_impl.cpp:257)
[FlightMode: Takeoff] Vehicle is at Lat: nan deg, Lon: nan deg.
[06:51:30|Debug] MAVLink: info: Takeoff detected (system_impl.cpp:257)
[FlightMode: Takeoff] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Takeoff] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Hold] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Hold] Vehicle is at Lat: nan deg, Lon: nan deg.
[06:51:34|Debug] Waiting for the system confirmation of the new configuration.. (follow_me_impl.cpp:93)
# of Target locations: 25 @ Rate: 1Hz.
[FlightMode: Follow Me] Vehicle is at Lat: nan deg, Lon: nan deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54567 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54569 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.5457 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54571 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54572 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54572 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54572 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54571 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.5457 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54568 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54567 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54564 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54552 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54559 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54557 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54554 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54553 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3997 deg, Lon: 8.5427 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3997 deg, Lon: 8.5427 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54555 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54554 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54554 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3978 deg, Lon: 8.54553 deg.
[FlightMode: Follow Me] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[FlightMode: Hold] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[FlightMode: Hold] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[06:52:01|Debug] MAVLink: info: Landing at current position (system_impl.cpp:257)
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[06:52:17|Debug] MAVLink: info: Landing detected (system_impl.cpp:257)
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[06:52:18|Debug] MAVLink: info: DISARMED by Auto disarm initiated (system_impl.cpp:257)
waiting for system to disarm
[FlightMode: Land] Vehicle is at Lat: 47.3977 deg, Lon: 8.54553 deg.
[ OK ] SitlTest.FollowMeMultiLocationWithConfig (54138 ms)
[----------] 2 tests from SitlTest (84250 ms total)
[----------] Global test environment tear-down
[==========] 2 tests from 1 test suite ran. (84251 ms total)
[ PASSED ] 2 tests.
Ok, it seems like that worked.
since after MAVSDK update FollowMe still doesn’t work I’ll send my whole code, because I don’t know what I’m doing wrong…
#include <chrono>
#include <future>
#include <iostream>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/offboard/offboard.h>
#include <mavsdk/plugins/follow_me/follow_me.h>
#include <mavsdk/mavsdk.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"
inline void handleActionError(Action::Result result, const string& message);
inline void handleFollowMeError(FollowMe::Result result, const string& message);
inline void handleOffboardError(Offboard::Result result, const string& message);
static int flyOffboard(System& system);
static int followDrone(System& leader, System& follower);
bool offboardTour(shared_ptr<Offboard> ob);
Telemetry::LandedStateCallback landedStateCallback(shared_ptr<Telemetry>& telemetry, promise<void>& promise);
Telemetry::PositionCallback positionCallback(shared_ptr<FollowMe>& followMe);
static int flyOffboard(System& system){
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 1 arming..."<<endl;
const Action::Result armResult = action->arm();
handleActionError(armResult,"ARM FAILED\n");
cout << "Drone 1 armed";
const Action::Result takeOffResult = action->takeoff();
handleActionError(takeOffResult,"TAKEOFF FAILED\n");
telemetry->subscribe_landed_state(landedStateCallback(telemetry,inAirPromise));
inAirFuture.wait();
bool ret= offboardTour(offboard);
if (!ret){
return EXIT_FAILURE;
}
const Action::Result landResult = action->land();
handleActionError(landResult,"LANDING FAILED\n");
while (telemetry->in_air()) {
cout << "Landing..." << endl;
sleep_for(seconds(1));
}
cout << "Landed!" << endl;
sleep_for(seconds(3));
cout << "Bye! ;)" << endl;
return EXIT_SUCCESS;
}
static int followDrone(System& leader, System& follower){
auto followMe = make_shared<FollowMe>(follower);
auto action = make_shared<Action>(follower);
auto followerTelemetry = make_shared<Telemetry>(follower);
auto leaderTelemetry = make_shared<Telemetry>(leader);
Telemetry::Result setRateResult = followerTelemetry->set_rate_position(1);
if(setRateResult!=Telemetry::Result::Success){
cout << ERROR_CONSOLE_TEXT
<< "Setting rate failed:" << setRateResult
<< NORMAL_CONSOLE_TEXT << endl;
return EXIT_FAILURE;
}
setRateResult = leaderTelemetry->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 (!followerTelemetry->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";
const Action::Result takeOffResult = action->takeoff();
handleActionError(takeOffResult,"TAKEOFF FAILED\n");
followerTelemetry->subscribe_landed_state(landedStateCallback(followerTelemetry, inAirPromise));
inAirFuture.wait();
FollowMe::Config followMeConfig;
followMeConfig.follow_distance_m = 5.0f;
followMeConfig.responsiveness = 0.2f;
followMeConfig.follow_direction = FollowMe::Config::FollowDirection::Behind;
FollowMe::Result configResult = followMe->set_config(followMeConfig);
if(configResult!=FollowMe::Result::Success){
cout << ERROR_CONSOLE_TEXT
<< "Setting configuration failed:" << configResult
<< NORMAL_CONSOLE_TEXT << endl;
return EXIT_FAILURE;
}
sleep_for(seconds(1));
FollowMe::Result followMeResult = followMe->start();
handleFollowMeError(followMeResult,"FOLLOWME PLUGIN FAILED\n");
leaderTelemetry->subscribe_position(positionCallback(followMe));
//followMe->set_target_location();
return 0;
}
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 handleFollowMeError(FollowMe::Result result, const string& message){
if(result != FollowMe::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);
}
};
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;
}
};
}
Telemetry::PositionCallback positionCallback(shared_ptr<FollowMe>& followMe){
return [&followMe](Telemetry::Position position){
FollowMe::TargetLocation target{};
target.latitude_deg = position.latitude_deg;
target.longitude_deg = position.longitude_deg;
target.absolute_altitude_m = position.absolute_altitude_m;
followMe->set_target_location(target);
cout<<target.latitude_deg<<target.longitude_deg<<target.absolute_altitude_m<<endl;
};
}
bool offboardTour(shared_ptr<Offboard> ob){
Offboard::VelocityNedYaw point;
point.north_m_s = 0;
point.east_m_s = 0;
point.down_m_s = 0;
point.yaw_deg = 0;
ob->set_velocity_ned(point);
Offboard::Result obResult = ob->start();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
cout<<"Offboard started"<<endl;
point.down_m_s = -5;
ob->set_velocity_ned(point);
sleep_for(seconds(1));
point.down_m_s = 0;
point.east_m_s = 5;
point.yaw_deg = 90;
ob->set_velocity_ned(point);
sleep_for(seconds(5));
point.north_m_s = -5;
point.east_m_s = 0;
point.yaw_deg = 180;
ob->set_velocity_ned(point);
sleep_for(seconds(5));
point.north_m_s = 0;
point.east_m_s = -5;
point.yaw_deg = 270;
ob->set_velocity_ned(point);
sleep_for(seconds(5));
point.north_m_s = 5;
point.east_m_s = 0;
point.yaw_deg = 0;
ob->set_velocity_ned(point);
sleep_for(seconds(5));
obResult = ob->stop();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
cout<<"Offboard stopped"<<endl;
return true;
}
int main() {
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(&flyOffboard, ref(system1));
thread t2(&followDrone, ref(system1),ref(system2));
//t1.join();
threads.push_back(move(t1));
threads.push_back(move(t2));
for (auto& t : threads){
t.join();
}
return 0;
}
Ok, when you say “it doesn’t work” what happens exactly?
The drone, which should follow the other one, just stops instead of that; in command line FollowMe stops at
[07:49:42|Debug] Waiting for the system confirmation of the new configuration.. (follow_me_impl.cpp:93)
and positionCallback is not working…
Ok, have you made sure that the follow me example for just one drone works properly?
just checked and example works fine; IMHO somewhere in my code is a error…
I think you’re locking yourself out somehow.
I suggest to do the following.
Start the application with gdb, so gdb --args yourapp --your_args, then do run and wait until it is stuck.
Then you hit Ctrl+C, and type: thread apply all backtrace, and then you copy the output here.
ok, since it’s multi thread app, something could escape the debugger; nonetheless here’s the output:
Thread 12 (Thread 0x7fffecff9700 (LWP 3900)):
#0 0x00007ffff6ab8c60 in __GI___nanosleep (requested_time=0x7fffecff8c30,
remaining=0x7fffecff8c30) at ../sysdeps/unix/sysv/linux/nanosleep.c:28
#1 0x0000555555577405 in std::this_thread::sleep_for<long, std::ratio<1l, 1l> > (__rtime=...) at /usr/include/c++/7/thread:373
#2 0x00005555555741b0 in offboardTour (
ob=std::shared_ptr<mavsdk::Offboard> (use count 2, weak count 0) = {...})
at /home/kris/CLionProjects/twoDrones/main.cpp:229
#3 0x0000555555573205 in flyOffboard (system=...)
at /home/kris/CLionProjects/twoDrones/main.cpp:66
#4 0x0000555555578dce in std::__invoke_impl<int, int (*)(mavsdk::System&), std::reference_wrapper<mavsdk::System> > (
__f=@0x55555579b8e0: 0x555555572f05 <flyOffboard(mavsdk::System&)>)
at /usr/include/c++/7/bits/invoke.h:60
#5 0x00005555555777ec in std::__invoke<int (*)(mavsdk::System&), std::reference_wrapper<mavsdk::System> > (
__fn=@0x55555579b8e0: 0x555555572f05 <flyOffboard(mavsdk::System&)>)
at /usr/include/c++/7/bits/invoke.h:96
#6 0x000055555557e433 in std::thread::_Invoker<std::tuple<int (*)(mavsdk::System&), std::reference_wrapper<mavsdk::System> > >::_M_invoke<0ul, 1ul> (
this=0x55555579b8d8) at /usr/include/c++/7/thread:234
---Type <return> to continue, or q <return> to quit---
m&), std::reference_wrapper<mavsdk::System> > >::operator() (this=0x55555579b8d8) at /usr/include/c++/7/thread:243
#8 0x000055555557e0b4 in std::thread::_State_impl<std::thread::_Invoker<std::tuple<int (*)(mavsdk::System&), std::reference_wrapper<mavsdk::System> > > >::_M_run (this=0x55555579b8d0)
at /usr/include/c++/7/thread:186
#9 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#10 0x00007ffff6aae6db in start_thread (arg=0x7fffecff9700) at pthread_create.c:463
#11 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 11 (Thread 0x7fffed7fa700 (LWP 3881)):
#0 0x00007ffff6ab49f3 in futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x7fffe80012f8) at ../sysdeps/unix/sysv/linux/futex-internal.h:88
#1 __pthread_cond_wait_common (abstime=0x0, mutex=0x7fffe80012a8, cond=0x7fffe80012d0) at pthread_cond_wait.c:502
#2 __pthread_cond_wait (cond=0x7fffe80012d0, mutex=0x7fffe80012a8) at pthread_cond_wait.c:655
#3 0x00007ffff6f958bc in std::condition_variable::wait(std::unique_lock<std::mutex>&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff7304369 in mavsdk::ThreadPool::worker() () from /usr/local/lib/libmavsdk.so
#5 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007ffff6aae6db in start_thread (arg=0x7fffed7fa700) at pthread_create.c:463
#7 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 10 (Thread 0x7fffedffb700 (LWP 3880)):
#0 0x00007ffff6ab49f3 in futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x7fffe80012fc) at ../sysdeps/unix/sysv/linux/futex-internal.h:88
#1 __pthread_cond_wait_common (abstime=0x0, mutex=0x7fffe80012a8, cond=0x7fffe80012d0) at pthread_cond_wait.c:502
#2 __pthread_cond_wait (cond=0x7fffe80012d0, mutex=0x7fffe80012a8) at pthread_cond_wait.c:655
#3 0x00007ffff6f958bc in std::condition_variable::wait(std::unique_lock<std::mutex>&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff7304369 in mavsdk::ThreadPool::worker() () from /usr/local/lib/libmavsdk.so
#5 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007ffff6aae6db in start_thread (arg=0x7fffedffb700) at pthread_create.c:463
#7 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 9 (Thread 0x7fffee7fc700 (LWP 3879)):
#0 0x00007ffff6ab49f3 in futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x7fffe80012fc) at ../sysdeps/unix/sysv/linux/futex-internal.h:88
#1 __pthread_cond_wait_common (abstime=0x0, mutex=0x7fffe80012a8, cond=0x7fffe80012d0) at pthread_cond_wait.c:502
#2 __pthread_cond_wait (cond=0x7fffe80012d0, mutex=0x7fffe80012a8) at pthread_cond_wait.c:655
#3 0x00007ffff6f958bc in std::condition_variable::wait(std::unique_lock<std::mutex>&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff7304369 in mavsdk::ThreadPool::worker() () from /usr/local/lib/libmavsdk.so
#5 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007ffff6aae6db in start_thread (arg=0x7fffee7fc700) at pthread_create.c:463
#7 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 8 (Thread 0x7fffeeffd700 (LWP 3878)):
#0 0x00007ffff6ab8c60 in __GI___nanosleep (requested_time=0x7fffeeffce90, remaining=0x7fffeeffce90) at ../sysdeps/unix/sysv/linux/nanosleep.c:28
#1 0x00007ffff72a37ba in mavsdk::SystemImpl::system_thread() () from /usr/local/lib/libmavsdk.so
#2 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3 0x00007ffff6aae6db in start_thread (arg=0x7fffeeffd700) at pthread_create.c:463
#4 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 7 (Thread 0x7fffef7fe700 (LWP 3877)):
#0 0x00007ffff6ab49f3 in futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x7ffff00012f8) at ../sysdeps/unix/sysv/linux/futex-internal.h:88
#1 __pthread_cond_wait_common (abstime=0x0, mutex=0x7ffff00012a8, cond=0x7ffff00012d0) at pthread_cond_wait.c:502
#2 __pthread_cond_wait (cond=0x7ffff00012d0, mutex=0x7ffff00012a8) at pthread_cond_wait.c:655
#3 0x00007ffff6f958bc in std::condition_variable::wait(std::unique_lock<std::mutex>&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff7304369 in mavsdk::ThreadPool::worker() () from /usr/local/lib/libmavsdk.so
#5 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007ffff6aae6db in start_thread (arg=0x7fffef7fe700) at pthread_create.c:463
#7 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
---Type <return> to continue, or q <return> to quit---
Thread 6 (Thread 0x7fffeffff700 (LWP 3876)):
#0 0x00007ffff6ab49f3 in futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x7ffff00012f8) at ../sysdeps/unix/sysv/linux/futex-internal.h:88
#1 __pthread_cond_wait_common (abstime=0x0, mutex=0x7ffff00012a8, cond=0x7ffff00012d0) at pthread_cond_wait.c:502
#2 __pthread_cond_wait (cond=0x7ffff00012d0, mutex=0x7ffff00012a8) at pthread_cond_wait.c:655
#3 0x00007ffff6f958bc in std::condition_variable::wait(std::unique_lock<std::mutex>&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff7304369 in mavsdk::ThreadPool::worker() () from /usr/local/lib/libmavsdk.so
#5 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007ffff6aae6db in start_thread (arg=0x7fffeffff700) at pthread_create.c:463
#7 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 5 (Thread 0x7ffff4b14700 (LWP 3875)):
#0 0x00007ffff6ab49f3 in futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x7ffff00012fc) at ../sysdeps/unix/sysv/linux/futex-internal.h:88
#1 __pthread_cond_wait_common (abstime=0x0, mutex=0x7ffff00012a8, cond=0x7ffff00012d0) at pthread_cond_wait.c:502
#2 __pthread_cond_wait (cond=0x7ffff00012d0, mutex=0x7ffff00012a8) at pthread_cond_wait.c:655
#3 0x00007ffff6f958bc in std::condition_variable::wait(std::unique_lock<std::mutex>&) () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff7304369 in mavsdk::ThreadPool::worker() () from /usr/local/lib/libmavsdk.so
#5 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007ffff6aae6db in start_thread (arg=0x7ffff4b14700) at pthread_create.c:463
#7 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 4 (Thread 0x7ffff5315700 (LWP 3874)):
#0 0x00007ffff6ab8c60 in __GI___nanosleep (requested_time=0x7ffff5314e90, remaining=0x7ffff5314e90) at ../sysdeps/unix/sysv/linux/nanosleep.c:28
#1 0x00007ffff72a37ba in mavsdk::SystemImpl::system_thread() () from /usr/local/lib/libmavsdk.so
#2 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3 0x00007ffff6aae6db in start_thread (arg=0x7ffff5315700) at pthread_create.c:463
#4 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 3 (Thread 0x7ffff5b16700 (LWP 3873)):
#0 0x00007ffff6ab88f2 in __libc_recvfrom (fd=4, buf=0x7ffff5b15690, len=2048, flags=0, addr=..., addrlen=0x7ffff5b153fc) at ../sysdeps/unix/sysv/linux/recvfrom.c:27
#1 0x00007ffff72fbc4a in mavsdk::UdpConnection::receive() () from /usr/local/lib/libmavsdk.so
#2 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3 0x00007ffff6aae6db in start_thread (arg=0x7ffff5b16700) at pthread_create.c:463
#4 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 2 (Thread 0x7ffff6317700 (LWP 3872)):
#0 0x00007ffff6ab88f2 in __libc_recvfrom (fd=3, buf=0x7ffff6316690, len=2048, flags=0, addr=..., addrlen=0x7ffff63163fc) at ../sysdeps/unix/sysv/linux/recvfrom.c:27
#1 0x00007ffff72fbc4a in mavsdk::UdpConnection::receive() () from /usr/local/lib/libmavsdk.so
#2 0x00007ffff6f9b6df in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3 0x00007ffff6aae6db in start_thread (arg=0x7ffff6317700) at pthread_create.c:463
#4 0x00007ffff67d788f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Thread 1 (Thread 0x7ffff7fac740 (LWP 3867)):
#0 0x00007ffff6aafd2d in __GI___pthread_timedjoin_ex (threadid=140737169561344, thread_return=0x0, abstime=0x0, block=<optimized out>) at pthread_join_common.c:89
#1 0x00007ffff6f9b933 in std::thread::join() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#2 0x00005555555748ae in main () at /home/kris/CLionProjects/twoDrones/main.cpp:296
I see that it’s going through your offboardTour function but I doubt that’s where it actually locks, right?
yep, IMO problem is in function followDrone at line
leaderTelemetry->subscribe_position(positionCallback(followMe));
it’s supposed to take position from leader drone for making targetPosition for Follow me plugin of follower drone…
the whole problem is I don’t know how to make subsrcibe_position work…
Is it never called? Or only once?
when debugged in CLion, it once goes to line leaderTelemetry->subscribe_position(positionCallback(followMe));, where callback is positioned but it doesn’t go to breakpoint in positionCallback function…