Merge pull request #743 from a6a3uh/add-attitude-to-backend

backend support for offboard/set_attitude

backend/src/plugins/offboard/offboard_service_impl.h

@@ -60,6 +60,31 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
         return grpc::Status::OK;
     }
 
+    grpc::Status SetAttitude(grpc::ServerContext * /* context */,
+                             const rpc::offboard::SetAttitudeRequest *request,
+                             rpc::offboard::SetAttitudeResponse * /* response */) override
+    {
+        if (request != nullptr) {
+            auto requested_attitude = translateRPCAttitude(request->attitude());
+            _offboard.set_attitude(requested_attitude);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    static dronecode_sdk::Offboard::Attitude
+    translateRPCAttitude(const rpc::offboard::Attitude &rpc_attitude)
+    {
+        dronecode_sdk::Offboard::Attitude attitude;
+
+        attitude.roll_deg = rpc_attitude.roll_deg();
+        attitude.pitch_deg = rpc_attitude.pitch_deg();
+        attitude.yaw_deg = rpc_attitude.yaw_deg();
+        attitude.thrust_value = rpc_attitude.thrust_value();
+
+        return attitude;
+    }
+
     grpc::Status SetAttitudeRate(grpc::ServerContext * /* context */,
                                  const rpc::offboard::SetAttitudeRateRequest *request,
                                  rpc::offboard::SetAttitudeRateResponse * /* response */) override

backend/test/offboard_service_impl_test.cpp

@@ -16,9 +16,12 @@ using OffboardServiceImpl = dronecode_sdk::backend::OffboardServiceImpl<MockOffb
 using OffboardResult = dronecode_sdk::rpc::offboard::OffboardResult;
 using InputPair = std::pair<std::string, dronecode_sdk::Offboard::Result>;
 
-static constexpr float ARBITRARY_ROLL = 2.5f;
-static constexpr float ARBITRARY_PITCH = 4.0f;
-static constexpr float ARBITRARY_YAW = 3.7f;
+static constexpr float ARBITRARY_ROLL = 25.0f;
+static constexpr float ARBITRARY_PITCH = 40.0f;
+static constexpr float ARBITRARY_YAW = 37.0f;
+static constexpr float ARBITRARY_ROLL_RATE = 2.5f;
+static constexpr float ARBITRARY_PITCH_RATE = 4.0f;
+static constexpr float ARBITRARY_YAW_RATE = 3.7f;
 static constexpr float ARBITRARY_THRUST = 0.5f;
 static constexpr float ARBITRARY_NORTH_M = 10.54f;
 static constexpr float ARBITRARY_EAST_M = 5.62f;
@@ -37,6 +40,7 @@ class OffboardServiceImplTest : public ::testing::TestWithParam<InputPair> {
 protected:
     void checkReturnsCorrectIsActiveStatus(const bool expected_is_active_status);
 
+    std::unique_ptr<dronecode_sdk::rpc::offboard::Attitude> createArbitraryRPCAttitude() const;
     std::unique_ptr<dronecode_sdk::rpc::offboard::AttitudeRate>
     createArbitraryRPCAttitudeRate() const;
     std::unique_ptr<dronecode_sdk::rpc::offboard::PositionNEDYaw>
@@ -138,6 +142,14 @@ TEST_F(OffboardServiceImplTest, isActiveDoesNotCrashWithNullResponse)
     offboardService.IsActive(nullptr, nullptr, nullptr);
 }
 
+TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithAllNullParams)
+{
+    MockOffboard offboard;
+    OffboardServiceImpl offboardService(offboard);
+
+    offboardService.SetAttitude(nullptr, nullptr, nullptr);
+}
+
 TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams)
 {
     MockOffboard offboard;
@@ -146,6 +158,18 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams)
     offboardService.SetAttitudeRate(nullptr, nullptr, nullptr);
 }
 
+TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithNullResponse)
+{
+    MockOffboard offboard;
+    OffboardServiceImpl offboardService(offboard);
+    dronecode_sdk::rpc::offboard::SetAttitudeRequest request;
+
+    auto rpc_attitude = createArbitraryRPCAttitude();
+    request.set_allocated_attitude(rpc_attitude.release());
+
+    offboardService.SetAttitude(nullptr, &request, nullptr);
+}
+
 TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse)
 {
     MockOffboard offboard;
@@ -158,19 +182,46 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithNullResponse)
     offboardService.SetAttitudeRate(nullptr, &request, nullptr);
 }
 
+std::unique_ptr<dronecode_sdk::rpc::offboard::Attitude>
+OffboardServiceImplTest::createArbitraryRPCAttitude() const
+{
+    auto rpc_attitude = std::unique_ptr<dronecode_sdk::rpc::offboard::Attitude>(
+        new dronecode_sdk::rpc::offboard::Attitude());
+    rpc_attitude->set_roll_deg(ARBITRARY_ROLL);
+    rpc_attitude->set_pitch_deg(ARBITRARY_PITCH);
+    rpc_attitude->set_yaw_deg(ARBITRARY_YAW);
+    rpc_attitude->set_thrust_value(ARBITRARY_THRUST);
+
+    return rpc_attitude;
+}
 std::unique_ptr<dronecode_sdk::rpc::offboard::AttitudeRate>
 OffboardServiceImplTest::createArbitraryRPCAttitudeRate() const
 {
     auto rpc_attitude_rate = std::unique_ptr<dronecode_sdk::rpc::offboard::AttitudeRate>(
         new dronecode_sdk::rpc::offboard::AttitudeRate());
-    rpc_attitude_rate->set_roll_deg_s(ARBITRARY_ROLL);
-    rpc_attitude_rate->set_pitch_deg_s(ARBITRARY_PITCH);
-    rpc_attitude_rate->set_yaw_deg_s(ARBITRARY_YAW);
+    rpc_attitude_rate->set_roll_deg_s(ARBITRARY_ROLL_RATE);
+    rpc_attitude_rate->set_pitch_deg_s(ARBITRARY_PITCH_RATE);
+    rpc_attitude_rate->set_yaw_deg_s(ARBITRARY_YAW_RATE);
     rpc_attitude_rate->set_thrust_value(ARBITRARY_THRUST);
 
     return rpc_attitude_rate;
 }
 
+TEST_F(OffboardServiceImplTest, setsAttitudeCorrectly)
+{
+    MockOffboard offboard;
+    OffboardServiceImpl offboardService(offboard);
+    dronecode_sdk::rpc::offboard::SetAttitudeRequest request;
+
+    auto rpc_attitude = createArbitraryRPCAttitude();
+    const auto expected_attitude = OffboardServiceImpl::translateRPCAttitude(*rpc_attitude);
+    EXPECT_CALL(offboard, set_attitude(expected_attitude)).Times(1);
+
+    request.set_allocated_attitude(rpc_attitude.release());
+
+    offboardService.SetAttitude(nullptr, &request, nullptr);
+}
+
 TEST_F(OffboardServiceImplTest, setsAttitudeRateCorrectly)
 {
     MockOffboard offboard;

plugins/offboard/include/plugins/offboard/offboard.h

@@ -228,13 +228,27 @@ class Offboard : public PluginBase {
     std::unique_ptr<OffboardImpl> _impl;
 };
 
+/**
+ * @brief Equal operator to compare two `Offboard::Attitude` objects.
+ *
+ * @return `true` if items are equal.
+ */
+bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs);
+
 /**
  * @brief Equal operator to compare two `Offboard::AttitudeRate` objects.
  *
  * @return `true` if items are equal.
  */
 bool operator==(const Offboard::AttitudeRate &lhs, const Offboard::AttitudeRate &rhs);
 
+/**
+ * @brief Stream operator to print information about a `Offboard::Attitude`.
+ *
+ * @return A reference to the stream.
+ */
+std::ostream &operator<<(std::ostream &str, Offboard::Attitude const &attitude);
+
 /**
  * @brief Stream operator to print information about a `Offboard::AttitudeRate`.
  *

plugins/offboard/mocks/offboard_mock.h

@@ -10,6 +10,7 @@ class MockOffboard {
     MOCK_CONST_METHOD0(start, Offboard::Result()){};
     MOCK_CONST_METHOD0(stop, Offboard::Result()){};
     MOCK_CONST_METHOD0(is_active, bool()){};
+    MOCK_CONST_METHOD1(set_attitude, void(Offboard::Attitude)){};
     MOCK_CONST_METHOD1(set_attitude_rate, void(Offboard::AttitudeRate)){};
     MOCK_CONST_METHOD1(set_position_ned, void(Offboard::PositionNEDYaw)){};
     MOCK_CONST_METHOD1(set_velocity_body, void(Offboard::VelocityBodyYawspeed)){};

plugins/offboard/offboard.cpp

@@ -80,6 +80,18 @@ const char *Offboard::result_str(Result result)
     }
 }
 
+bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs)
+{
+    return lhs.roll_deg == rhs.roll_deg && lhs.pitch_deg == rhs.pitch_deg &&
+           lhs.yaw_deg == rhs.yaw_deg && lhs.thrust_value == rhs.thrust_value;
+}
+
+std::ostream &operator<<(std::ostream &str, Offboard::Attitude const &attitude)
+{
+    return str << "[roll_deg: " << attitude.roll_deg << ", pitch_deg: " << attitude.pitch_deg
+               << ", yaw_deg: " << attitude.yaw_deg << ", thrust_value " << attitude.thrust_value
+               << "]";
+}
 bool operator==(const Offboard::AttitudeRate &lhs, const Offboard::AttitudeRate &rhs)
 {
     return lhs.roll_deg_s == rhs.roll_deg_s && lhs.pitch_deg_s == rhs.pitch_deg_s &&