diff --git a/Cargo.lock b/Cargo.lock index 124efab..02e9950 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -71,6 +71,27 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +[[package]] +name = "csv" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac574ff4d437a7b5ad237ef331c17ccca63c46479e5b5453eb8e10bb99a759fe" +dependencies = [ + "csv-core", + "itoa", + "ryu", + "serde", +] + +[[package]] +name = "csv-core" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5efa2b3d7902f4b634a20cae3c9c4e6209dc4779feb6863329607560143efa70" +dependencies = [ + "memchr", +] + [[package]] name = "erased-serde" version = "0.3.31" @@ -85,6 +106,7 @@ name = "fsds-rs" version = "0.1.0" dependencies = [ "anyhow", + "csv", "fsds-rs-derive", "msgpack-rpc", "rmp", @@ -202,6 +224,12 @@ version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d231dfb89cfffdbc30e7fc41579ed6066ad03abda9e567ccafae602b97ec5024" +[[package]] +name = "itoa" +version = "1.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" + [[package]] name = "libc" version = "0.2.158" @@ -336,6 +364,12 @@ version = "0.1.24" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "719b953e2095829ee67db738b3bfa9fa368c94900df327b3f07fe6e794d2fe1f" +[[package]] +name = "ryu" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f3cb5ba0dc43242ce17de99c180e96db90b235b8a9fdc9543c96d2209116bd9f" + [[package]] name = "serde" version = "1.0.209" diff --git a/Cargo.toml b/Cargo.toml index 4928b3b..276362b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,6 +15,7 @@ tokio-util = "0.7.11" # Utils anyhow = "=1.0.86" struct_iterable = "0.1.1" +csv = "1.3.0" # Derive fsds-rs-derive = { path = "./fsds-rs-derive" } diff --git a/examples/control.rs b/examples/control.rs new file mode 100644 index 0000000..f29c14e --- /dev/null +++ b/examples/control.rs @@ -0,0 +1,33 @@ +use fsds_rs::{client, types::CarControls}; +use std::{thread::sleep, time::Duration}; + +/// The name of the vehicle to control. +const VEHICLE_NAME: &str = "FSCar"; + +#[tokio::main] +async fn main() -> Result<(), anyhow::Error> { + // ---------- // + // CONNECTION // + // ---------- // + // Connect to the simulator. + let mut client = client::FSDSClient::init(None, None) + .await + .expect("Cannot establish a connection with the simulator"); + // Check network connection, exit if not connected. + client.ping().await?; + // Enable control of the vehicle via the API. + client.enable_api_control(VEHICLE_NAME).await?; + + // ---------------- // + // CONTROL THE CAR! // + // ---------------- // + // Set the throttle to 1.0. + let mut controls = CarControls::default(); + controls.throttle = 1.0; + client.set_car_controls(controls, VEHICLE_NAME).await; + + // Loop to keep the program running. + loop { + sleep(Duration::from_secs(1)); + } +} diff --git a/examples/gather_data.rs b/examples/gather_data.rs new file mode 100644 index 0000000..71b6a1b --- /dev/null +++ b/examples/gather_data.rs @@ -0,0 +1,131 @@ +use std::{fs::File, str::FromStr}; + +use fsds_rs::{ + client, + types::{KinematicsState, Vector3r}, +}; +use msgpack_rpc::Value; + +/// The name of the vehicle to control. +const VEHICLE_NAME: &str = "FSCar"; +/// The path to the CSV file containing the ground truth cones. +const CSV_PATH: &str = + "../Formula-Student-Driverless-Simulator/maps/FormulaElectricBelgium/track_droneport.csv"; + +enum Class { + Vehicle, + ConeYellow, + ConeBlue, + ConeBigOrange, +} + +impl FromStr for Class { + type Err = String; + + fn from_str(s: &str) -> Result { + match s { + "yellow" => Ok(Class::ConeYellow), + "blue" => Ok(Class::ConeBlue), + "big_orange" => Ok(Class::ConeBigOrange), + _ => Err(format!("Unknown class: {}", s)), + } + } +} + +/// An object in the 3D space. +/// Distances in meters. +struct Object { + vector: Vector3r, + class: Class, +} + +impl Object { + fn new(x_val: f64, y_val: f64, z_val: f64, class: Class) -> Self { + Self { + vector: Vector3r { + x_val, + y_val, + z_val, + }, + class, + } + } + + fn from_vector3r(vector: Vector3r, class: Class) -> Self { + Self { vector, class } + } +} + +#[tokio::main] +async fn main() -> Result<(), anyhow::Error> { + // ---------- // + // CONNECTION // + // ---------- // + // Connect to the simulator. + let mut client = client::FSDSClient::init(None, None) + .await + .expect("Cannot establish a connection with the simulator"); + // Check network connection, exit if not connected. + client.ping().await?; + + // Get cones ground truth from CSV file. + let mut cones_gt = Vec::new(); + // Open and read the CSV file. + let csv_file = File::open(CSV_PATH)?; + let mut csv_reader = csv::Reader::from_reader(csv_file); + for row in csv_reader.records() { + // Create a dictionary for each row. + let row = row?; + let entry = Object::new( + row[1].parse().unwrap(), + row[2].parse().unwrap(), + 0.0, // The cones are on the ground. + row[0].parse().unwrap(), + ); + // Append the dictionary to the list. + cones_gt.push(entry); + } + + // --------- // + // MAIN LOOP // + // --------- // + let mut instant = std::time::Instant::now(); + let mut fps = 0; + loop { + // Get the ground truth kinematics of the vehicle. + let kinematics_gt: KinematicsState = client + .sim_get_ground_truth_kinematics(VEHICLE_NAME) + .await? + .try_into() + .unwrap(); + let car_position = kinematics_gt.position; + let car_orientation = kinematics_gt.orientation; + + let _relative_cones_gt = cones_gt + .iter() + .map(|cone| cone.vector - car_position) + .collect::>(); + + // Get onboard image. + let image = client + .sim_get_image("cam1", fsds_rs::types::ImageType::Scene, VEHICLE_NAME) + .await?; + + if let Value::Binary(_image) = image { + // Save the image to a file. + //let mut file = File::create("image.png")?; + //file.write_all(&image)?; + + // Do something with the image and the cones. + + // Calculate the FPS. + if instant.elapsed().as_secs() >= 2 { + println!("FPS: {}", fps / 2); + fps = 0; + instant = std::time::Instant::now(); + } else { + fps += 1; + } + } + } +} diff --git a/examples/ping.rs b/examples/ping.rs deleted file mode 100644 index 1a176ac..0000000 --- a/examples/ping.rs +++ /dev/null @@ -1,27 +0,0 @@ -use fsds_rs::{client, types::CarControls}; -use std::{thread::sleep, time::Duration}; - -/// The name of the vehicle to control. -const VEHICLE_NAME: &str = "FSCar"; - -#[tokio::main] -async fn main() { - let mut client = client::FSDSClient::init(None, None) - .await - .expect("Cannot establish a connection with the simulator"); - - // Trying connection. - client.ping().await; - - client.enable_api_control(VEHICLE_NAME).await; - - let mut controls = CarControls::default(); - - controls.throttle = 1.0; - let value = client.set_car_controls(controls, VEHICLE_NAME).await; - println!("{:?}", value); - - loop { - sleep(Duration::from_secs(1)); - } -} diff --git a/fsds-rs-derive/src/lib.rs b/fsds-rs-derive/src/lib.rs index 6e36a34..3afb209 100644 --- a/fsds-rs-derive/src/lib.rs +++ b/fsds-rs-derive/src/lib.rs @@ -2,21 +2,20 @@ extern crate proc_macro; use proc_macro::TokenStream; use quote::quote; -use syn::{parse_macro_input, DeriveInput}; +use syn::{parse_macro_input, Data, DataStruct, DeriveInput, Fields, FieldsNamed}; /// Implements From for Value for an iterable struct. -#[proc_macro_derive(IntoValue)] -pub fn from_for_value_derive(input: TokenStream) -> TokenStream { +#[proc_macro_derive(FromIntoValue)] +pub fn from_and_into_for_value_derive(input: TokenStream) -> TokenStream { let input = parse_macro_input!(input as DeriveInput); let name = input.ident; - let expanded = quote! { + let from_impl = quote! { impl From<#name> for Value { fn from(value: #name) -> Self { let vec = value .iter() .map(|(k, v)| { - v.type_id(); (k.into(), any_to_value(v)) }) .collect(); @@ -26,5 +25,66 @@ pub fn from_for_value_derive(input: TokenStream) -> TokenStream { } }; + let fields = if let Data::Struct(DataStruct { fields: Fields::Named(FieldsNamed { named, .. }), .. }) = &input.data { + named + } else { + // Works only for structs with named fields. + unimplemented!(); + }; + + let try_from_impl = { + + let fields_def = fields.iter().map(|field| { + let field_name = &field.ident; + quote! { + let pos = map.iter().position(|(k, _)| k + .as_str() + .unwrap_or("") // TODO: throw instead this error for better + // debugging: Value::Map should be Vec<(String, _)> to be converted to a struct, but {} was found as a key + == stringify!(#field_name) + ).ok_or(anyhow::anyhow!("Field {} not found in Value::Map.", stringify!(#field_name)))?; + let #field_name = map + .remove(pos) + .1 + .try_into() + .map_err(|_| anyhow::anyhow!("Every field of {} should be convertible to Value.", stringify!(#name)))?; + } + }); + + let fields = fields.iter().map(|field| { + let field_name = &field.ident; + quote! { + #field_name + } + }); + + quote! { + impl TryFrom for #name { + type Error = anyhow::Error; + + fn try_from(value: Value) -> Result { + match value { + Value::Map(mut map) => { + #(#fields_def)* + if map.is_empty() { + Ok(#name { + #(#fields),* + }) + } else { + Err(anyhow::anyhow!("Value::Map contains extra fields: {:?}", map)) + } + } + _ => Err(anyhow::anyhow!("Value should be a Map to be converted to {}", stringify!(#name))), + } + } + } + } + }; + + let expanded = quote! { + #from_impl + #try_from_impl + }; + TokenStream::from(expanded) } \ No newline at end of file diff --git a/image.png b/image.png new file mode 100644 index 0000000..aa52869 Binary files /dev/null and b/image.png differ diff --git a/src/client.rs b/src/client.rs index 24221a2..5010d9d 100644 --- a/src/client.rs +++ b/src/client.rs @@ -30,28 +30,39 @@ impl FSDSClient { /// /// Note that you must call `enable_api_control` again after the call to /// reset. - pub async fn reset(&mut self) -> Result { - self.client.request("reset", &[]).await + pub async fn reset(&mut self) -> Result { + self.client + .request("reset", &[]) + .await + .map_err(|e| anyhow::anyhow!(e)) } /// If connection is established then this call will return Ok(_) otherwise /// it will be blocked until timeout. - pub async fn ping(&mut self) -> Result { - self.client.request("ping", &[]).await + pub async fn ping(&mut self) -> Result { + self.client + .request("ping", &[]) + .await + .map_err(|e| anyhow::anyhow!(e)) } /// Enables API control for vehicle corresponding to vehicle_name. - pub async fn enable_api_control(&mut self, vehicle_name: &str) -> Result { + pub async fn enable_api_control(&mut self, vehicle_name: &str) -> Result { self.client .request("enableApiControl", &[true.into(), vehicle_name.into()]) .await + .map_err(|e| anyhow::anyhow!(e)) } /// Disable API control for vehicle corresponding to vehicle_name. - pub async fn disable_api_control(&mut self, vehicle_name: &str) -> Result { + pub async fn disable_api_control( + &mut self, + vehicle_name: &str, + ) -> Result { self.client .request("enableApiControl", &[false.into(), vehicle_name.into()]) .await + .map_err(|e| anyhow::anyhow!(e)) } /// Returns true if API control is established. @@ -59,10 +70,14 @@ impl FSDSClient { /// If false (which is default) then API calls would be ignored. After a /// successful call to `enableApiControl`, `isApiControlEnabled` should /// return true. - pub async fn is_api_control_enabled(&mut self, vehicle_name: &str) -> Result { + pub async fn is_api_control_enabled( + &mut self, + vehicle_name: &str, + ) -> Result { self.client .request("isApiControlEnabled", &[vehicle_name.into()]) .await + .map_err(|e| anyhow::anyhow!(e)) } /// Get a single image. @@ -76,13 +91,14 @@ impl FSDSClient { camera_name: &str, image_type: ImageType, vehicle_name: &str, - ) -> Result { + ) -> Result { self.client .request( "simGetImage", &[camera_name.into(), image_type.into(), vehicle_name.into()], ) .await + .map_err(|e| anyhow::anyhow!(e)) } /// Get multiple images. @@ -93,27 +109,28 @@ impl FSDSClient { &mut self, requests: &[ImageRequest], vehicle_name: &str, - ) -> Result { + ) -> Result { self.client .request( "simGetImages", &[ - Value::Array(requests.into_iter().map(|r| r.clone().into()).collect()), + Value::Array(requests.iter().map(|r| r.clone().into()).collect()), vehicle_name.into(), ], ) .await + .map_err(|e| anyhow::anyhow!(e)) } /// Get Ground truth kinematics of the vehicle. pub async fn sim_get_ground_truth_kinematics( &mut self, vehicle_name: &str, - ) -> Result { + ) -> Result { self.client .request("simGetGroundTruthKinematics", &[vehicle_name.into()]) .await - .map(|v| v.into()) + .map_err(|e| anyhow::anyhow!(e)) } pub async fn set_car_controls(&mut self, controls: CarControls, vehicle_name: &str) { @@ -121,9 +138,10 @@ impl FSDSClient { .request("setCarControls", &[controls.into(), vehicle_name.into()]); } - pub async fn get_car_state(&mut self, vehicle_name: &str) -> Result { + pub async fn get_car_state(&mut self, vehicle_name: &str) -> Result { self.client .request("getCarState", &[vehicle_name.into()]) .await + .map_err(|e| anyhow::anyhow!(e)) } } diff --git a/src/lib.rs b/src/lib.rs index 7b6f5fa..6b87bc4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -4,8 +4,6 @@ pub mod utils; #[cfg(test)] mod tests { - use super::*; - #[test] fn client() {} diff --git a/src/types.rs b/src/types.rs index 9213de8..d052126 100644 --- a/src/types.rs +++ b/src/types.rs @@ -4,7 +4,7 @@ //! Struct are serialized to `msgpack_rpc::Value::Map` and vice versa in order //! to be sent and received from the simulator. -use fsds_rs_derive::IntoValue; +use fsds_rs_derive::FromIntoValue; use msgpack_rpc::Value; use std::{ any::Any, @@ -20,6 +20,21 @@ fn any_to_value(value: &dyn Any) -> Value { Value::from(*value) } else if let Some(value) = value.downcast_ref::() { Value::from(*value) + } else if let Some(value) = value.downcast_ref::() { + Value::String(value.clone().into()) + } else if let Some(value) = value.downcast_ref::>() { + Value::Array(value.iter().map(|v| Value::from(*v)).collect()) + } else if let Some(value) = value.downcast_ref::>() { + Value::Array(value.iter().map(|v| Value::from(*v)).collect()) + } else if let Some(value) = value.downcast_ref::>() { + Value::Array(value.iter().map(|v| Value::from(*v)).collect()) + } else if let Some(value) = value.downcast_ref::>() { + Value::Array( + value + .iter() + .map(|v| Value::String(v.clone().into())) + .collect(), + ) } else { Value::Nil } @@ -46,14 +61,36 @@ impl From for Value { } } +impl TryFrom for ImageType { + type Error = anyhow::Error; + + fn try_from(value: Value) -> Result { + match value { + // TODO: removeunwrap below + Value::Integer(value) => Ok(match value.as_u64().unwrap() { + 0 => ImageType::Scene, + 1 => ImageType::DepthPlanner, + 2 => ImageType::DepthPerspective, + 3 => ImageType::DepthVis, + 4 => ImageType::DisparityNormalized, + 5 => ImageType::Segmentation, + 6 => ImageType::SurfaceNormals, + 7 => ImageType::Infrared, + _ => return Err(anyhow::anyhow!("Invalid ImageType")), + }), + _ => Err(anyhow::anyhow!("Invalid ImageType")), + } + } +} + /// --------- /// /// VECTOR 3R /// /// --------- /// -#[derive(Copy, Clone, Default, Iterable, IntoValue)] +#[derive(Copy, Clone, Default, Iterable, FromIntoValue, Debug)] pub struct Vector3r { - x_val: f64, - y_val: f64, - z_val: f64, + pub x_val: f64, + pub y_val: f64, + pub z_val: f64, } impl Vector3r { @@ -130,7 +167,7 @@ impl MulAssign for Vector3r { /// QUATERNIONR /// /// ----------- /// -#[derive(Copy, Clone, Default, Iterable, IntoValue)] +#[derive(Copy, Clone, Default, Iterable, FromIntoValue, Debug)] pub struct Quaternionr { w_val: f64, x_val: f64, @@ -261,6 +298,7 @@ impl Mul for Quaternionr { impl Div for Quaternionr { type Output = Self; + #[allow(clippy::suspicious_arithmetic_impl)] fn div(self, other: Self) -> Self { self * other.inverse() } @@ -289,7 +327,7 @@ impl From for Quaternionr { /// ---- /// /// POSE /// /// ---- /// -#[derive(Copy, Clone, Default, Iterable, IntoValue)] +#[derive(Copy, Clone, Default, Iterable, FromIntoValue)] pub struct Pose { position: Vector3r, orientation: Quaternionr, @@ -314,7 +352,7 @@ impl Pose { /// --------- /// /// GEO POINT /// /// --------- /// -#[derive(Copy, Clone, Default, Iterable, IntoValue)] +#[derive(Copy, Clone, Default, Iterable, FromIntoValue)] pub struct GeoPoint { latitude: f64, longitude: f64, @@ -324,7 +362,7 @@ pub struct GeoPoint { /// ------------- /// /// IMAGE REQUEST /// /// ------------- /// -#[derive(Iterable, Clone, IntoValue)] +#[derive(Iterable, Clone, FromIntoValue)] pub struct ImageRequest { camera_name: String, image_type: ImageType, @@ -332,7 +370,7 @@ pub struct ImageRequest { compress: bool, } -impl<'a> Default for ImageRequest { +impl Default for ImageRequest { fn default() -> Self { Self { camera_name: "0".to_string(), @@ -346,9 +384,10 @@ impl<'a> Default for ImageRequest { /// -------------- /// /// IMAGE RESPONSE /// /// -------------- /// -#[derive(Iterable, IntoValue)] +#[derive(Iterable, FromIntoValue)] pub struct ImageResponse { - image_data: (u8, f64), + image_data_uint8: u64, + image_data_float: f64, camera_position: Vector3r, camera_orientation: Quaternionr, timestamp: u64, // TODO: SystemTime? @@ -363,7 +402,8 @@ pub struct ImageResponse { impl Default for ImageResponse { fn default() -> Self { Self { - image_data: (0, 0.0), + image_data_uint8: 0, + image_data_float: 0.0, camera_position: Default::default(), camera_orientation: Default::default(), timestamp: 0, @@ -380,7 +420,7 @@ impl Default for ImageResponse { /// ------------ /// /// CAR CONTROLS /// /// ------------ /// -#[derive(Iterable, IntoValue)] +#[derive(Iterable, FromIntoValue)] pub struct CarControls { pub throttle: f64, pub steering: f64, @@ -408,20 +448,20 @@ impl Default for CarControls { /// ---------------- /// /// KINEMATICS STATE /// /// ---------------- /// -#[derive(Iterable, IntoValue, Default)] +#[derive(Iterable, FromIntoValue, Default, Debug)] pub struct KinematicsState { - position: Vector3r, - orientation: Quaternionr, - linear_velocity: Vector3r, - angular_velocity: Vector3r, - linear_acceleration: Vector3r, - angular_acceleration: Vector3r, + pub position: Vector3r, + pub orientation: Quaternionr, + pub linear_velocity: Vector3r, + pub angular_velocity: Vector3r, + pub linear_acceleration: Vector3r, + pub angular_acceleration: Vector3r, } /// ----------------- /// /// ENVIRONMENT STATE /// /// ----------------- /// -#[derive(Iterable, IntoValue, Default)] +#[derive(Iterable, FromIntoValue, Default)] pub struct EnvironmentState { pub position: Vector3r, pub geo_point: GeoPoint, @@ -434,7 +474,7 @@ pub struct EnvironmentState { /// -------------- /// /// COLLISION INFO /// /// -------------- /// -#[derive(Iterable, IntoValue)] +#[derive(Iterable, FromIntoValue)] pub struct CollisionInfo { pub has_collided: bool, pub normal: Vector3r, @@ -449,7 +489,7 @@ pub struct CollisionInfo { /// --------- /// /// CAR STATE /// /// --------- /// -#[derive(Iterable, IntoValue)] +#[derive(Iterable, FromIntoValue)] pub struct CarState { pub speed: f64, pub kinematics_estimated: KinematicsState, @@ -459,7 +499,7 @@ pub struct CarState { /// ----------- /// /// POSITION 2D /// /// ----------- /// -#[derive(Iterable, IntoValue, Default)] +#[derive(Iterable, FromIntoValue, Default)] pub struct Position2D { pub x_val: f64, pub y_val: f64, @@ -468,19 +508,19 @@ pub struct Position2D { /// ------------- /// /// REFEREE STATE /// /// ------------- /// -#[derive(Iterable, IntoValue, Default)] +#[derive(Iterable, Default)] pub struct RefereeState { pub doo_counter: u64, pub laps: f64, pub initial_position: Position2D, - pub cones: Vec, + pub cones: Vec, // TODO: Vec does not implement Into } // TODO: // ----------------- /// // PROJECTION MATRIX /// // ----------------- /// -// #[derive(Iterable, IntoValue, Default)] +// #[derive(Iterable, FromIntoValue, Default)] // pub struct ProjectionMatrix { // pub matrix: Vec<_>, // }