Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adding FollowMe plugin #142

Merged
merged 100 commits into from
Dec 1, 2017
Merged

Adding FollowMe plugin #142

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
100 commits
Select commit Hold shift + click to select a range
922f695
Implementation of Follow Me mode.
Oct 16, 2017
0e1d251
Adding FollowMe plugin
Oct 25, 2017
b56da72
Adding FollowMe plugin
Oct 25, 2017
8ac0ca1
follow_me: ran fix_style
julianoes Oct 26, 2017
559d389
Refactored FollowMe plugin.
Oct 27, 2017
ace4d0b
follow_me: fixed type conversion error.
Oct 27, 2017
0710365
follow_me: fixed error "implicit conversion from ‘float’ to ‘double’"
Oct 27, 2017
84a5ec4
follow_me: ran `make fix_style`
Oct 27, 2017
a71893a
followme: hope this should fix "conversion from `double` to `uint64_t`"
Oct 27, 2017
df5f81e
converted elapsed time to milliseconds. some minor change in example.
Oct 27, 2017
71de007
Implementation of Follow Me mode.
Oct 16, 2017
46c4d23
Adding FollowMe plugin
Oct 25, 2017
f300155
Adding FollowMe plugin
Oct 25, 2017
3deee7e
follow_me: ran fix_style
julianoes Oct 26, 2017
6a71d0a
Refactored FollowMe plugin.
Oct 27, 2017
b18bcc8
follow_me: fixed type conversion error.
Oct 27, 2017
a7c1fa5
follow_me: fixed error "implicit conversion from ‘float’ to ‘double’"
Oct 27, 2017
4f6512f
follow_me: ran `make fix_style`
Oct 27, 2017
28720f0
followme: hope this should fix "conversion from `double` to `uint64_t`"
Oct 27, 2017
98e8408
converted elapsed time to milliseconds. some minor change in example.
Oct 27, 2017
fb0a6d0
integration_tests: Gimbal entry was missed accidently. Restored now.
Oct 27, 2017
7100d4c
follow_me: Resolved most of the review comments.
Oct 30, 2017
f412ce6
follow_me: fixed ununsed const variable
Oct 30, 2017
817ae09
ran `make fix_style`
Oct 30, 2017
c0b6aec
follow_me: Refactoring
Nov 3, 2017
0f2fc8d
follow_me: making drone to oscillate around a mocked GCS position
Nov 3, 2017
d08d0da
core: added get elapsed time in msec: needed for mavlink commands.
Nov 9, 2017
d133418
Merge branch 'follow_me' of https://github.com/dronecore/DroneCore in…
Nov 10, 2017
53860c0
Merge branch 'develop' of https://github.com/dronecore/DroneCore into…
Nov 10, 2017
9334259
Restored integration_tests/CMakeLists.txt
Nov 10, 2017
248d951
integration_tests: follow_me changes
Nov 10, 2017
eecb514
travis-ci fix: included <array>
Nov 10, 2017
a614eed
Merge branch 'develop' into follow_me
Nov 13, 2017
f704978
Merge branch 'develop' into follow_me
Nov 14, 2017
4dd5d67
Resolved review comments.
Nov 13, 2017
5f100c7
integration_tests: indentation
Nov 14, 2017
08c5df7
follow_me: fixed Windows build error: conversion from 'double' to 'in…
Nov 14, 2017
d4a5c28
Fixed Windows build error, removed repeated timer registration.
Nov 14, 2017
83ef11b
PX4 param used for FollowMe Min Height was misspelled. Corrected it.
Nov 14, 2017
6c44ab4
Implementation of Follow Me mode.
Oct 16, 2017
4e8fae0
Adding FollowMe plugin
Oct 25, 2017
2c85bef
Adding FollowMe plugin
Oct 25, 2017
5a75f39
follow_me: ran fix_style
julianoes Oct 26, 2017
f061886
Refactored FollowMe plugin.
Oct 27, 2017
a579047
follow_me: fixed type conversion error.
Oct 27, 2017
e8d7273
follow_me: fixed error "implicit conversion from ‘float’ to ‘double’"
Oct 27, 2017
e812480
follow_me: ran `make fix_style`
Oct 27, 2017
fb381cf
followme: hope this should fix "conversion from `double` to `uint64_t`"
Oct 27, 2017
6aaea69
converted elapsed time to milliseconds. some minor change in example.
Oct 27, 2017
69f6ad3
Refactored FollowMe plugin.
Oct 27, 2017
33c37ed
follow_me: Resolved most of the review comments.
Oct 30, 2017
6fe7100
follow_me: fixed ununsed const variable
Oct 30, 2017
62df17f
ran `make fix_style`
Oct 30, 2017
19c7692
follow_me: Refactoring
Nov 3, 2017
31ed769
follow_me: making drone to oscillate around a mocked GCS position
Nov 3, 2017
d68baf0
core: added get elapsed time in msec: needed for mavlink commands.
Nov 9, 2017
1517860
integration_tests: follow_me changes
Nov 10, 2017
87579cd
travis-ci fix: included <array>
Nov 10, 2017
0ebf6aa
Resolved review comments.
Nov 13, 2017
bfdfa41
integration_tests: indentation
Nov 14, 2017
a668194
follow_me: fixed Windows build error: conversion from 'double' to 'in…
Nov 14, 2017
a6765d1
Fixed Windows build error, removed repeated timer registration.
Nov 14, 2017
8a57a58
PX4 param used for FollowMe Min Height was misspelled. Corrected it.
Nov 14, 2017
c64edaa
Merge branch 'develop' into follow_me
Nov 15, 2017
6000cbc
Merge branch 'follow_me' of https://github.com/dronecore/DroneCore in…
Nov 15, 2017
65ea22a
follow_me: Added doxygen tags for See also. API name changes for resp…
Nov 15, 2017
ee718f2
follow_me: fixed shadow member error.
Nov 15, 2017
13eed58
Merge branch 'develop' into follow_me
Nov 17, 2017
724f101
Major changes.
Nov 17, 2017
67861eb
follow_me: Added range for responsiveness
Nov 17, 2017
999c37f
follow_me: Added doxygen comments for FollowMe config
Nov 17, 2017
cc114c2
follow_me: Fixes Travis-CI error: 'FollowInfo' in 'class dronecore::F…
Nov 17, 2017
3e120e5
follow_me: removed `last_request_time` tentatively
Nov 17, 2017
5016a71
follow_me: Fixed type conversion error during Windows build
Nov 17, 2017
5b285ee
follow_me: minor changes related to FOLLOW_ME_TESTIING
Nov 17, 2017
a7e1a79
Merge branch 'develop' into follow_me
Nov 20, 2017
53e67c9
follow_me: Major changes
Nov 20, 2017
a7300cc
follow_me: Added API for de-registering follow target callback.
Nov 20, 2017
5ff8365
follow_me: changes
Nov 21, 2017
c7d6c92
follow_me: Fixed doxygen error: 'un-documented param'
Nov 21, 2017
6e159f6
Merge branch 'develop' into follow_me
Nov 21, 2017
872faf4
follow_me: Removed callback mechanism.
Nov 24, 2017
3fe4e07
Merge branch 'develop' into follow_me
Nov 24, 2017
5c78ddb
Merge branch 'follow_me' of https://github.com/dronecore/DroneCore in…
Nov 24, 2017
ba5e36a
Added follow_me at the end
Nov 25, 2017
0127d34
Merge branch 'develop' into follow_me
Nov 28, 2017
c015b6f
follow_me: changes below
Nov 28, 2017
99a733c
follow_me: changes below
Nov 28, 2017
dff991f
follow_me: ran fix_style, fixed type conversion of float to double is…
Nov 28, 2017
2894a14
follow_me: Below changes
Nov 28, 2017
529cc70
follow_me: changes
Nov 29, 2017
cd9d9e8
Merge branch 'develop' into follow_me
Nov 29, 2017
ea1f808
Merge branch 'develop' into follow_me
Nov 30, 2017
f8ef3ba
follow_me: Resolved review comments. Minor changes in integration tests.
Nov 30, 2017
6a9e144
Merge branch 'follow_me' of https://github.com/dronecore/DroneCore in…
Nov 30, 2017
68abac1
follow_me: Doxygen tags @sa, @note, list items used.
Nov 30, 2017
aba29e9
Merge branch 'develop' into follow_me
Nov 30, 2017
02462ec
follow_me: fixed clang error: implicit conversion float to double
Dec 1, 2017
5486a93
Merge branch 'develop' into follow_me
Dec 1, 2017
815f6da
Merge branch 'follow_me' of https://github.com/dronecore/DroneCore in…
Dec 1, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,6 @@ logs/
.vscode
# auto-generated files (below) from Qt Creator
*.config
*.creator
*.creator*
*.files
*.includes
19 changes: 10 additions & 9 deletions core/mavlink_parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ void MavlinkParameters::do_work()
return;
}

// _last_request_time = steady_time();
// _last_request_time = _parent->get_time().steady_time();

// We want to get notified if a timeout happens
_parent->register_timeout_handler(std::bind(&MavlinkParameters::receive_timeout, this),
Expand Down Expand Up @@ -195,7 +195,7 @@ void MavlinkParameters::do_work()
return;
}

// _last_request_time = steady_time();
// _last_request_time = _parent->get_time().steady_time();

// We want to get notified if a timeout happens
_parent->register_timeout_handler(std::bind(&MavlinkParameters::receive_timeout, this),
Expand Down Expand Up @@ -232,7 +232,7 @@ void MavlinkParameters::process_param_value(const mavlink_message_t &message)
}
_state = State::NONE;
_parent->unregister_timeout_handler(_timeout_cookie);
// LogDebug() << "time taken: " << elapsed_since_s(_last_request_time);
// LogDebug() << "time taken: " << _parent->get_time().elapsed_since_s(_last_request_time);
_get_param_queue.pop_front();
}
}
Expand All @@ -254,7 +254,7 @@ void MavlinkParameters::process_param_value(const mavlink_message_t &message)

_state = State::NONE;
_parent->unregister_timeout_handler(_timeout_cookie);
// LogDebug() << "time taken: " << elapsed_since_s(_last_request_time);
// LogDebug() << "time taken: " << _parent->get_time().elapsed_since_s(_last_request_time);
_set_param_queue.pop_front();
}
}
Expand Down Expand Up @@ -288,7 +288,7 @@ void MavlinkParameters::process_param_ext_value(const mavlink_message_t &message
}
_state = State::NONE;
_parent->unregister_timeout_handler(_timeout_cookie);
// LogDebug() << "time taken: " << elapsed_since_s(_last_request_time);
// LogDebug() << "time taken: " << _parent->get_time().elapsed_since_s(_last_request_time);
_get_param_queue.pop_front();
}
}
Expand All @@ -311,7 +311,7 @@ void MavlinkParameters::process_param_ext_value(const mavlink_message_t &message

_state = State::NONE;
_parent->unregister_timeout_handler(_timeout_cookie);
// LogDebug() << "time taken: " << elapsed_since_s(_last_request_time);
// LogDebug() << "time taken: " << _parent->get_time().elapsed_since_s(_last_request_time);
_set_param_queue.pop_front();
}
}
Expand Down Expand Up @@ -347,7 +347,7 @@ void MavlinkParameters::process_param_ext_ack(const mavlink_message_t &message)

_state = State::NONE;
_parent->unregister_timeout_handler(_timeout_cookie);
// LogDebug() << "time taken: " << elapsed_since_s(_last_request_time);
// LogDebug() << "time taken: " << _parent->get_time().elapsed_since_s(_last_request_time);
_set_param_queue.pop_front();

} else if (param_ext_ack.param_result == PARAM_ACK_IN_PROGRESS) {
Expand All @@ -367,7 +367,7 @@ void MavlinkParameters::process_param_ext_ack(const mavlink_message_t &message)

_state = State::NONE;
_parent->unregister_timeout_handler(_timeout_cookie);
// LogDebug() << "time taken: " << elapsed_since_s(_last_request_time);
// LogDebug() << "time taken: " << _parent->get_time().elapsed_since_s(_last_request_time);
_set_param_queue.pop_front();
}

Expand All @@ -394,7 +394,7 @@ void MavlinkParameters::receive_timeout()
ParamValue empty_value;
// Notify about timeout
LogErr() << "Error: get param busy timeout: " << work.param_name;
// LogDebug() << "Got it after: " << elapsed_since_s(_last_request_time);
// LogErr() << "Got it after: " << _parent->get_time().elapsed_since_s(_last_request_time);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, these are good changes.

work.callback(false, empty_value);
}
_state = State::NONE;
Expand All @@ -411,6 +411,7 @@ void MavlinkParameters::receive_timeout()
if (work.callback) {
// Notify about timeout
LogErr() << "Error: set param busy timeout: " << work.param_name;
// LogErr() << "Got it after: " << _parent->get_time().elapsed_since_s(_last_request_time);
work.callback(false);
}
_state = State::NONE;
Expand Down
5 changes: 3 additions & 2 deletions integration_tests/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,13 @@ list(APPEND integration_tests
offboard_velocity
logging
info
mission
mission
mission_change_speed
mission_survey
mission_survey
gimbal
curl
transition_multicopter_fixedwing
follow_me
)


Expand Down
281 changes: 281 additions & 0 deletions integration_tests/follow_me.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,281 @@
#include <iostream>
#include <cmath>
#include <thread>
#include <chrono>
#include <array>
#include "integration_test_helper.h"
#include "global_include.h"
#include "dronecore.h"

using namespace dronecore;
using namespace std::chrono;
using namespace std::this_thread;
using namespace std::placeholders;

void print(const FollowMe::Config &config);
void send_location_updates(FollowMe &follow_me_handle, size_t count = 25ul, float rate = 1.f);

const size_t N_LOCATIONS = 100ul;

TEST_F(SitlTest, FollowMeOneLocation)
{
DroneCore dc;

DroneCore::ConnectionResult ret = dc.add_udp_connection();
ASSERT_EQ(DroneCore::ConnectionResult::SUCCESS, ret);

// Wait for device to connect via heartbeat.
sleep_for(seconds(2));
Device &device = dc.device();

while (!device.telemetry().health_all_ok()) {
std::cout << "waiting for device to be ready" << std::endl;
sleep_for(seconds(1));
}

Action::Result action_ret = device.action().arm();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);

device.telemetry().flight_mode_async(
std::bind([&](Telemetry::FlightMode flight_mode) {
FollowMe::TargetLocation last_location;
device.follow_me().get_last_location(last_location);

std::cout << "FlightMode: " << Telemetry::flight_mode_str(flight_mode)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think for integration tests we could re-use LogInfo(), etc. but we can do that in a later pull request because it's currently not consistent.

<< " @Lat: " << last_location.latitude_deg << ", " <<
"Lon: " << last_location.longitude_deg << std::endl;

}, std::placeholders::_1));

action_ret = device.action().takeoff();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);

sleep_for(seconds(5)); // let it reach takeoff altitude

auto curr_config = device.follow_me().get_config();
print(curr_config);

// Set just a single location before starting FollowMe (optional)
device.follow_me().set_curr_target_location({47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f});

// Start following with default configuration
FollowMe::Result follow_me_result = device.follow_me().start();
ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
sleep_for(seconds(1));

std::cout << "We're waiting (for 5s) to see the drone moving target location set." << std::endl;
sleep_for(seconds(5));

// stop following
follow_me_result = device.follow_me().stop();
ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
sleep_for(seconds(2)); // to watch flight mode change from "FollowMe" to default "HOLD"

action_ret = device.action().land();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);
sleep_for(seconds(2)); // let the device land
}

TEST_F(SitlTest, FollowMeMultiLocationWithConfig)
{
DroneCore dc;

DroneCore::ConnectionResult ret = dc.add_udp_connection();
ASSERT_EQ(DroneCore::ConnectionResult::SUCCESS, ret);

// Wait for device to connect via heartbeat.
sleep_for(seconds(2));
Device &device = dc.device();

while (!device.telemetry().health_all_ok()) {
std::cout << "Waiting for device to be ready" << std::endl;
sleep_for(seconds(1));
}

Action::Result action_ret = device.action().arm();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);

device.telemetry().flight_mode_async(
std::bind([&](Telemetry::FlightMode flight_mode) {
FollowMe::TargetLocation last_location;
device.follow_me().get_last_location(last_location);

std::cout << "FlightMode: " << Telemetry::flight_mode_str(flight_mode)
<< " @Lat: " << last_location.latitude_deg << ", " <<
"Lon: " << last_location.longitude_deg << std::endl;

}, std::placeholders::_1));

action_ret = device.action().takeoff();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);

sleep_for(seconds(5));

// configure follow me behaviour
FollowMe::Config config;
config.min_height_m = 12.f; // increase min height
config.follow_dist_m = 20.f; // set distance b/w device and target during FollowMe mode
config.responsiveness = 0.2f; // set to higher responsiveness
config.follow_direction =
FollowMe::Config::FollowDirection::FRONT; // Device follows target from FRONT side

// Apply configuration
bool configured = device.follow_me().set_config(config);
ASSERT_EQ(true, configured);

sleep_for(seconds(2)); // until config is applied

// Verify your configuration
auto curr_config = device.follow_me().get_config();
print(curr_config);

// Start following
FollowMe::Result follow_me_result = device.follow_me().start();
ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);

// send location update every second
send_location_updates(device.follow_me());

// Stop following
follow_me_result = device.follow_me().stop();
ASSERT_EQ(FollowMe::Result::SUCCESS, follow_me_result);
sleep_for(seconds(2)); // to watch flight mode change from "FollowMe" to default "HOLD"

action_ret = device.action().land();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);
sleep_for(seconds(2)); // let it land
}

void print(const FollowMe::Config &config)
{
std::cout << "Current FollowMe configuration of the device" << std::endl;
std::cout << "---------------------------" << std::endl;
std::cout << "Min Height: " << config.min_height_m << "m" << std::endl;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Units! Yay :)

std::cout << "Distance: " << config.follow_dist_m << "m" << std::endl;
std::cout << "Responsiveness: " << config.responsiveness << std::endl;
std::cout << "Following from: " << FollowMe::Config::to_str(config.follow_direction) << std::endl;
std::cout << "---------------------------" << std::endl;
}

void send_location_updates(FollowMe &follow_me, size_t count, float rate)
{
// TODO: Generate these co-ordinates from an algorithm
// Altitude here is ignored by PX4, as we've set min altitude in configuration.
std::array<FollowMe::TargetLocation, N_LOCATIONS> spiral_path = {
{
{ 47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f },
{ 47.39769035, 8.54566569, 0.0, 0.f, 0.f, 0.f },
{ 47.39770759, 8.54568983, 0.0, 0.f, 0.f, 0.f },
{ 47.39772757, 8.54569922, 0.0, 0.f, 0.f, 0.f },
{ 47.39774481, 8.54570727, 0.0, 0.f, 0.f, 0.f },
{ 47.39776025, 8.54572202, 0.0, 0.f, 0.f, 0.f },
{ 47.39778567, 8.54572336, 0.0, 0.f, 0.f, 0.f },
{ 47.39780291, 8.54572202, 0.0, 0.f, 0.f, 0.f },
{ 47.39782107, 8.54571263, 0.0, 0.f, 0.f, 0.f },
{ 47.39783469, 8.54569788, 0.0, 0.f, 0.f, 0.f },
{ 47.39783832, 8.54568179, 0.0, 0.f, 0.f, 0.f },
{ 47.39784104, 8.54566569, 0.0, 0.f, 0.f, 0.f },
{ 47.39784376, 8.54564424, 0.0, 0.f, 0.f, 0.f },
{ 47.39772938, 8.54552488, 0.0, 0.f, 0.f, 0.f },
{ 47.39782475, 8.54559193, 0.0, 0.f, 0.f, 0.f },
{ 47.39780291, 8.54557048, 0.0, 0.f, 0.f, 0.f },
{ 47.39771304, 8.54554231, 0.0, 0.f, 0.f, 0.f },
{ 47.39780836, 8.54552756, 0.0, 0.f, 0.f, 0.f },
{ 47.39973737, 8.54269845, 0.0, 0.f, 0.f, 0.f },
{ 47.39973730, 8.54269780, 0.0, 0.f, 0.f, 0.f },
{ 47.39779838, 8.54555174, 0.0, 0.f, 0.f, 0.f },
{ 47.39778748, 8.54554499, 0.0, 0.f, 0.f, 0.f },
{ 47.39777659, 8.54553561, 0.0, 0.f, 0.f, 0.f },
{ 47.39776569, 8.54553292, 0.0, 0.f, 0.f, 0.f },
{ 47.39774663, 8.54552622, 0.0, 0.f, 0.f, 0.f },
{ 47.39772938, 8.54552488, 0.0, 0.f, 0.f, 0.f },
{ 47.39771304, 8.54554231, 0.0, 0.f, 0.f, 0.f },
{ 47.39770578, 8.54557445, 0.0, 0.f, 0.f, 0.f },
{ 47.39770487, 8.54559596, 0.0, 0.f, 0.f, 0.f },
{ 47.39770578, 8.54561741, 0.0, 0.f, 0.f, 0.f },
{ 47.39770669, 8.54563887, 0.0, 0.f, 0.f, 0.f },
{ 47.39771486, 8.54565765, 0.0, 0.f, 0.f, 0.f },
{ 47.39773029, 8.54567642, 0.0, 0.f, 0.f, 0.f },
{ 47.39775026, 8.54568447, 0.0, 0.f, 0.f, 0.f },
{ 47.39776751, 8.54569118, 0.0, 0.f, 0.f, 0.f },
{ 47.39778203, 8.54569118, 0.0, 0.f, 0.f, 0.f },
{ 47.39779838, 8.54568447, 0.0, 0.f, 0.f, 0.f },
{ 47.39781835, 8.54566972, 0.0, 0.f, 0.f, 0.f },
{ 47.39782107, 8.54564692, 0.0, 0.f, 0.f, 0.f },
{ 47.39782474, 8.54561876, 0.0, 0.f, 0.f, 0.f },
{ 47.39782474, 8.54556511, 0.0, 0.f, 0.f, 0.f },
{ 47.39782107, 8.54553427, 0.0, 0.f, 0.f, 0.f },
{ 47.39779656, 8.54551549, 0.0, 0.f, 0.f, 0.f },
{ 47.39777568, 8.54550342, 0.0, 0.f, 0.f, 0.f },
{ 47.39775482, 8.54549671, 0.0, 0.f, 0.f, 0.f },
{ 47.39773755, 8.54549671, 0.0, 0.f, 0.f, 0.f },
{ 47.39771849, 8.54550208, 0.0, 0.f, 0.f, 0.f },
{ 47.39770396, 8.54551415, 0.0, 0.f, 0.f, 0.f },
{ 47.39769398, 8.54554097, 0.0, 0.f, 0.f, 0.f },
{ 47.39768762, 8.54556243, 0.0, 0.f, 0.f, 0.f },
{ 47.39768672, 8.54557852, 0.0, 0.f, 0.f, 0.f },
{ 47.39768493, 8.54559998, 0.0, 0.f, 0.f, 0.f },
{ 47.39768399, 8.54562278, 0.0, 0.f, 0.f, 0.f },
{ 47.39768399, 8.54564155, 0.0, 0.f, 0.f, 0.f },
{ 47.39769035, 8.54566569, 0.0, 0.f, 0.f, 0.f },
{ 47.39770759, 8.54568983, 0.0, 0.f, 0.f, 0.f },
{ 47.39772757, 8.54569922, 0.0, 0.f, 0.f, 0.f },
{ 47.39774481, 8.54570727, 0.0, 0.f, 0.f, 0.f },
{ 47.39776025, 8.54572202, 0.0, 0.f, 0.f, 0.f },
{ 47.39778567, 8.54572336, 0.0, 0.f, 0.f, 0.f },
{ 47.39780291, 8.54572202, 0.0, 0.f, 0.f, 0.f },
{ 47.39782107, 8.54571263, 0.0, 0.f, 0.f, 0.f },
{ 47.39783469, 8.54569788, 0.0, 0.f, 0.f, 0.f },
{ 47.39783832, 8.54568179, 0.0, 0.f, 0.f, 0.f },
{ 47.39784104, 8.54566569, 0.0, 0.f, 0.f, 0.f },
{ 47.39784376, 8.54564424, 0.0, 0.f, 0.f, 0.f },
{ 47.39784386, 8.54564435, 0.0, 0.f, 0.f, 0.f },
{ 47.39784396, 8.54564444, 0.0, 0.f, 0.f, 0.f },
{ 47.39784386, 8.54564454, 0.0, 0.f, 0.f, 0.f },
{ 47.39784346, 8.54564464, 0.0, 0.f, 0.f, 0.f },
{ 47.39784336, 8.54564424, 0.0, 0.f, 0.f, 0.f },
{ 47.39772757, 8.54569922, 0.0, 0.f, 0.f, 0.f },
{ 47.39774481, 8.54570727, 0.0, 0.f, 0.f, 0.f },
{ 47.39776025, 8.54572202, 0.0, 0.f, 0.f, 0.f },
{ 47.39778567, 8.54572336, 0.0, 0.f, 0.f, 0.f },
{ 47.39770396, 8.54551415, 0.0, 0.f, 0.f, 0.f },
{ 47.39769398, 8.54554097, 0.0, 0.f, 0.f, 0.f },
{ 47.39768762, 8.54556243, 0.0, 0.f, 0.f, 0.f },
{ 47.39768672, 8.54557852, 0.0, 0.f, 0.f, 0.f },
{ 47.39768494, 8.54559998, 0.0, 0.f, 0.f, 0.f },
{ 47.39779454, 8.54559464, 0.0, 0.f, 0.f, 0.f },
{ 47.39780291, 8.54557048, 0.0, 0.f, 0.f, 0.f },
{ 47.39779838, 8.54555173, 0.0, 0.f, 0.f, 0.f },
{ 47.39778748, 8.54554499, 0.0, 0.f, 0.f, 0.f },
{ 47.39777659, 8.54553561, 0.0, 0.f, 0.f, 0.f },
{ 47.39776569, 8.54553292, 0.0, 0.f, 0.f, 0.f },
{ 47.39774663, 8.54552622, 0.0, 0.f, 0.f, 0.f },
{ 47.39771304, 8.54554231, 0.0, 0.f, 0.f, 0.f },
{ 47.39772938, 8.54552488, 0.0, 0.f, 0.f, 0.f },
{ 47.39771304, 8.54554231, 0.0, 0.f, 0.f, 0.f },
{ 47.39770578, 8.54557445, 0.0, 0.f, 0.f, 0.f },
{ 47.39770487, 8.54559596, 0.0, 0.f, 0.f, 0.f },
{ 47.39770578, 8.54561741, 0.0, 0.f, 0.f, 0.f },
{ 47.39770669, 8.54563887, 0.0, 0.f, 0.f, 0.f },
{ 47.39771486, 8.54565765, 0.0, 0.f, 0.f, 0.f },
{ 47.39773029, 8.54567642, 0.0, 0.f, 0.f, 0.f },
{ 47.39775026, 8.54568447, 0.0, 0.f, 0.f, 0.f },
{ 47.39776751, 8.54569118, 0.0, 0.f, 0.f, 0.f },
{ 47.39784346, 8.54564464, 0.0, 0.f, 0.f, 0.f },
{ 47.39776569, 8.54553292, 0.0, 0.f, 0.f, 0.f }
}
};

// We're limiting to N_LOCATIONS for testing.
count = (count > N_LOCATIONS) ? N_LOCATIONS : count;
std::cout << "# of Target locations: " << count << " @ Rate: " << rate << "Hz." << std::endl;

for (auto pos : spiral_path) {
if (count-- == 0) {
return;
}
follow_me.set_curr_target_location(pos);
auto sleep_duration_ms = static_cast<int>(1 / rate * 1000);
sleep_for(milliseconds(sleep_duration_ms));
}
}
Loading