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.

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

Download mission capability #145

Merged
merged 13 commits into from
Nov 10, 2017
4 changes: 2 additions & 2 deletions example/fly_mission/fly_mission.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,11 +112,11 @@ int main(int /*argc*/, char ** /*argv*/)

{
std::cout << "Uploading mission..." << std::endl;
// We only have the send_mission function asynchronous for now, so we wrap it using
// We only have the upload_mission function asynchronous for now, so we wrap it using
// std::future.
auto prom = std::make_shared<std::promise<Mission::Result>>();
auto future_result = prom->get_future();
device.mission().send_mission_async(
device.mission().upload_mission_async(
mission_items, [prom](Mission::Result result) {
prom->set_value(result);
});
Expand Down
2 changes: 1 addition & 1 deletion grpc/server/dronecore_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ class DroneCoreRPCImpl final : public DroneCoreRPC::Service
mission_items.push_back(new_item);
}

dc.device().mission().send_mission_async(
dc.device().mission().upload_mission_async(
mission_items, [prom, response](Mission::Result result) {
response->set_result(static_cast<dronecorerpc::MissionResult::Result>(result));
response->set_result_str(Mission::result_str(result));
Expand Down
67 changes: 64 additions & 3 deletions integration_tests/mission.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@ static std::shared_ptr<MissionItem> add_mission_item(double latitude_deg,
float gimbal_yaw_deg,
MissionItem::CameraAction camera_action);

static void compare_mission_items(const std::shared_ptr<MissionItem> original,
const std::shared_ptr<MissionItem> downloaded);


TEST_F(SitlTest, MissionAddWaypointsAndFly)
{
Expand Down Expand Up @@ -96,11 +99,11 @@ TEST_F(SitlTest, MissionAddWaypointsAndFly)

{
LogInfo() << "Uploading mission...";
// We only have the send_mission function asynchronous for now, so we wrap it using
// We only have the upload_mission function asynchronous for now, so we wrap it using
// std::future.
auto prom = std::make_shared<std::promise<void>>();
auto future_result = prom->get_future();
device.mission().send_mission_async(
device.mission().upload_mission_async(
mission_items, [prom](Mission::Result result) {
ASSERT_EQ(result, Mission::Result::SUCCESS);
prom->set_value();
Expand All @@ -110,6 +113,34 @@ TEST_F(SitlTest, MissionAddWaypointsAndFly)
future_result.get();
}

{
// Download the mission again and compare it.
LogInfo() << "Downloading mission...";
// We only have the download_mission function asynchronous for now, so we wrap it using
// std::future.
auto prom = std::make_shared<std::promise<void>>();
auto future_result = prom->get_future();
device.mission().download_mission_async(
[prom, mission_items](
Mission::Result result,
std::vector<std::shared_ptr<MissionItem>> mission_items_downloaded) {
EXPECT_EQ(result, Mission::Result::SUCCESS);
prom->set_value();
LogInfo() << "Mission downloaded (to check it).";

EXPECT_EQ(mission_items.size(), mission_items_downloaded.size());

if (mission_items.size() == mission_items_downloaded.size()) {
for (unsigned i = 0; i < mission_items.size(); ++i) {
compare_mission_items(mission_items.at(i), mission_items_downloaded.at(i));
}
}
});

future_result.get();

}

LogInfo() << "Arming...";
const Action::Result arm_result = device.action().arm();
EXPECT_EQ(arm_result, Action::Result::SUCCESS);
Expand Down Expand Up @@ -211,12 +242,42 @@ std::shared_ptr<MissionItem> add_mission_item(double latitude_deg,
float gimbal_yaw_deg,
MissionItem::CameraAction camera_action)
{
std::shared_ptr<MissionItem> new_item(new MissionItem());
auto new_item = std::make_shared<MissionItem>();
new_item->set_position(latitude_deg, longitude_deg);
new_item->set_relative_altitude(relative_altitude_m);
new_item->set_speed(speed_m_s);
new_item->set_fly_through(is_fly_through);
new_item->set_gimbal_pitch_and_yaw(gimbal_pitch_deg, gimbal_yaw_deg);
new_item->set_camera_action(camera_action);

// In order to test setting the interval, add it here.
if (camera_action == MissionItem::CameraAction::START_PHOTO_INTERVAL) {
new_item->set_camera_photo_interval(1.5);
}

return new_item;
}

void compare_mission_items(const std::shared_ptr<MissionItem> original,
const std::shared_ptr<MissionItem> downloaded)
{
EXPECT_NEAR(original->get_latitude_deg(), downloaded->get_latitude_deg(), 1e-6);
EXPECT_NEAR(original->get_longitude_deg(), downloaded->get_longitude_deg(), 1e-6);

EXPECT_FLOAT_EQ(original->get_relative_altitude_m(),
downloaded->get_relative_altitude_m());

EXPECT_EQ(original->get_fly_through(), downloaded->get_fly_through());
if (std::isfinite(original->get_speed_m_s())) {
EXPECT_FLOAT_EQ(original->get_speed_m_s(), downloaded->get_speed_m_s());
}

EXPECT_EQ(original->get_camera_action(), downloaded->get_camera_action());

if (original->get_camera_action() == MissionItem::CameraAction::START_PHOTO_INTERVAL &&
std::isfinite(original->get_camera_photo_interval_s())) {

EXPECT_DOUBLE_EQ(original->get_camera_photo_interval_s(),
downloaded->get_camera_photo_interval_s());
}
}
8 changes: 4 additions & 4 deletions integration_tests/mission_change_speed.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
using namespace dronecore;
using namespace std::placeholders; // for `_1`

static void receive_send_mission_result(Mission::Result result);
static void receive_upload_mission_result(Mission::Result result);
static void receive_start_mission_result(Mission::Result result);
static void receive_mission_progress(int current, int total);

Expand Down Expand Up @@ -53,8 +53,8 @@ TEST_F(SitlTest, MissionChangeSpeed)
mission_items.push_back(add_waypoint(47.39824201089737, 8.5447561722784542, 10, speeds[2]));
mission_items.push_back(add_waypoint(47.397733642793433, 8.5447776308767516, 10, speeds[3]));

device.mission().send_mission_async(mission_items,
std::bind(&receive_send_mission_result, _1));
device.mission().upload_mission_async(mission_items,
std::bind(&receive_upload_mission_result, _1));
std::this_thread::sleep_for(std::chrono::seconds(1));
ASSERT_TRUE(_mission_sent_ok);

Expand Down Expand Up @@ -106,7 +106,7 @@ TEST_F(SitlTest, MissionChangeSpeed)
ASSERT_EQ(result, Action::Result::SUCCESS);
}

void receive_send_mission_result(Mission::Result result)
void receive_upload_mission_result(Mission::Result result)
{
EXPECT_EQ(result, Mission::Result::SUCCESS);

Expand Down
8 changes: 4 additions & 4 deletions integration_tests/mission_survey.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ enum class MissionState : unsigned {
static MissionState _mission_state = MissionState::INIT;

//Mission::mission_result_t receive_mission_result;
static void receive_send_mission_result(Mission::Result result);
static void receive_upload_mission_result(Mission::Result result);
static void receive_start_mission_result(Mission::Result result);
static void receive_mission_progress(int current, int total);
static void receive_arm_result(Action::Result result);
Expand Down Expand Up @@ -135,9 +135,9 @@ TEST_F(SitlTest, MissionSurvey)
}
switch (_mission_state) {
case MissionState::INIT:
device.mission().send_mission_async(
device.mission().upload_mission_async(
mis,
std::bind(&receive_send_mission_result, _1));
std::bind(&receive_upload_mission_result, _1));
_mission_state = MissionState::UPLOADING;
break;
case MissionState::UPLOADING:
Expand Down Expand Up @@ -199,7 +199,7 @@ TEST_F(SitlTest, MissionSurvey)
EXPECT_EQ(_mission_state, MissionState::DONE);
}

void receive_send_mission_result(Mission::Result result)
void receive_upload_mission_result(Mission::Result result)
{
EXPECT_EQ(result, Mission::Result::SUCCESS);

Expand Down
11 changes: 8 additions & 3 deletions plugins/mission/mission.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,15 @@ Mission::~Mission()
}


void Mission::send_mission_async(const std::vector<std::shared_ptr<MissionItem>> &mission_items,
result_callback_t callback)
void Mission::upload_mission_async(const std::vector<std::shared_ptr<MissionItem>> &mission_items,
result_callback_t callback)
{
_impl->send_mission_async(mission_items, callback);
_impl->upload_mission_async(mission_items, callback);
}

void Mission::download_mission_async(Mission::mission_items_and_result_callback_t callback)
{
_impl->download_mission_async(callback);
}

void Mission::start_mission_async(result_callback_t callback)
Expand Down
44 changes: 31 additions & 13 deletions plugins/mission/mission.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@ class Mission
BUSY, /**< @brief %Vehicle busy. */
TIMEOUT, /**< @brief Request timed out. */
INVALID_ARGUMENT, /**< @brief Invalid argument. */
UNSUPPORTED, /**< @brief The mission downloaded from the device is not supported. */
NO_MISSION_AVAILABLE, /**< @brief No mission available on device. */
UNKNOWN /**< @brief Unknown error. */
};

Expand All @@ -49,24 +51,40 @@ class Mission
typedef std::function<void(Result)> result_callback_t;

/**
* @brief Sends a vector of mission items to the device (asynchronous).
* @brief Uploads a vector of mission items to the device (asynchronous).
*
* The mission items are uploaded to a drone. Once uploaded the mission can be started and
* executed even if a connection is lost.
*
* @param mission_items reference to vector of mission items.
* @param callback callback to receive result of this request
* @param mission_items Reference to vector of mission items.
* @param callback Callback to receive result of this request.
*/
void send_mission_async(const std::vector<std::shared_ptr<MissionItem>> &mission_items,
result_callback_t callback);
void upload_mission_async(const std::vector<std::shared_ptr<MissionItem>> &mission_items,
result_callback_t callback);

/**
* @brief Callback type for `download_mission_async` call to get mission items and result.
*/
typedef std::function<void(Result, std::vector<std::shared_ptr<MissionItem>>)>
mission_items_and_result_callback_t;

/**
* @brief Downloads a vector of mission items from the device (asynchronous).
*
* The mission items are downloaded from a drone.
*
* @param mission_items Reference to vector of mission items.
* @param callback Callback to receive result of this request.
*/
void download_mission_async(mission_items_and_result_callback_t callback);

/**
* @brief Starts the mission (asynchronous).
*
* Note that the mission must be uplaoded to the vehicle using `send_mission_async()` before
* Note that the mission must be uplaoded to the vehicle using `upload_mission_async()` before
* this method is called.
*
* @param callback callback to receive result of this request
* @param callback callback to receive result of this request.
*/
void start_mission_async(result_callback_t callback);

Expand All @@ -78,7 +96,7 @@ class Mission
* A multicopter should just hover at the spot while a fixedwing vehicle should loiter
* around the location where it paused.
*
* @param callback callback to receive result of this request
* @param callback Callback to receive result of this request.
*/
void pause_mission_async(result_callback_t callback);

Expand All @@ -91,8 +109,8 @@ class Mission
* Note that this is not necessarily true for general missions using mavlink if loop counters
* are used.
*
* @param current index for mission index to go to next (0 based)
* @param callback callback to receive result of this request.
* @param current Index for mission index to go to next (0 based).
* @param callback Callback to receive result of this request.
*/
void set_current_mission_item_async(int current, result_callback_t callback);

Expand Down Expand Up @@ -125,15 +143,15 @@ class Mission
*
* The mission is finished if current == total.
*
* @param current current mission item index (0 based)
* @param total total number of mission items
* @param current Current mission item index (0 based).
* @param total Total number of mission items.
*/
typedef std::function<void(int current, int total)> progress_callback_t;

/**
* @brief Subscribes to mission progress (asynchronous).
*
* @param callback callback to receive mission progress
* @param callback Callback to receive mission progress.
*/
void subscribe_progress(progress_callback_t callback);

Expand Down
Loading