diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 2af2a223..f1017a7c 100644 --- a/.github/workflows/noetic.yml +++ b/.github/workflows/noetic.yml @@ -39,4 +39,4 @@ jobs: - name: Start rosbridge run: source /opt/ros/noetic/setup.bash; roslaunch rosbridge_server rosbridge_websocket.launch & disown; rosrun rosapi rosapi_node & sleep 1 - name: Integration Tests - run: source /opt/ros/noetic/setup.bash; source /root/.cargo/env; RUST_LOG=debug cargo test --features ros1_test,ros1,running_bridge,rosapi -- --test-threads 1 \ No newline at end of file + run: source /opt/ros/noetic/setup.bash; source /root/.cargo/env; RUST_LOG=debug cargo test --features ros1_test,ros1,running_bridge,rosapi,rosbridge,zenoh -- --test-threads 1 \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index 0f7b52a0..957df061 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - roslibrust_mock now provides a basic mock implementation of roslibrust's generic traits for use in building automated testing of nodes. - roslibrust_zenoh now proivides a Zenoh client that is compatible with the zenoh-ros1-plugin / zenoh-ros1-bridge +- roslibrust_ros1 now provides a ROS1 native client as a standalone crate +- roslibrust_rosbridge now provides a rosbridge client as a standalone crate +- roslibrust_rosapi now provides a generic interface for the rosapi node compatible with both rosbridge and ros1 backends ### Fixed @@ -36,6 +39,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed +- A major reorganization of the internals of roslibrust was done to improve our ability to support multiple backends. + As part of the organization the rosbridge backend has been moved under the the `rosbridge` module, so + `roslibrust::ClientHandle` now becomes `roslibrust::rosbridge::ClientHandle`. - Internal integral type Time changed from u32 to i32 representation to better align with ROS1 - Conversions between ROS Time and Duration to std::time::Time and std::time::Duration switched to TryFrom as they can be fallible. diff --git a/Cargo.lock b/Cargo.lock index e2a6fbef..c0be7886 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -450,16 +450,6 @@ version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5b63caa9aa9397e2d9480a9b13673856c78d8ac123288526c37d7839f2a86990" -[[package]] -name = "colored" -version = "2.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cbf2150cce219b664a8a70df7a1f933836724b503f8a413af9365b4dcc4d90b8" -dependencies = [ - "lazy_static", - "windows-sys 0.48.0", -] - [[package]] name = "combine" version = "4.6.7" @@ -852,6 +842,7 @@ version = "0.1.0" dependencies = [ "cargo-emit", "roslibrust_codegen", + "roslibrust_common", ] [[package]] @@ -860,6 +851,7 @@ version = "0.1.0" dependencies = [ "roslibrust_codegen", "roslibrust_codegen_macro", + "roslibrust_common", ] [[package]] @@ -1892,15 +1884,6 @@ dependencies = [ "libc", ] -[[package]] -name = "num_threads" -version = "0.1.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c7398b9c8b70908f6371f47ed36737907c87c52af34c268fed0bf0ceb92ead9" -dependencies = [ - "libc", -] - [[package]] name = "object" version = "0.36.5" @@ -2570,35 +2553,19 @@ name = "roslibrust" version = "0.11.1" dependencies = [ "abort-on-drop", - "anyhow", - "byteorder", - "dashmap", - "deadqueue", - "env_logger 0.10.2", - "futures", - "futures-util", - "gethostname", - "hyper", - "lazy_static", + "env_logger 0.11.5", "log", - "proc-macro2", - "rand", - "regex", - "reqwest", "roslibrust_codegen", "roslibrust_codegen_macro", - "roslibrust_serde_rosmsg", + "roslibrust_common", + "roslibrust_mock", + "roslibrust_ros1", + "roslibrust_rosapi", + "roslibrust_rosbridge", + "roslibrust_zenoh", "serde", - "serde-big-array", - "serde_json", - "serde_xmlrpc", - "simple_logger", - "smart-default 0.6.0", "test-log", - "thiserror 1.0.69", "tokio", - "tokio-tungstenite 0.17.2", - "uuid", ] [[package]] @@ -2611,12 +2578,13 @@ dependencies = [ "md5", "proc-macro2", "quote", + "roslibrust_common", "serde", "serde-big-array", "serde_bytes", "serde_json", "simple-error", - "smart-default 0.7.1", + "smart-default", "syn 1.0.109", "test-log", "tokio", @@ -2634,6 +2602,20 @@ dependencies = [ "syn 1.0.109", ] +[[package]] +name = "roslibrust_common" +version = "0.1.0" +dependencies = [ + "anyhow", + "futures", + "md5", + "serde", + "serde_json", + "thiserror 2.0.6", + "tokio", + "tokio-tungstenite 0.17.2", +] + [[package]] name = "roslibrust_genmsg" version = "0.9.0" @@ -2656,12 +2638,68 @@ version = "0.1.0" dependencies = [ "bincode", "log", - "roslibrust", "roslibrust_codegen", "roslibrust_codegen_macro", + "roslibrust_common", "tokio", ] +[[package]] +name = "roslibrust_ros1" +version = "0.1.0" +dependencies = [ + "abort-on-drop", + "anyhow", + "byteorder", + "gethostname", + "hyper", + "lazy_static", + "log", + "regex", + "reqwest", + "roslibrust_codegen", + "roslibrust_codegen_macro", + "roslibrust_common", + "roslibrust_serde_rosmsg", + "serde", + "serde_xmlrpc", + "test-log", + "thiserror 2.0.6", + "tokio", +] + +[[package]] +name = "roslibrust_rosapi" +version = "0.1.0" +dependencies = [ + "roslibrust_codegen", + "roslibrust_codegen_macro", + "roslibrust_common", + "roslibrust_rosbridge", + "test-log", + "tokio", +] + +[[package]] +name = "roslibrust_rosbridge" +version = "0.1.0" +dependencies = [ + "anyhow", + "dashmap", + "deadqueue", + "futures", + "futures-util", + "log", + "roslibrust_codegen", + "roslibrust_codegen_macro", + "roslibrust_common", + "serde_json", + "test-log", + "tokio", + "tokio-tungstenite 0.17.2", + "uuid", +] + [[package]] name = "roslibrust_serde_rosmsg" version = "0.4.0" @@ -2685,6 +2723,7 @@ dependencies = [ "pprof", "roslibrust", "roslibrust_codegen", + "roslibrust_common", "tokio", ] @@ -2699,6 +2738,7 @@ dependencies = [ "roslibrust", "roslibrust_codegen", "roslibrust_codegen_macro", + "roslibrust_common", "roslibrust_serde_rosmsg", "tokio", "zenoh", @@ -3157,18 +3197,6 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7e2accd2c41a0e920d2abd91b2badcfa1da784662f54fbc47e0e3a51f1e2e1cf" -[[package]] -name = "simple_logger" -version = "5.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e8c5dfa5e08767553704aa0ffd9d9794d527103c736aba9854773851fd7497eb" -dependencies = [ - "colored", - "log", - "time", - "windows-sys 0.48.0", -] - [[package]] name = "siphasher" version = "0.3.11" @@ -3190,17 +3218,6 @@ version = "1.13.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3c5e1a9a646d36c3599cd173a41282daf47c44583ad367b8e6837255952e5c67" -[[package]] -name = "smart-default" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "133659a15339456eeeb07572eb02a91c91e9815e9cbc89566944d2c8d3efdbf6" -dependencies = [ - "proc-macro2", - "quote", - "syn 1.0.109", -] - [[package]] name = "smart-default" version = "0.7.1" @@ -3462,9 +3479,7 @@ checksum = "35e7868883861bd0e56d9ac6efcaaca0d6d5d82a2a7ec8209ff492c07cf37b21" dependencies = [ "deranged", "itoa", - "libc", "num-conv", - "num_threads", "powerfmt", "serde", "time-core", diff --git a/Cargo.toml b/Cargo.toml index 3990f2b1..c665a45a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,14 +1,25 @@ [workspace] - members = [ "example_package", "example_package_macro", - "roslibrust", - "roslibrust_codegen", "roslibrust_codegen_macro", + "roslibrust_codegen", + "roslibrust_common", "roslibrust_genmsg", - "roslibrust_test", "roslibrust_mock", + "roslibrust_ros1", + "roslibrust_rosbridge", + "roslibrust_test", "roslibrust_zenoh", + "roslibrust", + "roslibrust_rosapi", ] resolver = "2" + +[workspace.dependencies] +log = "0.4" +tokio = {version = "1", features = ["full"] } +serde = { version = "1.0", features = ["derive"] } +# Someday we may move this crate into this workspace +# For now this is how we keep from repeating the verison everywhere +roslibrust_serde_rosmsg = "0.4" diff --git a/example_package/Cargo.toml b/example_package/Cargo.toml index f8ae9618..64cddbff 100644 --- a/example_package/Cargo.toml +++ b/example_package/Cargo.toml @@ -6,6 +6,8 @@ edition = "2021" [dependencies] # The code generated by roslibrust_codegen has dependendencies # We need to depend on the crate at build time so that the generate code has access to these dependencies +roslibrust_common = { path = "../roslibrust_common" } +# THIS SHOULDN'T BE NEEDED, BUT IS FOR NOW GOTTA FIX LEAKING DEPENDENCIES roslibrust_codegen = { path = "../roslibrust_codegen" } [build-dependencies] diff --git a/example_package_macro/Cargo.toml b/example_package_macro/Cargo.toml index edb0c4c1..1b0e89ae 100644 --- a/example_package_macro/Cargo.toml +++ b/example_package_macro/Cargo.toml @@ -6,6 +6,9 @@ edition = "2021" [dependencies] # The code generated by roslibrust_codegen has dependendencies # We need to depend on the crate at build time so that the generate code has access to these dependencies -roslibrust_codegen = { path = "../roslibrust_codegen" } +# TODO figure out how to rexport these types through the macro +roslibrust_common = { path = "../roslibrust_common" } # This crate contains the actual macro we will invoke roslibrust_codegen_macro = { path = "../roslibrust_codegen_macro" } +# THIS SHOULDN'T BE NEEDED, BUT IS FOR NOW GOTTA FIX LEAKING DEPENDENCIES +roslibrust_codegen = { path = "../roslibrust_codegen" } \ No newline at end of file diff --git a/roslibrust/Cargo.toml b/roslibrust/Cargo.toml index 7bc8db04..3415942c 100644 --- a/roslibrust/Cargo.toml +++ b/roslibrust/Cargo.toml @@ -6,82 +6,50 @@ edition = "2021" license = "MIT" readme = "../README.md" description = "An library for interfacing with the ROS's rosbridge_server" -repository = "https://github.com/Carter12s/roslibrust" +repository = "https://github.com/roslibrust/roslibrust" keywords = ["ROS", "robotics", "websocket", "json", "async"] categories = ["science::robotics"] [dependencies] -abort-on-drop = "0.2" -anyhow = "1.0" -byteorder = "1.4" -dashmap = "5.3" -deadqueue = "0.2.4" # .4+ is required to fix bug with missing tokio dep -futures = "0.3" -futures-util = "0.3" -lazy_static = "1.4" -log = "0.4" -proc-macro2 = "1.0" -rand = "0.8" -serde = { version = "1.0", features = ["derive"] } -serde_json = "1.0" -smart-default = "0.6" -thiserror = "1.0" -tokio = { version = "1.20", features = [ - "net", - "macros", - "time", - "rt-multi-thread", -] } -tokio-tungstenite = { version = "0.17" } -uuid = { version = "1.1", features = ["v4"] } -roslibrust_codegen_macro = { path = "../roslibrust_codegen_macro", version = "0.11.1" } -roslibrust_codegen = { path = "../roslibrust_codegen", version = "0.11.1" } -reqwest = { version = "0.11", optional = true } # Only used with native ros1 -serde_xmlrpc = { version = "0.2", optional = true } # Only used with native ros1 -roslibrust_serde_rosmsg = { version = "0.4", optional = true } # Only used with native ros1 -hyper = { version = "0.14", features = [ - "server", -], optional = true } # Only used with native ros1 -gethostname = { version = "0.4", optional = true } # Only used with native ros1 -regex = { version = "1.9", optional = true } # Only used with native ros1 -# TODO I think we should move rosapi into its own crate... -serde-big-array = { version = "0.5", optional = true } # Only used with rosapi +# TODO versions here +roslibrust_common = { path = "../roslibrust_common" } +roslibrust_ros1 = { path = "../roslibrust_ros1", optional = true } +roslibrust_rosbridge = { path = "../roslibrust_rosbridge", optional = true } +roslibrust_zenoh = { path = "../roslibrust_zenoh", optional = true } +roslibrust_mock = { path = "../roslibrust_mock", optional = true } +roslibrust_rosapi = { path = "../roslibrust_rosapi", optional = true } [dev-dependencies] -env_logger = "0.10" +env_logger = "0.11" test-log = "0.2" -simple_logger = "5.0" -serde-big-array = "0.5" +abort-on-drop = "0.2" +log = { workspace = true } +tokio = { workspace = true } +serde = { workspace = true } +# Used to generate messages for the examples +roslibrust_codegen = { path = "../roslibrust_codegen" } +roslibrust_codegen_macro = { path = "../roslibrust_codegen_macro" } [features] default = [] # Note: all does not include running_bridge as that is only intended for CI all = [] # Provides a rosapi rust interface -rosapi = ["serde-big-array"] +rosapi = ["roslibrust_rosapi"] # Intended for use with tests, includes tests that rely on a locally running rosbridge running_bridge = [] # For use with integration tests, indicating we are testing integration with a ros1 bridge ros1_test = ["running_bridge", "ros1"] # For use with integration tests, indicates we are testing integration with a ros2 bridge ros2_test = ["running_bridge"] -# Provides access to experimental abstract trait topic_provider -topic_provider = [] # Provides a ros1 xmlrpc / TCPROS client -ros1 = [ - "dep:serde_xmlrpc", - "dep:reqwest", - "dep:hyper", - "dep:gethostname", - "dep:regex", - "dep:roslibrust_serde_rosmsg", -] - - -[[test]] -name = "ros1_xmlrpc" -path = "tests/ros1_xmlrpc.rs" -required-features = ["ros1_test", "ros1"] +ros1 = ["roslibrust_ros1"] +# Provides a backend using the rosbridge websocket protocol +rosbridge = ["roslibrust_rosbridge"] +# Provides a backend using zenoh's ros1 format from zenoh-bridge-ros1 +zenoh = ["roslibrust_zenoh"] +# Provides a mock backend useful for writing tests around nodes +mock = ["roslibrust_mock"] [package.metadata.docs.rs] features = ["ros1"] diff --git a/roslibrust/examples/basic_publisher.rs b/roslibrust/examples/basic_publisher.rs index 1ec002a6..4981a06a 100644 --- a/roslibrust/examples/basic_publisher.rs +++ b/roslibrust/examples/basic_publisher.rs @@ -1,19 +1,16 @@ -use log::*; -use roslibrust::ClientHandle; - +#[cfg(feature = "rosbridge")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces/std_msgs"); /// This example creates a client, and publishes a message to the topic "talker" /// Running this example at the same time as subscribe_and_log will have the two examples /// pass messages between each other. /// To run this example a rosbridge websocket server should be running at the deafult port (9090). +#[cfg(feature = "rosbridge")] #[tokio::main(flavor = "multi_thread")] -async fn main() -> Result<(), anyhow::Error> { - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running wsl2 - .init() - .unwrap(); +async fn main() -> Result<(), Box> { + use log::*; + use roslibrust::rosbridge::ClientHandle; + env_logger::init(); let client = ClientHandle::new("ws://localhost:9090").await?; let publisher = client.advertise::("talker").await?; @@ -33,3 +30,8 @@ async fn main() -> Result<(), anyhow::Error> { tokio::time::sleep(tokio::time::Duration::from_secs(1)).await; } } + +#[cfg(not(feature = "rosbridge"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'rosbridge'"); +} diff --git a/roslibrust/examples/calling_service.rs b/roslibrust/examples/calling_service.rs index 2128ac7e..6793db2d 100644 --- a/roslibrust/examples/calling_service.rs +++ b/roslibrust/examples/calling_service.rs @@ -1,19 +1,17 @@ -use log::*; -use roslibrust::ClientHandle; - -roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces",); +#[cfg(feature = "rosbridge")] +roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces/rosapi"); /// This example shows calling a service /// To run this example rosbridge and a roscore should be running /// As well as the rosapi node. /// This node calls a service on the rosapi node to get the current ros time. +#[cfg(feature = "rosbridge")] #[tokio::main(flavor = "multi_thread")] -async fn main() -> Result<(), anyhow::Error> { - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // Required for running in wsl2 - .init() - .unwrap(); +async fn main() -> Result<(), Box> { + use log::*; + use roslibrust::rosbridge::ClientHandle; + + env_logger::init(); let client = ClientHandle::new("ws://localhost:9090").await?; @@ -25,3 +23,8 @@ async fn main() -> Result<(), anyhow::Error> { info!("Got time: {:?}", result); Ok(()) } + +#[cfg(not(feature = "rosbridge"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'rosbridge'"); +} diff --git a/roslibrust/examples/generic_client.rs b/roslibrust/examples/generic_client.rs index 50b4081e..f86c334e 100644 --- a/roslibrust/examples/generic_client.rs +++ b/roslibrust/examples/generic_client.rs @@ -1,17 +1,16 @@ //! Purpose of this example is to show how the TopicProvider trait can be use //! to create code that is generic of which communication backend it will use. -use roslibrust::topic_provider::*; +// Important to bring these traits into scope so we can use them + +#[cfg(all(feature = "rosbridge", feature = "ros1"))] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces/std_msgs"); -#[cfg(feature = "topic_provider")] +#[cfg(all(feature = "rosbridge", feature = "ros1"))] #[tokio::main] async fn main() { - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running wsl2 - .init() - .unwrap(); + use roslibrust::{Publish, Subscribe, TopicProvider}; + env_logger::init(); // TopicProvider cannot be an "Object Safe Trait" due to its generic parameters // This means we can't do: @@ -60,7 +59,7 @@ async fn main() { } // create a rosbridge handle and start node - let ros = roslibrust::ClientHandle::new("ws://localhost:9090") + let ros = roslibrust::rosbridge::ClientHandle::new("ws://localhost:9090") .await .unwrap(); let node = MyNode { @@ -90,5 +89,9 @@ async fn main() { // Note: this will not run without rosbridge running } -#[cfg(not(feature = "topic_provider"))] -fn main() {} +#[cfg(not(all(feature = "rosbridge", feature = "ros1")))] +fn main() { + eprintln!( + "This example does nothing without compiling with the feature 'rosbridge' and 'ros1'" + ); +} diff --git a/roslibrust/examples/generic_client_services.rs b/roslibrust/examples/generic_client_services.rs index 28fec204..f755e863 100644 --- a/roslibrust/examples/generic_client_services.rs +++ b/roslibrust/examples/generic_client_services.rs @@ -1,17 +1,15 @@ //! Purpose of this example is to show how the ServiceProvider trait can be use //! to create code that is generic of which communication backend it will use. -use roslibrust::topic_provider::*; +#[cfg(all(feature = "rosbridge", feature = "ros1"))] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); -#[cfg(feature = "topic_provider")] +#[cfg(all(feature = "rosbridge", feature = "ros1"))] #[tokio::main] async fn main() { - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running wsl2 - .init() - .unwrap(); + // Important to bring these traits into scope so we can use them + use roslibrust::{Service, ServiceProvider}; + env_logger::init(); // TopicProvider cannot be an "Object Safe Trait" due to its generic parameters // This means we can't do: @@ -56,16 +54,17 @@ async fn main() { tokio::time::sleep(std::time::Duration::from_millis(500)).await; println!("sleeping"); - client + let response = client .call(&std_srvs::SetBoolRequest { data: true }) .await .unwrap(); + println!("Got response: {response:?}"); } } } // create a rosbridge handle and start node - let ros = roslibrust::ClientHandle::new("ws://localhost:9090") + let ros = roslibrust::rosbridge::ClientHandle::new("ws://localhost:9090") .await .unwrap(); let node = MyNode { ros }; @@ -88,5 +87,9 @@ async fn main() { // You should see log output from both nodes } -#[cfg(not(feature = "topic_provider"))] -fn main() {} +#[cfg(not(all(feature = "rosbridge", feature = "ros1")))] +fn main() { + eprintln!( + "This example does nothing without compiling with the feature 'rosbridge' and 'ros1'" + ); +} diff --git a/roslibrust/examples/generic_message.rs b/roslibrust/examples/generic_message.rs index 1a1ba67a..612dbf56 100644 --- a/roslibrust/examples/generic_message.rs +++ b/roslibrust/examples/generic_message.rs @@ -1,8 +1,6 @@ //! This example shows off creating a custom generic message, and leveraging serde_json's parsing resolution //! to decode to the right type. -use log::*; -use roslibrust::ClientHandle; -use roslibrust_codegen::RosMessageType; +use roslibrust_common::RosMessageType; /// We place the ros1 generate code into a module to prevent name collisions with the identically /// named ros2 types. @@ -34,23 +32,20 @@ impl RosMessageType for GenericHeader { /// the same ROS type name. const ROS_TYPE_NAME: &'static str = "std_msgs/Header"; - // TODO these fields should be removed and not required for this example see - // https://github.com/Carter12s/roslibrust/issues/124 - const MD5SUM: &'static str = ""; + // "*" is used as a wildcard to match any md5sum + const MD5SUM: &'static str = "*"; const DEFINITION: &'static str = ""; } /// Sets up a subscriber that could get either of two versions of a message +#[cfg(feature = "rosbridge")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running in wsl2 - .init() - .unwrap(); +async fn main() -> Result<(), Box> { + use log::*; + env_logger::init(); // An instance of rosbridge needs to be running at this address for the example to work - let client = ClientHandle::new("ws://localhost:9090").await?; + let client = roslibrust::rosbridge::ClientHandle::new("ws://localhost:9090").await?; info!("ClientHandle connected"); let rx = client.subscribe::("talker").await?; @@ -70,3 +65,8 @@ async fn main() -> Result<(), anyhow::Error> { } } } + +#[cfg(not(feature = "rosbridge"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'rosbridge'"); +} diff --git a/roslibrust/examples/native_ros1.rs b/roslibrust/examples/native_ros1.rs deleted file mode 100644 index 7a9518be..00000000 --- a/roslibrust/examples/native_ros1.rs +++ /dev/null @@ -1,31 +0,0 @@ -/** - * Testing ground for hitting on rosmaster directly - */ - -#[cfg(feature = "ros1")] -#[tokio::main] -async fn main() -> Result<(), anyhow::Error> { - use roslibrust::ros1::NodeHandle; - - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running wsl2 - .init() - .unwrap(); - - let _nh = NodeHandle::new("http://localhost:11311", "native_ros1").await?; - - // Work to do: - // * [DONE] Take the ros MasterApi and create valid in/out types for each api call - // * [DONE] Build out a test suite for the materapi - // * Build out a host for the slaveapi (maybe skip some features if we can? stats?) - // * Build out a test against our own slaveapi (maybe skip some features if we can? stats?) - // * Actually make a connection with TCPROS - - Ok(()) -} - -#[cfg(not(feature = "ros1"))] -fn main() { - // Provide a dummy main for this example when ros1 is disabled -} diff --git a/roslibrust/examples/ros1_listener.rs b/roslibrust/examples/ros1_listener.rs index 08e92fdb..75e279bc 100644 --- a/roslibrust/examples/ros1_listener.rs +++ b/roslibrust/examples/ros1_listener.rs @@ -1,15 +1,12 @@ +#[cfg(feature = "ros1")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces/std_msgs"); #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { +async fn main() -> Result<(), Box> { use roslibrust::ros1::NodeHandle; - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() - .init() - .unwrap(); + env_logger::init(); let nh = NodeHandle::new("http://localhost:11311", "listener_rs").await?; let mut subscriber = nh.subscribe::("/chatter", 1).await?; diff --git a/roslibrust/examples/ros1_publish_any.rs b/roslibrust/examples/ros1_publish_any.rs index cc5db23a..5915de2d 100644 --- a/roslibrust/examples/ros1_publish_any.rs +++ b/roslibrust/examples/ros1_publish_any.rs @@ -1,3 +1,4 @@ +#[cfg(feature = "ros1")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); /// This example demonstrates ths usage of the .advertise_any() function @@ -8,7 +9,7 @@ roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_in #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { +async fn main() -> Result<(), Box> { // Note: this example needs a ros master running to work let node = roslibrust::ros1::NodeHandle::new("http://localhost:11311", "/ros1_publish_any").await?; diff --git a/roslibrust/examples/ros1_ros2_bridge_example.rs b/roslibrust/examples/ros1_ros2_bridge_example.rs index d2c5232a..48634147 100644 --- a/roslibrust/examples/ros1_ros2_bridge_example.rs +++ b/roslibrust/examples/ros1_ros2_bridge_example.rs @@ -2,9 +2,6 @@ //! both ROS1 types and ROS2 types, can use two clients to communicate //! with both versions of ROS at the same time. -use log::*; -use roslibrust::ClientHandle; - /// We place the ros1 generate code into a module to prevent name collisions with the identically /// named ros2 types. mod ros1 { @@ -52,14 +49,12 @@ mod ros2 { /// After both bridges are up and running, run this example. /// With this example running you should then be able to use the ros1 command line tools to publish a message, /// and see them appear in ros2 with its command line tools +#[cfg(feature = "rosbridge")] #[tokio::main(flavor = "multi_thread")] -async fn main() -> Result<(), anyhow::Error> { - // Initialize a basic logger useful if anything goes wrong while running the example - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // Required for running in wsl2 - .init() - .unwrap(); +async fn main() -> Result<(), Box> { + use log::*; + use roslibrust::rosbridge::ClientHandle; + env_logger::init(); info!("Attempting to connect to ros1..."); let ros1_client = ClientHandle::new("ws://localhost:9090").await?; @@ -101,3 +96,8 @@ async fn main() -> Result<(), anyhow::Error> { Ok(()) } + +#[cfg(not(feature = "rosbridge"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'rosbridge'"); +} diff --git a/roslibrust/examples/ros1_service_client.rs b/roslibrust/examples/ros1_service_client.rs index 27074fbe..f13fdd24 100644 --- a/roslibrust/examples/ros1_service_client.rs +++ b/roslibrust/examples/ros1_service_client.rs @@ -1,15 +1,12 @@ +#[cfg(feature = "ros1")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { +async fn main() -> Result<(), Box> { use roslibrust::ros1::NodeHandle; - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() - .init() - .unwrap(); + env_logger::init(); let nh = NodeHandle::new("http://localhost:11311", "service_client_rs").await?; log::info!("Connected!"); diff --git a/roslibrust/examples/ros1_service_server.rs b/roslibrust/examples/ros1_service_server.rs index 016f826f..0b658dbd 100644 --- a/roslibrust/examples/ros1_service_server.rs +++ b/roslibrust/examples/ros1_service_server.rs @@ -1,17 +1,14 @@ +#[cfg(feature = "ros1")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { +async fn main() -> Result<(), Box> { use log::*; use roslibrust::ros1::NodeHandle; use std::sync::{Arc, Mutex}; - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() - .init() - .unwrap(); + env_logger::init(); let nh = NodeHandle::new("http://localhost:11311", "service_server_rs").await?; log::info!("Connected!"); diff --git a/roslibrust/examples/ros1_talker.rs b/roslibrust/examples/ros1_talker.rs index b57613e4..800055af 100644 --- a/roslibrust/examples/ros1_talker.rs +++ b/roslibrust/examples/ros1_talker.rs @@ -1,15 +1,12 @@ +#[cfg(feature = "ros1")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { +async fn main() -> Result<(), Box> { use roslibrust::ros1::NodeHandle; - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running wsl2 - .init() - .unwrap(); + env_logger::init(); let nh = NodeHandle::new("http://localhost:11311", "talker_rs") .await diff --git a/roslibrust/examples/service_server.rs b/roslibrust/examples/service_server.rs index aa3ad45c..cb3fc5ef 100644 --- a/roslibrust/examples/service_server.rs +++ b/roslibrust/examples/service_server.rs @@ -1,10 +1,10 @@ -use roslibrust::ClientHandle; - // One way to import message definitions: +#[cfg(feature = "rosbridge")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); // A basic service server exampple, that logs the request is recieves and returns // a canned response. +#[cfg(feature = "rosbridge")] fn my_service( request: std_srvs::SetBoolRequest, my_string: &str, @@ -39,17 +39,13 @@ fn my_service( /// /// To actually exercise our service we need to call it with something. /// One option would be to use a ros commaline tool: `rosservice call /my_set_bool "data: false"` or `ros2 service call /my_set_bool std_srvs/srv/SetBool data:\ false\ `. +#[cfg(feature = "rosbridge")] #[tokio::main(flavor = "multi_thread")] -async fn main() -> Result<(), anyhow::Error> { - // Initialize a basic logger useful if anything goes wrong while running the example - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // Required for running in wsl2 - .init() - .unwrap(); +async fn main() -> Result<(), Box> { + env_logger::init(); // Create a new client - let client = ClientHandle::new("ws://localhost:9090").await?; + let client = roslibrust::rosbridge::ClientHandle::new("ws://localhost:9090").await?; // The string you want to pass in to the closure let my_string = "Some string"; @@ -71,3 +67,8 @@ async fn main() -> Result<(), anyhow::Error> { tokio::signal::ctrl_c().await?; Ok(()) } + +#[cfg(not(feature = "rosbridge"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'rosbridge'"); +} diff --git a/roslibrust/examples/subscribe_and_log.rs b/roslibrust/examples/subscribe_and_log.rs index 35296bd3..d2651037 100644 --- a/roslibrust/examples/subscribe_and_log.rs +++ b/roslibrust/examples/subscribe_and_log.rs @@ -1,20 +1,16 @@ -use log::*; -use roslibrust::ClientHandle; - +#[cfg(feature = "rosbridge")] roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); /// A basic example of connecting and subscribing to data. /// This example will log received messages if run at the same time as "basic_publisher". /// A running rosbridge websocket server at the default port (9090) is required to run this example. +#[cfg(feature = "rosbridge")] #[tokio::main] -async fn main() -> Result<(), anyhow::Error> { - simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() // required for running in wsl2 - .init() - .unwrap(); +async fn main() -> Result<(), Box> { + use log::*; + env_logger::init(); - let client = ClientHandle::new("ws://localhost:9090").await?; + let client = roslibrust::rosbridge::ClientHandle::new("ws://localhost:9090").await?; info!("ClientHandle connected"); let rx = client.subscribe::("talker").await?; @@ -25,3 +21,8 @@ async fn main() -> Result<(), anyhow::Error> { info!("Got msg: {:?}", msg); } } + +#[cfg(not(feature = "rosbridge"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'rosbridge'"); +} diff --git a/roslibrust/src/lib.rs b/roslibrust/src/lib.rs index 21423a37..5c572ee1 100644 --- a/roslibrust/src/lib.rs +++ b/roslibrust/src/lib.rs @@ -98,79 +98,25 @@ //! Specifically, roslibrust attempts to follow "good" ros error handling convention and be as compatible as possible //! with various error types; however, due to the async nature of the crate `Box` is needed. -mod rosbridge; -pub use rosbridge::*; - -// Re export the codegen traits so that crates that only interact with abstract messages -// don't need to depend on the codegen crate -pub use roslibrust_codegen::RosMessageType; -pub use roslibrust_codegen::RosServiceType; +// Re-export common types and traits under the roslibrust namespace +pub use roslibrust_common::*; +// If the rosapi feature is enabled, export the roslibrust_rosapi crate under rosapi #[cfg(feature = "rosapi")] -pub mod rosapi; +pub use roslibrust_rosapi as rosapi; +// If the ros1 feature is enabled, export the roslibrust_ros1 crate under ros1 #[cfg(feature = "ros1")] -pub mod ros1; - -// Topic provider is locked behind a feature until it is stabalized -// additionally because of its use of generic associated types, it requires rust >1.65 -#[cfg(feature = "topic_provider")] -/// Provides a generic trait for building clients / against either the rosbridge, -/// ros1, or a mock backend -pub mod topic_provider; - -/// For now starting with a central error type, may break this up more in future -#[derive(thiserror::Error, Debug)] -pub enum RosLibRustError { - #[error("Not currently connected to ros master / bridge")] - Disconnected, - // TODO we probably want to eliminate tungstenite from this and hide our - // underlying websocket implementation from the API - // currently we "technically" break the API when we change tungstenite verisons - #[error("Websocket communication error: {0}")] - CommFailure(#[from] tokio_tungstenite::tungstenite::Error), - #[error("Operation timed out: {0}")] - Timeout(#[from] tokio::time::error::Elapsed), - #[error("Failed to parse message from JSON: {0}")] - InvalidMessage(#[from] serde_json::Error), - #[error("TCPROS serialization error: {0}")] - SerializationError(String), - #[error("Rosbridge server reported an error: {0}")] - ServerError(String), - #[error("IO error: {0}")] - IoError(#[from] std::io::Error), - #[error("Name does not meet ROS requirements: {0}")] - InvalidName(String), - // Generic catch-all error type for not-yet-handled errors - // TODO ultimately this type will be removed from API of library - #[error(transparent)] - Unexpected(#[from] anyhow::Error), -} +pub use roslibrust_ros1 as ros1; -/// Generic result type used as standard throughout library. -/// Note: many functions which currently return this will be updated to provide specific error -/// types in the future instead of the generic error here. -pub type RosLibRustResult = Result; +// If the rosbridge feature is enabled, export the roslibrust_rosbridge crate under rosbridge +#[cfg(feature = "rosbridge")] +pub use roslibrust_rosbridge as rosbridge; -// Note: service Fn is currently defined here as it used by ros1 and roslibrust impls -/// This trait describes a function which can validly act as a ROS service -/// server with roslibrust. We're really just using this as a trait alias -/// as the full definition is overly verbose and trait aliases are unstable. -pub trait ServiceFn: - Fn(T::Request) -> Result> - + Send - + Sync - + 'static -{ -} +// If the zenoh feature is enabled, export the roslibrust_zenoh crate under zenoh +#[cfg(feature = "zenoh")] +pub use roslibrust_zenoh as zenoh; -/// Automatic implementation of ServiceFn for Fn -impl ServiceFn for F -where - T: RosServiceType, - F: Fn(T::Request) -> Result> - + Send - + Sync - + 'static, -{ -} +// If the mock feature is enabled, export the roslibrust_mock crate under mock +#[cfg(feature = "mock")] +pub use roslibrust_mock as mock; diff --git a/roslibrust/src/ros1/mod.rs b/roslibrust/src/ros1/mod.rs deleted file mode 100644 index e82b4e18..00000000 --- a/roslibrust/src/ros1/mod.rs +++ /dev/null @@ -1,29 +0,0 @@ -//! This module holds all content for directly working with ROS1 natively - -/// [master_client] module contains code for calling xmlrpc functions on the master -mod master_client; -pub use master_client::*; - -mod names; - -/// [node] module contains the central Node and NodeHandle APIs -mod node; -pub use node::*; - -mod publisher; -pub use publisher::Publisher; -pub use publisher::PublisherAny; -mod service_client; -pub use service_client::ServiceClient; -mod subscriber; -pub use subscriber::Subscriber; -mod service_server; -pub use service_server::ServiceServer; -mod tcpros; - -/// Provides a common type alias for type erased service server functions. -/// Internally we use this type to store collections of server functions. -pub(crate) type TypeErasedCallback = dyn Fn(Vec) -> Result, Box> - + Send - + Sync - + 'static; diff --git a/roslibrust/src/topic_provider.rs b/roslibrust/src/topic_provider.rs deleted file mode 100644 index 98ee253d..00000000 --- a/roslibrust/src/topic_provider.rs +++ /dev/null @@ -1,281 +0,0 @@ -use roslibrust_codegen::{RosMessageType, RosServiceType}; - -use crate::{RosLibRustResult, ServiceClient, ServiceFn}; - -/// Indicates that something is a publisher and has our expected publish -/// Implementors of this trait are expected to auto-cleanup the publisher when dropped -pub trait Publish { - // Note: this is really just syntactic de-sugared `async fn` - // However see: https://blog.rust-lang.org/2023/12/21/async-fn-rpit-in-traits.html - // This generates a warning is rust as of writing due to ambiguity around the "Send-ness" of the return type - // We only plan to work with multi-threaded work stealing executors (e.g. tokio) so we're manually specifying Send - fn publish(&self, data: &T) -> impl futures::Future> + Send; -} - -// Provide an implementation of publish for rosbridge backend -impl Publish for crate::Publisher { - async fn publish(&self, data: &T) -> RosLibRustResult<()> { - self.publish(data).await - } -} - -// Provide an implementation of publish for ros1 backend -#[cfg(feature = "ros1")] -impl Publish for crate::ros1::Publisher { - async fn publish(&self, data: &T) -> RosLibRustResult<()> { - // TODO error type conversion here is terrible and we need to standardize error stuff badly - self.publish(data) - .await - .map_err(|e| crate::RosLibRustError::SerializationError(e.to_string())) - } -} - -/// Indicates that something is a subscriber and has our expected subscribe method -/// Implementors of this trait are expected to auto-cleanup the subscriber when dropped -pub trait Subscribe { - // TODO need to solidify how we want errors to work with subscribers... - // TODO ros1 currently requires mut for next, we should change that - fn next(&mut self) -> impl futures::Future> + Send; -} - -impl Subscribe for crate::Subscriber { - async fn next(&mut self) -> RosLibRustResult { - // TODO: rosbridge subscribe really should emit errors... - Ok(crate::Subscriber::next(self).await) - } -} - -#[cfg(feature = "ros1")] -impl Subscribe for crate::ros1::Subscriber { - async fn next(&mut self) -> RosLibRustResult { - let res = crate::ros1::Subscriber::next(self).await; - match res { - Some(Ok(msg)) => Ok(msg), - Some(Err(e)) => { - log::error!("Subscriber got error: {e:?}"); - // TODO gotta do better error conversion / error types here - Err(crate::RosLibRustError::Unexpected(anyhow::anyhow!( - "Subscriber got error: {e:?}" - ))) - } - None => { - log::error!("Subscriber hit dropped channel"); - Err(crate::RosLibRustError::Unexpected(anyhow::anyhow!( - "Channel closed, something was dropped?" - ))) - } - } - } -} - -/// This trait generically describes the capability of something to act as an async interface to a set of topics -/// -/// This trait is largely based on ROS concepts, but could be extended to other protocols / concepts. -/// Fundamentally, it assumes that topics are uniquely identified by a string name (likely an ASCII assumption is buried in here...). -/// It assumes topics only carry one data type, but is not expected to enforce that. -/// It assumes that all actions can fail due to a variety of causes, and by network interruption specifically. -pub trait TopicProvider { - // These associated types makeup the other half of the API - // They are expected to be "self-deregistering", where dropping them results in unadvertise or unsubscribe operations as appropriate - // We require Publisher and Subscriber types to be Send + 'static so they can be sent into different tokio tasks once created - type Publisher: Publish + Send + 'static; - type Subscriber: Subscribe + Send + 'static; - - /// Advertises a topic to be published to and returns a type specific publisher to use. - /// - /// The returned publisher is expected to be "self de-registering", where dropping the publisher results in the appropriate unadvertise operation. - fn advertise( - &self, - topic: &str, - ) -> impl futures::Future>> + Send; - - /// Subscribes to a topic and returns a type specific subscriber to use. - /// - /// The returned subscriber is expected to be "self de-registering", where dropping the subscriber results in the appropriate unsubscribe operation. - fn subscribe( - &self, - topic: &str, - ) -> impl futures::Future>> + Send; -} - -// Implementation of TopicProvider trait for rosbridge client -impl TopicProvider for crate::ClientHandle { - type Publisher = crate::Publisher; - type Subscriber = crate::Subscriber; - - async fn advertise( - &self, - topic: &str, - ) -> RosLibRustResult> { - self.advertise::(topic.as_ref()).await - } - - async fn subscribe( - &self, - topic: &str, - ) -> RosLibRustResult> { - self.subscribe(topic).await - } -} - -#[cfg(feature = "ros1")] -impl TopicProvider for crate::ros1::NodeHandle { - type Publisher = crate::ros1::Publisher; - type Subscriber = crate::ros1::Subscriber; - - async fn advertise( - &self, - topic: &str, - ) -> RosLibRustResult> { - // TODO MAJOR: consider promoting queue size, making unlimited default - self.advertise::(topic.as_ref(), 10, false) - .await - .map_err(|e| e.into()) - } - - async fn subscribe( - &self, - topic: &str, - ) -> RosLibRustResult> { - // TODO MAJOR: consider promoting queue size, making unlimited default - self.subscribe(topic, 10).await.map_err(|e| e.into()) - } -} - -/// Defines what it means to be something that is callable as a service -pub trait Service { - fn call( - &self, - request: &T::Request, - ) -> impl futures::Future> + Send; -} - -impl Service for crate::ServiceClient { - async fn call(&self, request: &T::Request) -> RosLibRustResult { - // TODO sort out the reference vs clone stuff here - ServiceClient::call(&self, request.clone()).await - } -} - -#[cfg(feature = "ros1")] -impl Service for crate::ros1::ServiceClient { - async fn call(&self, request: &T::Request) -> RosLibRustResult { - self.call(request).await - } -} - -/// This trait is analogous to TopicProvider, but instead provides the capability to create service servers and service clients -pub trait ServiceProvider { - type ServiceClient: Service + Send + 'static; - type ServiceServer; - - fn service_client( - &self, - topic: &str, - ) -> impl futures::Future>> + Send; - - fn advertise_service( - &self, - topic: &str, - server: F, - ) -> impl futures::Future> + Send - where - F: ServiceFn; -} - -impl ServiceProvider for crate::ClientHandle { - type ServiceClient = crate::ServiceClient; - type ServiceServer = crate::ServiceHandle; - - async fn service_client( - &self, - topic: &str, - ) -> RosLibRustResult> { - self.service_client::(topic).await - } - - fn advertise_service( - &self, - topic: &str, - server: F, - ) -> impl futures::Future> + Send - where - F: ServiceFn, - { - self.advertise_service(topic, server) - } -} - -#[cfg(feature = "ros1")] -impl ServiceProvider for crate::ros1::NodeHandle { - type ServiceClient = crate::ros1::ServiceClient; - type ServiceServer = crate::ros1::ServiceServer; - - async fn service_client( - &self, - topic: &str, - ) -> RosLibRustResult> { - // TODO bad error mapping here... - self.service_client::(topic).await.map_err(|e| e.into()) - } - - async fn advertise_service( - &self, - topic: &str, - server: F, - ) -> RosLibRustResult - where - F: ServiceFn, - { - self.advertise_service::(topic, server) - .await - .map_err(|e| e.into()) - } -} - -#[cfg(test)] -mod test { - use super::TopicProvider; - use crate::{ros1::NodeHandle, ClientHandle}; - - // This test specifically fails because TopicProvider is not object safe - // Traits that have methods with generic parameters cannot be object safe in rust (currently) - // #[test] - // fn topic_provider_can_be_constructed() -> TestResult { - // let x: Box = Box::new(ClientHandle::new("")); - // Ok(()) - // } - - // This tests proves that you could use topic provider in a compile time api, but not object safe... - #[test_log::test] - #[should_panic] - fn topic_provider_can_be_used_at_compile_time() { - struct MyClient { - _client: T, - } - - // Kinda a hack way to make the compiler prove it could construct a MyClient with out actually - // constructing one at runtime - let new_mock: Result = Err(anyhow::anyhow!("Expected error")); - - let _x = MyClient { - _client: new_mock.unwrap(), // panic - }; - } - - #[test_log::test] - #[should_panic] - fn topic_provider_can_be_used_with_ros1() { - struct MyClient { - _client: T, - } - - // Kinda a hack way to make the compiler prove it could construct a MyClient with out actually - // constructing one at runtime - let new_mock: Result = Err(anyhow::anyhow!("Expected error")); - - let _x = MyClient { - _client: new_mock.unwrap(), // panic - }; - } -} diff --git a/roslibrust/tests/ros1_native_integration_tests.rs b/roslibrust/tests/ros1_native_integration_tests.rs index 2e2facfb..b62d95e4 100644 --- a/roslibrust/tests/ros1_native_integration_tests.rs +++ b/roslibrust/tests/ros1_native_integration_tests.rs @@ -1,10 +1,11 @@ //! This test file is intended to contain all integration tests of ROS1 native fuctionality. //! Any test which interacts with actual running ros nodes should be in this file. -#[cfg(feature = "ros1_test")] +#[cfg(all(feature = "ros1_test", feature = "ros1", feature = "rosbridge"))] mod tests { use log::*; use roslibrust::ros1::{NodeError, NodeHandle}; + use roslibrust::rosbridge::ClientHandle; use tokio::time::timeout; roslibrust_codegen_macro::find_and_generate_ros_messages!( @@ -436,7 +437,7 @@ mod tests { .await .unwrap(); - let master_client = roslibrust::ros1::MasterClient::new( + let master_client = roslibrust_ros1::MasterClient::new( "http://localhost:11311", "NAN", "/test_dropping_publisher_mc", @@ -467,10 +468,9 @@ mod tests { } #[test_log::test(tokio::test)] - #[cfg(all(feature = "ros1_test", feature = "topic_provider"))] + #[cfg(feature = "ros1_test")] async fn topic_provider_publish_functionality_test() { - use roslibrust::topic_provider::*; - use roslibrust::ClientHandle; + use roslibrust_common::*; // Dropping watchdog at end of function cancels watchdog // This test can hang which gives crappy debug output @@ -490,7 +490,7 @@ mod tests { async fn test_main( ros: &T, msg: &str, - ) -> Result<(), Box> { + ) -> std::result::Result<(), Box> { // In the body we'll publish a message let publisher = ros .advertise::("/topic_provider_func_test") @@ -569,7 +569,7 @@ mod tests { .await .unwrap(); - let master_client = roslibrust::ros1::MasterClient::new( + let master_client = roslibrust_ros1::MasterClient::new( "http://localhost:11311", "NAN", "/test_node_cleanup_checker", diff --git a/roslibrust_codegen/Cargo.toml b/roslibrust_codegen/Cargo.toml index 52364561..370f8396 100644 --- a/roslibrust_codegen/Cargo.toml +++ b/roslibrust_codegen/Cargo.toml @@ -13,16 +13,17 @@ categories = ["science::robotics"] # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] +tokio = { workspace = true, optional = true} +log = { workspace = true } +serde = { workspace = true } +roslibrust_common = { path = "../roslibrust_common" } lazy_static = "1.4" -log = "0.4" md5 = "0.7" proc-macro2 = "1.0" quote = "1.0" -serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" simple-error = "0.3" syn = "1.0" -tokio = { version = "1.0", features = ["time", "signal"], optional = true} walkdir = "2.3" xml-rs = "0.8" # Not a direct dependencies of the crate, but something generated code uses diff --git a/roslibrust_codegen/src/gen.rs b/roslibrust_codegen/src/gen.rs index 1f4de44f..c5b9ae03 100644 --- a/roslibrust_codegen/src/gen.rs +++ b/roslibrust_codegen/src/gen.rs @@ -42,7 +42,7 @@ pub fn generate_service(service: ServiceFile) -> Result { pub struct #struct_name { } - impl ::roslibrust_codegen::RosServiceType for #struct_name { + impl ::roslibrust_common::RosServiceType for #struct_name { const ROS_SERVICE_NAME: &'static str = #service_type_name; const MD5SUM: &'static str = #service_md5sum; type Request = #request_name; @@ -99,7 +99,7 @@ pub fn generate_struct(msg: MessageFile) -> Result { #(#fields )* } - impl ::roslibrust_codegen::RosMessageType for #struct_name { + impl ::roslibrust_common::RosMessageType for #struct_name { const ROS_TYPE_NAME: &'static str = #ros_type_name; const MD5SUM: &'static str = #md5sum; const DEFINITION: &'static str = #raw_message_definition; diff --git a/roslibrust_codegen/src/integral_types.rs b/roslibrust_codegen/src/integral_types.rs index 9a984005..7480073b 100644 --- a/roslibrust_codegen/src/integral_types.rs +++ b/roslibrust_codegen/src/integral_types.rs @@ -1,6 +1,6 @@ use simple_error::{bail, SimpleError}; -use crate::RosMessageType; +use roslibrust_common::RosMessageType; /// Matches the integral ros1 type time, with extensions for ease of use /// NOTE: in ROS1 "Time" is not a message in and of itself and std_msgs/Time should be used. @@ -78,16 +78,6 @@ impl RosMessageType for Time { const DEFINITION: &'static str = ""; } -#[derive(:: serde :: Deserialize, :: serde :: Serialize, Debug, Default, Clone, PartialEq)] -pub struct ShapeShifter(Vec); - -// The equivalent of rospy AnyMsg or C++ ShapeShifter, subscribe_any() uses this type -impl RosMessageType for ShapeShifter { - const ROS_TYPE_NAME: &'static str = "*"; - const MD5SUM: &'static str = "*"; - const DEFINITION: &'static str = ""; -} - // TODO provide chrono conversions here behind a cfg flag /// Matches the integral ros1 duration type, with extensions for ease of use diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index d1ae5adb..eda36396 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -2,7 +2,7 @@ use log::*; use proc_macro2::TokenStream; use quote::quote; use simple_error::{bail, SimpleError as Error}; -use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, VecDeque}; +use std::collections::{BTreeMap, BTreeSet, VecDeque}; use std::fmt::{Debug, Display}; use std::path::PathBuf; use utils::Package; @@ -27,209 +27,8 @@ pub use serde_big_array::BigArray; // Used in generated code for large fixed siz pub use serde_bytes; pub use smart_default::SmartDefault; // Used in generated code for default values // Used in generated code for faster Vec serialization -/// Fundamental traits for message types this crate works with -/// This trait will be satisfied for any types generated with this crate's message_gen functionality -pub trait RosMessageType: - 'static + DeserializeOwned + Send + Serialize + Sync + Clone + Debug -{ - /// Expected to be the combination pkg_name/type_name string describing the type to ros - /// Example: std_msgs/Header - const ROS_TYPE_NAME: &'static str; - - /// The computed md5sum of the message file and its dependencies - /// This field is optional, and only needed when using ros1 native communication - const MD5SUM: &'static str = ""; - - /// The definition from the msg, srv, or action file - /// This field is optional, and only needed when using ros1 native communication - const DEFINITION: &'static str = ""; -} - -// This special impl allows for services with no args / returns -impl RosMessageType for () { - const ROS_TYPE_NAME: &'static str = ""; - const MD5SUM: &'static str = ""; - const DEFINITION: &'static str = ""; -} - -/// Fundamental traits for service types this crate works with -/// This trait will be satisfied for any services definitions generated with this crate's message_gen functionality -pub trait RosServiceType: 'static + Send + Sync { - /// Name of the ros service e.g. `rospy_tutorials/AddTwoInts` - const ROS_SERVICE_NAME: &'static str; - /// The computed md5sum of the message file and its dependencies - const MD5SUM: &'static str; - /// The type of data being sent in the request - type Request: RosMessageType; - /// The type of the data - type Response: RosMessageType; -} - -/// Taking in a message definition -/// reformat it according to the md5sum algorithm: -/// - Comments removed -/// - Extra whitespace removed -/// - package names of dependencies removed -/// - constants reordered to be at the front -fn clean_msg(msg: &str) -> String { - let mut result_params = vec![]; - let mut result_constants = vec![]; - for line in msg.lines() { - let line = line.trim(); - // Skip comment lines - if line.starts_with('#') { - continue; - } - // Strip comment from the end of the line (if present) - let line = line.split('#').collect::>()[0].trim(); - // Remove any extra whitespace from inside the line - let line = line.split_whitespace().collect::>().join(" "); - // Remove any whitespace on either side of the "=" for constants - let line = line.replace(" = ", "="); - // Skip any empty lines - if line.is_empty() { - continue; - } - // Determine if constant or not - if line.contains('=') { - result_constants.push(line); - } else { - result_params.push(line); - } - } - format!( - "{}\n{}", - result_constants.join("\n"), - result_params.join("\n") - ) - .trim() - .to_string() // Last trim here is lazy, but gets job done -} - -// TODO(lucasw) this deserves a lot of str vs String cleanup -// TODO(lucasw) the msg_type isn't actually needed - Carter (it actually is, or we need to know the package name at least) -/// This function will calculate the md5sum of an expanded message definition. -/// The expanded message definition is the output of `gendeps --cat` see: -/// This definition is typically sent in the connection header of a ros1 topic and is also stored in bag files. -/// This can be used to calculate the md5sum when message definitions aren't available at compile time. -pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result { - if full_def.is_empty() { - return Err("empty input definition".into()); - } - - // Split the full definition into sections per message - let sep: &str = - "================================================================================\n"; - let sections = full_def.split(sep).collect::>(); - if sections.is_empty() { - // Carter: this error is impossible, split only gives empty iterator when input string is empty - // which we've already checked for above - return Err("empty sections".into()); - } - - // Split the overall definition into seperate sub-messages sorted by message type (incluidng package name) - let mut sub_messages: HashMap<&str, String> = HashMap::new(); - // Note: the first section doesn't contain the "MSG: " line so we don't need to strip it here - let clean_root = clean_msg(sections[0]); - if clean_root.is_empty() { - return Err("empty cleaned root definition".into()); - } - sub_messages.insert(msg_name, clean_root); - - for section in §ions[1..] { - let line0 = section.lines().next().ok_or("empty section")?; - if !line0.starts_with("MSG: ") { - return Err("bad section {section} -> {line0} doesn't start with 'MSG: '".into()); - } - // TODO(lucasw) the full text definition doesn't always have the full message types with - // the package name, - // but I think this is only when the message type is Header or the package of the message - // being define is the same as the message in the field - // Carter: I agree with this, we found the same when dealing with this previously - let section_type = line0.split_whitespace().collect::>()[1]; - let end_of_first_line = section.find('\n').ok_or("No body found in section")?; - let body = clean_msg(§ion[end_of_first_line + 1..]); - sub_messages.insert(section_type, body); - } - - // TODO MAJOR(carter): I'd like to convert this loop to a recursive function where we pass in the map of hashes - // and update them as we go, this tripple loop is stinky to my eye. - // TODO(carter) we should be able to do this in close to one pass if we iterate the full_def backwards - let mut hashed = HashMap::new(); - let hash = message_definition_to_md5sum_recursive(msg_name, &sub_messages, &mut hashed)?; - - Ok(hash) -} - -/// Calculates the hash of the specified message type by recursively calling itself on all dependencies -/// Uses defs as the list of message definitions available for it (expects them to already be cleaned) -/// Uses hashes as the cache of already calculated hashes so we don't redo work -fn message_definition_to_md5sum_recursive( - msg_type: &str, - defs: &HashMap<&str, String>, - hashes: &mut HashMap, -) -> Result { - let base_types: HashSet = HashSet::from_iter( - [ - "bool", "byte", "int8", "int16", "int32", "int64", "uint8", "uint16", "uint32", - "uint64", "float32", "float64", "time", "duration", "string", - ] - .map(|name| name.to_string()), - ); - let def = defs.get(msg_type).ok_or(simple_error::simple_error!( - "Couldn't find message type: {msg_type}" - ))?; - let pkg_name = msg_type.split('/').collect::>()[0]; - // We'll store the expanded hash definition in this string as we go - let mut field_def = "".to_string(); - for line_raw in def.lines() { - let line_split = line_raw.split_whitespace().collect::>(); - if line_split.len() < 2 { - log::error!("bad line to split '{line_raw}'"); - // TODO(lucasw) or error out - continue; - } - let (raw_field_type, _field_name) = (line_split[0], line_split[1]); - // leave array characters alone, could be [] [C] where C is a constant - let field_type = raw_field_type.split('[').collect::>()[0].to_string(); - - let full_field_type; - let line; - if base_types.contains(&field_type) { - line = line_raw.to_string(); - } else { - // TODO(lucasw) are there other special message types besides header- or is it anything in std_msgs? - if field_type == "Header" { - full_field_type = "std_msgs/Header".to_string(); - } else if !field_type.contains('/') { - full_field_type = format!("{pkg_name}/{field_type}"); - } else { - full_field_type = field_type; - } - - match hashes.get(&full_field_type) { - Some(hash_value) => { - // Hash already exists in cache so we can use it - line = line_raw.replace(raw_field_type, hash_value).to_string(); - } - None => { - // Recurse! To calculate hash of this field type - let hash = - message_definition_to_md5sum_recursive(&full_field_type, defs, hashes)?; - line = line_raw.replace(raw_field_type, &hash).to_string(); - } - } - } - field_def += &format!("{line}\n"); - } - field_def = field_def.trim().to_string(); - let md5sum = md5::compute(field_def.trim_end().as_bytes()); - let md5sum_text = format!("{md5sum:x}"); - // Insert our hash into the cache before we return - hashes.insert(msg_type.to_string(), md5sum_text.clone()); - - Ok(md5sum_text) -} +// Export the common types so they can be found under this namespace for backwards compatibility reasons +// pub use roslibrust_common::*; #[derive(Clone, Debug)] pub struct MessageFile { @@ -959,606 +758,4 @@ mod test { assert!(!source.is_empty()); assert!(!paths.is_empty()); } - - /// Confirm md5sum from the connection header message definition matches normally - /// generated checksums - #[test_log::test] - fn msg_def_to_md5() { - { - let def = "byte DEBUG=1\nbyte INFO=2\nbyte WARN=4\nbyte ERROR=8\nbyte FATAL=16\n\ - 2176decaecbce78abc3b96ef049fabed header\n\ - byte level\nstring name\nstring msg\nstring file\nstring function\nuint32 line\nstring[] topics"; - let expected = "acffd30cd6b6de30f120938c17c593fb"; - let md5sum = format!("{:x}", md5::compute(def.trim_end().as_bytes())); - assert_eq!(md5sum, expected, "partially checksumed rosgraph_msgs/Log"); - } - - { - let msg_type = "bad_msgs/Empty"; - let def = ""; - let _md5sum = - crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); - } - - { - let msg_type = "bad_msgs/CommentSpacesOnly"; - let def = - "# message with only comments and whitespace\n# another line comment\n\n \n"; - let _md5sum = - crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); - } - - { - let msg_type = "fake_msgs/MissingSectionMsg"; - let def = "string name\nstring msg\n================================================================================\n# message with only comments and whitespace\n# another line comment\n\n \n"; - let _md5sum = - crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); - } - - { - let msg_type = "bad_msgs/BadLog"; - let def = "## -## Severity level constants -byte DEUG=1 #debug level -byte FATAL=16 #fatal/critical level -## -## Fields -## -Header header -byte level -string name # name of the node -uint32 line # line the message came from -string[] topics # topic names that the node publishes - -================================================================================ -MSG: std_msgs/badHeader -# Standard metadata for higher-level stamped data types. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -time stamp -"; - let _md5sum = - crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); - } - - { - // TODO(lucasw) not sure if this is an ok message, currently it passes - let expected = "96c44a027b586ee888fe95ac325151ae"; - let msg_type = "fake_msgs/CommentSpacesOnlySection"; - let def = "string name\nstring msg\n================================================================================\nMSG: foo/bar\n# message with only comments and whitespace\n# another line comment\n\n \n"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected, "{msg_type}"); - } - - { - let msg_type = "fake_msgs/Garbage"; - let def = r#" -fsdajklf - -== #fdjkl - -MSG: jklfd -# -================================================================================ -f - -vjk -"#; - let _md5sum = - crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); - } - - // TODO(lucasw) it would be nice to pull these out of the real messages, but to avoid - // dependencies just storing expected definition and md5sum - // from roslib.message import get_message_class - // msg = get_message_class("std_msgs/Header") - // md5 = msg._md5sum - // def = msg._full_text - // - - { - let msg_type = "sensor_msgs/CameraInfo"; - // This definition contains double quotes, so representing it with r# and newlines which is nicer - // for limited width text editing anyhow - let def = r#" -# This message defines meta information for a camera. It should be in a -# camera namespace on topic "camera_info" and accompanied by up to five -# image topics named: -# -# image_raw - raw data from the camera driver, possibly Bayer encoded -# image - monochrome, distorted -# image_color - color, distorted -# image_rect - monochrome, rectified -# image_rect_color - color, rectified -# -# The image_pipeline contains packages (image_proc, stereo_image_proc) -# for producing the four processed image topics from image_raw and -# camera_info. The meaning of the camera parameters are described in -# detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. -# -# The image_geometry package provides a user-friendly interface to -# common operations using this meta information. If you want to, e.g., -# project a 3d point into image coordinates, we strongly recommend -# using image_geometry. -# -# If the camera is uncalibrated, the matrices D, K, R, P should be left -# zeroed out. In particular, clients may assume that K[0] == 0.0 -# indicates an uncalibrated camera. - -####################################################################### -# Image acquisition info # -####################################################################### - -# Time of image acquisition, camera coordinate frame ID -Header header # Header timestamp should be acquisition time of image - # Header frame_id should be optical frame of camera - # origin of frame should be optical center of camera - # +x should point to the right in the image - # +y should point down in the image - # +z should point into the plane of the image - - -####################################################################### -# Calibration Parameters # -####################################################################### -# These are fixed during camera calibration. Their values will be the # -# same in all messages until the camera is recalibrated. Note that # -# self-calibrating systems may "recalibrate" frequently. # -# # -# The internal parameters can be used to warp a raw (distorted) image # -# to: # -# 1. An undistorted image (requires D and K) # -# 2. A rectified image (requires D, K, R) # -# The projection matrix P projects 3D points into the rectified image.# -####################################################################### - -# The image dimensions with which the camera was calibrated. Normally -# this will be the full camera resolution in pixels. -uint32 height -uint32 width - -# The distortion model used. Supported models are listed in -# sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a -# simple model of radial and tangential distortion - is sufficient. -string distortion_model - -# The distortion parameters, size depending on the distortion model. -# For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). -float64[] D - -# Intrinsic camera matrix for the raw (distorted) images. -# [fx 0 cx] -# K = [ 0 fy cy] -# [ 0 0 1] -# Projects 3D points in the camera coordinate frame to 2D pixel -# coordinates using the focal lengths (fx, fy) and principal point -# (cx, cy). -float64[9] K # 3x3 row-major matrix - -# Rectification matrix (stereo cameras only) -# A rotation matrix aligning the camera coordinate system to the ideal -# stereo image plane so that epipolar lines in both stereo images are -# parallel. -float64[9] R # 3x3 row-major matrix - -# Projection/camera matrix -# [fx' 0 cx' Tx] -# P = [ 0 fy' cy' Ty] -# [ 0 0 1 0] -# By convention, this matrix specifies the intrinsic (camera) matrix -# of the processed (rectified) image. That is, the left 3x3 portion -# is the normal camera intrinsic matrix for the rectified image. -# It projects 3D points in the camera coordinate frame to 2D pixel -# coordinates using the focal lengths (fx', fy') and principal point -# (cx', cy') - these may differ from the values in K. -# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will -# also have R = the identity and P[1:3,1:3] = K. -# For a stereo pair, the fourth column [Tx Ty 0]' is related to the -# position of the optical center of the second camera in the first -# camera's frame. We assume Tz = 0 so both cameras are in the same -# stereo image plane. The first camera always has Tx = Ty = 0. For -# the right (second) camera of a horizontal stereo pair, Ty = 0 and -# Tx = -fx' * B, where B is the baseline between the cameras. -# Given a 3D point [X Y Z]', the projection (x, y) of the point onto -# the rectified image is given by: -# [u v w]' = P * [X Y Z 1]' -# x = u / w -# y = v / w -# This holds for both images of a stereo pair. -float64[12] P # 3x4 row-major matrix - - -####################################################################### -# Operational Parameters # -####################################################################### -# These define the image region actually captured by the camera # -# driver. Although they affect the geometry of the output image, they # -# may be changed freely without recalibrating the camera. # -####################################################################### - -# Binning refers here to any camera setting which combines rectangular -# neighborhoods of pixels into larger "super-pixels." It reduces the -# resolution of the output image to -# (width / binning_x) x (height / binning_y). -# The default values binning_x = binning_y = 0 is considered the same -# as binning_x = binning_y = 1 (no subsampling). -uint32 binning_x -uint32 binning_y - -# Region of interest (subwindow of full camera resolution), given in -# full resolution (unbinned) image coordinates. A particular ROI -# always denotes the same window of pixels on the camera sensor, -# regardless of binning settings. -# The default setting of roi (all values 0) is considered the same as -# full resolution (roi.width = width, roi.height = height). -RegionOfInterest roi - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -string frame_id - -================================================================================ -MSG: sensor_msgs/RegionOfInterest -# This message is used to specify a region of interest within an image. -# -# When used to specify the ROI setting of the camera when the image was -# taken, the height and width fields should either match the height and -# width fields for the associated image; or height = width = 0 -# indicates that the full resolution image was captured. - -uint32 x_offset # Leftmost pixel of the ROI - # (0 if the ROI includes the left edge of the image) -uint32 y_offset # Topmost pixel of the ROI - # (0 if the ROI includes the top edge of the image) -uint32 height # Height of ROI -uint32 width # Width of ROI - -# True if a distinct rectified ROI should be calculated from the "raw" -# ROI in this message. Typically this should be False if the full image -# is captured (ROI not used), and True if a subwindow is captured (ROI -# used). -bool do_rectify - -"#; - let expected = "c9a58c1b0b154e0e6da7578cb991d214"; - let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected, "{msg_type}"); - } - - { - let msg_type = "std_msgs/Header"; - let def = "# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; - let expected = "2176decaecbce78abc3b96ef049fabed"; - let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected, "{msg_type}"); - } - - { - let msg_type = "rosgraph_msgs/Log"; - let def = "##\n## Severity level constants\n##\nbyte DEBUG=1 #debug level\nbyte INFO=2 #general level\nbyte WARN=4 #warning level\nbyte ERROR=8 #error level\nbyte FATAL=16 #fatal/critical level\n##\n## Fields\n##\nHeader header\nbyte level\nstring name # name of the node\nstring msg # message \nstring file # file the message came from\nstring function # function the message came from\nuint32 line # line the message came from\nstring[] topics # topic names that the node publishes\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; - let expected = "acffd30cd6b6de30f120938c17c593fb"; - let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected, "{msg_type}"); - } - - { - let msg_type = "nav_msgs/Odometry"; - let def = "# This represents an estimate of a position and velocity in free space. \n# The pose in this message should be specified in the coordinate frame given by header.frame_id.\n# The twist in this message should be specified in the coordinate frame given by the child_frame_id\nHeader header\nstring child_frame_id\ngeometry_msgs/PoseWithCovariance pose\ngeometry_msgs/TwistWithCovariance twist\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/PoseWithCovariance\n# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n================================================================================\nMSG: geometry_msgs/TwistWithCovariance\n# This expresses velocity in free space with uncertainty.\n\nTwist twist\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Twist\n# This expresses velocity in free space broken into its linear and angular parts.\nVector3 linear\nVector3 angular\n\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space. \n# It is only meant to represent a direction. Therefore, it does not\n# make sense to apply a translation to it (e.g., when applying a \n# generic rigid transformation to a Vector3, tf2 will only apply the\n# rotation). If you want your data to be translatable too, use the\n# geometry_msgs/Point message instead.\n\nfloat64 x\nfloat64 y\nfloat64 z"; - let expected = "cd5e73d190d741a2f92e81eda573aca7"; - let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected); - } - - { - let msg_type = "tf2_msgs/TFMessage"; - let def = r#" -geometry_msgs/TransformStamped[] transforms - -================================================================================ -MSG: geometry_msgs/TransformStamped -# This expresses a transform from coordinate frame header.frame_id -# to the coordinate frame child_frame_id -# -# This message is mostly used by the -# tf package. -# See its documentation for more information. - -Header header -string child_frame_id # the frame id of the child frame -Transform transform - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -string frame_id - -================================================================================ -MSG: geometry_msgs/Transform -# This represents the transform between two coordinate frames in free space. - -Vector3 translation -Quaternion rotation - -================================================================================ -MSG: geometry_msgs/Vector3 -# This represents a vector in free space. -# It is only meant to represent a direction. Therefore, it does not -# make sense to apply a translation to it (e.g., when applying a -# generic rigid transformation to a Vector3, tf2 will only apply the -# rotation). If you want your data to be translatable too, use the -# geometry_msgs/Point message instead. - -float64 x -float64 y -float64 z -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -"#; - let expected = "94810edda583a504dfda3829e70d7eec"; - let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected); - } - - { - let msg_type = "vision_msgs/Detection3DArray"; - let def = r#" -# A list of 3D detections, for a multi-object 3D detector. - -Header header - -# A list of the detected proposals. A multi-proposal detector might generate -# this list with many candidate detections generated from a single input. -Detection3D[] detections - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. -# -# sequence ID: consecutively increasing ID -uint32 seq -#Two-integer timestamp that is expressed as: -# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') -# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') -# time-handling sugar is provided by the client library -time stamp -#Frame this data is associated with -string frame_id - -================================================================================ -MSG: vision_msgs/Detection3D -# Defines a 3D detection result. -# -# This extends a basic 3D classification by including position information, -# allowing a classification result for a specific position in an image to -# to be located in the larger image. - -Header header - -# Class probabilities. Does not have to include hypotheses for all possible -# object ids, the scores for any ids not listed are assumed to be 0. -ObjectHypothesisWithPose[] results - -# 3D bounding box surrounding the object. -BoundingBox3D bbox - -# The 3D data that generated these results (i.e. region proposal cropped out of -# the image). This information is not required for all detectors, so it may -# be empty. -sensor_msgs/PointCloud2 source_cloud - -================================================================================ -MSG: vision_msgs/ObjectHypothesisWithPose -# An object hypothesis that contains position information. - -# The unique numeric ID of object detected. To get additional information about -# this ID, such as its human-readable name, listeners should perform a lookup -# in a metadata database. See vision_msgs/VisionInfo.msg for more detail. -int64 id - -# The probability or confidence value of the detected object. By convention, -# this value should lie in the range [0-1]. -float64 score - -# The 6D pose of the object hypothesis. This pose should be -# defined as the pose of some fixed reference point on the object, such a -# the geometric center of the bounding box or the center of mass of the -# object. -# Note that this pose is not stamped; frame information can be defined by -# parent messages. -# Also note that different classes predicted for the same input data may have -# different predicted 6D poses. -geometry_msgs/PoseWithCovariance pose -================================================================================ -MSG: geometry_msgs/PoseWithCovariance -# This represents a pose in free space with uncertainty. - -Pose pose - -# Row-major representation of the 6x6 covariance matrix -# The orientation parameters use a fixed-axis representation. -# In order, the parameters are: -# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) -float64[36] covariance - -================================================================================ -MSG: geometry_msgs/Pose -# A representation of pose in free space, composed of position and orientation. -Point position -Quaternion orientation - -================================================================================ -MSG: geometry_msgs/Point -# This contains the position of a point in free space -float64 x -float64 y -float64 z - -================================================================================ -MSG: geometry_msgs/Quaternion -# This represents an orientation in free space in quaternion form. - -float64 x -float64 y -float64 z -float64 w - -================================================================================ -MSG: vision_msgs/BoundingBox3D -# A 3D bounding box that can be positioned and rotated about its center (6 DOF) -# Dimensions of this box are in meters, and as such, it may be migrated to -# another package, such as geometry_msgs, in the future. - -# The 3D position and orientation of the bounding box center -geometry_msgs/Pose center - -# The size of the bounding box, in meters, surrounding the object's center -# pose. -geometry_msgs/Vector3 size - -================================================================================ -MSG: geometry_msgs/Vector3 -# This represents a vector in free space. -# It is only meant to represent a direction. Therefore, it does not -# make sense to apply a translation to it (e.g., when applying a -# generic rigid transformation to a Vector3, tf2 will only apply the -# rotation). If you want your data to be translatable too, use the -# geometry_msgs/Point message instead. - -float64 x -float64 y -float64 z -================================================================================ -MSG: sensor_msgs/PointCloud2 -# This message holds a collection of N-dimensional points, which may -# contain additional information such as normals, intensity, etc. The -# point data is stored as a binary blob, its layout described by the -# contents of the "fields" array. - -# The point cloud data may be organized 2d (image-like) or 1d -# (unordered). Point clouds organized as 2d images may be produced by -# camera depth sensors such as stereo or time-of-flight. - -# Time of sensor data acquisition, and the coordinate frame ID (for 3d -# points). -Header header - -# 2D structure of the point cloud. If the cloud is unordered, height is -# 1 and width is the length of the point cloud. -uint32 height -uint32 width - -# Describes the channels and their layout in the binary data blob. -PointField[] fields - -bool is_bigendian # Is this data bigendian? -uint32 point_step # Length of a point in bytes -uint32 row_step # Length of a row in bytes -uint8[] data # Actual point data, size is (row_step*height) - -bool is_dense # True if there are no invalid points - -================================================================================ -MSG: sensor_msgs/PointField -# This message holds the description of one point entry in the -# PointCloud2 message format. -uint8 INT8 = 1 -uint8 UINT8 = 2 -uint8 INT16 = 3 -uint8 UINT16 = 4 -uint8 INT32 = 5 -uint8 UINT32 = 6 -uint8 FLOAT32 = 7 -uint8 FLOAT64 = 8 - -string name # Name of field -uint32 offset # Offset from start of point struct -uint8 datatype # Datatype enumeration, see above -uint32 count # How many elements in the field - -"#; - let expected = "05c51d9aea1fb4cfdc8effb94f197b6f"; - let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); - println!("{msg_type}, computed {md5sum}, expected {expected}"); - assert_eq!(md5sum, expected, "{msg_type}"); - } - } - - // Basic test of clean_msg function - #[test] - fn clean_msg_test() { - let test_msg = r#" -# This message holds the description of one point entry in the -# PointCloud2 message format. -uint8 INT8 = 1 -uint8 UINT8 = 2 -uint8 INT16 = 3 -uint8 UINT16 = 4 -uint8 INT32 = 5 -uint8 UINT32 = 6 # Random Comment - -string name # Name of field -uint32 offset # Offset from start of point struct -uint8 datatype # Datatype enumeration, see above -uint32 count # How many elements in the field - - -uint8 FLOAT32 = 7 -uint8 FLOAT64 = 8 - -"#; - let result = crate::clean_msg(test_msg); - let expected = r#"uint8 INT8=1 -uint8 UINT8=2 -uint8 INT16=3 -uint8 UINT16=4 -uint8 INT32=5 -uint8 UINT32=6 -uint8 FLOAT32=7 -uint8 FLOAT64=8 -string name -uint32 offset -uint8 datatype -uint32 count"#; - assert_eq!(result, expected); - } } diff --git a/roslibrust_common/Cargo.toml b/roslibrust_common/Cargo.toml new file mode 100644 index 00000000..f2aed082 --- /dev/null +++ b/roslibrust_common/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "roslibrust_common" +version = "0.1.0" +edition = "2021" + +[dependencies] +# Used for error handling +thiserror = "2.0" +anyhow = "1.0" +# Used as basis for serialization +serde = { workspace = true } +# Used for md5sum calculation +md5 = "0.7" +# Used for async trait definitions +futures = "0.3" + +# THESE DEPENDENCIES WILL BE REMOVED +# We're currently leaking these error types in the "generic error type" +# We'll clean this up +tokio = { workspace = true } +tokio-tungstenite = { version = "0.17" } +serde_json = "1.0" diff --git a/roslibrust_common/src/lib.rs b/roslibrust_common/src/lib.rs new file mode 100644 index 00000000..085190df --- /dev/null +++ b/roslibrust_common/src/lib.rs @@ -0,0 +1,121 @@ +//! # roslibrust_common +//! This crate provides common types and traits used throughout the roslibrust ecosystem. + +/// For now starting with a central error type, may break this up more in future +#[derive(thiserror::Error, Debug)] +pub enum Error { + #[error("Not currently connected to ros master / bridge")] + Disconnected, + // TODO we probably want to eliminate tungstenite from this and hide our + // underlying websocket implementation from the API + // currently we "technically" break the API when we change tungstenite verisons + #[error("Websocket communication error: {0}")] + CommFailure(#[from] tokio_tungstenite::tungstenite::Error), + #[error("Operation timed out: {0}")] + Timeout(#[from] tokio::time::error::Elapsed), + #[error("Failed to parse message from JSON: {0}")] + InvalidMessage(#[from] serde_json::Error), + #[error("TCPROS serialization error: {0}")] + SerializationError(String), + #[error("Rosbridge server reported an error: {0}")] + ServerError(String), + #[error("IO error: {0}")] + IoError(#[from] std::io::Error), + #[error("Name does not meet ROS requirements: {0}")] + InvalidName(String), + // Generic catch-all error type for not-yet-handled errors + // TODO ultimately this type will be removed from API of library + #[error(transparent)] + Unexpected(#[from] anyhow::Error), +} + +/// Generic result type used as standard throughout library. +/// Note: many functions which currently return this will be updated to provide specific error +/// types in the future instead of the generic error here. +pub type Result = std::result::Result; + +/// Fundamental traits for message types this crate works with +/// This trait will be satisfied for any types generated with this crate's message_gen functionality +pub trait RosMessageType: + 'static + serde::de::DeserializeOwned + Send + serde::Serialize + Sync + Clone + std::fmt::Debug +{ + /// Expected to be the combination pkg_name/type_name string describing the type to ros + /// Example: std_msgs/Header + const ROS_TYPE_NAME: &'static str; + + /// The computed md5sum of the message file and its dependencies + /// This field is optional, and only needed when using ros1 native communication + const MD5SUM: &'static str = ""; + + /// The definition from the msg, srv, or action file + /// This field is optional, and only needed when using ros1 native communication + const DEFINITION: &'static str = ""; +} + +// This special impl allows for services with no args / returns +impl RosMessageType for () { + const ROS_TYPE_NAME: &'static str = ""; + const MD5SUM: &'static str = ""; + const DEFINITION: &'static str = ""; +} + +/// Fundamental traits for service types this crate works with +/// This trait will be satisfied for any services definitions generated with this crate's message_gen functionality +pub trait RosServiceType: 'static + Send + Sync { + /// Name of the ros service e.g. `rospy_tutorials/AddTwoInts` + const ROS_SERVICE_NAME: &'static str; + /// The computed md5sum of the message file and its dependencies + const MD5SUM: &'static str; + /// The type of data being sent in the request + type Request: RosMessageType; + /// The type of the data + type Response: RosMessageType; +} + +// Note: service Fn is currently defined here as it used by ros1 and roslibrust impls +/// This trait describes a function which can validly act as a ROS service +/// server with roslibrust. We're really just using this as a trait alias +/// as the full definition is overly verbose and trait aliases are unstable. +pub trait ServiceFn: + Fn( + T::Request, + ) -> std::result::Result> + + Send + + Sync + + 'static +{ +} + +/// Automatic implementation of ServiceFn for Fn +impl ServiceFn for F +where + T: RosServiceType, + F: Fn( + T::Request, + ) + -> std::result::Result> + + Send + + Sync + + 'static, +{ +} + +/// A generic message type used by some implementations to provide a generic subscriber / publisher without serialization +#[derive(:: serde :: Deserialize, :: serde :: Serialize, Debug, Default, Clone, PartialEq)] +pub struct ShapeShifter(Vec); + +// The equivalent of rospy AnyMsg or C++ ShapeShifter, subscribe_any() uses this type +impl RosMessageType for ShapeShifter { + const ROS_TYPE_NAME: &'static str = "*"; + const MD5SUM: &'static str = "*"; + const DEFINITION: &'static str = ""; +} + +/// Contains functions for calculating md5sums of message definitions +/// These functions are needed both in roslibrust_ros1 and roslibrust_codegen so they're in this crate +pub mod md5sum; + +/// Contains the generic traits represent a pubsub system and service system +/// These traits will be implemented for specific backends to provides access to "ROS Like" functionality +pub mod topic_provider; +pub use topic_provider::*; // Bring topic provider traits into root namespace diff --git a/roslibrust_common/src/md5sum.rs b/roslibrust_common/src/md5sum.rs new file mode 100644 index 00000000..a7fdd4dd --- /dev/null +++ b/roslibrust_common/src/md5sum.rs @@ -0,0 +1,769 @@ +use anyhow::{anyhow, bail, Error}; +use std::collections::{HashMap, HashSet}; + +// TODO(lucasw) this deserves a lot of str vs String cleanup +/// This function will calculate the md5sum of an expanded message definition. +/// The expanded message definition is the output of `gendeps --cat` see: +/// This definition is typically sent in the connection header of a ros1 topic and is also stored in bag files. +/// This can be used to calculate the md5sum when message definitions aren't available at compile time. +pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result { + if full_def.is_empty() { + bail!("empty input definition"); + } + + // Split the full definition into sections per message + let sep: &str = + "================================================================================\n"; + let sections = full_def.split(sep).collect::>(); + if sections.is_empty() { + // Carter: this error is impossible, split only gives empty iterator when input string is empty + // which we've already checked for above + bail!("empty sections"); + } + + // Split the overall definition into separate sub-messages sorted by message type (including package name) + let mut sub_messages: HashMap<&str, String> = HashMap::new(); + // Note: the first section doesn't contain the "MSG: " line so we don't need to strip it here + let clean_root = clean_msg(sections[0]); + if clean_root.is_empty() { + bail!("empty cleaned root definition"); + } + sub_messages.insert(msg_name, clean_root); + + for section in §ions[1..] { + let line0 = section.lines().next().ok_or(anyhow!("empty section"))?; + if !line0.starts_with("MSG: ") { + bail!("bad section {section} -> {line0} doesn't start with 'MSG: '"); + } + // TODO(lucasw) the full text definition doesn't always have the full message types with + // the package name, + // but I think this is only when the message type is Header or the package of the message + // being define is the same as the message in the field + // Carter: I agree with this, we found the same when dealing with this previously + let section_type = line0.split_whitespace().collect::>()[1]; + let end_of_first_line = section + .find('\n') + .ok_or(anyhow!("No body found in section"))?; + let body = clean_msg(§ion[end_of_first_line + 1..]); + sub_messages.insert(section_type, body); + } + + // TODO MAJOR(carter): I'd like to convert this loop to a recursive function where we pass in the map of hashes + // and update them as we go, this tripple loop is stinky to my eye. + // TODO(carter) we should be able to do this in close to one pass if we iterate the full_def backwards + let mut hashed = HashMap::new(); + let hash = message_definition_to_md5sum_recursive(msg_name, &sub_messages, &mut hashed)?; + + Ok(hash) +} + +/// Calculates the hash of the specified message type by recursively calling itself on all dependencies +/// Uses defs as the list of message definitions available for it (expects them to already be cleaned) +/// Uses hashes as the cache of already calculated hashes so we don't redo work +fn message_definition_to_md5sum_recursive( + msg_type: &str, + defs: &HashMap<&str, String>, + hashes: &mut HashMap, +) -> Result { + let base_types: HashSet = HashSet::from_iter( + [ + "bool", "byte", "int8", "int16", "int32", "int64", "uint8", "uint16", "uint32", + "uint64", "float32", "float64", "time", "duration", "string", + ] + .map(|name| name.to_string()), + ); + let def = defs + .get(msg_type) + .ok_or(anyhow!("Couldn't find message type: {msg_type}"))?; + let pkg_name = msg_type.split('/').collect::>()[0]; + // We'll store the expanded hash definition in this string as we go + let mut field_def = "".to_string(); + for line_raw in def.lines() { + let line_split = line_raw.split_whitespace().collect::>(); + if line_split.len() < 2 { + bail!("bad line to split '{line_raw}'"); + } + let (raw_field_type, _field_name) = (line_split[0], line_split[1]); + // leave array characters alone, could be [] [C] where C is a constant + let field_type = raw_field_type.split('[').collect::>()[0].to_string(); + + let full_field_type; + let line; + if base_types.contains(&field_type) { + line = line_raw.to_string(); + } else { + // TODO(lucasw) are there other special message types besides header- or is it anything in std_msgs? + if field_type == "Header" { + full_field_type = "std_msgs/Header".to_string(); + } else if !field_type.contains('/') { + full_field_type = format!("{pkg_name}/{field_type}"); + } else { + full_field_type = field_type; + } + + match hashes.get(&full_field_type) { + Some(hash_value) => { + // Hash already exists in cache so we can use it + line = line_raw.replace(raw_field_type, hash_value).to_string(); + } + None => { + // Recurse! To calculate hash of this field type + let hash = + message_definition_to_md5sum_recursive(&full_field_type, defs, hashes)?; + line = line_raw.replace(raw_field_type, &hash).to_string(); + } + } + } + field_def += &format!("{line}\n"); + } + field_def = field_def.trim().to_string(); + let md5sum = md5::compute(field_def.trim_end().as_bytes()); + let md5sum_text = format!("{md5sum:x}"); + // Insert our hash into the cache before we return + hashes.insert(msg_type.to_string(), md5sum_text.clone()); + + Ok(md5sum_text) +} + +/// Taking in a message definition +/// reformat it according to the md5sum algorithm: +/// - Comments removed +/// - Extra whitespace removed +/// - package names of dependencies removed +/// - constants reordered to be at the front +fn clean_msg(msg: &str) -> String { + let mut result_params = vec![]; + let mut result_constants = vec![]; + for line in msg.lines() { + let line = line.trim(); + // Skip comment lines + if line.starts_with('#') { + continue; + } + // Strip comment from the end of the line (if present) + let line = line.split('#').collect::>()[0].trim(); + // Remove any extra whitespace from inside the line + let line = line.split_whitespace().collect::>().join(" "); + // Remove any whitespace on either side of the "=" for constants + let line = line.replace(" = ", "="); + // Skip any empty lines + if line.is_empty() { + continue; + } + // Determine if constant or not + if line.contains('=') { + result_constants.push(line); + } else { + result_params.push(line); + } + } + format!( + "{}\n{}", + result_constants.join("\n"), + result_params.join("\n") + ) + .trim() + .to_string() // Last trim here is lazy, but gets job done +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Confirm md5sum from the connection header message definition matches normally + /// generated checksums + #[test] + fn msg_def_to_md5() { + { + let def = "byte DEBUG=1\nbyte INFO=2\nbyte WARN=4\nbyte ERROR=8\nbyte FATAL=16\n\ + 2176decaecbce78abc3b96ef049fabed header\n\ + byte level\nstring name\nstring msg\nstring file\nstring function\nuint32 line\nstring[] topics"; + let expected = "acffd30cd6b6de30f120938c17c593fb"; + let md5sum = format!("{:x}", md5::compute(def.trim_end().as_bytes())); + assert_eq!(md5sum, expected, "partially checksumed rosgraph_msgs/Log"); + } + + { + let msg_type = "bad_msgs/Empty"; + let def = ""; + let _md5sum = message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + let msg_type = "bad_msgs/CommentSpacesOnly"; + let def = + "# message with only comments and whitespace\n# another line comment\n\n \n"; + let _md5sum = message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + let msg_type = "fake_msgs/MissingSectionMsg"; + let def = "string name\nstring msg\n================================================================================\n# message with only comments and whitespace\n# another line comment\n\n \n"; + let _md5sum = message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + let msg_type = "bad_msgs/BadLog"; + let def = "## +## Severity level constants +byte DEUG=1 #debug level +byte FATAL=16 #fatal/critical level +## +## Fields +## +Header header +byte level +string name # name of the node +uint32 line # line the message came from +string[] topics # topic names that the node publishes + +================================================================================ +MSG: std_msgs/badHeader +# Standard metadata for higher-level stamped data types. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +time stamp +"; + let _md5sum = message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + // TODO(lucasw) not sure if this is an ok message, currently it passes + let expected = "96c44a027b586ee888fe95ac325151ae"; + let msg_type = "fake_msgs/CommentSpacesOnlySection"; + let def = "string name\nstring msg\n================================================================================\nMSG: foo/bar\n# message with only comments and whitespace\n# another line comment\n\n \n"; + let md5sum = message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "fake_msgs/Garbage"; + let def = r#" +fsdajklf + +== #fdjkl + +MSG: jklfd +# +================================================================================ +f + +vjk +"#; + let _md5sum = message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + // TODO(lucasw) it would be nice to pull these out of the real messages, but to avoid + // dependencies just storing expected definition and md5sum + // from roslib.message import get_message_class + // msg = get_message_class("std_msgs/Header") + // md5 = msg._md5sum + // def = msg._full_text + // + + { + let msg_type = "sensor_msgs/CameraInfo"; + // This definition contains double quotes, so representing it with r# and newlines which is nicer + // for limited width text editing anyhow + let def = r#" +# This message defines meta information for a camera. It should be in a +# camera namespace on topic "camera_info" and accompanied by up to five +# image topics named: +# +# image_raw - raw data from the camera driver, possibly Bayer encoded +# image - monochrome, distorted +# image_color - color, distorted +# image_rect - monochrome, rectified +# image_rect_color - color, rectified +# +# The image_pipeline contains packages (image_proc, stereo_image_proc) +# for producing the four processed image topics from image_raw and +# camera_info. The meaning of the camera parameters are described in +# detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. +# +# The image_geometry package provides a user-friendly interface to +# common operations using this meta information. If you want to, e.g., +# project a 3d point into image coordinates, we strongly recommend +# using image_geometry. +# +# If the camera is uncalibrated, the matrices D, K, R, P should be left +# zeroed out. In particular, clients may assume that K[0] == 0.0 +# indicates an uncalibrated camera. + +####################################################################### +# Image acquisition info # +####################################################################### + +# Time of image acquisition, camera coordinate frame ID +Header header # Header timestamp should be acquisition time of image + # Header frame_id should be optical frame of camera + # origin of frame should be optical center of camera + # +x should point to the right in the image + # +y should point down in the image + # +z should point into the plane of the image + + +####################################################################### +# Calibration Parameters # +####################################################################### +# These are fixed during camera calibration. Their values will be the # +# same in all messages until the camera is recalibrated. Note that # +# self-calibrating systems may "recalibrate" frequently. # +# # +# The internal parameters can be used to warp a raw (distorted) image # +# to: # +# 1. An undistorted image (requires D and K) # +# 2. A rectified image (requires D, K, R) # +# The projection matrix P projects 3D points into the rectified image.# +####################################################################### + +# The image dimensions with which the camera was calibrated. Normally +# this will be the full camera resolution in pixels. +uint32 height +uint32 width + +# The distortion model used. Supported models are listed in +# sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a +# simple model of radial and tangential distortion - is sufficient. +string distortion_model + +# The distortion parameters, size depending on the distortion model. +# For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). +float64[] D + +# Intrinsic camera matrix for the raw (distorted) images. +# [fx 0 cx] +# K = [ 0 fy cy] +# [ 0 0 1] +# Projects 3D points in the camera coordinate frame to 2D pixel +# coordinates using the focal lengths (fx, fy) and principal point +# (cx, cy). +float64[9] K # 3x3 row-major matrix + +# Rectification matrix (stereo cameras only) +# A rotation matrix aligning the camera coordinate system to the ideal +# stereo image plane so that epipolar lines in both stereo images are +# parallel. +float64[9] R # 3x3 row-major matrix + +# Projection/camera matrix +# [fx' 0 cx' Tx] +# P = [ 0 fy' cy' Ty] +# [ 0 0 1 0] +# By convention, this matrix specifies the intrinsic (camera) matrix +# of the processed (rectified) image. That is, the left 3x3 portion +# is the normal camera intrinsic matrix for the rectified image. +# It projects 3D points in the camera coordinate frame to 2D pixel +# coordinates using the focal lengths (fx', fy') and principal point +# (cx', cy') - these may differ from the values in K. +# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will +# also have R = the identity and P[1:3,1:3] = K. +# For a stereo pair, the fourth column [Tx Ty 0]' is related to the +# position of the optical center of the second camera in the first +# camera's frame. We assume Tz = 0 so both cameras are in the same +# stereo image plane. The first camera always has Tx = Ty = 0. For +# the right (second) camera of a horizontal stereo pair, Ty = 0 and +# Tx = -fx' * B, where B is the baseline between the cameras. +# Given a 3D point [X Y Z]', the projection (x, y) of the point onto +# the rectified image is given by: +# [u v w]' = P * [X Y Z 1]' +# x = u / w +# y = v / w +# This holds for both images of a stereo pair. +float64[12] P # 3x4 row-major matrix + + +####################################################################### +# Operational Parameters # +####################################################################### +# These define the image region actually captured by the camera # +# driver. Although they affect the geometry of the output image, they # +# may be changed freely without recalibrating the camera. # +####################################################################### + +# Binning refers here to any camera setting which combines rectangular +# neighborhoods of pixels into larger "super-pixels." It reduces the +# resolution of the output image to +# (width / binning_x) x (height / binning_y). +# The default values binning_x = binning_y = 0 is considered the same +# as binning_x = binning_y = 1 (no subsampling). +uint32 binning_x +uint32 binning_y + +# Region of interest (subwindow of full camera resolution), given in +# full resolution (unbinned) image coordinates. A particular ROI +# always denotes the same window of pixels on the camera sensor, +# regardless of binning settings. +# The default setting of roi (all values 0) is considered the same as +# full resolution (roi.width = width, roi.height = height). +RegionOfInterest roi + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: sensor_msgs/RegionOfInterest +# This message is used to specify a region of interest within an image. +# +# When used to specify the ROI setting of the camera when the image was +# taken, the height and width fields should either match the height and +# width fields for the associated image; or height = width = 0 +# indicates that the full resolution image was captured. + +uint32 x_offset # Leftmost pixel of the ROI + # (0 if the ROI includes the left edge of the image) +uint32 y_offset # Topmost pixel of the ROI + # (0 if the ROI includes the top edge of the image) +uint32 height # Height of ROI +uint32 width # Width of ROI + +# True if a distinct rectified ROI should be calculated from the "raw" +# ROI in this message. Typically this should be False if the full image +# is captured (ROI not used), and True if a subwindow is captured (ROI +# used). +bool do_rectify + +"#; + let expected = "c9a58c1b0b154e0e6da7578cb991d214"; + let md5sum = message_definition_to_md5sum(msg_type, def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "std_msgs/Header"; + let def = "# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; + let expected = "2176decaecbce78abc3b96ef049fabed"; + let md5sum = message_definition_to_md5sum(msg_type, def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "rosgraph_msgs/Log"; + let def = "##\n## Severity level constants\n##\nbyte DEBUG=1 #debug level\nbyte INFO=2 #general level\nbyte WARN=4 #warning level\nbyte ERROR=8 #error level\nbyte FATAL=16 #fatal/critical level\n##\n## Fields\n##\nHeader header\nbyte level\nstring name # name of the node\nstring msg # message \nstring file # file the message came from\nstring function # function the message came from\nuint32 line # line the message came from\nstring[] topics # topic names that the node publishes\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; + let expected = "acffd30cd6b6de30f120938c17c593fb"; + let md5sum = message_definition_to_md5sum(msg_type, def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "nav_msgs/Odometry"; + let def = "# This represents an estimate of a position and velocity in free space. \n# The pose in this message should be specified in the coordinate frame given by header.frame_id.\n# The twist in this message should be specified in the coordinate frame given by the child_frame_id\nHeader header\nstring child_frame_id\ngeometry_msgs/PoseWithCovariance pose\ngeometry_msgs/TwistWithCovariance twist\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/PoseWithCovariance\n# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n================================================================================\nMSG: geometry_msgs/TwistWithCovariance\n# This expresses velocity in free space with uncertainty.\n\nTwist twist\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Twist\n# This expresses velocity in free space broken into its linear and angular parts.\nVector3 linear\nVector3 angular\n\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space. \n# It is only meant to represent a direction. Therefore, it does not\n# make sense to apply a translation to it (e.g., when applying a \n# generic rigid transformation to a Vector3, tf2 will only apply the\n# rotation). If you want your data to be translatable too, use the\n# geometry_msgs/Point message instead.\n\nfloat64 x\nfloat64 y\nfloat64 z"; + let expected = "cd5e73d190d741a2f92e81eda573aca7"; + let md5sum = message_definition_to_md5sum(msg_type, def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected); + } + + { + let msg_type = "tf2_msgs/TFMessage"; + let def = r#" +geometry_msgs/TransformStamped[] transforms + +================================================================================ +MSG: geometry_msgs/TransformStamped +# This expresses a transform from coordinate frame header.frame_id +# to the coordinate frame child_frame_id +# +# This message is mostly used by the +# tf package. +# See its documentation for more information. + +Header header +string child_frame_id # the frame id of the child frame +Transform transform + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Transform +# This represents the transform between two coordinate frames in free space. + +Vector3 translation +Quaternion rotation + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +"#; + let expected = "94810edda583a504dfda3829e70d7eec"; + let md5sum = message_definition_to_md5sum(msg_type, def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected); + } + + { + let msg_type = "vision_msgs/Detection3DArray"; + let def = r#" +# A list of 3D detections, for a multi-object 3D detector. + +Header header + +# A list of the detected proposals. A multi-proposal detector might generate +# this list with many candidate detections generated from a single input. +Detection3D[] detections + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: vision_msgs/Detection3D +# Defines a 3D detection result. +# +# This extends a basic 3D classification by including position information, +# allowing a classification result for a specific position in an image to +# to be located in the larger image. + +Header header + +# Class probabilities. Does not have to include hypotheses for all possible +# object ids, the scores for any ids not listed are assumed to be 0. +ObjectHypothesisWithPose[] results + +# 3D bounding box surrounding the object. +BoundingBox3D bbox + +# The 3D data that generated these results (i.e. region proposal cropped out of +# the image). This information is not required for all detectors, so it may +# be empty. +sensor_msgs/PointCloud2 source_cloud + +================================================================================ +MSG: vision_msgs/ObjectHypothesisWithPose +# An object hypothesis that contains position information. + +# The unique numeric ID of object detected. To get additional information about +# this ID, such as its human-readable name, listeners should perform a lookup +# in a metadata database. See vision_msgs/VisionInfo.msg for more detail. +int64 id + +# The probability or confidence value of the detected object. By convention, +# this value should lie in the range [0-1]. +float64 score + +# The 6D pose of the object hypothesis. This pose should be +# defined as the pose of some fixed reference point on the object, such a +# the geometric center of the bounding box or the center of mass of the +# object. +# Note that this pose is not stamped; frame information can be defined by +# parent messages. +# Also note that different classes predicted for the same input data may have +# different predicted 6D poses. +geometry_msgs/PoseWithCovariance pose +================================================================================ +MSG: geometry_msgs/PoseWithCovariance +# This represents a pose in free space with uncertainty. + +Pose pose + +# Row-major representation of the 6x6 covariance matrix +# The orientation parameters use a fixed-axis representation. +# In order, the parameters are: +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) +float64[36] covariance + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: vision_msgs/BoundingBox3D +# A 3D bounding box that can be positioned and rotated about its center (6 DOF) +# Dimensions of this box are in meters, and as such, it may be migrated to +# another package, such as geometry_msgs, in the future. + +# The 3D position and orientation of the bounding box center +geometry_msgs/Pose center + +# The size of the bounding box, in meters, surrounding the object's center +# pose. +geometry_msgs/Vector3 size + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +================================================================================ +MSG: sensor_msgs/PointCloud2 +# This message holds a collection of N-dimensional points, which may +# contain additional information such as normals, intensity, etc. The +# point data is stored as a binary blob, its layout described by the +# contents of the "fields" array. + +# The point cloud data may be organized 2d (image-like) or 1d +# (unordered). Point clouds organized as 2d images may be produced by +# camera depth sensors such as stereo or time-of-flight. + +# Time of sensor data acquisition, and the coordinate frame ID (for 3d +# points). +Header header + +# 2D structure of the point cloud. If the cloud is unordered, height is +# 1 and width is the length of the point cloud. +uint32 height +uint32 width + +# Describes the channels and their layout in the binary data blob. +PointField[] fields + +bool is_bigendian # Is this data bigendian? +uint32 point_step # Length of a point in bytes +uint32 row_step # Length of a row in bytes +uint8[] data # Actual point data, size is (row_step*height) + +bool is_dense # True if there are no invalid points + +================================================================================ +MSG: sensor_msgs/PointField +# This message holds the description of one point entry in the +# PointCloud2 message format. +uint8 INT8 = 1 +uint8 UINT8 = 2 +uint8 INT16 = 3 +uint8 UINT16 = 4 +uint8 INT32 = 5 +uint8 UINT32 = 6 +uint8 FLOAT32 = 7 +uint8 FLOAT64 = 8 + +string name # Name of field +uint32 offset # Offset from start of point struct +uint8 datatype # Datatype enumeration, see above +uint32 count # How many elements in the field + +"#; + let expected = "05c51d9aea1fb4cfdc8effb94f197b6f"; + let md5sum = message_definition_to_md5sum(msg_type, def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + } + + // Basic test of clean_msg function + #[test] + fn clean_msg_test() { + let test_msg = r#" +# This message holds the description of one point entry in the +# PointCloud2 message format. +uint8 INT8 = 1 +uint8 UINT8 = 2 +uint8 INT16 = 3 +uint8 UINT16 = 4 +uint8 INT32 = 5 +uint8 UINT32 = 6 # Random Comment + +string name # Name of field +uint32 offset # Offset from start of point struct +uint8 datatype # Datatype enumeration, see above +uint32 count # How many elements in the field + + +uint8 FLOAT32 = 7 +uint8 FLOAT64 = 8 + +"#; + let result = clean_msg(test_msg); + let expected = r#"uint8 INT8=1 +uint8 UINT8=2 +uint8 INT16=3 +uint8 UINT16=4 +uint8 INT32=5 +uint8 UINT32=6 +uint8 FLOAT32=7 +uint8 FLOAT64=8 +string name +uint32 offset +uint8 datatype +uint32 count"#; + assert_eq!(result, expected); + } +} diff --git a/roslibrust_common/src/topic_provider.rs b/roslibrust_common/src/topic_provider.rs new file mode 100644 index 00000000..add41e80 --- /dev/null +++ b/roslibrust_common/src/topic_provider.rs @@ -0,0 +1,102 @@ +use crate::{Result, RosMessageType, RosServiceType, ServiceFn}; + +/// Indicates that something is a publisher and has our expected publish +/// Implementors of this trait are expected to auto-cleanup the publisher when dropped +pub trait Publish { + // Note: this is really just syntactic de-sugared `async fn` + // However see: https://blog.rust-lang.org/2023/12/21/async-fn-rpit-in-traits.html + // This generates a warning is rust as of writing due to ambiguity around the "Send-ness" of the return type + // We only plan to work with multi-threaded work stealing executors (e.g. tokio) so we're manually specifying Send + fn publish(&self, data: &T) -> impl futures::Future> + Send; +} + +/// Indicates that something is a subscriber and has our expected subscribe method +/// Implementors of this trait are expected to auto-cleanup the subscriber when dropped +pub trait Subscribe { + // TODO need to solidify how we want errors to work with subscribers... + // TODO ros1 currently requires mut for next, we should change that + fn next(&mut self) -> impl futures::Future> + Send; +} + +/// This trait generically describes the capability of something to act as an async interface to a set of topics +/// +/// This trait is largely based on ROS concepts, but could be extended to other protocols / concepts. +/// Fundamentally, it assumes that topics are uniquely identified by a string name (likely an ASCII assumption is buried in here...). +/// It assumes topics only carry one data type, but is not expected to enforce that. +/// It assumes that all actions can fail due to a variety of causes, and by network interruption specifically. +pub trait TopicProvider { + // These associated types makeup the other half of the API + // They are expected to be "self-deregistering", where dropping them results in unadvertise or unsubscribe operations as appropriate + // We require Publisher and Subscriber types to be Send + 'static so they can be sent into different tokio tasks once created + type Publisher: Publish + Send + 'static; + type Subscriber: Subscribe + Send + 'static; + + /// Advertises a topic to be published to and returns a type specific publisher to use. + /// + /// The returned publisher is expected to be "self de-registering", where dropping the publisher results in the appropriate unadvertise operation. + fn advertise( + &self, + topic: &str, + ) -> impl futures::Future>> + Send; + + /// Subscribes to a topic and returns a type specific subscriber to use. + /// + /// The returned subscriber is expected to be "self de-registering", where dropping the subscriber results in the appropriate unsubscribe operation. + fn subscribe( + &self, + topic: &str, + ) -> impl futures::Future>> + Send; +} + +/// Defines what it means to be something that is callable as a service +pub trait Service { + fn call( + &self, + request: &T::Request, + ) -> impl futures::Future> + Send; +} + +/// This trait is analogous to TopicProvider, but instead provides the capability to create service servers and service clients +pub trait ServiceProvider { + type ServiceClient: Service + Send + 'static; + type ServiceServer; + + /// A "oneshot" service call good for low frequency calls or where the service_provider may not always be available. + fn call_service( + &self, + topic: &str, + request: T::Request, + ) -> impl futures::Future> + Send; + + /// An optimized version of call_service that returns a persistent client that can be used to repeatedly call a service. + /// Depending on backend this may provide a performance benefit over call_service. + /// Dropping the returned client will perform all needed cleanup. + fn service_client( + &self, + topic: &str, + ) -> impl futures::Future>> + Send; + + /// Advertise a service function to be available for clients to call. + /// A handle is returned that manages the lifetime of the service. + /// Dropping the handle will perform all needed cleanup. + /// The service will be active until the handle is dropped. + /// Currently this function only accepts non-async functions, but with the stabilization of async closures this may change. + fn advertise_service( + &self, + topic: &str, + server: F, + ) -> impl futures::Future> + Send + where + F: ServiceFn; +} + +#[cfg(test)] +mod test { + // This test specifically fails because TopicProvider is not object safe + // Traits that have methods with generic parameters cannot be object safe in rust (currently) + // #[test] + // fn topic_provider_can_be_constructed() -> TestResult { + // let x: Box = Box::new(ClientHandle::new("")); + // Ok(()) + // } +} diff --git a/roslibrust_genmsg/Cargo.toml b/roslibrust_genmsg/Cargo.toml index db8dc53d..bbc81284 100644 --- a/roslibrust_genmsg/Cargo.toml +++ b/roslibrust_genmsg/Cargo.toml @@ -18,14 +18,14 @@ name = "gencpp" path = "src/main.rs" [dependencies] +log = { workspace = true } +serde = { workspace = true } +roslibrust_codegen = { path = "../roslibrust_codegen", version = "0.11.0" } clap = { version = "4.1", features = ["derive"] } env_logger = "0.11" itertools = "0.12" lazy_static = "1.4" -log = "0.4" minijinja = "2.0" -roslibrust_codegen = { path = "../roslibrust_codegen", version = "0.11.0" } -serde = { version = "1", features = ["derive"] } serde_json = "1" [dev-dependencies] diff --git a/roslibrust_mock/Cargo.toml b/roslibrust_mock/Cargo.toml index beba67ca..5c882c56 100644 --- a/roslibrust_mock/Cargo.toml +++ b/roslibrust_mock/Cargo.toml @@ -4,12 +4,12 @@ version = "0.1.0" edition = "2021" [dependencies] -roslibrust = { path = "../roslibrust", features = ["topic_provider"] } -tokio = { version = "1.41", features = ["sync", "rt-multi-thread", "macros"] } +roslibrust_common = { path = "../roslibrust_common" } +tokio = { workspace = true } # Used for serializing messages bincode = "1.3" # We add logging to aid in debugging tests -log = "0.4" +log = { workspace = true } [dev-dependencies] roslibrust_codegen = { path = "../roslibrust_codegen" } diff --git a/roslibrust_mock/src/lib.rs b/roslibrust_mock/src/lib.rs index b8f2ba4c..9430b17c 100644 --- a/roslibrust_mock/src/lib.rs +++ b/roslibrust_mock/src/lib.rs @@ -1,20 +1,15 @@ use std::collections::BTreeMap; use std::sync::Arc; -use roslibrust::topic_provider::*; -use roslibrust::RosLibRustError; -use roslibrust::RosLibRustResult; -use roslibrust::RosMessageType; +use roslibrust_common::*; -use roslibrust::RosServiceType; -use roslibrust::ServiceFn; use tokio::sync::broadcast as Channel; use tokio::sync::RwLock; use log::*; type TypeErasedCallback = Arc< - dyn Fn(Vec) -> Result, Box> + dyn Fn(Vec) -> std::result::Result, Box> + Send + Sync + 'static, @@ -42,10 +37,7 @@ impl TopicProvider for MockRos { type Publisher = MockPublisher; type Subscriber = MockSubscriber; - async fn advertise( - &self, - topic: &str, - ) -> RosLibRustResult> { + async fn advertise(&self, topic: &str) -> Result> { // Check if we already have this channel { let topics = self.topics.read().await; @@ -72,7 +64,7 @@ impl TopicProvider for MockRos { async fn subscribe( &self, topic: &str, - ) -> RosLibRustResult> { + ) -> roslibrust_common::Result> { // Check if we already have this channel { let topics = self.topics.read().await; @@ -103,13 +95,13 @@ pub struct MockServiceClient { } impl Service for MockServiceClient { - async fn call(&self, request: &T::Request) -> RosLibRustResult { - let data = bincode::serialize(request) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; - let response = (self.callback)(data) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; + async fn call(&self, request: &T::Request) -> roslibrust_common::Result { + let data = + bincode::serialize(request).map_err(|e| Error::SerializationError(e.to_string()))?; + let response = + (self.callback)(data).map_err(|e| Error::SerializationError(e.to_string()))?; let response = bincode::deserialize(&response[..]) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; + .map_err(|e| Error::SerializationError(e.to_string()))?; Ok(response) } } @@ -118,10 +110,19 @@ impl ServiceProvider for MockRos { type ServiceClient = MockServiceClient; type ServiceServer = (); + async fn call_service( + &self, + topic: &str, + request: T::Request, + ) -> roslibrust_common::Result { + let client = self.service_client::(topic).await?; + client.call(&request).await + } + async fn service_client( &self, topic: &str, - ) -> RosLibRustResult> { + ) -> roslibrust_common::Result> { let services = self.services.read().await; if let Some(callback) = services.get(topic) { return Ok(MockServiceClient { @@ -129,27 +130,29 @@ impl ServiceProvider for MockRos { _marker: Default::default(), }); } - Err(RosLibRustError::Disconnected) + Err(Error::Disconnected) } async fn advertise_service( &self, topic: &str, server: F, - ) -> RosLibRustResult + ) -> roslibrust_common::Result where F: ServiceFn, { // Type erase the service function here - let erased_closure = - move |message: Vec| -> Result, Box> { - let request = bincode::deserialize(&message[..]) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; - let response = server(request)?; - let bytes = bincode::serialize(&response) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; - Ok(bytes) - }; + let erased_closure = move |message: Vec| -> std::result::Result< + Vec, + Box, + > { + let request = bincode::deserialize(&message[..]) + .map_err(|e| Error::SerializationError(e.to_string()))?; + let response = server(request)?; + let bytes = bincode::serialize(&response) + .map_err(|e| Error::SerializationError(e.to_string()))?; + Ok(bytes) + }; let erased_closure = Arc::new(erased_closure); let mut services = self.services.write().await; services.insert(topic.to_string(), erased_closure); @@ -166,12 +169,10 @@ pub struct MockPublisher { } impl Publish for MockPublisher { - async fn publish(&self, data: &T) -> RosLibRustResult<()> { - let data = bincode::serialize(data) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; - self.sender - .send(data) - .map_err(|_e| RosLibRustError::Disconnected)?; + async fn publish(&self, data: &T) -> roslibrust_common::Result<()> { + let data = + bincode::serialize(data).map_err(|e| Error::SerializationError(e.to_string()))?; + self.sender.send(data).map_err(|_e| Error::Disconnected)?; debug!("Sent data on topic {}", T::ROS_TYPE_NAME); Ok(()) } @@ -183,14 +184,14 @@ pub struct MockSubscriber { } impl Subscribe for MockSubscriber { - async fn next(&mut self) -> RosLibRustResult { + async fn next(&mut self) -> roslibrust_common::Result { let data = self .receiver .recv() .await - .map_err(|_| RosLibRustError::Disconnected)?; + .map_err(|_| Error::Disconnected)?; let msg = bincode::deserialize(&data[..]) - .map_err(|e| RosLibRustError::SerializationError(e.to_string()))?; + .map_err(|e| Error::SerializationError(e.to_string()))?; debug!("Received data on topic {}", T::ROS_TYPE_NAME); Ok(msg) } diff --git a/roslibrust_ros1/Cargo.toml b/roslibrust_ros1/Cargo.toml new file mode 100644 index 00000000..defcc53d --- /dev/null +++ b/roslibrust_ros1/Cargo.toml @@ -0,0 +1,40 @@ +[package] +name = "roslibrust_ros1" +version = "0.1.0" +edition = "2021" + +[dependencies] +# Provides common types and traits used throughout the roslibrust ecosystem. +roslibrust_common = { path = "../roslibrust_common" } + +# Standard dependencies: +tokio = { workspace = true } +log = { workspace = true } +serde = { workspace = true } + +# Should probably become workspace members: +lazy_static = "1.4" +abort-on-drop = "0.2" +test-log = "0.2" + +# These are definitely needed by this crate: +reqwest = { version = "0.11" } +serde_xmlrpc = { version = "0.2" } +roslibrust_serde_rosmsg = { version = "0.4" } +hyper = { version = "0.14", features = ["server"] } +gethostname = { version = "0.4" } +regex = { version = "1.9" } +byteorder = "1.4" +thiserror = "2.0" +anyhow = "1.0" + +[dev-dependencies] +# Used to provide message types for the examples +# TODO generate a roslibrust_std_msgs crate we can depend on instead of this +roslibrust_codegen_macro = { path = "../roslibrust_codegen_macro" } +# Relied on by generate types in the macro this should be cleaned up +roslibrust_codegen = { path = "../roslibrust_codegen" } + +[features] +# Used for enabling tests that rely on a running ros1 master +ros1_test = [] diff --git a/roslibrust_ros1/src/lib.rs b/roslibrust_ros1/src/lib.rs new file mode 100644 index 00000000..acaf3ea3 --- /dev/null +++ b/roslibrust_ros1/src/lib.rs @@ -0,0 +1,154 @@ +//! This module holds all content for directly working with ROS1 natively +use roslibrust_common::Error; +use roslibrust_common::{ + Publish, RosMessageType, RosServiceType, Service, ServiceFn, ServiceProvider, Subscribe, + TopicProvider, +}; + +/// [master_client] module contains code for calling xmlrpc functions on the master +mod master_client; +pub use master_client::*; + +mod names; + +/// [node] module contains the central Node and NodeHandle APIs +mod node; +pub use node::*; + +mod publisher; +pub use publisher::Publisher; +pub use publisher::PublisherAny; +mod service_client; +pub use service_client::ServiceClient; +mod subscriber; +pub use subscriber::Subscriber; +mod service_server; +pub use service_server::ServiceServer; +mod tcpros; + +/// Provides a common type alias for type erased service server functions. +/// Internally we use this type to store collections of server functions. +pub(crate) type TypeErasedCallback = dyn Fn(Vec) -> Result, Box> + + Send + + Sync + + 'static; + +// Implement the generic roslibrust trait +impl TopicProvider for NodeHandle { + type Publisher = crate::Publisher; + type Subscriber = crate::Subscriber; + + async fn advertise( + &self, + topic: &str, + ) -> roslibrust_common::Result> { + // TODO MAJOR: consider promoting queue size, making unlimited default + self.advertise::(topic.as_ref(), 10, false) + .await + .map_err(|e| e.into()) + } + + async fn subscribe( + &self, + topic: &str, + ) -> roslibrust_common::Result> { + // TODO MAJOR: consider promoting queue size, making unlimited default + self.subscribe(topic, 10).await.map_err(|e| e.into()) + } +} + +impl Service for ServiceClient { + async fn call(&self, request: &T::Request) -> roslibrust_common::Result { + self.call(request).await + } +} + +impl ServiceProvider for crate::NodeHandle { + type ServiceClient = crate::ServiceClient; + type ServiceServer = crate::ServiceServer; + + async fn call_service( + &self, + topic: &str, + request: T::Request, + ) -> roslibrust_common::Result { + // TODO should have a more optimized version of this... + let client = self.service_client::(topic).await?; + client.call(&request).await + } + + async fn service_client( + &self, + topic: &str, + ) -> roslibrust_common::Result> { + // TODO bad error mapping here... + self.service_client::(topic).await.map_err(|e| e.into()) + } + + async fn advertise_service( + &self, + topic: &str, + server: F, + ) -> roslibrust_common::Result + where + F: ServiceFn, + { + self.advertise_service::(topic, server) + .await + .map_err(|e| e.into()) + } +} + +impl Subscribe for crate::Subscriber { + async fn next(&mut self) -> roslibrust_common::Result { + let res = crate::Subscriber::next(self).await; + match res { + Some(Ok(msg)) => Ok(msg), + Some(Err(e)) => { + log::error!("Subscriber got error: {e:?}"); + // TODO gotta do better error conversion / error types here + Err(Error::Unexpected(anyhow::anyhow!( + "Subscriber got error: {e:?}" + ))) + } + None => { + log::error!("Subscriber hit dropped channel"); + Err(Error::Unexpected(anyhow::anyhow!( + "Channel closed, something was dropped?" + ))) + } + } + } +} + +// Provide an implementation of publish for ros1 backend +impl Publish for Publisher { + async fn publish(&self, data: &T) -> roslibrust_common::Result<()> { + // TODO error type conversion here is terrible and we need to standardize error stuff badly + self.publish(data) + .await + .map_err(|e| Error::SerializationError(e.to_string())) + } +} + +#[cfg(test)] +mod test { + use roslibrust_common::TopicProvider; + + // Prove that we've implemented the topic provider trait fully for NodeHandle + #[test] + #[should_panic] + fn topic_provider_can_be_used_with_ros1() { + struct MyClient { + _client: T, + } + + // Kinda a hack way to make the compiler prove it could construct a MyClient with out actually + // constructing one at runtime + let new_mock: Result = Err(anyhow::anyhow!("Expected error")); + + let _x = MyClient { + _client: new_mock.unwrap(), // panic + }; + } +} diff --git a/roslibrust_ros1/src/main.rs b/roslibrust_ros1/src/main.rs new file mode 100644 index 00000000..e7a11a96 --- /dev/null +++ b/roslibrust_ros1/src/main.rs @@ -0,0 +1,3 @@ +fn main() { + println!("Hello, world!"); +} diff --git a/roslibrust/src/ros1/master_client.rs b/roslibrust_ros1/src/master_client.rs similarity index 100% rename from roslibrust/src/ros1/master_client.rs rename to roslibrust_ros1/src/master_client.rs diff --git a/roslibrust/src/ros1/names.rs b/roslibrust_ros1/src/names.rs similarity index 100% rename from roslibrust/src/ros1/names.rs rename to roslibrust_ros1/src/names.rs diff --git a/roslibrust/src/ros1/node/actor.rs b/roslibrust_ros1/src/node/actor.rs similarity index 98% rename from roslibrust/src/ros1/node/actor.rs rename to roslibrust_ros1/src/node/actor.rs index 4ec61e7c..e13ba63d 100644 --- a/roslibrust/src/ros1/node/actor.rs +++ b/roslibrust_ros1/src/node/actor.rs @@ -1,18 +1,15 @@ use crate::{ - ros1::{ - names::Name, - node::{XmlRpcServer, XmlRpcServerHandle}, - publisher::Publication, - service_client::ServiceClientLink, - service_server::ServiceServerLink, - subscriber::Subscription, - MasterClient, NodeError, ProtocolParams, ServiceClient, TypeErasedCallback, - }, - RosLibRustError, ServiceFn, + names::Name, + node::{XmlRpcServer, XmlRpcServerHandle}, + publisher::Publication, + service_client::ServiceClientLink, + service_server::ServiceServerLink, + subscriber::Subscription, + MasterClient, NodeError, ProtocolParams, ServiceClient, TypeErasedCallback, }; use abort_on_drop::ChildTask; use log::*; -use roslibrust_codegen::{RosMessageType, RosServiceType}; +use roslibrust_common::{Error, RosMessageType, RosServiceType, ServiceFn}; use std::{collections::HashMap, io, net::Ipv4Addr, sync::Arc}; use tokio::sync::{broadcast, mpsc, oneshot}; @@ -197,7 +194,7 @@ impl NodeServerHandle { let md5sum; let md5sum_res = - roslibrust_codegen::message_definition_to_md5sum(topic_type, msg_definition); + roslibrust_common::md5sum::message_definition_to_md5sum(topic_type, msg_definition); match md5sum_res { // TODO(lucasw) make a new error type for this? Err(err) => { @@ -288,10 +285,10 @@ impl NodeServerHandle { let server_typeless = move |message: Vec| -> Result, Box> { let request = roslibrust_serde_rosmsg::from_slice::(&message) - .map_err(|err| RosLibRustError::SerializationError(err.to_string()))?; + .map_err(|err| Error::SerializationError(err.to_string()))?; let response = server(request)?; Ok(roslibrust_serde_rosmsg::to_vec(&response) - .map_err(|err| RosLibRustError::SerializationError(err.to_string()))?) + .map_err(|err| Error::SerializationError(err.to_string()))?) }; let server_typeless = Box::new(server_typeless); diff --git a/roslibrust/src/ros1/node/handle.rs b/roslibrust_ros1/src/node/handle.rs similarity index 92% rename from roslibrust/src/ros1/node/handle.rs rename to roslibrust_ros1/src/node/handle.rs index f58d1352..edfe5fd8 100644 --- a/roslibrust/src/ros1/node/handle.rs +++ b/roslibrust_ros1/src/node/handle.rs @@ -1,11 +1,9 @@ use super::actor::{Node, NodeServerHandle}; use crate::{ - ros1::{ - names::Name, publisher::Publisher, publisher::PublisherAny, service_client::ServiceClient, - subscriber::Subscriber, subscriber::SubscriberAny, NodeError, ServiceServer, - }, - ServiceFn, + names::Name, publisher::Publisher, publisher::PublisherAny, service_client::ServiceClient, + subscriber::Subscriber, subscriber::SubscriberAny, NodeError, ServiceServer, }; +use roslibrust_common::ServiceFn; /// Represents a handle to an underlying Node. NodeHandle's can be freely cloned, moved, copied, etc. /// This class provides the user facing API for interacting with ROS. @@ -97,7 +95,7 @@ impl NodeHandle { /// Subsequent calls will simply be given additional handles to the underlying publication. /// This behavior was chosen to mirror ROS1's API, however it is recommended to .clone() the returned publisher /// instead of calling this function multiple times. - pub async fn advertise( + pub async fn advertise( &self, topic_name: &str, queue_size: usize, @@ -117,12 +115,12 @@ impl NodeHandle { ) -> Result { let receiver = self .inner - .register_subscriber::(topic_name, queue_size) + .register_subscriber::(topic_name, queue_size) .await?; Ok(SubscriberAny::new(receiver)) } - pub async fn subscribe( + pub async fn subscribe( &self, topic_name: &str, queue_size: usize, @@ -134,7 +132,7 @@ impl NodeHandle { Ok(Subscriber::new(receiver)) } - pub async fn service_client( + pub async fn service_client( &self, service_name: &str, ) -> Result, NodeError> { @@ -152,7 +150,7 @@ impl NodeHandle { server: F, ) -> Result where - T: roslibrust_codegen::RosServiceType, + T: roslibrust_common::RosServiceType, F: ServiceFn, { let service_name = Name::new(service_name)?; diff --git a/roslibrust/src/ros1/node/mod.rs b/roslibrust_ros1/src/node/mod.rs similarity index 89% rename from roslibrust/src/ros1/node/mod.rs rename to roslibrust_ros1/src/node/mod.rs index 95719365..7cbdf09e 100644 --- a/roslibrust/src/ros1/node/mod.rs +++ b/roslibrust_ros1/src/node/mod.rs @@ -1,7 +1,7 @@ //! This module contains the top level Node and NodeHandle classes. //! These wrap the lower level management of a ROS Node connection into a higher level and thread safe API. -use crate::RosLibRustError; +use roslibrust_common::Error; use super::{names::InvalidNameError, RosMasterError}; use std::{ @@ -107,16 +107,16 @@ impl From> for NodeError { // we produced two different error types for the two different impls (ros1, rosbridge) // This allows fusing the two error types together so that TopicProider can work // but we should just better design all the error handling -impl From for RosLibRustError { +impl From for Error { fn from(value: NodeError) -> Self { match value { - NodeError::RosMasterError(e) => RosLibRustError::ServerError(e.to_string()), + NodeError::RosMasterError(e) => Error::ServerError(e.to_string()), NodeError::ChannelClosedError => { - RosLibRustError::Unexpected(anyhow!("Channel closed, something was dropped?")) + Error::Unexpected(anyhow!("Channel closed, something was dropped?")) } - NodeError::InvalidName(e) => RosLibRustError::InvalidName(e.to_string()), - NodeError::XmlRpcError(e) => RosLibRustError::SerializationError(e.to_string().into()), - NodeError::IoError(e) => RosLibRustError::IoError(e), + NodeError::InvalidName(e) => Error::InvalidName(e.to_string()), + NodeError::XmlRpcError(e) => Error::SerializationError(e.to_string().into()), + NodeError::IoError(e) => Error::IoError(e), } } } diff --git a/roslibrust/src/ros1/node/xmlrpc.rs b/roslibrust_ros1/src/node/xmlrpc.rs similarity index 100% rename from roslibrust/src/ros1/node/xmlrpc.rs rename to roslibrust_ros1/src/node/xmlrpc.rs diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust_ros1/src/publisher.rs similarity index 99% rename from roslibrust/src/ros1/publisher.rs rename to roslibrust_ros1/src/publisher.rs index 4bb11e9b..1f35af2c 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust_ros1/src/publisher.rs @@ -1,10 +1,10 @@ -use crate::ros1::{ +use crate::{ names::Name, tcpros::{self, ConnectionHeader}, }; use abort_on_drop::ChildTask; use log::*; -use roslibrust_codegen::RosMessageType; +use roslibrust_common::RosMessageType; use std::{ marker::PhantomData, net::{Ipv4Addr, SocketAddr}, diff --git a/roslibrust/src/ros1/service_client.rs b/roslibrust_ros1/src/service_client.rs similarity index 90% rename from roslibrust/src/ros1/service_client.rs rename to roslibrust_ros1/src/service_client.rs index 3efa9f18..9def9099 100644 --- a/roslibrust/src/ros1/service_client.rs +++ b/roslibrust_ros1/src/service_client.rs @@ -1,12 +1,9 @@ use crate::{ - ros1::{ - names::Name, - tcpros::{establish_connection, ConnectionHeader}, - }, - RosLibRustError, RosLibRustResult, + names::Name, + tcpros::{establish_connection, ConnectionHeader}, }; use abort_on_drop::ChildTask; -use roslibrust_codegen::RosServiceType; +use roslibrust_common::{Error, RosServiceType}; use std::{marker::PhantomData, sync::Arc}; use tokio::{ io::{AsyncReadExt, AsyncWriteExt}, @@ -20,7 +17,7 @@ use tokio::{ use super::tcpros; pub type CallServiceRequest = (Vec, oneshot::Sender); -pub type CallServiceResponse = RosLibRustResult>; +pub type CallServiceResponse = roslibrust_common::Result>; // Note: ServiceClient is clone, and this is expressly different behavior than calling .service_client() twice on NodeHandle // clonning a ServiceClient does not create a new connection to the service, but instead creates a second handle to the @@ -53,14 +50,14 @@ impl ServiceClient { &self.service_name } - pub async fn call(&self, request: &T::Request) -> RosLibRustResult { + pub async fn call(&self, request: &T::Request) -> std::result::Result { let request_payload = roslibrust_serde_rosmsg::to_vec(request) - .map_err(|err| RosLibRustError::SerializationError(err.to_string()))?; + .map_err(|err| Error::SerializationError(err.to_string()))?; let (response_tx, response_rx) = oneshot::channel(); self.sender .send((request_payload, response_tx)) - .map_err(|_err| RosLibRustError::Disconnected)?; + .map_err(|_err| Error::Disconnected)?; match response_rx.await { Ok(Ok(result_payload)) => { @@ -70,14 +67,14 @@ impl ServiceClient { result_payload ); let response: T::Response = roslibrust_serde_rosmsg::from_slice(&result_payload) - .map_err(|err| RosLibRustError::SerializationError(err.to_string()))?; + .map_err(|err| Error::SerializationError(err.to_string()))?; return Ok(response); } Ok(Err(err)) => { return Err(err); } Err(_err) => { - return Err(RosLibRustError::Disconnected); + return Err(Error::Disconnected); } } } @@ -96,7 +93,7 @@ impl ServiceClientLink { service_uri: &str, srv_definition: &str, md5sum: &str, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { let header = ConnectionHeader { caller_id: node_name.to_string(), latching: false, @@ -116,7 +113,7 @@ impl ServiceClientLink { let stream = establish_connection(&node_name, &service_name, &service_uri, header).await.map_err(|err| { log::error!("Failed to establish connection to service URI {service_uri} for service {service_name}: {err}"); - RosLibRustError::from(err) + Error::from(err) })?; let actor_context = Self::actor_context(stream, service_name.to_owned(), call_rx); @@ -164,7 +161,7 @@ impl ServiceClientLink { log::error!( "Failed to send and receive service call for service {service_name}: {err:?}" ); - RosLibRustError::from(err) + Error::from(err) }); let send_result = response_sender.send(response); if let Err(_err) = send_result { diff --git a/roslibrust/src/ros1/service_server.rs b/roslibrust_ros1/src/service_server.rs similarity index 99% rename from roslibrust/src/ros1/service_server.rs rename to roslibrust_ros1/src/service_server.rs index affa6394..208b0cdb 100644 --- a/roslibrust/src/ros1/service_server.rs +++ b/roslibrust_ros1/src/service_server.rs @@ -7,7 +7,7 @@ use abort_on_drop::ChildTask; use log::*; use tokio::io::AsyncWriteExt; -use crate::ros1::tcpros::{self, ConnectionHeader}; +use crate::tcpros::{self, ConnectionHeader}; use super::{names::Name, NodeHandle, TypeErasedCallback}; diff --git a/roslibrust/src/ros1/subscriber.rs b/roslibrust_ros1/src/subscriber.rs similarity index 98% rename from roslibrust/src/ros1/subscriber.rs rename to roslibrust_ros1/src/subscriber.rs index b352d9ff..bbf10680 100644 --- a/roslibrust/src/ros1/subscriber.rs +++ b/roslibrust_ros1/src/subscriber.rs @@ -1,7 +1,7 @@ -use crate::ros1::{names::Name, tcpros::ConnectionHeader}; +use crate::{names::Name, tcpros::ConnectionHeader}; use abort_on_drop::ChildTask; use log::*; -use roslibrust_codegen::{RosMessageType, ShapeShifter}; +use roslibrust_common::{RosMessageType, ShapeShifter}; use std::{marker::PhantomData, sync::Arc}; use tokio::{ io::AsyncWriteExt, diff --git a/roslibrust/src/ros1/tcpros.rs b/roslibrust_ros1/src/tcpros.rs similarity index 100% rename from roslibrust/src/ros1/tcpros.rs rename to roslibrust_ros1/src/tcpros.rs diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust_ros1/tests/ros1_xmlrpc.rs similarity index 97% rename from roslibrust/tests/ros1_xmlrpc.rs rename to roslibrust_ros1/tests/ros1_xmlrpc.rs index 4b2f2eac..56ca47ce 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust_ros1/tests/ros1_xmlrpc.rs @@ -1,7 +1,7 @@ -#[cfg(all(feature = "ros1", feature = "ros1_test"))] +#[cfg(feature = "ros1_test")] mod tests { - use roslibrust::ros1::NodeHandle; - use roslibrust_codegen::RosMessageType; + use roslibrust_common::RosMessageType; + use roslibrust_ros1::NodeHandle; use serde::de::DeserializeOwned; use serde_xmlrpc::Value; roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); diff --git a/roslibrust_rosapi/Cargo.toml b/roslibrust_rosapi/Cargo.toml new file mode 100644 index 00000000..1f79160b --- /dev/null +++ b/roslibrust_rosapi/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "roslibrust_rosapi" +version = "0.1.0" +edition = "2021" + +[dependencies] +# TODO shouldn't need to depend on codegen should be able to use _macro directly +roslibrust_codegen = { path = "../roslibrust_codegen" } +roslibrust_codegen_macro = { path = "../roslibrust_codegen_macro" } +roslibrust_common = { path = "../roslibrust_common" } +# Note: this crate doesn't depend on any specific backend implementations +# but can work with any backend that implements the ServiceProvider trait and has a RosApi node running + +[dev-dependencies] +tokio = { workspace = true } +test-log = "0.2" +# Only backend we're currently testing with +roslibrust_rosbridge = { path = "../roslibrust_rosbridge" } + +[features] +# Used to indicate we're executing tests with a running ros1 rosapi node +ros1_test = [] \ No newline at end of file diff --git a/roslibrust_rosapi/README.md b/roslibrust_rosapi/README.md new file mode 100644 index 00000000..bc2bea0f --- /dev/null +++ b/roslibrust_rosapi/README.md @@ -0,0 +1,7 @@ +# roslibrust_rosapi + +Provides a trait representing the capabilities of the rosapi node. + +And provides implementations of that trait for roslibrust_ros1::NodeHandle and roslibrust_rosbridge::ClientHandle + +The future of this crate is a little uncertain given direction of the roslibrust project. diff --git a/roslibrust/src/rosapi/mod.rs b/roslibrust_rosapi/src/lib.rs similarity index 76% rename from roslibrust/src/rosapi/mod.rs rename to roslibrust_rosapi/src/lib.rs index d7d078d3..45471e06 100644 --- a/roslibrust/src/rosapi/mod.rs +++ b/roslibrust_rosapi/src/lib.rs @@ -1,9 +1,9 @@ -//! This module is intended to provide a convenience wrapper around the capabilities +//! This crate provides a convenience wrapper around the capabilities //! provided by the [rosapi](http://wiki.ros.org/rosapi) node. //! //! Ensure rosapi is running on your target system before attempting to utilize these features! -use crate::{ClientHandle, RosLibRustResult}; +use roslibrust_common::topic_provider::ServiceProvider; // TODO major issue here for folks who actually try to use rosapi in their project // This macro isn't going to expand correctly when not used from this crate's workspace @@ -13,99 +13,117 @@ roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_in /// Represents the ability to interact with the interfaces provided by the rosapi node. /// This trait is implemented for ClientHandle when the `rosapi` feature is enabled. -trait RosApi { - async fn get_time(&self) -> RosLibRustResult; - async fn topics(&self) -> RosLibRustResult; - async fn get_topic_type( +pub trait RosApi { + fn get_time( + &self, + ) -> impl std::future::Future> + Send; + fn topics( + &self, + ) -> impl std::future::Future> + Send; + fn get_topic_type( &self, topic: impl Into + Send, - ) -> RosLibRustResult; - async fn get_topics_for_type( + ) -> impl std::future::Future> + Send; + fn get_topics_for_type( &self, topic_type: impl Into + Send, - ) -> RosLibRustResult; - async fn get_nodes(&self) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; + fn get_nodes( + &self, + ) -> impl std::future::Future> + Send; - async fn get_node_details( + fn get_node_details( &self, node: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn get_node_for_service( + fn get_node_for_service( &self, service: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn set_param( + fn set_param( &self, param_name: impl Into + Send, param_value: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn get_param( + fn get_param( &self, param_name: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn get_param_names(&self) -> RosLibRustResult; + fn get_param_names( + &self, + ) -> impl std::future::Future> + Send; - async fn has_param( + fn has_param( &self, param: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn delete_param( + fn delete_param( &self, name: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn message_details( + fn message_details( &self, message_name: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + + Send; - async fn publishers( + fn publishers( &self, topic: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn service_host( + fn service_host( &self, service: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn service_providers( + fn service_providers( &self, service_type: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + + Send; - async fn get_service_request_details( + fn get_service_request_details( &self, service_type: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future< + Output = roslibrust_common::Result, + > + Send; - async fn get_service_response_details( + fn get_service_response_details( &self, service_type: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future< + Output = roslibrust_common::Result, + > + Send; - async fn get_service_type( + fn get_service_type( &self, service_name: impl Into + Send, - ) -> RosLibRustResult; + ) -> impl std::future::Future> + Send; - async fn get_services(&self) -> RosLibRustResult; + fn get_services( + &self, + ) -> impl std::future::Future> + Send; } -impl RosApi for ClientHandle { +/// A Generic implementation of the RosApi trait for any type that implements ServiceProvider +/// Note, this will not work on ROS2 systems or systems without the rosapi node running. +impl RosApi for T { /// Get the current time - async fn get_time(&self) -> RosLibRustResult { + async fn get_time(&self) -> roslibrust_common::Result { self.call_service::("/rosapi/get_time", rosapi::GetTimeRequest {}) .await } /// Get the list of topics active - async fn topics(&self) -> RosLibRustResult { + async fn topics(&self) -> roslibrust_common::Result { self.call_service::("/rosapi/topics", rosapi::TopicsRequest {}) .await } @@ -114,7 +132,7 @@ impl RosApi for ClientHandle { async fn get_topic_type( &self, topic: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/topic_type", rosapi::TopicTypeRequest { @@ -128,7 +146,7 @@ impl RosApi for ClientHandle { async fn get_topics_for_type( &self, topic_type: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/topics_for_type", rosapi::TopicsForTypeRequest { @@ -139,7 +157,7 @@ impl RosApi for ClientHandle { } /// Returns list of nodes active in a system - async fn get_nodes(&self) -> RosLibRustResult { + async fn get_nodes(&self) -> roslibrust_common::Result { self.call_service::("/rosapi/nodes", rosapi::NodesRequest {}) .await } @@ -149,7 +167,7 @@ impl RosApi for ClientHandle { async fn get_node_details( &self, node: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/node_details", rosapi::NodeDetailsRequest { node: node.into() }, @@ -161,7 +179,7 @@ impl RosApi for ClientHandle { async fn get_node_for_service( &self, service: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/service_node", rosapi::ServiceNodeRequest { @@ -177,7 +195,7 @@ impl RosApi for ClientHandle { &self, param_name: impl Into + Send, param_value: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/set_param", rosapi::SetParamRequest { @@ -193,7 +211,7 @@ impl RosApi for ClientHandle { async fn get_param( &self, param_name: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/get_param", rosapi::GetParamRequest { @@ -205,7 +223,7 @@ impl RosApi for ClientHandle { } /// Gets the list of currently known parameters. - async fn get_param_names(&self) -> RosLibRustResult { + async fn get_param_names(&self) -> roslibrust_common::Result { self.call_service::( "/rosapi/get_param_names", rosapi::GetParamNamesRequest {}, @@ -217,7 +235,7 @@ impl RosApi for ClientHandle { async fn has_param( &self, param: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/has_param", rosapi::HasParamRequest { name: param.into() }, @@ -229,7 +247,7 @@ impl RosApi for ClientHandle { async fn delete_param( &self, name: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/delete_param", rosapi::DeleteParamRequest { name: name.into() }, @@ -241,7 +259,7 @@ impl RosApi for ClientHandle { async fn message_details( &self, message_name: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/message_details", rosapi::MessageDetailsRequest { @@ -255,7 +273,7 @@ impl RosApi for ClientHandle { async fn publishers( &self, topic: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/publishers", rosapi::PublishersRequest { @@ -269,7 +287,7 @@ impl RosApi for ClientHandle { async fn service_host( &self, service: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/service_host", rosapi::ServiceHostRequest { @@ -283,7 +301,7 @@ impl RosApi for ClientHandle { async fn service_providers( &self, service_type: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/service_providers", rosapi::ServiceProvidersRequest { @@ -297,7 +315,7 @@ impl RosApi for ClientHandle { async fn get_service_request_details( &self, service_type: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/service_request_details", rosapi::ServiceRequestDetailsRequest { @@ -311,7 +329,7 @@ impl RosApi for ClientHandle { async fn get_service_response_details( &self, service_type: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/service_response_details", rosapi::ServiceRequestDetailsRequest { @@ -325,7 +343,7 @@ impl RosApi for ClientHandle { async fn get_service_type( &self, service_name: impl Into + Send, - ) -> RosLibRustResult { + ) -> roslibrust_common::Result { self.call_service::( "/rosapi/service_type", rosapi::ServiceTypeRequest { @@ -336,7 +354,7 @@ impl RosApi for ClientHandle { } /// Get the list of services active on the system - async fn get_services(&self) -> RosLibRustResult { + async fn get_services(&self) -> roslibrust_common::Result { self.call_service::("/rosapi/services", rosapi::ServicesRequest {}) .await } @@ -374,13 +392,10 @@ impl RosApi for ClientHandle { } #[cfg(test)] -#[cfg(feature = "running_bridge")] -// TODO currently rosapi only supports ros1, we should try to figure out a way to fix that -// ros1api and ros2api may make sense as their own crates? #[cfg(feature = "ros1_test")] mod test { use super::RosApi; - use crate::{ClientHandle, ClientHandleOptions}; + use roslibrust_rosbridge::{ClientHandle, ClientHandleOptions}; async fn fixture_client() -> ClientHandle { // Tiny sleep to throttle rate at which tests are run to try to make CI more consistent diff --git a/roslibrust_rosbridge/Cargo.toml b/roslibrust_rosbridge/Cargo.toml new file mode 100644 index 00000000..877a18b0 --- /dev/null +++ b/roslibrust_rosbridge/Cargo.toml @@ -0,0 +1,30 @@ +[package] +name = "roslibrust_rosbridge" +version = "0.1.0" +edition = "2021" + +[dependencies] +roslibrust_common = { path = "../roslibrust_common" } +tokio = { workspace = true } +log = { workspace = true } +tokio-tungstenite = { version = "0.17" } +uuid = { version = "1.1", features = ["v4"] } +serde_json = "1.0" +anyhow = "1.0" +futures = "0.3" +futures-util = "0.3" +dashmap = "5.3" +deadqueue = "0.2.4" # .4+ is required to fix bug with missing tokio dep + +[dev-dependencies] +test-log = "0.2" +roslibrust_codegen = { path = "../roslibrust_codegen" } +roslibrust_codegen_macro = { path = "../roslibrust_codegen_macro" } + +[features] +# Used to enable tests that rely on a locally running rosbridge +running_bridge = [] +# Indicates we're testing with running ROS1 bridge +ros1_test = ["running_bridge"] +# Indicates we're testing with running ROS2 bridge +ros2_test = ["running_bridge"] diff --git a/roslibrust/src/rosbridge/client.rs b/roslibrust_rosbridge/src/client.rs similarity index 91% rename from roslibrust/src/rosbridge/client.rs rename to roslibrust_rosbridge/src/client.rs index 337534c0..d1c90f8d 100644 --- a/roslibrust/src/rosbridge/client.rs +++ b/roslibrust_rosbridge/src/client.rs @@ -1,12 +1,11 @@ -use crate::{ - rosbridge::comm::{self, RosBridgeComm}, - Publisher, RosLibRustError, RosLibRustResult, ServiceFn, ServiceHandle, Subscriber, -}; +use crate::comm::Ops; +use crate::comm::RosBridgeComm; +use crate::{Publisher, ServiceHandle, Subscriber}; use anyhow::anyhow; use dashmap::DashMap; use futures::StreamExt; use log::*; -use roslibrust_codegen::{RosMessageType, RosServiceType}; +use roslibrust_common::*; use serde_json::Value; use std::collections::HashMap; use std::str::FromStr; @@ -59,7 +58,7 @@ impl ClientHandleOptions { /// # #[tokio::main] /// # async fn main() -> Result<(), Box> { /// // Create a new client -/// let mut handle = roslibrust::ClientHandle::new("ws://localhost:9090").await?; +/// let mut handle = roslibrust_rosbridge::ClientHandle::new("ws://localhost:9090").await?; /// // Create a copy of the handle (does not create a seperate connection) /// let mut handle2 = handle.clone(); /// tokio::spawn(async move { @@ -86,7 +85,7 @@ impl ClientHandle { /// Like [ClientHandle::new] this function does not resolve until the connection is established for the first time. /// This function respects the [ClientHandleOptions] timeout and will return with an error if a connection is not /// established within the timeout. - pub async fn new_with_options(opts: ClientHandleOptions) -> RosLibRustResult { + pub async fn new_with_options(opts: ClientHandleOptions) -> Result { let inner = Arc::new(RwLock::new(timeout(opts.timeout, Client::new(opts)).await?)); let inner_weak = Arc::downgrade(&inner); @@ -106,19 +105,19 @@ impl ClientHandle { /// Connects a rosbridge instance at the given url /// Expects a fully describe websocket url, e.g. 'ws://localhost:9090' /// When awaited will not resolve until connection is successfully made. - pub async fn new>(url: S) -> RosLibRustResult { + pub async fn new>(url: S) -> Result { Self::new_with_options(ClientHandleOptions::new(url)).await } - fn check_for_disconnect(&self) -> RosLibRustResult<()> { + fn check_for_disconnect(&self) -> Result<()> { match self.is_disconnected.load(Ordering::Relaxed) { false => Ok(()), - true => Err(RosLibRustError::Disconnected), + true => Err(Error::Disconnected), } } // Internal implementation of subscribe - async fn _subscribe(&self, topic_name: &str) -> RosLibRustResult> + async fn _subscribe(&self, topic_name: &str) -> Result> where Msg: RosMessageType, { @@ -205,11 +204,11 @@ impl ClientHandle { /// # #[tokio::main] /// # async fn main() -> Result<(), Box> { /// // Create a new client - /// let mut handle = roslibrust::ClientHandle::new("ws://localhost:9090").await?; + /// let mut handle = roslibrust_rosbridge::ClientHandle::new("ws://localhost:9090").await?; /// // Subscribe using :: style /// let subscriber1 = handle.subscribe::("/topic").await?; /// // Subscribe using explicit type style - /// let subscriber2: roslibrust::Subscriber = handle.subscribe::("/topic").await?; + /// let subscriber2: roslibrust_rosbridge::Subscriber = handle.subscribe("/topic").await?; /// # Ok(()) /// # } /// ``` @@ -238,7 +237,7 @@ impl ClientHandle { /// # #[tokio::main] /// # async fn main() -> Result<(), Box> { /// // Create a new client - /// let mut handle = roslibrust::ClientHandle::new("ws://localhost:9090").await?; + /// let mut handle = roslibrust_rosbridge::ClientHandle::new("ws://localhost:9090").await?; /// // Subscribe to the same topic with two different types /// let ros1_subscriber = handle.subscribe::("/topic").await?; /// let ros2_subscriber = handle.subscribe::("/topic").await?; @@ -250,7 +249,7 @@ impl ClientHandle { /// # Ok(()) /// # } /// ``` - pub async fn subscribe(&self, topic_name: &str) -> RosLibRustResult> + pub async fn subscribe(&self, topic_name: &str) -> Result> where Msg: RosMessageType, { @@ -265,7 +264,7 @@ impl ClientHandle { // Publishes a message // Fails immediately(ish) if disconnected // Returns success when message is put on websocket (no confirmation of receipt) - pub(crate) async fn publish(&self, topic: &str, msg: &T) -> RosLibRustResult<()> + pub(crate) async fn publish(&self, topic: &str, msg: &T) -> Result<()> where T: RosMessageType, { @@ -296,15 +295,15 @@ impl ClientHandle { /// # #[tokio::main] /// # async fn main() -> Result<(), Box> { /// // Create a new client - /// let mut handle = roslibrust::ClientHandle::new("ws://localhost:9090").await?; + /// let mut handle = roslibrust_rosbridge::ClientHandle::new("ws://localhost:9090").await?; /// // Advertise using :: style /// let mut publisher = handle.advertise::("/topic").await?; /// // Advertise using explicit type - /// let mut publisher2: roslibrust::Publisher = handle.advertise("/other_topic").await?; + /// let mut publisher2: roslibrust_rosbridge::Publisher = handle.advertise("/other_topic").await?; /// # Ok(()) /// # } /// ``` - pub async fn advertise(&self, topic: &str) -> RosLibRustResult> + pub async fn advertise(&self, topic: &str) -> Result> where T: RosMessageType, { @@ -312,7 +311,7 @@ impl ClientHandle { let client = self.inner.read().await; if client.publishers.contains_key(topic) { // TODO if we ever remove this restriction we should still check types match - return Err(RosLibRustError::Unexpected(anyhow!( + return Err(Error::Unexpected(anyhow!( "Attempted to create two publisher to same topic, this is not supported" ))); } else { @@ -348,7 +347,7 @@ impl ClientHandle { /// # #[tokio::main] /// # async fn main() -> Result<(), Box> { /// // Create a new client - /// let mut handle = roslibrust::ClientHandle::new("ws://localhost:9090").await?; + /// let mut handle = roslibrust_rosbridge::ClientHandle::new("ws://localhost:9090").await?; /// // Call service, type of response will be rosapi::GetTimeResponse (alternatively named rosapi::GetTime::Response) /// let response = handle.call_service::("/rosapi/get_time", rosapi::GetTimeRequest{}).await?; /// # Ok(()) @@ -358,7 +357,7 @@ impl ClientHandle { &self, service: &str, req: S::Request, - ) -> RosLibRustResult { + ) -> Result { self.check_for_disconnect()?; let (tx, rx) = tokio::sync::oneshot::channel(); let rand_string: String = uuid::Uuid::new_v4().to_string(); @@ -403,10 +402,10 @@ impl ClientHandle { // We failed to parse the value as an expected type, before just giving up, try to parse as string // if we got a string it indicates a server side error, otherwise we got the wrong datatype back match serde_json::from_value(msg) { - Ok(s) => return Err(RosLibRustError::ServerError(s)), + Ok(s) => return Err(Error::ServerError(s)), Err(_) => { // Return the error from the origional parse - return Err(RosLibRustError::InvalidMessage(e)); + return Err(Error::InvalidMessage(e)); } } } @@ -417,11 +416,7 @@ impl ClientHandle { /// Service will be active until the handle is dropped! /// /// See examples/service_server.rs for usage. - pub async fn advertise_service( - &self, - topic: &str, - server: F, - ) -> RosLibRustResult + pub async fn advertise_service(&self, topic: &str, server: F) -> Result where T: RosServiceType, F: ServiceFn, @@ -435,11 +430,11 @@ impl ClientHandle { error!( "Re-registering a server for the pre-existing topic: {topic} This will fail!" ); - return Err(RosLibRustError::Unexpected(anyhow!("roslibrust does not support re-advertising a service without first dropping the previous Service"))); + return Err(Error::Unexpected(anyhow!("roslibrust does not support re-advertising a service without first dropping the previous Service"))); } // We need to do type erasure and hide the request by wrapping their closure in a generic closure - let erased_closure = move |message: &str| -> Result< + let erased_closure = move |message: &str| -> std::result::Result< serde_json::Value, Box, > { @@ -471,7 +466,7 @@ impl ClientHandle { /// /// Note: Unlike with ROS1 native service, this provides no performance benefit over call_service, /// and is just a thin wrapper around call_service. - pub async fn service_client(&self, topic: &str) -> RosLibRustResult> + pub async fn service_client(&self, topic: &str) -> Result> where T: RosServiceType, { @@ -532,7 +527,7 @@ impl ClientHandle { // This function removes the entry for a subscriber in from the client, and if it is the last // subscriber for a given topic then dispatches an unsubscribe message to the master/bridge - pub(crate) fn unsubscribe(&self, topic_name: &str, id: &uuid::Uuid) -> RosLibRustResult<()> { + pub(crate) fn unsubscribe(&self, topic_name: &str, id: &uuid::Uuid) -> Result<()> { // Copy so we can move into closure let client = self.clone(); let topic_name = topic_name.to_string(); @@ -585,7 +580,7 @@ pub(crate) struct Client { impl Client { // internal implementation of new - async fn new(opts: ClientHandleOptions) -> RosLibRustResult { + async fn new(opts: ClientHandleOptions) -> Result { let (writer, reader) = stubborn_connect(&opts.url).await; let client = Self { reader: RwLock::new(reader), @@ -600,7 +595,7 @@ impl Client { Ok(client) } - async fn handle_message(&self, msg: Message) -> RosLibRustResult<()> { + async fn handle_message(&self, msg: Message) -> Result<()> { match msg { Message::Text(text) => { debug!("got message: {}", text); @@ -614,17 +609,17 @@ impl Client { .expect("Op field not present on returned object.") .as_str() .expect("Op field was not of string type."); - let op = comm::Ops::from_str(op)?; + let op = Ops::from_str(op)?; match op { - comm::Ops::Publish => { + Ops::Publish => { trace!("handling publish for {:?}", &parsed); self.handle_publish(parsed).await; } - comm::Ops::ServiceResponse => { + Ops::ServiceResponse => { trace!("handling service response for {:?}", &parsed); self.handle_response(parsed).await; } - comm::Ops::CallService => { + Ops::CallService => { trace!("handling call_service for {:?}", &parsed); self.handle_service(parsed).await; } @@ -694,7 +689,7 @@ impl Client { // Now we need to send the service_response back } - async fn spin_once(&self) -> RosLibRustResult<()> { + async fn spin_once(&self) -> Result<()> { let read = { let mut stream = self.reader.write().await; match stream.next().await { @@ -703,9 +698,7 @@ impl Client { return Err(e.into()); } None => { - return Err(RosLibRustError::Unexpected(anyhow!( - "Wtf does none mean here?" - ))); + return Err(Error::Unexpected(anyhow!("Wtf does none mean here?"))); } } }; @@ -735,7 +728,7 @@ impl Client { } } - async fn reconnect(&mut self) -> RosLibRustResult<()> { + async fn reconnect(&mut self) -> Result<()> { // Reconnect stream let (writer, reader) = stubborn_connect(&self.opts.url).await; self.reader = RwLock::new(reader); @@ -772,7 +765,7 @@ impl Client { async fn stubborn_spin( client: std::sync::Weak>, is_disconnected: Arc, -) -> RosLibRustResult<()> { +) -> Result<()> { debug!("Starting stubborn_spin"); while let Some(client) = client.upgrade() { const SPIN_DURATION: Duration = Duration::from_millis(10); @@ -801,9 +794,9 @@ async fn stubborn_spin( // Implementation of timeout that is a no-op if timeout is 0 or un-configured // Only works on functions that already return our result type // This might not be needed but reading tokio::timeout docs I couldn't confirm this -async fn timeout(timeout: Option, future: F) -> RosLibRustResult +async fn timeout(timeout: Option, future: F) -> Result where - F: futures::Future>, + F: futures::Future>, { if let Some(t) = timeout { tokio::time::timeout(t, future).await? @@ -832,7 +825,7 @@ async fn stubborn_connect(url: &str) -> (Writer, Reader) { } // Basic connection attempt and error wrapping -async fn connect(url: &str) -> RosLibRustResult { +async fn connect(url: &str) -> Result { let attempt = tokio_tungstenite::connect_async(url).await; match attempt { Ok((stream, _response)) => Ok(stream), diff --git a/roslibrust/src/rosbridge/comm.rs b/roslibrust_rosbridge/src/comm.rs similarity index 88% rename from roslibrust/src/rosbridge/comm.rs rename to roslibrust_rosbridge/src/comm.rs index c7685654..6792521b 100644 --- a/roslibrust/src/rosbridge/comm.rs +++ b/roslibrust_rosbridge/src/comm.rs @@ -1,8 +1,8 @@ -use crate::{rosbridge::Writer, RosLibRustResult}; +use crate::Writer; use anyhow::bail; use futures_util::SinkExt; use log::debug; -use roslibrust_codegen::RosMessageType; +use roslibrust_common::{Result, RosMessageType}; use serde_json::json; use std::{fmt::Display, str::FromStr, string::ToString}; use tokio_tungstenite::tungstenite::Message; @@ -64,7 +64,7 @@ impl Into<&str> for &Ops { // TODO should replace this with deserialize impl FromStr for Ops { type Err = anyhow::Error; - fn from_str(s: &str) -> Result { + fn from_str(s: &str) -> std::result::Result { Ok(match s { "advertise" => Ops::Advertise, "unadvertise" => Ops::Unadvertise, @@ -88,31 +88,31 @@ impl FromStr for Ops { /// using this trait for mocking. I'm inclined to replace it, and move the /// impls directly into some wrapper around [Writer] pub(crate) trait RosBridgeComm { - async fn subscribe(&mut self, topic: &str, msg_type: &str) -> RosLibRustResult<()>; - async fn unsubscribe(&mut self, topic: &str) -> RosLibRustResult<()>; - async fn publish(&mut self, topic: &str, msg: &T) -> RosLibRustResult<()>; - async fn advertise(&mut self, topic: &str) -> RosLibRustResult<()>; - async fn advertise_str(&mut self, topic: &str, msg_type: &str) -> RosLibRustResult<()>; + async fn subscribe(&mut self, topic: &str, msg_type: &str) -> Result<()>; + async fn unsubscribe(&mut self, topic: &str) -> Result<()>; + async fn publish(&mut self, topic: &str, msg: &T) -> Result<()>; + async fn advertise(&mut self, topic: &str) -> Result<()>; + async fn advertise_str(&mut self, topic: &str, msg_type: &str) -> Result<()>; async fn call_service( &mut self, service: &str, id: &str, req: Req, - ) -> RosLibRustResult<()>; - async fn unadvertise(&mut self, topic: &str) -> RosLibRustResult<()>; - async fn advertise_service(&mut self, topic: &str, srv_type: &str) -> RosLibRustResult<()>; - async fn unadvertise_service(&mut self, topic: &str) -> RosLibRustResult<()>; + ) -> Result<()>; + async fn unadvertise(&mut self, topic: &str) -> Result<()>; + async fn advertise_service(&mut self, topic: &str, srv_type: &str) -> Result<()>; + async fn unadvertise_service(&mut self, topic: &str) -> Result<()>; async fn service_response( &mut self, topic: &str, id: Option, is_success: bool, response: serde_json::Value, - ) -> RosLibRustResult<()>; + ) -> Result<()>; } impl RosBridgeComm for Writer { - async fn subscribe(&mut self, topic: &str, msg_type: &str) -> RosLibRustResult<()> { + async fn subscribe(&mut self, topic: &str, msg_type: &str) -> Result<()> { let msg = json!( { "op": Ops::Subscribe.to_string(), @@ -126,7 +126,7 @@ impl RosBridgeComm for Writer { Ok(()) } - async fn unsubscribe(&mut self, topic: &str) -> RosLibRustResult<()> { + async fn unsubscribe(&mut self, topic: &str) -> Result<()> { let msg = json!( { "op": Ops::Unsubscribe.to_string(), @@ -139,7 +139,7 @@ impl RosBridgeComm for Writer { Ok(()) } - async fn publish(&mut self, topic: &str, msg: &T) -> RosLibRustResult<()> { + async fn publish(&mut self, topic: &str, msg: &T) -> Result<()> { let msg = json!( { "op": Ops::Publish.to_string(), @@ -154,14 +154,14 @@ impl RosBridgeComm for Writer { Ok(()) } - async fn advertise(&mut self, topic: &str) -> RosLibRustResult<()> { + async fn advertise(&mut self, topic: &str) -> Result<()> { self.advertise_str(topic, T::ROS_TYPE_NAME).await } // Identical to advertise, but allows providing a string argument for the topic type // This is important as the type is erased in our list of publishers, and not available // when we try to reconnect - async fn advertise_str(&mut self, topic: &str, topic_type: &str) -> RosLibRustResult<()> { + async fn advertise_str(&mut self, topic: &str, topic_type: &str) -> Result<()> { let msg = json!( { "op": Ops::Advertise.to_string(), @@ -180,7 +180,7 @@ impl RosBridgeComm for Writer { service: &str, id: &str, req: Req, - ) -> RosLibRustResult<()> { + ) -> Result<()> { let msg = json!( { "op": Ops::CallService.to_string(), @@ -195,7 +195,7 @@ impl RosBridgeComm for Writer { Ok(()) } - async fn unadvertise(&mut self, topic: &str) -> RosLibRustResult<()> { + async fn unadvertise(&mut self, topic: &str) -> Result<()> { debug!("Sending unadvertise on {}", topic); let msg = json! { { @@ -209,7 +209,7 @@ impl RosBridgeComm for Writer { Ok(()) } - async fn advertise_service(&mut self, srv_name: &str, srv_type: &str) -> RosLibRustResult<()> { + async fn advertise_service(&mut self, srv_name: &str, srv_type: &str) -> Result<()> { debug!("Sending advertise service on {} w/ {}", srv_name, srv_type); let msg = json! { { @@ -223,7 +223,7 @@ impl RosBridgeComm for Writer { Ok(()) } - async fn unadvertise_service(&mut self, topic: &str) -> RosLibRustResult<()> { + async fn unadvertise_service(&mut self, topic: &str) -> Result<()> { debug!("Sending unadvertise service on {topic}"); let msg = json! { { @@ -242,7 +242,7 @@ impl RosBridgeComm for Writer { id: Option, is_success: bool, response: serde_json::Value, - ) -> RosLibRustResult<()> { + ) -> Result<()> { debug!( "Sending service response on {:?} with {:?}, {:?}, {:?}", topic, id, is_success, response diff --git a/roslibrust/src/rosbridge/integration_tests.rs b/roslibrust_rosbridge/src/integration_tests.rs similarity index 97% rename from roslibrust/src/rosbridge/integration_tests.rs rename to roslibrust_rosbridge/src/integration_tests.rs index de7e612b..761cc594 100644 --- a/roslibrust/src/rosbridge/integration_tests.rs +++ b/roslibrust_rosbridge/src/integration_tests.rs @@ -11,9 +11,7 @@ mod integration_tests { use std::sync::Arc; - use crate::{ - rosbridge::TestResult, ClientHandle, ClientHandleOptions, RosLibRustError, Subscriber, - }; + use crate::{ClientHandle, ClientHandleOptions, Error, Subscriber, TestResult}; use log::debug; use tokio::time::{timeout, Duration}; // On my laptop test was ~90% reliable at 10ms @@ -177,11 +175,6 @@ mod integration_tests { // of advertise / unadvertise status. Unclear how to confirm whether unadvertise was sent or not #[ignore] async fn unadvertise() -> TestResult { - let _ = simple_logger::SimpleLogger::new() - .with_level(log::LevelFilter::Debug) - .without_timestamps() - .init(); - // Flow: // 1. Create a publisher and subscriber // 2. Send a message and confirm connection works (topic was advertised) @@ -316,7 +309,7 @@ mod integration_tests { .store(true, std::sync::atomic::Ordering::Relaxed); let res = client.advertise::