AP2/client.cpp

#include <condition_variable>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <unistd.h>

#include <grpcpp/grpcpp.h>

#include "grpc_ex.grpc.pb.h"

using grpc::Channel;
using grpc::ClientContext;
using grpc::Status;
using grpc_ex::MainGateway;
using grpc_ex::Main_Ctrl_Takeoff_Reply;
using grpc_ex::Main_Ctrl_Takeoff_Request;
using grpc_ex::Main_Ctrl_Land_Reply;
using grpc_ex::Main_Ctrl_Land_Request;
using grpc_ex::Main_Ctrl_RTL_Reply;
using grpc_ex::Main_Ctrl_RTL_Request;
using grpc_ex::Main_Ctrl_PositionGlobal_Reply;
using grpc_ex::Main_Ctrl_PositionGlobal_Request;
using grpc_ex::Main_Ctrl_PositionNED_Reply;
using grpc_ex::Main_Ctrl_PositionNED_Request;

class MainGatewayClient {
 public:
  MainGatewayClient(std::shared_ptr<Channel> channel)
      : stub_(MainGateway::NewStub(channel)) {}

  std::string Ctrl_Takeoff(double alt_m) {
    Main_Ctrl_Takeoff_Request request;
    request.set_alt_m(alt_m);

    Main_Ctrl_Takeoff_Reply reply;
    ClientContext context;
    std::mutex mu;
    std::condition_variable cv;
    bool done = false;
    Status status;
    
    stub_->async()->Ctrl_Takeoff(&context, &request, &reply,
                             [&mu, &cv, &done, &status](Status s) {
                               status = std::move(s);
                               std::lock_guard<std::mutex> lock(mu);
                               done = true;
                               cv.notify_one();
                             });

    std::unique_lock<std::mutex> lock(mu);
    while (!done) {
      cv.wait(lock);
    }
    if (status.ok()) {
      return reply.message();
    } else {
      std::cout << status.error_code() << ": " << status.error_message()
                << std::endl;
      return "RPC failed";
    }
  }
  
  std::string Ctrl_Land(const std::string& message) {
    Main_Ctrl_Land_Request request;
    request.set_message(message);

    Main_Ctrl_Land_Reply reply;
    ClientContext context;
    std::mutex mu;
    std::condition_variable cv;
    bool done = false;
    Status status;
    
    stub_->async()->Ctrl_Land(&context, &request, &reply,
                             [&mu, &cv, &done, &status](Status s) {
                               status = std::move(s);
                               std::lock_guard<std::mutex> lock(mu);
                               done = true;
                               cv.notify_one();
                             });

    std::unique_lock<std::mutex> lock(mu);
    while (!done) {
      cv.wait(lock);
    }
    if (status.ok()) {
      return reply.message();
    } else {
      std::cout << status.error_code() << ": " << status.error_message()
                << std::endl;
      return "RPC failed";
    }
  }

  std::string Ctrl_RTL(const std::string& message) {
    Main_Ctrl_RTL_Request request;
    request.set_message(message);

    Main_Ctrl_RTL_Reply reply;
    ClientContext context;
    std::mutex mu;
    std::condition_variable cv;
    bool done = false;
    Status status;
    
    stub_->async()->Ctrl_RTL(&context, &request, &reply,
                             [&mu, &cv, &done, &status](Status s) {
                               status = std::move(s);
                               std::lock_guard<std::mutex> lock(mu);
                               done = true;
                               cv.notify_one();
                             });

    std::unique_lock<std::mutex> lock(mu);
    while (!done) {
      cv.wait(lock);
    }
    if (status.ok()) {
      return reply.message();
    } else {
      std::cout << status.error_code() << ": " << status.error_message()
                << std::endl;
      return "RPC failed";
    }
  }

  std::string Ctrl_PositionGlobal(double lat_deg, double lon_deg, double alt_m, double yaw_deg) {
    Main_Ctrl_PositionGlobal_Request request;
    request.set_lat_deg(lat_deg);
    request.set_lon_deg(lon_deg);
    request.set_alt_m(alt_m);
    request.set_yaw_deg(yaw_deg);

    Main_Ctrl_PositionGlobal_Reply reply;
    ClientContext context;
    std::mutex mu;
    std::condition_variable cv;
    bool done = false;
    Status status;
    
    stub_->async()->Ctrl_PositionGlobal(&context, &request, &reply,
                             [&mu, &cv, &done, &status](Status s) {
                               status = std::move(s);
                               std::lock_guard<std::mutex> lock(mu);
                               done = true;
                               cv.notify_one();
                             });

    std::unique_lock<std::mutex> lock(mu);
    while (!done) {
      cv.wait(lock);
    }
    if (status.ok()) {
      return reply.message();
    } else {
      std::cout << status.error_code() << ": " << status.error_message()
                << std::endl;
      return "RPC failed";
    }
  }

  std::string Ctrl_PositionNED(double north_m, double east_m, double down_m, double yaw_deg) {
    Main_Ctrl_PositionNED_Request request;
    request.set_north_m(north_m);
    request.set_east_m(east_m);
    request.set_down_m(down_m);
    request.set_yaw_deg(yaw_deg);

    Main_Ctrl_PositionNED_Reply reply;
    ClientContext context;
    std::mutex mu;
    std::condition_variable cv;
    bool done = false;
    Status status;
    
    stub_->async()->Ctrl_PositionNED(&context, &request, &reply,
                             [&mu, &cv, &done, &status](Status s) {
                               status = std::move(s);
                               std::lock_guard<std::mutex> lock(mu);
                               done = true;
                               cv.notify_one();
                             });

    std::unique_lock<std::mutex> lock(mu);
    while (!done) {
      cv.wait(lock);
    }
    if (status.ok()) {
      return reply.message();
    } else {
      std::cout << status.error_code() << ": " << status.error_message()
                << std::endl;
      return "RPC failed";
    }
  }

 private:
  std::unique_ptr<MainGateway::Stub> stub_;
};

int main(int argc, char** argv) {

  std::string target_str = "127.0.0.1:8081";

  MainGatewayClient client(
      grpc::CreateChannel(target_str, grpc::InsecureChannelCredentials()));

  std::string reply{};
  for(int i = 0; i < 1000; ++i){
    reply = client.Ctrl_Takeoff(i);
    std::cout << reply << std::endl;

    reply = client.Ctrl_Land("Ctrl_Land " + std::to_string(i));
    std::cout << reply << std::endl;

    reply = client.Ctrl_RTL("Ctrl_RTL " + std::to_string(i));
    std::cout << reply << std::endl;

    reply = client.Ctrl_PositionGlobal(i, i, i, i);
    std::cout << reply << std::endl;

    reply = client.Ctrl_PositionNED(i, i, i, i);
    std::cout << reply << std::endl;

    std::cout << std::endl;
    sleep(1);
  }

  return 0;
}