Merge pull request #735 from cswkim/add-offboard-grpc-backend-support

Add gRPC backend support for Offboard plugin

backend/src/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.1)
 
-set(COMPONENTS_LIST core action calibration gimbal camera mission telemetry info param)
+set(COMPONENTS_LIST core action calibration gimbal camera mission offboard telemetry info param)
 
 include(cmake/compile_proto.cmake)
 
@@ -41,6 +41,7 @@ target_link_libraries(backend
     dronecode_sdk_gimbal
     dronecode_sdk_camera
     dronecode_sdk_mission
+    dronecode_sdk_offboard
     dronecode_sdk_telemetry
     dronecode_sdk_info
     dronecode_sdk_param

backend/src/core/core_service_impl.h

@@ -43,15 +43,16 @@ class CoreServiceImpl final : public dronecode_sdk::rpc::core::CoreService::Serv
                        const rpc::core::ListRunningPluginsRequest * /* request */,
                        dronecode_sdk::rpc::core::ListRunningPluginsResponse *response) override
     {
-        std::string plugin_names[9] = {"action",
-                                       "calibration",
-                                       "camera",
-                                       "core",
-                                       "gimbal",
-                                       "info",
-                                       "mission",
-                                       "param",
-                                       "telemetry"};
+        std::string plugin_names[10] = {"action",
+                                        "calibration",
+                                        "camera",
+                                        "core",
+                                        "gimbal",
+                                        "info",
+                                        "mission",
+                                        "offboard",
+                                        "param",
+                                        "telemetry"};
 
         for (const auto plugin_name : plugin_names) {
             auto plugin_info = response->add_plugin_info();

backend/src/grpc_server.cpp

@@ -19,6 +19,7 @@ void GRPCServer::run()
     builder.RegisterService(&_gimbal_service);
     builder.RegisterService(&_camera_service);
     builder.RegisterService(&_mission_service);
+    builder.RegisterService(&_offboard_service);
     builder.RegisterService(&_telemetry_service);
     builder.RegisterService(&_info_service);
     builder.RegisterService(&_param_service);

backend/src/grpc_server.h

@@ -19,6 +19,8 @@
 #include "gimbal/gimbal_service_impl.h"
 #include "plugins/param/param.h"
 #include "param/param_service_impl.h"
+#include "plugins/offboard/offboard.h"
+#include "offboard/offboard_service_impl.h"
 
 namespace dronecode_sdk {
 namespace backend {
@@ -38,6 +40,8 @@ class GRPCServer {
         _camera_service(_camera),
         _mission(_dc.system()),
         _mission_service(_mission),
+        _offboard(_dc.system()),
+        _offboard_service(_offboard),
         _telemetry(_dc.system()),
         _telemetry_service(_telemetry),
         _info(_dc.system()),
@@ -65,6 +69,8 @@ class GRPCServer {
     CameraServiceImpl<> _camera_service;
     Mission _mission;
     MissionServiceImpl<> _mission_service;
+    Offboard _offboard;
+    OffboardServiceImpl<> _offboard_service;
     Telemetry _telemetry;
     TelemetryServiceImpl<> _telemetry_service;
     Info _info;

backend/src/plugins/offboard/offboard_service_impl.h

@@ -0,0 +1,171 @@
+#include "plugins/offboard/offboard.h"
+#include "offboard/offboard.grpc.pb.h"
+
+namespace dronecode_sdk {
+namespace backend {
+
+template<typename Offboard = Offboard>
+class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service {
+public:
+    OffboardServiceImpl(Offboard &offboard) : _offboard(offboard) {}
+
+    template<typename ResponseType>
+    void fillResponseWithResult(ResponseType *response,
+                                dronecode_sdk::Offboard::Result &offboard_result) const
+    {
+        auto rpc_result = static_cast<rpc::offboard::OffboardResult::Result>(offboard_result);
+
+        auto *rpc_offboard_result = new rpc::offboard::OffboardResult();
+        rpc_offboard_result->set_result(rpc_result);
+        rpc_offboard_result->set_result_str(dronecode_sdk::Offboard::result_str(offboard_result));
+
+        response->set_allocated_offboard_result(rpc_offboard_result);
+    }
+
+    grpc::Status Start(grpc::ServerContext * /* context */,
+                       const rpc::offboard::StartRequest * /* request */,
+                       rpc::offboard::StartResponse *response) override
+    {
+        auto offboard_result = _offboard.start();
+
+        if (response != nullptr) {
+            fillResponseWithResult(response, offboard_result);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    grpc::Status Stop(grpc::ServerContext * /* context */,
+                      const rpc::offboard::StopRequest * /* request */,
+                      rpc::offboard::StopResponse *response) override
+    {
+        auto offboard_result = _offboard.stop();
+
+        if (response != nullptr) {
+            fillResponseWithResult(response, offboard_result);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    grpc::Status IsActive(grpc::ServerContext * /* context */,
+                          const rpc::offboard::IsActiveRequest * /* request */,
+                          rpc::offboard::IsActiveResponse *response) override
+    {
+        if (response != nullptr) {
+            auto is_active = _offboard.is_active();
+            response->set_is_active(is_active);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    grpc::Status SetAttitudeRate(grpc::ServerContext * /* context */,
+                                 const rpc::offboard::SetAttitudeRateRequest *request,
+                                 rpc::offboard::SetAttitudeRateResponse * /* response */) override
+    {
+        if (request != nullptr) {
+            auto requested_attitude_rate = translateRPCAttitudeRate(request->attitude_rate());
+            _offboard.set_attitude_rate(requested_attitude_rate);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    static dronecode_sdk::Offboard::AttitudeRate
+    translateRPCAttitudeRate(const rpc::offboard::AttitudeRate &rpc_attitude_rate)
+    {
+        dronecode_sdk::Offboard::AttitudeRate attitude_rate;
+
+        attitude_rate.roll_deg_s = rpc_attitude_rate.roll_deg_s();
+        attitude_rate.pitch_deg_s = rpc_attitude_rate.pitch_deg_s();
+        attitude_rate.yaw_deg_s = rpc_attitude_rate.yaw_deg_s();
+        attitude_rate.thrust_value = rpc_attitude_rate.thrust_value();
+
+        return attitude_rate;
+    }
+
+    grpc::Status SetPositionNed(grpc::ServerContext * /* context */,
+                                const rpc::offboard::SetPositionNedRequest *request,
+                                rpc::offboard::SetPositionNedResponse * /* response */) override
+    {
+        if (request != nullptr) {
+            auto requested_position_ned_yaw =
+                translateRPCPositionNEDYaw(request->position_ned_yaw());
+            _offboard.set_position_ned(requested_position_ned_yaw);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    static dronecode_sdk::Offboard::PositionNEDYaw
+    translateRPCPositionNEDYaw(const rpc::offboard::PositionNEDYaw &rpc_position_ned_yaw)
+    {
+        dronecode_sdk::Offboard::PositionNEDYaw position_ned_yaw;
+
+        position_ned_yaw.north_m = rpc_position_ned_yaw.north_m();
+        position_ned_yaw.east_m = rpc_position_ned_yaw.east_m();
+        position_ned_yaw.down_m = rpc_position_ned_yaw.down_m();
+        position_ned_yaw.yaw_deg = rpc_position_ned_yaw.yaw_deg();
+
+        return position_ned_yaw;
+    }
+
+    grpc::Status SetVelocityBody(grpc::ServerContext * /* context */,
+                                 const rpc::offboard::SetVelocityBodyRequest *request,
+                                 rpc::offboard::SetVelocityBodyResponse * /* response */) override
+    {
+        if (request != nullptr) {
+            auto requested_velocity_body_yawspeed =
+                translateRPCVelocityBodyYawspeed(request->velocity_body_yawspeed());
+            _offboard.set_velocity_body(requested_velocity_body_yawspeed);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    static dronecode_sdk::Offboard::VelocityBodyYawspeed translateRPCVelocityBodyYawspeed(
+        const rpc::offboard::VelocityBodyYawspeed &rpc_velocity_body_yawspeed)
+    {
+        dronecode_sdk::Offboard::VelocityBodyYawspeed velocity_body_yawspeed;
+
+        velocity_body_yawspeed.forward_m_s = rpc_velocity_body_yawspeed.forward_m_s();
+        velocity_body_yawspeed.right_m_s = rpc_velocity_body_yawspeed.right_m_s();
+        velocity_body_yawspeed.down_m_s = rpc_velocity_body_yawspeed.down_m_s();
+        velocity_body_yawspeed.yawspeed_deg_s = rpc_velocity_body_yawspeed.yawspeed_deg_s();
+
+        return velocity_body_yawspeed;
+    }
+
+    grpc::Status SetVelocityNed(grpc::ServerContext * /* context */,
+                                const rpc::offboard::SetVelocityNedRequest *request,
+                                rpc::offboard::SetVelocityNedResponse * /* response */) override
+    {
+        if (request != nullptr) {
+            auto requested_velocity_ned_yaw =
+                translateRPCVelocityNEDYaw(request->velocity_ned_yaw());
+            _offboard.set_velocity_ned(requested_velocity_ned_yaw);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    static dronecode_sdk::Offboard::VelocityNEDYaw
+    translateRPCVelocityNEDYaw(const rpc::offboard::VelocityNEDYaw &rpc_velocity_ned_yaw)
+    {
+        dronecode_sdk::Offboard::VelocityNEDYaw velocity_ned_yaw;
+
+        velocity_ned_yaw.north_m_s = rpc_velocity_ned_yaw.north_m_s();
+        velocity_ned_yaw.east_m_s = rpc_velocity_ned_yaw.east_m_s();
+        velocity_ned_yaw.down_m_s = rpc_velocity_ned_yaw.down_m_s();
+        velocity_ned_yaw.yaw_deg = rpc_velocity_ned_yaw.yaw_deg();
+
+        return velocity_ned_yaw;
+    }
+
+private:
+    Offboard &_offboard;
+};
+
+} // namespace backend
+} // namespace dronecode_sdk

backend/test/CMakeLists.txt

@@ -7,6 +7,7 @@ add_executable(unit_tests_backend
     connection_initiator_test.cpp
     core_service_impl_test.cpp
     mission_service_impl_test.cpp
+    offboard_service_impl_test.cpp
     telemetry_service_impl_test.cpp
     info_service_impl_test.cpp
 )