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.

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

use 'thiserror' to derive errors #20

Merged
merged 2 commits into from
Oct 28, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions Cargo.lock

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

1 change: 1 addition & 0 deletions Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ bytes = "1.1.0"
prost = "0.8.0"

futures-util = { version = "0.3", default-features = false }
thiserror = "1.0.30"

[dev-dependencies]
tokio = { version = "1.12.0", features = ["rt-multi-thread", "macros", "time"] }
Expand Down
27 changes: 8 additions & 19 deletions examples/info.rs
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
use libmavsdk::{info, RequestError, System};
use libmavsdk::System;
use std::io::{self, Write};

#[tokio::main]
async fn main() {
async fn main() -> Result<(), Box<dyn std::error::Error>> {
let args: Vec<String> = std::env::args().skip(1).collect();

if args.len() > 1 {
Expand All @@ -14,22 +14,11 @@ async fn main() {

let url = args.get(0).cloned();

let mut system = match System::connect(url).await {
Ok(system) => system,
Err(err) => {
println!("Connection error: {:?}", err);
return;
}
};
let mut system = System::connect(url).await?;

match system.info.get_version().await {
Ok(v) => println!("Version received: {:?}", v),
Err(RequestError::MavErr(info::InfoError::Unknown(s))) => {
println!("Unknown MAVLink error ({:?})", s);
}
Err(RequestError::MavErr(info::InfoError::InformationNotReceivedYet(s))) => {
println!("{}", s);
}
Err(RequestError::RpcErr(rpc_err)) => println!("RPC error: {:?}", rpc_err),
};
let version = system.info.get_version().await?;

println!("Version received: {:?}", version);

Ok(())
}
31 changes: 9 additions & 22 deletions examples/mocap.rs
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
use libmavsdk::{mocap, RequestError, System};
use libmavsdk::{mocap, System};
use std::io::{self, Write};
use std::time::Duration;

#[tokio::main]
async fn main() {
async fn main() -> Result<(), Box<dyn std::error::Error>> {
let args: Vec<String> = std::env::args().skip(1).collect();

if args.len() > 1 {
Expand All @@ -15,32 +15,17 @@ async fn main() {

let url = args.get(0).cloned();

let mut system = match System::connect(url).await {
Ok(system) => system,
Err(err) => {
println!("Connection error: {:?}", err);
return;
}
};
let mut system = System::connect(url).await?;

let mut vision_position_estimate = mocap::VisionPositionEstimate::default();
vision_position_estimate.pose_covariance.covariance_matrix = vec![std::f32::NAN];

let mut counter: i32 = 500;

while counter > 0 {
if let Err(error) = system
for _ in 0..500 {
system
.mocap
.set_vision_position_estimate(vision_position_estimate.clone())
.await
{
match error {
RequestError::MavErr(mav_err) => println!("MAVLink error: {:?}", mav_err),
RequestError::RpcErr(rpc_err) => println!("RPC error: {:?}", rpc_err.message()),
}
return;
};
counter -= 1;
.await?;

vision_position_estimate.position_body.x_m += 0.1;
vision_position_estimate.position_body.y_m -= 0.1;
vision_position_estimate.position_body.z_m -= 0.01;
Expand All @@ -49,4 +34,6 @@ async fn main() {
}

println!("All sended successfully!");

Ok(())
}
21 changes: 5 additions & 16 deletions examples/telemetry.rs
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ use libmavsdk::System;
use std::io::{self, Write};

#[tokio::main]
async fn main() {
async fn main() -> Result<(), Box<dyn std::error::Error>> {
let args: Vec<String> = std::env::args().skip(1).collect();

if args.len() > 1 {
Expand All @@ -15,23 +15,12 @@ async fn main() {

let url = args.get(0).cloned();

let mut system = match System::connect(url).await {
Ok(system) => system,
Err(err) => {
println!("Connection error: {:?}", err);
return;
}
};
let mut system = System::connect(url).await?;

let mut stream_odometry = system.telemetry.subscribe_odometry().await.unwrap();
let mut stream_odometry = system.telemetry.subscribe_odometry().await?;
while let Some(odometry) = stream_odometry.next().await {
match odometry {
Ok(odometry) => println!("Received: {:?}", odometry),
Err(err) => {
println!("Break: {:?}", err);
break;
}
}
println!("Received: {:?}", odometry?);
}
println!("Exit");
Ok(())
}
36 changes: 19 additions & 17 deletions src/generated/info/mod.rs
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
use super::super::FromRpcResponse;
use super::super::RequestError::{MavErr, RpcErr};
use super::super::RequestError;
use super::super::RequestResult;
use super::super::TonicResult;

Expand Down Expand Up @@ -45,42 +45,44 @@ impl From<&pb::Version> for Version {
}
}

#[derive(PartialEq, Clone, Debug)]
#[derive(PartialEq, Clone, Debug, thiserror::Error)]
pub enum InfoError {
#[error("Unknown error: {0}")]
Unknown(String),
#[error("Information not yet received: {0}")]
InformationNotReceivedYet(String),
}

impl From<InfoError> for RequestError<InfoError> {
fn from(e: InfoError) -> Self {
Self::Mav(e)
}
}

pub type GetVersionResult = RequestResult<Version, InfoError>;

impl FromRpcResponse<pb::GetVersionResponse> for GetVersionResult {
fn from_rpc_response(rpc_get_version_response: TonicResult<pb::GetVersionResponse>) -> Self {
let get_version_response = rpc_get_version_response.map_err(RpcErr)?.into_inner();
let get_version_response = rpc_get_version_response?.into_inner();

let rpc_info_result = get_version_response
.info_result
.ok_or_else(|| MavErr(InfoError::Unknown("InfoResult does not received".into())))?;
.ok_or_else(|| InfoError::Unknown("InfoResult does not received".into()))?;

let info_result =
pb::info_result::Result::from_i32(rpc_info_result.result).ok_or_else(|| {
MavErr(InfoError::Unknown(
"Unsupported InfoResult.result value".into(),
))
})?;
let info_result = pb::info_result::Result::from_i32(rpc_info_result.result)
.ok_or_else(|| InfoError::Unknown("Unsupported InfoResult.result value".into()))?;

match info_result {
pb::info_result::Result::Success => match get_version_response.version {
Some(ref rpc_version) => Ok(Version::from(rpc_version)),
None => Err(MavErr(InfoError::Unknown(
"Version does not received".into(),
))),
None => Err(InfoError::Unknown("Version does not received".into()).into()),
},
pb::info_result::Result::Unknown => {
Err(MavErr(InfoError::Unknown(rpc_info_result.result_str)))
Err(InfoError::Unknown(rpc_info_result.result_str).into())
}
pb::info_result::Result::InformationNotReceivedYet => {
Err(InfoError::InformationNotReceivedYet(rpc_info_result.result_str).into())
}
pb::info_result::Result::InformationNotReceivedYet => Err(MavErr(
InfoError::InformationNotReceivedYet(rpc_info_result.result_str),
)),
}
}
}
Expand Down
43 changes: 24 additions & 19 deletions src/generated/mocap/mod.rs
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
use super::super::FromRpcResponse;
use super::super::RequestError::{MavErr, RpcErr};
use super::super::RequestError;
use super::super::RequestResult;
use super::super::TonicResult;
use std::convert::Into;
Expand Down Expand Up @@ -225,18 +225,28 @@ impl Into<pb::Quaternion> for Quaternion {
}
}

#[derive(PartialEq, Clone, Debug)]
#[derive(PartialEq, Clone, Debug, thiserror::Error)]
pub enum MocapError {
/// Unknown error
#[error("Unknown error: {0}")]
Unknown(String),
/// No system is connected
#[error("No system is connected: {0}")]
NoSystem(String),
/// Connection error
#[error("Connection error: {0}")]
ConnectionError(String),
/// Invalid request data
#[error("Invalid request: {0}")]
InvalidRequestData(String),
}

impl From<MocapError> for RequestError<MocapError> {
fn from(e: MocapError) -> Self {
Self::Mav(e)
}
}

pub type SetVisionPositionEstimateResult = RequestResult<(), MocapError>;

impl FromRpcResponse<pb::SetVisionPositionEstimateResponse> for SetVisionPositionEstimateResult {
Expand All @@ -245,33 +255,28 @@ impl FromRpcResponse<pb::SetVisionPositionEstimateResponse> for SetVisionPositio
pb::SetVisionPositionEstimateResponse,
>,
) -> Self {
let rpc_mocap_result = rpc_set_vision_position_estimate_response
.map_err(RpcErr)?
let rpc_mocap_result = rpc_set_vision_position_estimate_response?
.into_inner()
.mocap_result
.ok_or_else(|| MavErr(MocapError::Unknown("MocapResult does not received".into())))?;
.ok_or_else(|| MocapError::Unknown("MocapResult does not received".into()))?;

let mocap_result =
pb::mocap_result::Result::from_i32(rpc_mocap_result.result).ok_or_else(|| {
MavErr(MocapError::Unknown(
"Unsupported MocapResult.result value".into(),
))
})?;
let mocap_result = pb::mocap_result::Result::from_i32(rpc_mocap_result.result)
.ok_or_else(|| MocapError::Unknown("Unsupported MocapResult.result value".into()))?;

match mocap_result {
pb::mocap_result::Result::Success => Ok(()),
pb::mocap_result::Result::Unknown => {
Err(MavErr(MocapError::Unknown(rpc_mocap_result.result_str)))
Err(MocapError::Unknown(rpc_mocap_result.result_str).into())
}
pb::mocap_result::Result::NoSystem => {
Err(MavErr(MocapError::NoSystem(rpc_mocap_result.result_str)))
Err(MocapError::NoSystem(rpc_mocap_result.result_str).into())
}
pb::mocap_result::Result::ConnectionError => {
Err(MocapError::ConnectionError(rpc_mocap_result.result_str).into())
}
pb::mocap_result::Result::InvalidRequestData => {
Err(MocapError::InvalidRequestData(rpc_mocap_result.result_str).into())
}
pb::mocap_result::Result::ConnectionError => Err(MavErr(MocapError::ConnectionError(
rpc_mocap_result.result_str,
))),
pb::mocap_result::Result::InvalidRequestData => Err(MavErr(
MocapError::InvalidRequestData(rpc_mocap_result.result_str),
)),
}
}
}
Expand Down
12 changes: 8 additions & 4 deletions src/generated/telemetry/mod.rs
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
use super::super::RequestError::{MavErr, RpcErr};
use super::super::RequestError;
use super::super::RequestResult;
use futures_util::stream::{Stream, StreamExt};
use std::pin::Pin;
Expand Down Expand Up @@ -212,15 +212,19 @@ impl Into<pb::Quaternion> for Quaternion {
}
}

#[derive(Clone, Debug)]
#[derive(Clone, Debug, thiserror::Error)]
pub enum TelemetryError {
/// Unknown error
#[error("Unknown error: {0}")]
Unknown(String),
/// No system is connected
#[error("No system is connected: {0}")]
NoSystem(String),
/// Connection error
#[error("Connection error: {0}")]
ConnectionError(String),
/// Invalid request data
#[error("Invalid request: {0}")]
InvalidRequestData(String),
}

Expand Down Expand Up @@ -257,11 +261,11 @@ impl Stream for OdometryStream {
Poll::Ready(Some(rpc_result)) => match rpc_result {
Ok(odometry_response) => match odometry_response.odometry {
Some(rpc_odometry) => Poll::Ready(Some(Ok(Odometry::from(rpc_odometry)))),
None => Poll::Ready(Some(Err(MavErr(TelemetryError::Unknown(
None => Poll::Ready(Some(Err(RequestError::Mav(TelemetryError::Unknown(
"Unexpected value".into(),
))))),
},
Err(err) => Poll::Ready(Some(Err(RpcErr(err)))),
Err(err) => Poll::Ready(Some(Err(RequestError::Rpc(err)))),
},
}
}
Expand Down
13 changes: 9 additions & 4 deletions src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,15 @@ pub use generated::info;
pub use generated::mocap;
pub use generated::telemetry;

#[derive(Debug)]
pub enum RequestError<PluginMavErr> {
MavErr(PluginMavErr),
RpcErr(tonic::Status),
#[derive(Debug, thiserror::Error)]
pub enum RequestError<PluginMavErr>
where
PluginMavErr: std::error::Error,
{
#[error("MAVLink error: {0}")]
Mav(PluginMavErr),
#[error("RPC error: {0}")]
Rpc(#[from] tonic::Status),
}

pub type RequestResult<SuccessType, PluginMavErr> = Result<SuccessType, RequestError<PluginMavErr>>;
Expand Down