diff --git a/backend/proto b/backend/proto index e7a285733a..6edc683472 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit e7a285733aa03e4b56a771551a68e3f2d4b91a6f +Subproject commit 6edc683472316715ea54c8d10d197bbeec9e81ee diff --git a/backend/src/plugins/camera/camera_service_impl.h b/backend/src/plugins/camera/camera_service_impl.h index 918756a4f5..f358b1726f 100644 --- a/backend/src/plugins/camera/camera_service_impl.h +++ b/backend/src/plugins/camera/camera_service_impl.h @@ -348,8 +348,10 @@ class CameraServiceImpl final : public rpc::camera::CameraService::Service { std::unique_ptr(new rpc::camera::CaptureInfo()); rpc_capture_info->set_allocated_position( translatePosition(capture_info.position).release()); - rpc_capture_info->set_allocated_quaternion( - translateQuaternion(capture_info.quaternion).release()); + rpc_capture_info->set_allocated_attitude_quaternion( + translateAttitudeQuaternion(capture_info.attitude_quaternion).release()); + rpc_capture_info->set_allocated_attitude_euler_angle( + translateAttitudeEulerAngle(capture_info.attitude_euler_angle).release()); rpc_capture_info->set_time_utc_us(capture_info.time_utc_us); rpc_capture_info->set_is_success(capture_info.success); rpc_capture_info->set_index(capture_info.index); @@ -370,25 +372,40 @@ class CameraServiceImpl final : public rpc::camera::CameraService::Service { return rpc_position; } - static std::unique_ptr - translateQuaternion(const dronecode_sdk::Camera::CaptureInfo::Quaternion &quaternion) + static std::unique_ptr translateAttitudeQuaternion( + const dronecode_sdk::Camera::CaptureInfo::Quaternion &attitude_quaternion) { auto rpc_quaternion = std::unique_ptr(new rpc::camera::Quaternion()); - rpc_quaternion->set_w(quaternion.w); - rpc_quaternion->set_x(quaternion.x); - rpc_quaternion->set_y(quaternion.y); - rpc_quaternion->set_z(quaternion.z); + rpc_quaternion->set_w(attitude_quaternion.w); + rpc_quaternion->set_x(attitude_quaternion.x); + rpc_quaternion->set_y(attitude_quaternion.y); + rpc_quaternion->set_z(attitude_quaternion.z); return rpc_quaternion; } + static std::unique_ptr translateAttitudeEulerAngle( + const dronecode_sdk::Camera::CaptureInfo::EulerAngle &attitude_euler_angle) + { + auto rpc_euler_angle = + std::unique_ptr(new rpc::camera::EulerAngle()); + rpc_euler_angle->set_yaw_deg(attitude_euler_angle.yaw_deg); + rpc_euler_angle->set_pitch_deg(attitude_euler_angle.pitch_deg); + rpc_euler_angle->set_roll_deg(attitude_euler_angle.roll_deg); + + return rpc_euler_angle; + } + static dronecode_sdk::Camera::CaptureInfo translateRPCCaptureInfo(const rpc::camera::CaptureInfo &rpc_capture_info) { dronecode_sdk::Camera::CaptureInfo capture_info; capture_info.position = translateRPCPosition(rpc_capture_info.position()); - capture_info.quaternion = translateRPCQuaternion(rpc_capture_info.quaternion()); + capture_info.attitude_quaternion = + translateRPCQuaternion(rpc_capture_info.attitude_quaternion()); + capture_info.attitude_euler_angle = + translateRPCEulerAngle(rpc_capture_info.attitude_euler_angle()); capture_info.time_utc_us = rpc_capture_info.time_utc_us(); capture_info.success = rpc_capture_info.is_success(); capture_info.index = rpc_capture_info.index(); @@ -421,6 +438,17 @@ class CameraServiceImpl final : public rpc::camera::CameraService::Service { return quaternion; } + static dronecode_sdk::Camera::CaptureInfo::EulerAngle + translateRPCEulerAngle(const rpc::camera::EulerAngle &rpc_euler_angle) + { + dronecode_sdk::Camera::CaptureInfo::EulerAngle euler_angle; + euler_angle.yaw_deg = rpc_euler_angle.yaw_deg(); + euler_angle.pitch_deg = rpc_euler_angle.pitch_deg(); + euler_angle.roll_deg = rpc_euler_angle.roll_deg(); + + return euler_angle; + } + grpc::Status SubscribeCameraStatus(grpc::ServerContext * /* context */, const rpc::camera::SubscribeCameraStatusRequest * /* request */, diff --git a/backend/test/camera_service_impl_test.cpp b/backend/test/camera_service_impl_test.cpp index 2b5df097b1..4d5344bd6e 100644 --- a/backend/test/camera_service_impl_test.cpp +++ b/backend/test/camera_service_impl_test.cpp @@ -89,7 +89,9 @@ class CameraServiceImplTest : public ::testing::TestWithParam { std::unique_ptr createRPCPosition( const double lat, const double lng, const float abs_alt, const float rel_alt) const; std::unique_ptr - createRPCQuaternion(const float w, const float x, const float y, const float z) const; + createRPCAttitudeQuaternion(const float w, const float x, const float y, const float z) const; + std::unique_ptr createRPCAttitudeEulerAngle( + const float yaw_deg, const float pitch_deg, const float roll_deg) const; void checkSendsCaptureInfo( const std::vector &capture_info_events) const; dronecode_sdk::Camera::CaptureInfo createArbitraryCaptureInfo() const; @@ -655,7 +657,10 @@ CameraServiceImplTest::createArbitraryRPCCaptureInfo() const new dronecode_sdk::rpc::camera::CaptureInfo()); rpc_info->set_allocated_position( createRPCPosition(41.848695, 75.132751, 3002.1f, 50.3f).release()); - rpc_info->set_allocated_quaternion(createRPCQuaternion(0.1f, 0.2f, 0.3f, 0.4f).release()); + rpc_info->set_allocated_attitude_quaternion( + createRPCAttitudeQuaternion(0.1f, 0.2f, 0.3f, 0.4f).release()); + rpc_info->set_allocated_attitude_euler_angle( + createRPCAttitudeEulerAngle(12.2f, 90.0f, 11.1f).release()); rpc_info->set_time_utc_us(ARBITRARY_INT); rpc_info->set_is_success(ARBITRARY_BOOL); rpc_info->set_index(ARBITRARY_INT); @@ -678,8 +683,11 @@ std::unique_ptr CameraServiceImplTest::cre return expected_position; } -std::unique_ptr CameraServiceImplTest::createRPCQuaternion( - const float w, const float x, const float y, const float z) const +std::unique_ptr +CameraServiceImplTest::createRPCAttitudeQuaternion(const float w, + const float x, + const float y, + const float z) const { auto quaternion = std::unique_ptr( new dronecode_sdk::rpc::camera::Quaternion()); @@ -692,6 +700,21 @@ std::unique_ptr CameraServiceImplTest::c return quaternion; } +std::unique_ptr +CameraServiceImplTest::createRPCAttitudeEulerAngle(const float yaw_deg, + const float pitch_deg, + const float roll_deg) const +{ + auto euler_angle = std::unique_ptr( + new dronecode_sdk::rpc::camera::EulerAngle()); + + euler_angle->set_yaw_deg(yaw_deg); + euler_angle->set_pitch_deg(pitch_deg); + euler_angle->set_roll_deg(roll_deg); + + return euler_angle; +} + TEST_F(CameraServiceImplTest, doesNotSendCaptureInfoIfCallbackNotCalled) { std::vector camera_info_events; @@ -768,9 +791,15 @@ dronecode_sdk::Camera::CaptureInfo CameraServiceImplTest::createArbitraryCapture quaternion.y = 1.1f; quaternion.z = -4.2f; + dronecode_sdk::Camera::CaptureInfo::EulerAngle euler_angle; + euler_angle.yaw_deg = 12.6f; + euler_angle.pitch_deg = 102.3f; + euler_angle.roll_deg = -24.0f; + dronecode_sdk::Camera::CaptureInfo capture_info; capture_info.position = position; - capture_info.quaternion = quaternion; + capture_info.attitude_quaternion = quaternion; + capture_info.attitude_euler_angle = euler_angle; capture_info.time_utc_us = ARBITRARY_INT; capture_info.success = ARBITRARY_BOOL; capture_info.index = ARBITRARY_INT; diff --git a/integration_tests/camera_take_photo.cpp b/integration_tests/camera_take_photo.cpp index a730d44727..d6c3a34557 100644 --- a/integration_tests/camera_take_photo.cpp +++ b/integration_tests/camera_take_photo.cpp @@ -92,8 +92,9 @@ void receive_capture_info(Camera::CaptureInfo capture_info) << capture_info.position.absolute_altitude_m << " m, " << capture_info.position.relative_altitude_m << " m (relative to home)."; LogInfo() << "Time: " << capture_info.time_utc_us << " us."; - LogInfo() << "Attitude: " << capture_info.quaternion.w << ", " << capture_info.quaternion.x - << ", " << capture_info.quaternion.y << ", " << capture_info.quaternion.z << "."; + LogInfo() << "Attitude: " << capture_info.attitude_quaternion.w << ", " + << capture_info.attitude_quaternion.x << ", " << capture_info.attitude_quaternion.y + << ", " << capture_info.attitude_quaternion.z << "."; LogInfo() << "Result: " << (capture_info.success ? "success" : "fail") << "."; LogInfo() << "Saved to " << capture_info.file_url << " (" << capture_info.index << ")."; diff --git a/plugins/camera/camera.cpp b/plugins/camera/camera.cpp index a93e01194d..a13cc1b5d9 100644 --- a/plugins/camera/camera.cpp +++ b/plugins/camera/camera.cpp @@ -238,7 +238,8 @@ std::ostream &operator<<(std::ostream &str, bool operator==(const Camera::CaptureInfo &lhs, const Camera::CaptureInfo &rhs) { - return lhs.position == rhs.position && lhs.quaternion == rhs.quaternion && + return lhs.position == rhs.position && lhs.attitude_quaternion == rhs.attitude_quaternion && + lhs.attitude_euler_angle == rhs.attitude_euler_angle && lhs.time_utc_us == rhs.time_utc_us && lhs.success == rhs.success && lhs.index == rhs.index && lhs.file_url == rhs.file_url; } @@ -247,7 +248,8 @@ std::ostream &operator<<(std::ostream &str, Camera::CaptureInfo const &capture_i { return str << "[" << std::endl << "position: " << capture_info.position << std::endl - << "quaternion: " << capture_info.quaternion << std::endl + << "attitude_quaternion: " << capture_info.attitude_quaternion << std::endl + << "attitude_euler_angle: " << capture_info.attitude_euler_angle << std::endl << "time_utc_us: " << capture_info.time_utc_us << std::endl << "is_success: " << capture_info.success << std::endl << "index: " << capture_info.index << std::endl @@ -277,8 +279,21 @@ bool operator==(const Camera::CaptureInfo::Quaternion &lhs, std::ostream &operator<<(std::ostream &str, Camera::CaptureInfo::Quaternion const &quaternion) { - return str << "[w: " << quaternion.w << "x: " << quaternion.x << "y: " << quaternion.y - << "z: " << quaternion.z << "]"; + return str << "[w: " << quaternion.w << ", x: " << quaternion.x << ", y: " << quaternion.y + << ", z: " << quaternion.z << "]"; +} + +bool operator==(const Camera::CaptureInfo::EulerAngle &lhs, + const Camera::CaptureInfo::EulerAngle &rhs) +{ + return lhs.yaw_deg == rhs.yaw_deg && lhs.pitch_deg == rhs.pitch_deg && + lhs.roll_deg == rhs.roll_deg; +} + +std::ostream &operator<<(std::ostream &str, Camera::CaptureInfo::EulerAngle const &euler_angle) +{ + return str << "[yaw_deg: " << euler_angle.yaw_deg << ", pitch_deg: " << euler_angle.pitch_deg + << ", roll_deg: " << euler_angle.roll_deg; } bool operator==(const Camera::Status &lhs, const Camera::Status &rhs) diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index 40f5a98b8c..5863349dcc 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -751,11 +751,12 @@ void CameraImpl::process_camera_image_captured(const mavlink_message_t &message) capture_info.position.absolute_altitude_m = image_captured.alt / 1e3f; capture_info.position.relative_altitude_m = image_captured.relative_alt / 1e3f; capture_info.time_utc_us = image_captured.time_utc; - capture_info.quaternion.w = image_captured.q[0]; - capture_info.quaternion.x = image_captured.q[1]; - capture_info.quaternion.y = image_captured.q[2]; - capture_info.quaternion.z = image_captured.q[3]; - capture_info.euler_angle = to_euler_angle_from_quaternion(capture_info.quaternion); + capture_info.attitude_quaternion.w = image_captured.q[0]; + capture_info.attitude_quaternion.x = image_captured.q[1]; + capture_info.attitude_quaternion.y = image_captured.q[2]; + capture_info.attitude_quaternion.z = image_captured.q[3]; + capture_info.attitude_euler_angle = + to_euler_angle_from_quaternion(capture_info.attitude_quaternion); capture_info.file_url = std::string(image_captured.file_url); capture_info.success = (image_captured.capture_result == 1); capture_info.index = image_captured.image_index; diff --git a/plugins/camera/include/plugins/camera/camera.h b/plugins/camera/include/plugins/camera/camera.h index 29512ecf70..719cc9ec96 100644 --- a/plugins/camera/include/plugins/camera/camera.h +++ b/plugins/camera/include/plugins/camera/camera.h @@ -258,7 +258,7 @@ class Camera : public PluginBase { float x; /**< @brief Quaternion entry 1 also denoted as b. */ float y; /**< @brief Quaternion entry 2 also denoted as c. */ float z; /**< @brief Quaternion entry 3 also denoted as d. */ - } quaternion; /**< @brief Quaternion of camera orientation. */ + } attitude_quaternion; /**< @brief Quaternion of camera orientation. */ /** * @brief Euler angle type. @@ -273,7 +273,7 @@ class Camera : public PluginBase { float pitch_deg; /**< @brief Pitch angle in degrees, positive is pitching nose up. */ float yaw_deg; /**< @brief Yaw angle in degrees, positive is clock-wise seen from above. */ - } euler_angle; /**< @brief Euler Angle of camera orientation. */ + } attitude_euler_angle; /**< @brief Euler Angle of camera orientation. */ uint64_t time_utc_us; /**< @brief Timestamp in UTC (since UNIX epoch) in microseconds. */ bool success; /**< @brief True if capture was successful. */ @@ -657,6 +657,21 @@ bool operator==(const Camera::CaptureInfo::Quaternion &lhs, */ std::ostream &operator<<(std::ostream &str, Camera::CaptureInfo::Quaternion const &quaternion); +/** + * @brief Equal operator to compare two `Camera::CaptureInfo::EulerAngle` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Camera::CaptureInfo::EulerAngle &lhs, + const Camera::CaptureInfo::EulerAngle &rhs); + +/** + * @brief Stream operator to print information about a `Camera::CaptureInfo::EulerAngle`. + * + * @return A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, Camera::CaptureInfo::EulerAngle const &euler_angle); + /** * @brief Equal operator to compare two `Camera::Status` objects. *