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

backend support for offboard/set_attitude #743

Merged
merged 4 commits into from
May 13, 2019
Merged

backend support for offboard/set_attitude #743

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ad57674
backend support for offboard/set_attitude
a6a3uh May 7, 2019
7db3ebd
Update backend/test/offboard_service_impl_test.cpp
a6a3uh May 10, 2019
7190e21
fix style
a6a3uh May 10, 2019
1f9043e
updated proto
a6a3uh May 10, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 1 addition & 1 deletion backend/proto
Open in desktop
Submodule proto updated from dccc60 to 3c5856
25 changes: 25 additions & 0 deletions backend/src/plugins/offboard/offboard_service_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
65 changes: 59 additions & 6 deletions backend/test/offboard_service_impl_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -37,6 +40,8 @@ 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>
Expand Down Expand Up @@ -138,6 +143,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;
Expand All @@ -146,6 +159,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;
Expand All @@ -158,19 +183,47 @@ 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, setAttitudeCorrectrly)
a6a3uh marked this conversation as resolved.
Show resolved Hide resolved
{
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;
Expand Down
14 changes: 14 additions & 0 deletions plugins/offboard/include/plugins/offboard/offboard.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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`.
*
Expand Down
1 change: 1 addition & 0 deletions plugins/offboard/mocks/offboard_mock.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)){};
Expand Down
13 changes: 13 additions & 0 deletions plugins/offboard/offboard.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,19 @@ 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 &&
Expand Down