From b411beaf0657b917b29bb2307927dd29597a3169 Mon Sep 17 00:00:00 2001 From: Carter Date: Thu, 5 Oct 2023 13:46:31 -0600 Subject: [PATCH 01/34] Trying to figure out which tests are flaky and fix, adding debug information to CI as some flakyness is not reproducing locally: --- .github/pull_request_template.md | 2 +- .github/workflows/galactic.yml | 2 ++ .github/workflows/humble.yml | 2 ++ .github/workflows/noetic.yml | 2 ++ .github/workflows/noetic_gencpp.yml | 2 ++ roslibrust/src/rosbridge/integration_tests.rs | 8 +++++--- roslibrust/tests/ros1_xmlrpc.rs | 14 +++++++++----- 7 files changed, 23 insertions(+), 9 deletions(-) diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index dac3605d..9c6d7f47 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -2,7 +2,7 @@ `Insert description` ## Fixes -Issue Number: `number` +Closes: `number` ## Checklist - [ ] Update CHANGELOG.md diff --git a/.github/workflows/galactic.yml b/.github/workflows/galactic.yml index 8472b672..0adc78c9 100644 --- a/.github/workflows/galactic.yml +++ b/.github/workflows/galactic.yml @@ -8,6 +8,8 @@ on: env: HOME: /root + # Coupled with our use of the test_log crate this should give us good CI output on failure + RUST_LOG: debug jobs: galactic: diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index 7c55d89a..4974f963 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -8,6 +8,8 @@ on: env: HOME: /root + # Coupled with our use of the test_log crate this should give us good CI output on failure + RUST_LOG: debug jobs: humble: diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 10a0e2f8..33101068 100644 --- a/.github/workflows/noetic.yml +++ b/.github/workflows/noetic.yml @@ -9,6 +9,8 @@ on: env: HOME: /root ROS_PACKAGE_PATH: /opt/ros/noetic/share + # Coupled with our use of the test_log crate this should give us good CI output on failure + RUST_LOG: debug jobs: noetic: diff --git a/.github/workflows/noetic_gencpp.yml b/.github/workflows/noetic_gencpp.yml index db077492..41e85bd5 100644 --- a/.github/workflows/noetic_gencpp.yml +++ b/.github/workflows/noetic_gencpp.yml @@ -8,6 +8,8 @@ on: env: HOME: /root + # Coupled with our use of the test_log crate this should give us good CI output on failure + RUST_LOG: debug jobs: noetic_gencpp: diff --git a/roslibrust/src/rosbridge/integration_tests.rs b/roslibrust/src/rosbridge/integration_tests.rs index d4b5c54a..44eb5d21 100644 --- a/roslibrust/src/rosbridge/integration_tests.rs +++ b/roslibrust/src/rosbridge/integration_tests.rs @@ -324,10 +324,12 @@ mod integration_tests { ClientHandle::new_with_options(ClientHandleOptions::new(LOCAL_WS).timeout(TIMEOUT)) .await?; - let subscriber = client.subscribe::("/char_topic").await?; - tokio::time::sleep(TIMEOUT).await; + // Thing for us to figure out, and don't ask me why, but this test is WAY more reliable if you advertise first then subscribe + // Unclear if this is our fault or rosbridge's let publisher = client.advertise("/char_topic").await?; tokio::time::sleep(TIMEOUT).await; + let subscriber = client.subscribe::("/char_topic").await?; + tokio::time::sleep(TIMEOUT).await; // Note because C++ char != rust char some care has to be taken when converting let x = std_msgs::Char { @@ -349,7 +351,7 @@ mod integration_tests { #[test_log::test(tokio::test)] async fn error_on_non_existent_service() -> TestResult { // This test is designed to catch a specific error raised in issue #88 - // When roslibrust expereiences a server side error, it returns a string instead of our message + // When roslibrust experiences a server side error, it returns a string instead of our message // We are trying to force that here, and ensure we correctly report the error let client = diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index fa06bc37..a3ef3d79 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -88,12 +88,16 @@ mod tests { .unwrap(); // Note: internally timeout()'d - let publications = call_node_api::>( - &node_uri, - "getPublications", - vec!["/verify_get_publications".into()], + let publications = timeout( + TIMEOUT, + call_node_api::>( + &node_uri, + "getPublications", + vec!["/verify_get_publications".into()], + ), ) - .await; + .await + .unwrap(); assert_eq!(publications.len(), 1); let (topic, topic_type) = publications.iter().nth(0).unwrap(); assert_eq!(topic, "/test_topic"); From 8c32d478f9225b7384906d4efef5bd4050f85f74 Mon Sep 17 00:00:00 2001 From: Carter Date: Thu, 5 Oct 2023 13:54:18 -0600 Subject: [PATCH 02/34] Add rust version confirmation to CI --- .github/workflows/galactic.yml | 2 ++ .github/workflows/humble.yml | 2 ++ .github/workflows/noetic.yml | 2 ++ 3 files changed, 6 insertions(+) diff --git a/.github/workflows/galactic.yml b/.github/workflows/galactic.yml index 0adc78c9..4e703f50 100644 --- a/.github/workflows/galactic.yml +++ b/.github/workflows/galactic.yml @@ -24,6 +24,8 @@ jobs: uses: actions/checkout@v3 with: submodules: 'true' + - name: Verify rust version + run: source /root/.cargo/env; rustc --version - name: Lint run: source /root/.cargo/env; cargo fmt --all -- --check - name: Build Main Lib diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index 4974f963..53831e37 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -24,6 +24,8 @@ jobs: uses: actions/checkout@v3 with: submodules: 'true' + - name: Verify rust version + run: source /root/.cargo/env; rustc --version - name: Lint run: source /root/.cargo/env; cargo fmt --all -- --check - name: Build Main Lib diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 33101068..6110e7f6 100644 --- a/.github/workflows/noetic.yml +++ b/.github/workflows/noetic.yml @@ -25,6 +25,8 @@ jobs: uses: actions/checkout@v3 with: submodules: 'true' + - name: Verify rust version + run: source /root/.cargo/env; rustc --version - name: Lint run: source /root/.cargo/env; cargo fmt --all -- --check - name: Build Main Lib From 869085d2bd12bf4a8404ffa63bca91fd82c0f188 Mon Sep 17 00:00:00 2001 From: Carter Date: Sun, 8 Oct 2023 15:18:58 -0600 Subject: [PATCH 03/34] Nuking rosapi, and the funky macro --- .github/workflows/noetic.yml | 2 +- CHANGELOG.md | 4 + roslibrust/Cargo.toml | 2 - roslibrust/src/lib.rs | 3 - roslibrust/src/rosapi/mod.rs | 544 ---------------------------- roslibrust_codegen_macro/src/lib.rs | 25 -- 6 files changed, 5 insertions(+), 575 deletions(-) delete mode 100644 roslibrust/src/rosapi/mod.rs diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 6110e7f6..7ac7c833 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 /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge,rosapi -- --test-threads 1 \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge -- --test-threads 1 \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index 401351b6..43828dfa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed + - Removed `rosapi` module and functionality. Will likely return in a separate crate in the future. + - Removed `find_and_generate_ros_messages_relative_to_manifest_dir!` this proc_macro was changing the current working directory of the compilation job resulting in a variety of strange compilation behaviors. Build.rs scripts are recommended for use cases requiring fine + grained control of message generation. + ## 0.8.0 - October 4th, 2023 ### Added diff --git a/roslibrust/Cargo.toml b/roslibrust/Cargo.toml index 23fca167..c531dc80 100644 --- a/roslibrust/Cargo.toml +++ b/roslibrust/Cargo.toml @@ -53,8 +53,6 @@ simple_logger = "2.1.0" [features] default = [] -# Provides a rosapi rust interface -rosapi = [] # Note: all does not include running_bridge as that is only intended for CI all = [] # Intended for use with tests, includes tests that rely on a locally running rosbridge diff --git a/roslibrust/src/lib.rs b/roslibrust/src/lib.rs index 8932461d..389a0aaf 100644 --- a/roslibrust/src/lib.rs +++ b/roslibrust/src/lib.rs @@ -101,9 +101,6 @@ mod rosbridge; pub use rosbridge::*; -#[cfg(feature = "rosapi")] -pub mod rosapi; - #[cfg(feature = "ros1")] mod ros1; #[cfg(feature = "ros1")] diff --git a/roslibrust/src/rosapi/mod.rs b/roslibrust/src/rosapi/mod.rs deleted file mode 100644 index 8e198208..00000000 --- a/roslibrust/src/rosapi/mod.rs +++ /dev/null @@ -1,544 +0,0 @@ -//! This module is intended to provide 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 async_trait::async_trait; - -// TODO Fix this... this sucks -roslibrust_codegen_macro::find_and_generate_ros_messages_relative_to_manifest_dir!( - "../assets/ros1_common_interfaces/rosapi" -); - -/// 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. -#[async_trait] -trait RosApi { - async fn get_time(&self) -> RosLibRustResult; - async fn topics(&self) -> RosLibRustResult; - async fn get_topic_type( - &self, - topic: impl Into + Send, - ) -> RosLibRustResult; - async fn get_topics_for_type( - &self, - topic_type: impl Into + Send, - ) -> RosLibRustResult; - async fn get_nodes(&self) -> RosLibRustResult; - - async fn get_node_details( - &self, - node: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_node_for_service( - &self, - service: impl Into + Send, - ) -> RosLibRustResult; - - async fn set_param( - &self, - param_name: impl Into + Send, - param_value: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_param( - &self, - param_name: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_param_names(&self) -> RosLibRustResult; - - async fn has_param( - &self, - param: impl Into + Send, - ) -> RosLibRustResult; - - async fn delete_param( - &self, - name: impl Into + Send, - ) -> RosLibRustResult; - - async fn message_details( - &self, - message_name: impl Into + Send, - ) -> RosLibRustResult; - - async fn publishers( - &self, - topic: impl Into + Send, - ) -> RosLibRustResult; - - async fn service_host( - &self, - service: impl Into + Send, - ) -> RosLibRustResult; - - async fn service_providers( - &self, - service_type: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_service_request_details( - &self, - service_type: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_service_response_details( - &self, - service_type: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_service_type( - &self, - service_name: impl Into + Send, - ) -> RosLibRustResult; - - async fn get_services(&self) -> RosLibRustResult; -} - -#[async_trait] -impl RosApi for ClientHandle { - /// Get the current time - async fn get_time(&self) -> RosLibRustResult { - self.call_service("/rosapi/get_time", rosapi::GetTimeRequest {}) - .await - } - - /// Get the list of topics active - async fn topics(&self) -> RosLibRustResult { - self.call_service("/rosapi/topics", rosapi::TopicsRequest {}) - .await - } - - /// Get the type of a given topic - async fn get_topic_type( - &self, - topic: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/topic_type", - rosapi::TopicTypeRequest { - topic: topic.into(), - }, - ) - .await - } - - /// Returns a list of the topics active in the system that are of the type provided - async fn get_topics_for_type( - &self, - topic_type: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/topics_for_type", - rosapi::TopicsForTypeRequest { - r#type: topic_type.into(), - }, - ) - .await - } - - /// Returns list of nodes active in a system - async fn get_nodes(&self) -> RosLibRustResult { - self.call_service("/rosapi/nodes", rosapi::NodesRequest {}) - .await - } - - /// Returns the subscriptions, publishers, and service servers for a given node - /// @param node Fully resolved ros node name e.g. "/rosapi", should match the names given by `rosnode list` - async fn get_node_details( - &self, - node: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/node_details", - rosapi::NodeDetailsRequest { node: node.into() }, - ) - .await - } - - /// Given the name of service, get the name of the node that is providing that service - async fn get_node_for_service( - &self, - service: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/service_node", - rosapi::ServiceNodeRequest { - service: service.into(), - }, - ) - .await - } - - /// Sets a parameter. Unclear exactly how 'types' of parameters will be handled here. - /// For now we're simply converting into a string. - async fn set_param( - &self, - param_name: impl Into + Send, - param_value: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/set_param", - rosapi::SetParamRequest { - name: param_name.into(), - value: param_value.into(), - }, - ) - .await - } - - /// Gets the current value for a parameter. - /// Not handling any type safety or conversion. - async fn get_param( - &self, - param_name: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/get_param", - rosapi::GetParamRequest { - name: param_name.into(), - default: "".to_string(), - }, - ) - .await - } - - /// Gets the list of currently known parameters. - async fn get_param_names(&self) -> RosLibRustResult { - self.call_service("/rosapi/get_param_names", rosapi::GetParamNamesRequest {}) - .await - } - - /// Checks whether the given parameter is defined. - async fn has_param( - &self, - param: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/has_param", - rosapi::HasParamRequest { name: param.into() }, - ) - .await - } - - /// Deletes a parameter by name. - async fn delete_param( - &self, - name: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/delete_param", - rosapi::DeleteParamRequest { name: name.into() }, - ) - .await - } - - /// Returns detailed information about a given message type e.g. 'std_msgs/Header' - async fn message_details( - &self, - message_name: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/message_details", - rosapi::MessageDetailsRequest { - r#type: message_name.into(), - }, - ) - .await - } - - /// Gets a list of all nodes that are publishing to a given topic. - async fn publishers( - &self, - topic: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/publishers", - rosapi::PublishersRequest { - topic: topic.into(), - }, - ) - .await - } - - /// Give the name of a service, returns the name of the machine on which that service is being hosted - async fn service_host( - &self, - service: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/service_host", - rosapi::ServiceHostRequest { - service: service.into(), - }, - ) - .await - } - - /// Given the type of a service, returns a list of nodes which are providing services with that type - async fn service_providers( - &self, - service_type: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/service_providers", - rosapi::ServiceProvidersRequest { - service: service_type.into(), - }, - ) - .await - } - - /// Given the type of a service (e.g. 'rosapi/SetParam'), returns details about the message format of the request - async fn get_service_request_details( - &self, - service_type: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/service_request_details", - rosapi::ServiceRequestDetailsRequest { - r#type: service_type.into(), - }, - ) - .await - } - - /// Given the type of a service (e.g. 'rosapi/SetParam'), returns details about the message format of the response - async fn get_service_response_details( - &self, - service_type: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/service_response_details", - rosapi::ServiceRequestDetailsRequest { - r#type: service_type.into(), - }, - ) - .await - } - - /// Given the name of a service (e.g. '/rosapi/publishers'), returns the type of the service ('rosapi/Publishers') - async fn get_service_type( - &self, - service_name: impl Into + Send, - ) -> RosLibRustResult { - self.call_service( - "/rosapi/service_type", - rosapi::ServiceTypeRequest { - service: service_name.into(), - }, - ) - .await - } - - /// Get the list of services active on the system - async fn get_services(&self) -> RosLibRustResult { - self.call_service("/rosapi/services", rosapi::ServicesRequest {}) - .await - } - - /* - List of rosapi services pulled from `rosservice list` - /rosapi/action_servers - Probably won't support - /rosapi/delete_param - Done - /rosapi/get_loggers - ?? - /rosapi/get_param - Done - /rosapi/get_param_names - Done - /rosapi/get_time - Done - /rosapi/has_param - Done - /rosapi/message_details - Done - /rosapi/node_details - Done - /rosapi/nodes - Done - /rosapi/publishers - Done - /rosapi/search_param - ?? What does this do and why? - /rosapi/service_host - Done - /rosapi/service_node - Done - /rosapi/service_providers - Done - /rosapi/service_request_details - Done - /rosapi/service_response_details - Done - /rosapi/service_type - Done - /rosapi/services - TODO - /rosapi/services_for_type - Done - /rosapi/set_logger_level - ?? - /rosapi/set_param - Done - /rosapi/subscribers - Done - /rosapi/topic_type - Done - /rosapi/topics - Done - /rosapi/topics_and_raw_types - ?? - /rosapi/topics_for_type - Done - */ -} - -#[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}; - - async fn fixture_client() -> ClientHandle { - // Tiny sleep to throttle rate at which tests are run to try to make CI more consistent - tokio::time::sleep(std::time::Duration::from_millis(50)).await; - let opts = ClientHandleOptions::new("ws://localhost:9090") - // 200 ms failed CI - .timeout(std::time::Duration::from_millis(500)); - ClientHandle::new_with_options(opts).await.unwrap() - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_time() { - let api = fixture_client().await; - api.get_time().await.unwrap(); - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_topic_type() { - let api = fixture_client().await; - let res = api - .get_topic_type("/rosout") - .await - .expect("Failed to get topic type for rosout"); - assert_eq!(res.r#type, "rosgraph_msgs/Log"); - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_topics_for_type() { - let api = fixture_client().await; - let res = api - .get_topics_for_type("rosgraph_msgs/Log") - .await - .expect("Failed to get topics for type Log"); - assert!(res.topics.iter().any(|f| f == "/rosout")); - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_nodes() { - let api = fixture_client().await; - let nodes = api.get_nodes().await.expect("Failed to get nodes"); - assert!(nodes.nodes.iter().any(|f| f == "/rosapi")); - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_node_details() { - let api = fixture_client().await; - assert!( - api.get_node_details("/rosapi") - .await - .expect("Failed to get node details for rosapi") - .services - .len() - > 0 - ); - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_node_for_service() { - let api = fixture_client().await; - assert_eq!( - api.get_node_for_service("/rosapi/service_node") - .await - .expect("Failed to call service_node") - .node, - "/rosapi" - ); - } - - #[test_log::test(tokio::test)] - async fn rosapi_param_roundtrip() { - let api = fixture_client().await; - const PARAM_NAME: &'static str = "/rosapi_param_roundtrip"; - // Set the parameter - api.set_param(PARAM_NAME, 1.0.to_string()) - .await - .expect("Failed to set"); - - // read back the value we set - let response = api - .get_param(PARAM_NAME) - .await - .expect("Failed to read param back"); - assert_eq!(1.0, response.value.parse::().unwrap()); - - // Confirm the parameter is in the list of parameters - let param_names = api - .get_param_names() - .await - .expect("Failed to get param names"); - assert!(param_names.names.contains(&PARAM_NAME.to_string())); - - // Confirm has_param sees it - assert!(api.has_param(PARAM_NAME).await.unwrap().exists); - - // Delete it! - api.delete_param(PARAM_NAME).await.unwrap(); - - // Confirm it is gone - assert!(!api.has_param(PARAM_NAME).await.unwrap().exists); - } - - #[test_log::test(tokio::test)] - async fn rosapi_message_details() { - let api = fixture_client().await; - let response = api.message_details("std_msgs/Header").await.unwrap(); - // Spot check we actually got some data back - assert!(response - .typedefs - .iter() - .any(|t| t.fieldtypes.contains(&"time".to_string()))); - } - - #[test_log::test(tokio::test)] - async fn rosapi_publishers() { - let api = fixture_client().await; - let response = api.publishers("/rosout").await.unwrap(); - assert!(response.publishers.iter().any(|p| p == "/rosapi")); - } - - #[test_log::test(tokio::test)] - async fn rosapi_service_providers() { - let api = fixture_client().await; - let response = api.service_providers("rosapi/ServiceHost").await.unwrap(); - assert!(response.providers.iter().any(|p| p == "/rosapi")); - } - - #[test_log::test(tokio::test)] - async fn rosapi_service_request_details() { - let api = fixture_client().await; - let response = api - .get_service_request_details("rosapi/SetParam") - .await - .unwrap(); - // Spot check we got some data back - assert!(response.typedefs[0].fieldnames.len() == 2); - } - - #[test_log::test(tokio::test)] - async fn rosapi_service_response_details() { - let api = fixture_client().await; - let response = api - .get_service_response_details("rosapi/GetParam") - .await - .unwrap(); - // Spot check we got some data back - assert_eq!(response.typedefs[0].fieldnames[0], "value"); - } - - #[test_log::test(tokio::test)] - async fn rosapi_get_service_type() { - let api = fixture_client().await; - let response = api.get_service_type("/rosapi/node_details").await.unwrap(); - assert_eq!(response.r#type, "rosapi/NodeDetails"); - } - - #[test_log::test(tokio::test)] - async fn rosapi_services() { - let api = fixture_client().await; - let response = api.get_services().await.unwrap(); - assert!(!response.services.is_empty()); - } -} diff --git a/roslibrust_codegen_macro/src/lib.rs b/roslibrust_codegen_macro/src/lib.rs index 63134f06..ad5549d7 100644 --- a/roslibrust_codegen_macro/src/lib.rs +++ b/roslibrust_codegen_macro/src/lib.rs @@ -41,31 +41,6 @@ pub fn find_and_generate_ros_messages(input_stream: TokenStream) -> TokenStream } } -/// Does the same as find_and_generate_ros_messages, but interprets relative paths -/// as relative to the root of this crate. No idea if this is useful, but I needed it! -#[proc_macro] -pub fn find_and_generate_ros_messages_relative_to_manifest_dir( - input_stream: TokenStream, -) -> TokenStream { - let RosLibRustMessagePaths { mut paths } = - parse_macro_input!(input_stream as RosLibRustMessagePaths); - - std::env::set_current_dir(env!("CARGO_MANIFEST_DIR")).expect("Failed to set working dir"); - for path in &mut paths { - *path = path.canonicalize().unwrap_or_else(|err| { - panic!("Failed to canonicalize path {path:?}: {}", err.to_string()) - }); - } - - match roslibrust_codegen::find_and_generate_ros_messages(paths) { - Ok(t) => t.into(), - Err(e) => { - let error_msg = e.to_string(); - quote::quote!(compile_error!(#error_msg);).into() - } - } -} - /// Similar to `find_and_generate_ros_messages`, but does not search the /// `ROS_PACKAGE_PATH` environment variable paths (useful in some situations). #[proc_macro] From d8540c9e76fcf9209f3de9ed63c9d53ae0e54c83 Mon Sep 17 00:00:00 2001 From: Carter Date: Sun, 8 Oct 2023 15:26:00 -0600 Subject: [PATCH 04/34] Fix ros1_test --- roslibrust/tests/ros1_xmlrpc.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index a3ef3d79..b02f66bb 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -6,8 +6,8 @@ mod tests { use tokio::time::timeout; const TIMEOUT: tokio::time::Duration = tokio::time::Duration::from_millis(500); - roslibrust_codegen_macro::find_and_generate_ros_messages_relative_to_manifest_dir!( - "../assets/ros1_common_interfaces" + roslibrust_codegen_macro::find_and_generate_ros_messages!( + "assets/ros1_common_interfaces" ); async fn call_node_api_raw(uri: &str, endpoint: &str, args: Vec) -> String { From 6bdceae852f5cd7417881521abf6b7366917fc62 Mon Sep 17 00:00:00 2001 From: Carter Date: Sun, 8 Oct 2023 15:29:56 -0600 Subject: [PATCH 05/34] Lint --- roslibrust/tests/ros1_xmlrpc.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index b02f66bb..a5f8c359 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -6,9 +6,7 @@ mod tests { use tokio::time::timeout; const TIMEOUT: tokio::time::Duration = tokio::time::Duration::from_millis(500); - roslibrust_codegen_macro::find_and_generate_ros_messages!( - "assets/ros1_common_interfaces" - ); + roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); async fn call_node_api_raw(uri: &str, endpoint: &str, args: Vec) -> String { let client = reqwest::Client::new(); From 269855a6fb93a7c436e4dada46a477a93895c5b4 Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 8 Oct 2023 17:24:35 -0600 Subject: [PATCH 06/34] Trying to get more info on failure --- .github/workflows/noetic.yml | 4 +++- roslibrust/tests/ros1_xmlrpc.rs | 4 ++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 7ac7c833..0d8e9147 100644 --- a/.github/workflows/noetic.yml +++ b/.github/workflows/noetic.yml @@ -39,4 +39,6 @@ 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 /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge -- --test-threads 1 \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge -- --test-threads 1 + - name: Ros1 Evil Integration Tests # Added because these specific tests were flaky in CI and trying to get more info + run: source /root/.cargo/env; cargo test --features ros1_test,ros1 -- --ignored \ No newline at end of file diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index a5f8c359..a97db98a 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -30,6 +30,7 @@ mod tests { } #[tokio::test] + #[ignore] async fn verify_get_master_uri() { let node = timeout( TIMEOUT, @@ -54,6 +55,7 @@ mod tests { } #[tokio::test] + #[ignore] async fn verify_get_publications() { let node = timeout( TIMEOUT, @@ -103,6 +105,7 @@ mod tests { } #[tokio::test] + #[ignore] async fn verify_shutdown() { let node = timeout( TIMEOUT, @@ -129,6 +132,7 @@ mod tests { } #[tokio::test] + #[ignore] async fn verify_request_topic() { let node = timeout( TIMEOUT, From bfea3c12063d2d1f442c2bae3e32cf969b10aa24 Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 8 Oct 2023 18:34:13 -0600 Subject: [PATCH 07/34] Disabing previously ignored test so I can test what I really want to --- roslibrust/src/rosbridge/integration_tests.rs | 112 +++++++++--------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/roslibrust/src/rosbridge/integration_tests.rs b/roslibrust/src/rosbridge/integration_tests.rs index 44eb5d21..4cca0423 100644 --- a/roslibrust/src/rosbridge/integration_tests.rs +++ b/roslibrust/src/rosbridge/integration_tests.rs @@ -168,62 +168,62 @@ mod integration_tests { }); } - /// Tests that dropping a publisher correctly unadvertises - #[test_log::test(tokio::test)] - // This test is currently broken, it seems that rosbridge still sends the message regardless - // 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) - // 3. Drop the publisher, unadvertise should be sent - // 4. Manually send a message without a publisher it should fail since topic was unadvertised - const TOPIC: &str = "/unadvertise"; - debug!("Start unadvertise test"); - - let opt = ClientHandleOptions::new(LOCAL_WS).timeout(TIMEOUT); - - let client = ClientHandle::new_with_options(opt).await?; - let publisher = client.advertise(TOPIC).await?; - debug!("Got publisher"); - - let sub = client.subscribe::
(TOPIC).await?; - debug!("Got subscriber"); - - let msg = Header::default(); - publisher.publish(msg).await?; - timeout(TIMEOUT, sub.next()).await?; - - debug!("Dropping publisher"); - // drop subscriber so it doesn't keep topic open - std::mem::drop(sub); - // unadvertise should happen here - std::mem::drop(publisher); - - // Wait for drop to complete - tokio::time::sleep(TIMEOUT).await; - - let sub = client.subscribe::
(TOPIC).await?; - // manually publishing using private api - let msg = Header::default(); - client.publish(TOPIC, msg).await?; - - match timeout(TIMEOUT, sub.next()).await { - Ok(_msg) => { - anyhow::bail!("Received message after unadvertised!"); - } - Err(_e) => { - // All good! Timeout should expire - } - } - Ok(()) - } + // /// Tests that dropping a publisher correctly unadvertises + // #[test_log::test(tokio::test)] + // // This test is currently broken, it seems that rosbridge still sends the message regardless + // // 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) + // // 3. Drop the publisher, unadvertise should be sent + // // 4. Manually send a message without a publisher it should fail since topic was unadvertised + // const TOPIC: &str = "/unadvertise"; + // debug!("Start unadvertise test"); + + // let opt = ClientHandleOptions::new(LOCAL_WS).timeout(TIMEOUT); + + // let client = ClientHandle::new_with_options(opt).await?; + // let publisher = client.advertise(TOPIC).await?; + // debug!("Got publisher"); + + // let sub = client.subscribe::
(TOPIC).await?; + // debug!("Got subscriber"); + + // let msg = Header::default(); + // publisher.publish(msg).await?; + // timeout(TIMEOUT, sub.next()).await?; + + // debug!("Dropping publisher"); + // // drop subscriber so it doesn't keep topic open + // std::mem::drop(sub); + // // unadvertise should happen here + // std::mem::drop(publisher); + + // // Wait for drop to complete + // tokio::time::sleep(TIMEOUT).await; + + // let sub = client.subscribe::
(TOPIC).await?; + // // manually publishing using private api + // let msg = Header::default(); + // client.publish(TOPIC, msg).await?; + + // match timeout(TIMEOUT, sub.next()).await { + // Ok(_msg) => { + // anyhow::bail!("Received message after unadvertised!"); + // } + // Err(_e) => { + // // All good! Timeout should expire + // } + // } + // Ok(()) + // } // This test currently doesn't work for ROS2, however all other service functionalities appear fine // It may be that ros2 prevents a "service_loop" where a node calls a service on itself? From ff6a7d3d42c20724cf723526193d868e16779854 Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 8 Oct 2023 18:51:46 -0600 Subject: [PATCH 08/34] Ignore is messy as it turns out --- roslibrust/src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/roslibrust/src/lib.rs b/roslibrust/src/lib.rs index 389a0aaf..6973ad7e 100644 --- a/roslibrust/src/lib.rs +++ b/roslibrust/src/lib.rs @@ -33,12 +33,12 @@ //! //! ## Message Generation //! Message generation is provided in two APIs. The first, which is visible in `roslibrust/examples`, is a proc-macro which can be invoked to generate ROS message structs in place: -//! ```ignore +//! ```compile_fail //! use roslibrust_codegen_macro::find_and_generate_ros_messages; //! find_and_generate_ros_messages!(); //! ``` //! If you have ROS installed, this macro will search for message files under paths in the `ROS_PACKAGE_PATH`. If you do not have ROS installed in your environment, you can specify search paths explicitly: -//! ```ignore +//! ```compile_fail //! use roslibrust_codegen_macro::find_and_generate_ros_messages; //! find_and_generate_ros_messages!("/path/to/noetic/packages", "/path/to/my/packages"); //! ``` From 4c858f01d611fdab9a1c5dc2c5a545b322e1271e Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 10:08:39 -0600 Subject: [PATCH 09/34] Trying to get some debug out of these tests --- roslibrust/tests/ros1_xmlrpc.rs | 265 +++++++++++++++++++------------- 1 file changed, 157 insertions(+), 108 deletions(-) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index a97db98a..8b55a9a3 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -29,148 +29,197 @@ mod tests { value } - #[tokio::test] + async fn test_with_watch_dog(test: impl std::future::Future) { + // Overall watchdog since I can't get these tests to timeout, but they take 20min in CI + // Need the task to fail so I can get any debug information out + // Can't get the failure to repro locally + let watchdog = async { + tokio::time::sleep(TIMEOUT * 5).await; + log::error!("Test watchdog tripped!"); + }; + tokio::select! { + _ = watchdog => { + panic!("Test failed due to watchdog"); + } + _ = test => { + // Happy days! + } + } + } + + #[test_log::test(tokio::test)] #[ignore] async fn verify_get_master_uri() { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_master_uri"), - ) - .await - .unwrap() - .unwrap(); - let node_uri = timeout(TIMEOUT, node.get_client_uri()) + test_with_watch_dog(async { + let node = timeout( + TIMEOUT, + roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_master_uri"), + ) .await .unwrap() .unwrap(); + log::info!("Got new handle"); + + let node_uri = timeout(TIMEOUT, node.get_client_uri()) + .await + .unwrap() + .unwrap(); + log::info!("Got uri"); - // Note: doesn't need timeout as it is internally timeout()'d - let master_uri = call_node_api::( - &node_uri, - "getMasterUri", - vec!["/get_master_uri_test".into()], - ) + // Note: doesn't need timeout as it is internally timeout()'d + let master_uri = call_node_api::( + &node_uri, + "getMasterUri", + vec!["/get_master_uri_test".into()], + ) + .await; + log::info!("Got master"); + assert_eq!(master_uri, "http://localhost:11311"); + }) .await; - assert_eq!(master_uri, "http://localhost:11311"); } - #[tokio::test] + #[test_log::test(tokio::test)] #[ignore] async fn verify_get_publications() { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications"), - ) - .await - .unwrap() - .unwrap(); - - let node_uri = timeout(TIMEOUT, node.get_client_uri()) + test_with_watch_dog(async { + let node = timeout( + TIMEOUT, + roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications"), + ) .await .unwrap() .unwrap(); + log::info!("Got new handle"); - // Note: timeout not needed as it is internally timeout'd - let publications = call_node_api::>( - &node_uri, - "getPublications", - vec!["/verify_get_publications".into()], - ) - .await; - assert_eq!(publications.len(), 0); - - let _publisher = timeout( - TIMEOUT, - node.advertise::("/test_topic", 1), - ) - .await - .unwrap() - .unwrap(); - - // Note: internally timeout()'d - let publications = timeout( - TIMEOUT, - call_node_api::>( + let node_uri = timeout(TIMEOUT, node.get_client_uri()) + .await + .unwrap() + .unwrap(); + log::info!("Got uri"); + + // Note: timeout not needed as it is internally timeout'd + let publications = call_node_api::>( &node_uri, "getPublications", vec!["/verify_get_publications".into()], - ), - ) - .await - .unwrap(); - assert_eq!(publications.len(), 1); - let (topic, topic_type) = publications.iter().nth(0).unwrap(); - assert_eq!(topic, "/test_topic"); - assert_eq!(topic_type, std_msgs::String::ROS_TYPE_NAME); + ) + .await; + assert_eq!(publications.len(), 0); + log::info!("Got publications"); + + let _publisher = timeout( + TIMEOUT, + node.advertise::("/test_topic", 1), + ) + .await + .unwrap() + .unwrap(); + log::info!("advertised"); + + // Note: internally timeout()'d + let publications = timeout( + TIMEOUT, + call_node_api::>( + &node_uri, + "getPublications", + vec!["/verify_get_publications".into()], + ), + ) + .await + .unwrap(); + log::info!("Got post advertise publications"); + + assert_eq!(publications.len(), 1); + let (topic, topic_type) = publications.iter().nth(0).unwrap(); + assert_eq!(topic, "/test_topic"); + assert_eq!(topic_type, std_msgs::String::ROS_TYPE_NAME); + }) + .await; } - #[tokio::test] + #[test_log::test(tokio::test)] #[ignore] async fn verify_shutdown() { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_shutdown"), - ) - .await - .unwrap() - .unwrap(); - let node_uri = timeout(TIMEOUT, node.get_client_uri()) + test_with_watch_dog(async { + let node = timeout( + TIMEOUT, + roslibrust::NodeHandle::new("http://localhost:11311", "verify_shutdown"), + ) .await .unwrap() .unwrap(); - assert!(node.is_ok()); - - // Note: internally timeout()'d - call_node_api::( - &node_uri, - "shutdown", - vec!["/verify_shutdown".into(), "".into()], - ) + log::info!("Got handle"); + let node_uri = timeout(TIMEOUT, node.get_client_uri()) + .await + .unwrap() + .unwrap(); + assert!(node.is_ok()); + log::info!("Got uri"); + + // Note: internally timeout()'d + call_node_api::( + &node_uri, + "shutdown", + vec!["/verify_shutdown".into(), "".into()], + ) + .await; + log::info!("shutdown?"); + + assert!(!node.is_ok()); + }) .await; - - assert!(!node.is_ok()); } - #[tokio::test] + #[test_log::test(tokio::test)] #[ignore] async fn verify_request_topic() { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_request_topic"), - ) - .await - .unwrap() - .unwrap(); - let node_uri = timeout(TIMEOUT, node.get_client_uri()) + test_with_watch_dog(async { + let node = timeout( + TIMEOUT, + roslibrust::NodeHandle::new("http://localhost:11311", "verify_request_topic"), + ) + .await + .unwrap() + .unwrap(); + log::info!("Got handle"); + let node_uri = timeout(TIMEOUT, node.get_client_uri()) + .await + .unwrap() + .unwrap(); + log::info!("Got uri {node_uri:?}"); + + let _publisher = timeout( + TIMEOUT, + node.advertise::("/test_topic", 1), + ) .await .unwrap() .unwrap(); + log::info!("Got publisher"); - let _publisher = timeout( - TIMEOUT, - node.advertise::("/test_topic", 1), - ) - .await - .unwrap() - .unwrap(); - - // Note: internally timeout()'d - let response = call_node_api_raw( - &node_uri, - "requestTopic", - vec![ - "verify_request_topic".into(), - "/test_topic".into(), - serde_xmlrpc::Value::Array(vec![serde_xmlrpc::Value::Array(vec!["TCPROS".into()])]), - ], - ) + // Note: internally timeout()'d + let response = call_node_api_raw( + &node_uri, + "requestTopic", + vec![ + "verify_request_topic".into(), + "/test_topic".into(), + serde_xmlrpc::Value::Array(vec![serde_xmlrpc::Value::Array(vec![ + "TCPROS".into() + ])]), + ], + ) + .await; + log::info!("Got response"); + + let (code, _description, (protocol, host, port)): (i8, String, (String, String, u16)) = + serde_xmlrpc::response_from_str(&response).unwrap(); + assert_eq!(code, 1); + assert_eq!(protocol, "TCPROS"); + assert!(!host.is_empty()); + assert!(port != 0); + }) .await; - - let (code, _description, (protocol, host, port)): (i8, String, (String, String, u16)) = - serde_xmlrpc::response_from_str(&response).unwrap(); - assert_eq!(code, 1); - assert_eq!(protocol, "TCPROS"); - assert!(!host.is_empty()); - assert!(port != 0); } } From f876f677ba3206bf2ffd802376c7e4ad8b1e0526 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 14:39:55 -0600 Subject: [PATCH 10/34] Convert to multi-thread test --- roslibrust/tests/ros1_xmlrpc.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 8b55a9a3..2729e173 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -47,7 +47,7 @@ mod tests { } } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_get_master_uri() { test_with_watch_dog(async { @@ -79,7 +79,7 @@ mod tests { .await; } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_get_publications() { test_with_watch_dog(async { @@ -138,7 +138,7 @@ mod tests { .await; } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_shutdown() { test_with_watch_dog(async { @@ -171,7 +171,7 @@ mod tests { .await; } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_request_topic() { test_with_watch_dog(async { From 21dd3b0e1dd48939d4e281f2d790e945248561e5 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 15:11:21 -0600 Subject: [PATCH 11/34] WAS IT DNS THE WHOLE TIME --- roslibrust/src/ros1/node.rs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 04c00d4a..6f261aff 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -513,7 +513,7 @@ impl NodeHandle { name: &str, ) -> Result> { // Follow ROS rules and determine our IP and hostname - let (addr, hostname) = determine_addr()?; + let (addr, hostname) = determine_addr().await?; let node = Node::new(master_uri, &hostname, name, addr).await?; let nh = NodeHandle { inner: node }; @@ -560,7 +560,7 @@ impl NodeHandle { /// Following ROS's idiomatic address rules uses ROS_HOSTNAME and ROS_IP to determine the address that server should be hosted at. /// Returns both the resolved IpAddress of the host (used for actually opening the socket), and the String "hostname" which should /// be used in the URI. -fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { +async fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { // If ROS_IP is set that trumps anything else if let Ok(ip_str) = std::env::var("ROS_IP") { let ip = ip_str.parse().map_err(|e| { @@ -572,7 +572,7 @@ fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { } // If ROS_HOSTNAME is set that is next highest precedent if let Ok(name) = std::env::var("ROS_HOSTNAME") { - let ip = hostname_to_ipv4(&name)?; + let ip = hostname_to_ipv4(&name).await?; return Ok((ip, name)); } // If neither env var is set, use the computers "hostname" @@ -580,14 +580,14 @@ fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { let name = name.into_string().map_err(|e| { RosMasterError::HostIpResolutionFailure(format!("This host's hostname is a string that cannot be validly converted into a Rust type, and therefore we cannot convert it into an IpAddrv4: {e:?}")) })?; - let ip = hostname_to_ipv4(&name)?; + let ip = hostname_to_ipv4(&name).await?; return Ok((ip, name)); } /// Given a the name of a host use's std::net::ToSocketAddrs to perform a DNS lookup and return the resulting IP address. /// This function is intended to be used to determine the correct IP host the socket for the xmlrpc server on. -fn hostname_to_ipv4(name: &str) -> Result { - let mut i = (name, 0).to_socket_addrs().map_err(|e| { +async fn hostname_to_ipv4(name: &str) -> Result { + let mut i = tokio::net::lookup_host(name).await.map_err(|e| { RosMasterError::HostIpResolutionFailure(format!( "Failure while attempting to lookup ROS_HOSTNAME: {e:?}" )) From 44106652f3a66c0b685b10f60ddb4f56d2811baa Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 15:20:10 -0600 Subject: [PATCH 12/34] Removing multi-threaded test --- roslibrust/tests/ros1_xmlrpc.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 2729e173..8b55a9a3 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -47,7 +47,7 @@ mod tests { } } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] + #[test_log::test(tokio::test)] #[ignore] async fn verify_get_master_uri() { test_with_watch_dog(async { @@ -79,7 +79,7 @@ mod tests { .await; } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] + #[test_log::test(tokio::test)] #[ignore] async fn verify_get_publications() { test_with_watch_dog(async { @@ -138,7 +138,7 @@ mod tests { .await; } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] + #[test_log::test(tokio::test)] #[ignore] async fn verify_shutdown() { test_with_watch_dog(async { @@ -171,7 +171,7 @@ mod tests { .await; } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] + #[test_log::test(tokio::test)] #[ignore] async fn verify_request_topic() { test_with_watch_dog(async { From 131ab1db52347596e17440c205b2d00f8233d050 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 15:24:15 -0600 Subject: [PATCH 13/34] Port fix --- roslibrust/src/ros1/node.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 6f261aff..3c77e894 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -587,7 +587,8 @@ async fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { /// Given a the name of a host use's std::net::ToSocketAddrs to perform a DNS lookup and return the resulting IP address. /// This function is intended to be used to determine the correct IP host the socket for the xmlrpc server on. async fn hostname_to_ipv4(name: &str) -> Result { - let mut i = tokio::net::lookup_host(name).await.map_err(|e| { + let name_with_port = &format!("{name}:0"); + let mut i = tokio::net::lookup_host(name_with_port).await.map_err(|e| { RosMasterError::HostIpResolutionFailure(format!( "Failure while attempting to lookup ROS_HOSTNAME: {e:?}" )) From e4623d60f975b8c7880ae8945b70ba8f4c7ce407 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 15:44:00 -0600 Subject: [PATCH 14/34] try_bind instead of bind for xmlrpc server for better error propagation --- roslibrust/src/ros1/node.rs | 2 +- roslibrust/src/ros1/xmlrpc_server.rs | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 3c77e894..3ab03b32 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -252,7 +252,7 @@ impl Node { node_server_sender: node_sender, }; // Create our xmlrpc server and bind our socket so we know our port and can determine our local URI - let xmlrpc_server = XmlRpcServer::new(addr, node_server_handle.clone()); + let xmlrpc_server = XmlRpcServer::new(addr, node_server_handle.clone())?; let client_uri = format!("http://{hostname}:{}", xmlrpc_server.port()); if let None = Name::new(node_name) { diff --git a/roslibrust/src/ros1/xmlrpc_server.rs b/roslibrust/src/ros1/xmlrpc_server.rs index 1ebc7d95..7d60c710 100644 --- a/roslibrust/src/ros1/xmlrpc_server.rs +++ b/roslibrust/src/ros1/xmlrpc_server.rs @@ -44,7 +44,10 @@ impl XmlRpcServerHandle { } impl XmlRpcServer { - pub fn new(host_addr: Ipv4Addr, node_server: NodeServerHandle) -> XmlRpcServerHandle { + pub fn new( + host_addr: Ipv4Addr, + node_server: NodeServerHandle, + ) -> Result> { let make_svc = hyper::service::make_service_fn(move |connection| { debug!("New node xmlrpc connection {connection:?}"); let node_server = node_server.clone(); @@ -55,7 +58,7 @@ impl XmlRpcServer { } }); let host_addr = SocketAddr::from((host_addr, 0)); - let server = hyper::server::Server::bind(&host_addr.into()); + let server = hyper::server::Server::try_bind(&host_addr.into())?; let server = server.serve(make_svc); let addr = server.local_addr(); @@ -65,10 +68,10 @@ impl XmlRpcServer { } }); - XmlRpcServerHandle { + Ok(XmlRpcServerHandle { port: addr.port(), _handle: handle.into(), - } + }) } // Our actual service handler with our error type From cb10c20c6f32612d4d73c9900e24c1bd007d03a9 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 15:48:13 -0600 Subject: [PATCH 15/34] Disable spurious test case --- roslibrust/src/rosbridge/integration_tests.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/roslibrust/src/rosbridge/integration_tests.rs b/roslibrust/src/rosbridge/integration_tests.rs index 4cca0423..6503319e 100644 --- a/roslibrust/src/rosbridge/integration_tests.rs +++ b/roslibrust/src/rosbridge/integration_tests.rs @@ -135,9 +135,13 @@ mod integration_tests { // Intentionally a port where there won't be a server at let opts = ClientHandleOptions::new("ws://localhost:9091").timeout(TIMEOUT); assert!(ClientHandle::new_with_options(opts).await.is_err()); + + // This case sometimes actually passed in CI and caused test to fail + // Removed as flaky, but left here for posterity. // Impossibly short to actually work - let opts = ClientHandleOptions::new(LOCAL_WS).timeout(Duration::from_nanos(1)); - assert!(ClientHandle::new_with_options(opts).await.is_err()); + // let opts = ClientHandleOptions::new(LOCAL_WS).timeout(Duration::from_nanos(1)); + // assert!(ClientHandle::new_with_options(opts).await.is_err()); + // Doesn't timeout if given enough time let opts = ClientHandleOptions::new(LOCAL_WS).timeout(TIMEOUT); assert!(ClientHandle::new_with_options(opts).await.is_ok()); From 1f73209e9c4e2e1699a2c254a78b310b9219f300 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 16:00:10 -0600 Subject: [PATCH 16/34] Adding back in multi-thread executor --- roslibrust/tests/ros1_xmlrpc.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 8b55a9a3..2729e173 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -47,7 +47,7 @@ mod tests { } } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_get_master_uri() { test_with_watch_dog(async { @@ -79,7 +79,7 @@ mod tests { .await; } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_get_publications() { test_with_watch_dog(async { @@ -138,7 +138,7 @@ mod tests { .await; } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_shutdown() { test_with_watch_dog(async { @@ -171,7 +171,7 @@ mod tests { .await; } - #[test_log::test(tokio::test)] + #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] #[ignore] async fn verify_request_topic() { test_with_watch_dog(async { From eb0e4c77e72dbafc8ebeaab009063f0c2d4b5ef9 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 9 Oct 2023 16:14:35 -0600 Subject: [PATCH 17/34] Add delay to ensure shutdown completes --- roslibrust/tests/ros1_xmlrpc.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 2729e173..ed705eb3 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -166,6 +166,8 @@ mod tests { .await; log::info!("shutdown?"); + tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; + assert!(!node.is_ok()); }) .await; From 8b43d2e5cd7fbbb76050509a98656153ac87146b Mon Sep 17 00:00:00 2001 From: Carter Date: Tue, 10 Oct 2023 19:58:11 -0600 Subject: [PATCH 18/34] FOUND IT BITCHES :poop: --- roslibrust/src/ros1/node.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 3ab03b32..604ca93c 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -275,7 +275,8 @@ impl Node { node_name: node_name.to_owned(), }; - tokio::spawn(async move { + // MAJOR TODO THIS TASK MUST GET STOPPED + let t: abort_on_drop::ChildTask<_> = tokio::spawn(async move { loop { match node.node_msg_rx.recv().await { Some(NodeMsg::Shutdown) => { From 55a7df75d275e5f9f9135001084b825192ddbe17 Mon Sep 17 00:00:00 2001 From: Carter Date: Sun, 22 Oct 2023 13:20:10 -0600 Subject: [PATCH 19/34] Commiting in-flight work to transfer between computers --- Cargo.lock | 1 + roslibrust/Cargo.toml | 1 + roslibrust/src/ros1/node.rs | 42 ++++++++++++------ roslibrust/src/ros1/xmlrpc_server.rs | 2 +- roslibrust/tests/ros1_xmlrpc.rs | 65 +++++++++++++--------------- 5 files changed, 61 insertions(+), 50 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 83e7cd47..c8914d93 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1252,6 +1252,7 @@ dependencies = [ "serde_json", "serde_rosmsg", "serde_xmlrpc", + "simple-error", "simple_logger", "smart-default 0.6.0", "test-log", diff --git a/roslibrust/Cargo.toml b/roslibrust/Cargo.toml index c531dc80..7448d340 100644 --- a/roslibrust/Cargo.toml +++ b/roslibrust/Cargo.toml @@ -50,6 +50,7 @@ regex = { version = "1.9", optional = true } # Only used with native ros1 env_logger = "0.10" test-log = "0.2" simple_logger = "2.1.0" +simple-error = "0.3" [features] default = [] diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 604ca93c..af289401 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -7,9 +7,10 @@ use super::{ subscriber::{Subscriber, Subscription}, }; use crate::{MasterClient, RosMasterError, ServiceCallback, XmlRpcServer, XmlRpcServerHandle}; +use abort_on_drop::ChildTask; use dashmap::DashMap; use roslibrust_codegen::RosMessageType; -use std::net::{IpAddr, Ipv4Addr, ToSocketAddrs}; +use std::{net::{IpAddr, Ipv4Addr, ToSocketAddrs}, sync::Arc}; use tokio::sync::{broadcast, mpsc, oneshot}; #[derive(Debug)] @@ -65,6 +66,10 @@ pub enum NodeMsg { #[derive(Clone)] pub(crate) struct NodeServerHandle { node_server_sender: mpsc::UnboundedSender, + // If this handle should keep the underlying node task alive it will hold an + // Arc to the underlying node task. This is an option because internal handles + // within the node shouldn't keep it alive (e.g. what we hand to xml server) + _node_task: Option>>, } impl NodeServerHandle { @@ -80,7 +85,7 @@ impl NodeServerHandle { } } - pub async fn get_client_uri(&self) -> Result> { + pub async fn get_client_uri(&self) -> Result> { let (sender, receiver) = oneshot::channel(); match self .node_server_sender @@ -111,12 +116,15 @@ impl NodeServerHandle { pub async fn get_publications( &self, ) -> Result, Box> { + log::debug!("get_publications reached"); let (sender, receiver) = oneshot::channel(); match self .node_server_sender .send(NodeMsg::GetPublications { reply: sender }) { - Ok(()) => Ok(receiver.await.map_err(|err| Box::new(err))?), + Ok(()) => { + log::debug!("Awaiting receiver"); + Ok(receiver.await.map_err(|err| Box::new(err))?)}, Err(e) => Err(Box::new(e)), } } @@ -146,7 +154,7 @@ impl NodeServerHandle { topic: &str, topic_type: &str, queue_size: usize, - ) -> Result>, Box> { + ) -> Result>, Box> { let (sender, receiver) = oneshot::channel(); match self.node_server_sender.send(NodeMsg::RegisterPublisher { reply: sender, @@ -246,13 +254,15 @@ impl Node { hostname: &str, node_name: &str, addr: Ipv4Addr, - ) -> Result> { + ) -> Result> { let (node_sender, node_receiver) = mpsc::unbounded_channel(); - let node_server_handle = NodeServerHandle { - node_server_sender: node_sender, + let xml_server_handle = NodeServerHandle { + node_server_sender: node_sender.clone(), + // None here because this handle should not keep task alive + _node_task: None, }; // Create our xmlrpc server and bind our socket so we know our port and can determine our local URI - let xmlrpc_server = XmlRpcServer::new(addr, node_server_handle.clone())?; + let xmlrpc_server = XmlRpcServer::new(addr, xml_server_handle)?; let client_uri = format!("http://{hostname}:{}", xmlrpc_server.port()); if let None = Name::new(node_name) { @@ -275,8 +285,7 @@ impl Node { node_name: node_name.to_owned(), }; - // MAJOR TODO THIS TASK MUST GET STOPPED - let t: abort_on_drop::ChildTask<_> = tokio::spawn(async move { + let t = Arc::new(tokio::spawn(async move { loop { match node.node_msg_rx.recv().await { Some(NodeMsg::Shutdown) => { @@ -291,8 +300,12 @@ impl Node { } } } - }); + }).into()); + let node_server_handle = NodeServerHandle { + node_server_sender: node_sender, + _node_task: Some(t), + }; Ok(node_server_handle) } @@ -512,7 +525,7 @@ impl NodeHandle { pub async fn new( master_uri: &str, name: &str, - ) -> Result> { + ) -> Result> { // Follow ROS rules and determine our IP and hostname let (addr, hostname) = determine_addr().await?; @@ -528,7 +541,7 @@ impl NodeHandle { !self.inner.node_server_sender.is_closed() } - pub async fn get_client_uri(&self) -> Result> { + pub async fn get_client_uri(&self) -> Result> { self.inner.get_client_uri().await } @@ -536,7 +549,8 @@ impl NodeHandle { &self, topic_name: &str, queue_size: usize, - ) -> Result, Box> { + ) -> Result, Box> { + log::trace!("Advertising "); let sender = self .inner .register_publisher::(topic_name, T::ROS_TYPE_NAME, queue_size) diff --git a/roslibrust/src/ros1/xmlrpc_server.rs b/roslibrust/src/ros1/xmlrpc_server.rs index 7d60c710..ea3a3701 100644 --- a/roslibrust/src/ros1/xmlrpc_server.rs +++ b/roslibrust/src/ros1/xmlrpc_server.rs @@ -47,7 +47,7 @@ impl XmlRpcServer { pub fn new( host_addr: Ipv4Addr, node_server: NodeServerHandle, - ) -> Result> { + ) -> Result> { let make_svc = hyper::service::make_service_fn(move |connection| { debug!("New node xmlrpc connection {connection:?}"); let node_server = node_server.clone(); diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index ed705eb3..137402a5 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -19,8 +19,10 @@ mod tests { } async fn call_node_api(uri: &str, endpoint: &str, args: Vec) -> T { + log::debug!("Start call api: {uri:?}, {endpoint:?}, {args:?}"); // Note: don't need timeout here as all operations in side call_node_api_raw are timeout()'d let response = call_node_api_raw(uri, endpoint, args).await; + log::debug!("Got api raw response"); let (error_code, error_description, value): (i8, String, T) = serde_xmlrpc::response_from_str(&response).unwrap(); @@ -29,7 +31,9 @@ mod tests { value } - async fn test_with_watch_dog(test: impl std::future::Future) { + async fn test_with_watch_dog( + test: impl std::future::Future>>, + ) -> Result<(), Box> { // Overall watchdog since I can't get these tests to timeout, but they take 20min in CI // Need the task to fail so I can get any debug information out // Can't get the failure to repro locally @@ -39,17 +43,16 @@ mod tests { }; tokio::select! { _ = watchdog => { - panic!("Test failed due to watchdog"); + simple_error::bail!("Bark!"); } _ = test => { - // Happy days! + Ok(()) } } } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] - #[ignore] - async fn verify_get_master_uri() { + #[test_log::test(tokio::test)] + async fn verify_get_master_uri() -> Result<(), Box> { test_with_watch_dog(async { let node = timeout( TIMEOUT, @@ -75,27 +78,23 @@ mod tests { .await; log::info!("Got master"); assert_eq!(master_uri, "http://localhost:11311"); + Ok(()) }) - .await; + .await } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] - #[ignore] - async fn verify_get_publications() { + #[tokio::test(flavor = "multi_thread")] + async fn verify_get_publications() -> Result<(), Box> { + // let _ = simple_logger::init_with_level(log::Level::Trace); test_with_watch_dog(async { let node = timeout( TIMEOUT, roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications"), ) - .await - .unwrap() - .unwrap(); + .await??; log::info!("Got new handle"); - let node_uri = timeout(TIMEOUT, node.get_client_uri()) - .await - .unwrap() - .unwrap(); + let node_uri = timeout(TIMEOUT, node.get_client_uri()).await??; log::info!("Got uri"); // Note: timeout not needed as it is internally timeout'd @@ -112,9 +111,7 @@ mod tests { TIMEOUT, node.advertise::("/test_topic", 1), ) - .await - .unwrap() - .unwrap(); + .await??; log::info!("advertised"); // Note: internally timeout()'d @@ -126,34 +123,31 @@ mod tests { vec!["/verify_get_publications".into()], ), ) - .await - .unwrap(); + .await?; log::info!("Got post advertise publications"); assert_eq!(publications.len(), 1); - let (topic, topic_type) = publications.iter().nth(0).unwrap(); + let (topic, topic_type) = publications + .iter() + .nth(0) + .ok_or(simple_error::SimpleError::new("wtf"))?; assert_eq!(topic, "/test_topic"); assert_eq!(topic_type, std_msgs::String::ROS_TYPE_NAME); + Ok(()) }) - .await; + .await } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] - #[ignore] + #[test_log::test(tokio::test)] async fn verify_shutdown() { test_with_watch_dog(async { let node = timeout( TIMEOUT, roslibrust::NodeHandle::new("http://localhost:11311", "verify_shutdown"), ) - .await - .unwrap() - .unwrap(); + .await??; log::info!("Got handle"); - let node_uri = timeout(TIMEOUT, node.get_client_uri()) - .await - .unwrap() - .unwrap(); + let node_uri = timeout(TIMEOUT, node.get_client_uri()).await??; assert!(node.is_ok()); log::info!("Got uri"); @@ -169,12 +163,12 @@ mod tests { tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; assert!(!node.is_ok()); + Ok(()) }) .await; } - #[test_log::test(tokio::test(flavor = "multi_thread", worker_threads = 2))] - #[ignore] + #[test_log::test(tokio::test)] async fn verify_request_topic() { test_with_watch_dog(async { let node = timeout( @@ -221,6 +215,7 @@ mod tests { assert_eq!(protocol, "TCPROS"); assert!(!host.is_empty()); assert!(port != 0); + Ok(()) }) .await; } From 40a701590a0aeb563edd0acec247f3ace4072f5d Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:02:24 -0600 Subject: [PATCH 20/34] Adding yet more debug --- roslibrust/examples/native_ros1.rs | 2 +- roslibrust/examples/ros1_listener.rs | 2 +- roslibrust/examples/ros1_talker.rs | 2 +- roslibrust/src/ros1/master_client.rs | 1 + roslibrust/src/ros1/node.rs | 71 +++++++++++++++++----------- roslibrust/src/ros1/publisher.rs | 8 +++- roslibrust/tests/ros1_xmlrpc.rs | 3 +- 7 files changed, 54 insertions(+), 35 deletions(-) diff --git a/roslibrust/examples/native_ros1.rs b/roslibrust/examples/native_ros1.rs index 3f454677..ddceef8b 100644 --- a/roslibrust/examples/native_ros1.rs +++ b/roslibrust/examples/native_ros1.rs @@ -4,7 +4,7 @@ #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), Box> { +async fn main() -> Result<(), Box> { use roslibrust::NodeHandle; simple_logger::SimpleLogger::new() diff --git a/roslibrust/examples/ros1_listener.rs b/roslibrust/examples/ros1_listener.rs index c0983a40..3d94b3e5 100644 --- a/roslibrust/examples/ros1_listener.rs +++ b/roslibrust/examples/ros1_listener.rs @@ -2,7 +2,7 @@ roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_in #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), Box> { +async fn main() -> Result<(), Box> { use roslibrust::NodeHandle; simple_logger::SimpleLogger::new() diff --git a/roslibrust/examples/ros1_talker.rs b/roslibrust/examples/ros1_talker.rs index 2671b068..3d40d479 100644 --- a/roslibrust/examples/ros1_talker.rs +++ b/roslibrust/examples/ros1_talker.rs @@ -2,7 +2,7 @@ roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_in #[cfg(feature = "ros1")] #[tokio::main] -async fn main() -> Result<(), Box> { +async fn main() -> Result<(), Box> { use roslibrust::NodeHandle; simple_logger::SimpleLogger::new() diff --git a/roslibrust/src/ros1/master_client.rs b/roslibrust/src/ros1/master_client.rs index 03de74d0..4ceaa819 100644 --- a/roslibrust/src/ros1/master_client.rs +++ b/roslibrust/src/ros1/master_client.rs @@ -254,6 +254,7 @@ impl MasterClient { self.client_uri.clone().into(), ], )?; + log::trace!("Posting {body:?} to register publisher"); self.post(body).await } diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index af289401..8c8cac5d 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -10,7 +10,10 @@ use crate::{MasterClient, RosMasterError, ServiceCallback, XmlRpcServer, XmlRpcS use abort_on_drop::ChildTask; use dashmap::DashMap; use roslibrust_codegen::RosMessageType; -use std::{net::{IpAddr, Ipv4Addr, ToSocketAddrs}, sync::Arc}; +use std::{ + net::{IpAddr, Ipv4Addr, ToSocketAddrs}, + sync::Arc, +}; use tokio::sync::{broadcast, mpsc, oneshot}; #[derive(Debug)] @@ -124,7 +127,8 @@ impl NodeServerHandle { { Ok(()) => { log::debug!("Awaiting receiver"); - Ok(receiver.await.map_err(|err| Box::new(err))?)}, + Ok(receiver.await.map_err(|err| Box::new(err))?) + } Err(e) => Err(Box::new(e)), } } @@ -165,7 +169,9 @@ impl NodeServerHandle { md5sum: T::MD5SUM.to_owned(), }) { Ok(()) => { + log::trace!("Recieved okay from node_server_send."); let received = receiver.await.map_err(|err| Box::new(err))?; + log::trace!("Awaiting receiver produced: {received:?}"); Ok(received.map_err(|err| { log::error!("Failed to register publisher: {err}"); Box::new(std::io::Error::from(std::io::ErrorKind::ConnectionAborted)) @@ -179,7 +185,7 @@ impl NodeServerHandle { &self, topic: &str, queue_size: usize, - ) -> Result>, Box> { + ) -> Result>, Box> { let (sender, receiver) = oneshot::channel(); match self.node_server_sender.send(NodeMsg::RegisterSubscriber { reply: sender, @@ -259,7 +265,7 @@ impl Node { let xml_server_handle = NodeServerHandle { node_server_sender: node_sender.clone(), // None here because this handle should not keep task alive - _node_task: None, + _node_task: None, }; // Create our xmlrpc server and bind our socket so we know our port and can determine our local URI let xmlrpc_server = XmlRpcServer::new(addr, xml_server_handle)?; @@ -285,22 +291,25 @@ impl Node { node_name: node_name.to_owned(), }; - let t = Arc::new(tokio::spawn(async move { - loop { - match node.node_msg_rx.recv().await { - Some(NodeMsg::Shutdown) => { - log::info!("Shutdown requested, shutting down node"); - break; - } - Some(node_msg) => { - node.handle_msg(node_msg).await; - } - None => { - break; + let t = Arc::new( + tokio::spawn(async move { + loop { + match node.node_msg_rx.recv().await { + Some(NodeMsg::Shutdown) => { + log::info!("Shutdown requested, shutting down node"); + break; + } + Some(node_msg) => { + node.handle_msg(node_msg).await; + } + None => { + break; + } } } - } - }).into()); + }) + .into(), + ); let node_server_handle = NodeServerHandle { node_server_sender: node_sender, @@ -356,10 +365,12 @@ impl Node { msg_definition, md5sum, } => { - match self + log::trace!("Got register publisher command from mpsc for {topic:?}"); + let res = self .register_publisher(topic, &topic_type, queue_size, msg_definition, md5sum) - .await - { + .await; + log::trace!("Result of register_publisher: {res:?}"); + match res { Ok(handle) => reply.send(Ok(handle)), Err(err) => reply.send(Err(err.to_string())), } @@ -471,7 +482,9 @@ impl Node { msg_definition: String, md5sum: String, ) -> Result>, Box> { + log::trace!("Registering publisher for: {topic:?}"); if let Some(handle) = self.publishers.iter().find_map(|entry| { + log::trace!("Found existing entry for: {topic:?}"); if entry.key().as_str() == &topic { if entry.topic_type() == topic_type { Some(Ok(entry.get_sender())) @@ -486,6 +499,7 @@ impl Node { }) { Ok(handle?) } else { + log::trace!("Creating new entry for {topic:?}"); let channel = Publication::new( &self.node_name, false, @@ -501,10 +515,12 @@ impl Node { log::error!("Failed to create publishing channel: {err:?}"); err })?; + log::trace!("Created new publication for {topic:?}"); let handle = channel.get_sender(); self.publishers.insert(topic.clone(), channel); - - let _current_subscribers = self.client.register_publisher(topic, topic_type).await?; + log::trace!("Inserted new publsiher into dashmap"); + let _current_subscribers = self.client.register_publisher(&topic, topic_type).await?; + log::trace!("Registered new publication for {topic:?}"); Ok(handle) } } @@ -518,7 +534,7 @@ pub struct NodeHandle { } impl NodeHandle { - // TODO builder, async, result, better error type + // TODO builder, result, better error type /// Creates a new node connect and returns a handle to it /// It is idiomatic to call this once per process and treat the created node as singleton. /// The returned handle can be freely clone'd to create additional handles without creating additional connections. @@ -532,8 +548,6 @@ impl NodeHandle { let node = Node::new(master_uri, &hostname, name, addr).await?; let nh = NodeHandle { inner: node }; - // TODO spawn our TcpManager here for TCPROS - Ok(nh) } @@ -550,11 +564,12 @@ impl NodeHandle { topic_name: &str, queue_size: usize, ) -> Result, Box> { - log::trace!("Advertising "); + log::trace!("Advertising: {topic_name:?}"); let sender = self .inner .register_publisher::(topic_name, T::ROS_TYPE_NAME, queue_size) .await?; + log::trace!("Advertised: {topic_name:?}"); Ok(Publisher::new(topic_name, sender)) } @@ -562,7 +577,7 @@ impl NodeHandle { &self, topic_name: &str, queue_size: usize, - ) -> Result, Box> { + ) -> Result, Box> { let receiver = self .inner .register_subscriber::(topic_name, queue_size) diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index 0a630104..bc1db0c6 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -1,3 +1,5 @@ +use crate::RosLibRustError; + use super::tcpros::ConnectionHeader; use abort_on_drop::ChildTask; use roslibrust_codegen::RosMessageType; @@ -26,8 +28,10 @@ impl Publisher { } } - pub async fn publish(&self, data: &T) -> Result<(), Box> { - let data = serde_rosmsg::to_vec(&data)?; + pub async fn publish(&self, data: &T) -> Result<(), Box> { + let data = serde_rosmsg::to_vec(&data) + // Gotta do some funny error mapping here as serde_rosmsg's error type is not sync + .map_err(|e| RosLibRustError::Unexpected(anyhow::anyhow!("{e:?}")))?; self.sender.send(data).await?; log::debug!("Publishing data on topic {}", self.topic_name); Ok(()) diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 137402a5..0a8e01f1 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -83,9 +83,8 @@ mod tests { .await } - #[tokio::test(flavor = "multi_thread")] + #[test_log::test(tokio::test(flavor = "multi_thread"))] async fn verify_get_publications() -> Result<(), Box> { - // let _ = simple_logger::init_with_level(log::Level::Trace); test_with_watch_dog(async { let node = timeout( TIMEOUT, From 48fdc3af65bd6960bba63ebe29620ea8636e294e Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:19:03 -0600 Subject: [PATCH 21/34] Seemed to actually have fixed the error (at least for one test) by switching away from dashmap --- roslibrust/src/ros1/node.rs | 49 +++++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 18 deletions(-) diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 8c8cac5d..ce6bd59e 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -243,7 +243,7 @@ pub struct Node { // Receiver for requests to the Node actor node_msg_rx: mpsc::UnboundedReceiver, // Map of topic names to the publishing channels associated with the topic - publishers: DashMap, + publishers: tokio::sync::RwLock>, // Record of subscriptions this node has subscriptions: DashMap, // Record of what services this node is serving @@ -283,7 +283,7 @@ impl Node { client: rosmaster_client, _xmlrpc_server: xmlrpc_server, node_msg_rx: node_receiver, - publishers: DashMap::new(), + publishers: tokio::sync::RwLock::new(std::collections::HashMap::new()), subscriptions: DashMap::new(), services: DashMap::new(), host_addr: addr, @@ -337,8 +337,10 @@ impl Node { NodeMsg::GetPublications { reply } => { let _ = reply.send( self.publishers + .read() + .await .iter() - .map(|entry| (entry.key().clone(), entry.topic_type().to_owned())) + .map(|(key, entry)| (key.clone(), entry.topic_type().to_owned())) .collect(), ); } @@ -408,10 +410,12 @@ impl Node { .find(|proto| proto.as_str() == "TCPROS") .is_some() { - if let Some(publishing_channel) = self + if let Some((_key, publishing_channel)) = self .publishers + .read() + .await .iter() - .find(|publisher| publisher.key() == &topic) + .find(|(key, _pub)| *key == &topic) { let protocol_params = ProtocolParams { hostname: self.hostname.clone(), @@ -483,20 +487,26 @@ impl Node { md5sum: String, ) -> Result>, Box> { log::trace!("Registering publisher for: {topic:?}"); - if let Some(handle) = self.publishers.iter().find_map(|entry| { - log::trace!("Found existing entry for: {topic:?}"); - if entry.key().as_str() == &topic { - if entry.topic_type() == topic_type { - Some(Ok(entry.get_sender())) + + let existing_entry = { + let read_lock = self.publishers.read().await; + read_lock.iter().find_map(|(key, value)| { + log::trace!("Found existing entry for: {topic:?}"); + if key.as_str() == &topic { + if value.topic_type() == topic_type { + Some(Ok(value.get_sender())) + } else { + Some(Err(Box::new(std::io::Error::from( + std::io::ErrorKind::AddrInUse, + )))) + } } else { - Some(Err(Box::new(std::io::Error::from( - std::io::ErrorKind::AddrInUse, - )))) + None } - } else { - None - } - }) { + }) + }; + + if let Some(handle) = existing_entry { Ok(handle?) } else { log::trace!("Creating new entry for {topic:?}"); @@ -517,7 +527,10 @@ impl Node { })?; log::trace!("Created new publication for {topic:?}"); let handle = channel.get_sender(); - self.publishers.insert(topic.clone(), channel); + { + let mut write_lock = self.publishers.write().await; + write_lock.insert(topic.clone(), channel); + } log::trace!("Inserted new publsiher into dashmap"); let _current_subscribers = self.client.register_publisher(&topic, topic_type).await?; log::trace!("Registered new publication for {topic:?}"); From 99d9e78be8ead05ec4eeef56b87e6285a4df1f33 Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:27:27 -0600 Subject: [PATCH 22/34] Remove unneeded dashmaps --- roslibrust/src/ros1/node.rs | 46 ++++++++++++++----------------------- 1 file changed, 17 insertions(+), 29 deletions(-) diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index ce6bd59e..c069267d 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -8,10 +8,10 @@ use super::{ }; use crate::{MasterClient, RosMasterError, ServiceCallback, XmlRpcServer, XmlRpcServerHandle}; use abort_on_drop::ChildTask; -use dashmap::DashMap; use roslibrust_codegen::RosMessageType; use std::{ - net::{IpAddr, Ipv4Addr, ToSocketAddrs}, + collections::HashMap, + net::{IpAddr, Ipv4Addr}, sync::Arc, }; use tokio::sync::{broadcast, mpsc, oneshot}; @@ -243,11 +243,11 @@ pub struct Node { // Receiver for requests to the Node actor node_msg_rx: mpsc::UnboundedReceiver, // Map of topic names to the publishing channels associated with the topic - publishers: tokio::sync::RwLock>, + publishers: HashMap, // Record of subscriptions this node has - subscriptions: DashMap, + subscriptions: HashMap, // Record of what services this node is serving - services: DashMap, + services: HashMap, // TODO need signal to shutdown xmlrpc server when node is dropped host_addr: Ipv4Addr, hostname: String, @@ -283,9 +283,9 @@ impl Node { client: rosmaster_client, _xmlrpc_server: xmlrpc_server, node_msg_rx: node_receiver, - publishers: tokio::sync::RwLock::new(std::collections::HashMap::new()), - subscriptions: DashMap::new(), - services: DashMap::new(), + publishers: std::collections::HashMap::new(), + subscriptions: std::collections::HashMap::new(), + services: std::collections::HashMap::new(), host_addr: addr, hostname: hostname.to_owned(), node_name: node_name.to_owned(), @@ -330,15 +330,15 @@ impl Node { let _ = reply.send( self.subscriptions .iter() - .map(|entry| (entry.key().clone(), entry.topic_type().to_owned())) + .map(|(topic_name, subscription)| { + (topic_name.clone(), subscription.topic_type().to_owned()) + }) .collect(), ); } NodeMsg::GetPublications { reply } => { let _ = reply.send( self.publishers - .read() - .await .iter() .map(|(key, entry)| (key.clone(), entry.topic_type().to_owned())) .collect(), @@ -410,12 +410,8 @@ impl Node { .find(|proto| proto.as_str() == "TCPROS") .is_some() { - if let Some((_key, publishing_channel)) = self - .publishers - .read() - .await - .iter() - .find(|(key, _pub)| *key == &topic) + if let Some((_key, publishing_channel)) = + self.publishers.iter().find(|(key, _pub)| *key == &topic) { let protocol_params = ProtocolParams { hostname: self.hostname.clone(), @@ -450,12 +446,8 @@ impl Node { msg_definition: &str, md5sum: &str, ) -> Result>, Box> { - match self - .subscriptions - .iter() - .find(|subscription| subscription.key() == topic) - { - Some(subscription) => Ok(subscription.get_receiver()), + match self.subscriptions.iter().find(|(key, _)| *key == topic) { + Some((_topic, subscription)) => Ok(subscription.get_receiver()), None => { let mut subscription = Subscription::new( &self.node_name, @@ -489,8 +481,7 @@ impl Node { log::trace!("Registering publisher for: {topic:?}"); let existing_entry = { - let read_lock = self.publishers.read().await; - read_lock.iter().find_map(|(key, value)| { + self.publishers.iter().find_map(|(key, value)| { log::trace!("Found existing entry for: {topic:?}"); if key.as_str() == &topic { if value.topic_type() == topic_type { @@ -527,10 +518,7 @@ impl Node { })?; log::trace!("Created new publication for {topic:?}"); let handle = channel.get_sender(); - { - let mut write_lock = self.publishers.write().await; - write_lock.insert(topic.clone(), channel); - } + self.publishers.insert(topic.clone(), channel); log::trace!("Inserted new publsiher into dashmap"); let _current_subscribers = self.client.register_publisher(&topic, topic_type).await?; log::trace!("Registered new publication for {topic:?}"); From 75354b2162f3cb81c1f722759144ac9350d707ec Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:40:19 -0600 Subject: [PATCH 23/34] Undoing a lot of the sins committed while debugging --- .github/workflows/noetic.yml | 4 +- roslibrust/src/lib.rs | 7 +- roslibrust/src/ros1/node.rs | 2 +- roslibrust/src/rosapi/mod.rs | 544 ++++++++++++++++++ roslibrust/src/rosbridge/integration_tests.rs | 112 ++-- roslibrust/tests/ros1_xmlrpc.rs | 269 ++++----- 6 files changed, 707 insertions(+), 231 deletions(-) create mode 100644 roslibrust/src/rosapi/mod.rs diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 0d8e9147..7ac7c833 100644 --- a/.github/workflows/noetic.yml +++ b/.github/workflows/noetic.yml @@ -39,6 +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 /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge -- --test-threads 1 - - name: Ros1 Evil Integration Tests # Added because these specific tests were flaky in CI and trying to get more info - run: source /root/.cargo/env; cargo test --features ros1_test,ros1 -- --ignored \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge -- --test-threads 1 \ No newline at end of file diff --git a/roslibrust/src/lib.rs b/roslibrust/src/lib.rs index 6973ad7e..8932461d 100644 --- a/roslibrust/src/lib.rs +++ b/roslibrust/src/lib.rs @@ -33,12 +33,12 @@ //! //! ## Message Generation //! Message generation is provided in two APIs. The first, which is visible in `roslibrust/examples`, is a proc-macro which can be invoked to generate ROS message structs in place: -//! ```compile_fail +//! ```ignore //! use roslibrust_codegen_macro::find_and_generate_ros_messages; //! find_and_generate_ros_messages!(); //! ``` //! If you have ROS installed, this macro will search for message files under paths in the `ROS_PACKAGE_PATH`. If you do not have ROS installed in your environment, you can specify search paths explicitly: -//! ```compile_fail +//! ```ignore //! use roslibrust_codegen_macro::find_and_generate_ros_messages; //! find_and_generate_ros_messages!("/path/to/noetic/packages", "/path/to/my/packages"); //! ``` @@ -101,6 +101,9 @@ mod rosbridge; pub use rosbridge::*; +#[cfg(feature = "rosapi")] +pub mod rosapi; + #[cfg(feature = "ros1")] mod ros1; #[cfg(feature = "ros1")] diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index c069267d..2e212d75 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -345,7 +345,7 @@ impl Node { ); } NodeMsg::SetPeerPublishers { topic, publishers } => { - if let Some(mut subscription) = self.subscriptions.get_mut(&topic) { + if let Some(subscription) = self.subscriptions.get_mut(&topic) { for publisher_uri in publishers { if let Err(err) = subscription.add_publisher_source(&publisher_uri).await { log::error!( diff --git a/roslibrust/src/rosapi/mod.rs b/roslibrust/src/rosapi/mod.rs new file mode 100644 index 00000000..8e198208 --- /dev/null +++ b/roslibrust/src/rosapi/mod.rs @@ -0,0 +1,544 @@ +//! This module is intended to provide 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 async_trait::async_trait; + +// TODO Fix this... this sucks +roslibrust_codegen_macro::find_and_generate_ros_messages_relative_to_manifest_dir!( + "../assets/ros1_common_interfaces/rosapi" +); + +/// 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. +#[async_trait] +trait RosApi { + async fn get_time(&self) -> RosLibRustResult; + async fn topics(&self) -> RosLibRustResult; + async fn get_topic_type( + &self, + topic: impl Into + Send, + ) -> RosLibRustResult; + async fn get_topics_for_type( + &self, + topic_type: impl Into + Send, + ) -> RosLibRustResult; + async fn get_nodes(&self) -> RosLibRustResult; + + async fn get_node_details( + &self, + node: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_node_for_service( + &self, + service: impl Into + Send, + ) -> RosLibRustResult; + + async fn set_param( + &self, + param_name: impl Into + Send, + param_value: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_param( + &self, + param_name: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_param_names(&self) -> RosLibRustResult; + + async fn has_param( + &self, + param: impl Into + Send, + ) -> RosLibRustResult; + + async fn delete_param( + &self, + name: impl Into + Send, + ) -> RosLibRustResult; + + async fn message_details( + &self, + message_name: impl Into + Send, + ) -> RosLibRustResult; + + async fn publishers( + &self, + topic: impl Into + Send, + ) -> RosLibRustResult; + + async fn service_host( + &self, + service: impl Into + Send, + ) -> RosLibRustResult; + + async fn service_providers( + &self, + service_type: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_service_request_details( + &self, + service_type: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_service_response_details( + &self, + service_type: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_service_type( + &self, + service_name: impl Into + Send, + ) -> RosLibRustResult; + + async fn get_services(&self) -> RosLibRustResult; +} + +#[async_trait] +impl RosApi for ClientHandle { + /// Get the current time + async fn get_time(&self) -> RosLibRustResult { + self.call_service("/rosapi/get_time", rosapi::GetTimeRequest {}) + .await + } + + /// Get the list of topics active + async fn topics(&self) -> RosLibRustResult { + self.call_service("/rosapi/topics", rosapi::TopicsRequest {}) + .await + } + + /// Get the type of a given topic + async fn get_topic_type( + &self, + topic: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/topic_type", + rosapi::TopicTypeRequest { + topic: topic.into(), + }, + ) + .await + } + + /// Returns a list of the topics active in the system that are of the type provided + async fn get_topics_for_type( + &self, + topic_type: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/topics_for_type", + rosapi::TopicsForTypeRequest { + r#type: topic_type.into(), + }, + ) + .await + } + + /// Returns list of nodes active in a system + async fn get_nodes(&self) -> RosLibRustResult { + self.call_service("/rosapi/nodes", rosapi::NodesRequest {}) + .await + } + + /// Returns the subscriptions, publishers, and service servers for a given node + /// @param node Fully resolved ros node name e.g. "/rosapi", should match the names given by `rosnode list` + async fn get_node_details( + &self, + node: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/node_details", + rosapi::NodeDetailsRequest { node: node.into() }, + ) + .await + } + + /// Given the name of service, get the name of the node that is providing that service + async fn get_node_for_service( + &self, + service: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/service_node", + rosapi::ServiceNodeRequest { + service: service.into(), + }, + ) + .await + } + + /// Sets a parameter. Unclear exactly how 'types' of parameters will be handled here. + /// For now we're simply converting into a string. + async fn set_param( + &self, + param_name: impl Into + Send, + param_value: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/set_param", + rosapi::SetParamRequest { + name: param_name.into(), + value: param_value.into(), + }, + ) + .await + } + + /// Gets the current value for a parameter. + /// Not handling any type safety or conversion. + async fn get_param( + &self, + param_name: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/get_param", + rosapi::GetParamRequest { + name: param_name.into(), + default: "".to_string(), + }, + ) + .await + } + + /// Gets the list of currently known parameters. + async fn get_param_names(&self) -> RosLibRustResult { + self.call_service("/rosapi/get_param_names", rosapi::GetParamNamesRequest {}) + .await + } + + /// Checks whether the given parameter is defined. + async fn has_param( + &self, + param: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/has_param", + rosapi::HasParamRequest { name: param.into() }, + ) + .await + } + + /// Deletes a parameter by name. + async fn delete_param( + &self, + name: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/delete_param", + rosapi::DeleteParamRequest { name: name.into() }, + ) + .await + } + + /// Returns detailed information about a given message type e.g. 'std_msgs/Header' + async fn message_details( + &self, + message_name: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/message_details", + rosapi::MessageDetailsRequest { + r#type: message_name.into(), + }, + ) + .await + } + + /// Gets a list of all nodes that are publishing to a given topic. + async fn publishers( + &self, + topic: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/publishers", + rosapi::PublishersRequest { + topic: topic.into(), + }, + ) + .await + } + + /// Give the name of a service, returns the name of the machine on which that service is being hosted + async fn service_host( + &self, + service: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/service_host", + rosapi::ServiceHostRequest { + service: service.into(), + }, + ) + .await + } + + /// Given the type of a service, returns a list of nodes which are providing services with that type + async fn service_providers( + &self, + service_type: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/service_providers", + rosapi::ServiceProvidersRequest { + service: service_type.into(), + }, + ) + .await + } + + /// Given the type of a service (e.g. 'rosapi/SetParam'), returns details about the message format of the request + async fn get_service_request_details( + &self, + service_type: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/service_request_details", + rosapi::ServiceRequestDetailsRequest { + r#type: service_type.into(), + }, + ) + .await + } + + /// Given the type of a service (e.g. 'rosapi/SetParam'), returns details about the message format of the response + async fn get_service_response_details( + &self, + service_type: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/service_response_details", + rosapi::ServiceRequestDetailsRequest { + r#type: service_type.into(), + }, + ) + .await + } + + /// Given the name of a service (e.g. '/rosapi/publishers'), returns the type of the service ('rosapi/Publishers') + async fn get_service_type( + &self, + service_name: impl Into + Send, + ) -> RosLibRustResult { + self.call_service( + "/rosapi/service_type", + rosapi::ServiceTypeRequest { + service: service_name.into(), + }, + ) + .await + } + + /// Get the list of services active on the system + async fn get_services(&self) -> RosLibRustResult { + self.call_service("/rosapi/services", rosapi::ServicesRequest {}) + .await + } + + /* + List of rosapi services pulled from `rosservice list` + /rosapi/action_servers - Probably won't support + /rosapi/delete_param - Done + /rosapi/get_loggers - ?? + /rosapi/get_param - Done + /rosapi/get_param_names - Done + /rosapi/get_time - Done + /rosapi/has_param - Done + /rosapi/message_details - Done + /rosapi/node_details - Done + /rosapi/nodes - Done + /rosapi/publishers - Done + /rosapi/search_param - ?? What does this do and why? + /rosapi/service_host - Done + /rosapi/service_node - Done + /rosapi/service_providers - Done + /rosapi/service_request_details - Done + /rosapi/service_response_details - Done + /rosapi/service_type - Done + /rosapi/services - TODO + /rosapi/services_for_type - Done + /rosapi/set_logger_level - ?? + /rosapi/set_param - Done + /rosapi/subscribers - Done + /rosapi/topic_type - Done + /rosapi/topics - Done + /rosapi/topics_and_raw_types - ?? + /rosapi/topics_for_type - Done + */ +} + +#[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}; + + async fn fixture_client() -> ClientHandle { + // Tiny sleep to throttle rate at which tests are run to try to make CI more consistent + tokio::time::sleep(std::time::Duration::from_millis(50)).await; + let opts = ClientHandleOptions::new("ws://localhost:9090") + // 200 ms failed CI + .timeout(std::time::Duration::from_millis(500)); + ClientHandle::new_with_options(opts).await.unwrap() + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_time() { + let api = fixture_client().await; + api.get_time().await.unwrap(); + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_topic_type() { + let api = fixture_client().await; + let res = api + .get_topic_type("/rosout") + .await + .expect("Failed to get topic type for rosout"); + assert_eq!(res.r#type, "rosgraph_msgs/Log"); + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_topics_for_type() { + let api = fixture_client().await; + let res = api + .get_topics_for_type("rosgraph_msgs/Log") + .await + .expect("Failed to get topics for type Log"); + assert!(res.topics.iter().any(|f| f == "/rosout")); + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_nodes() { + let api = fixture_client().await; + let nodes = api.get_nodes().await.expect("Failed to get nodes"); + assert!(nodes.nodes.iter().any(|f| f == "/rosapi")); + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_node_details() { + let api = fixture_client().await; + assert!( + api.get_node_details("/rosapi") + .await + .expect("Failed to get node details for rosapi") + .services + .len() + > 0 + ); + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_node_for_service() { + let api = fixture_client().await; + assert_eq!( + api.get_node_for_service("/rosapi/service_node") + .await + .expect("Failed to call service_node") + .node, + "/rosapi" + ); + } + + #[test_log::test(tokio::test)] + async fn rosapi_param_roundtrip() { + let api = fixture_client().await; + const PARAM_NAME: &'static str = "/rosapi_param_roundtrip"; + // Set the parameter + api.set_param(PARAM_NAME, 1.0.to_string()) + .await + .expect("Failed to set"); + + // read back the value we set + let response = api + .get_param(PARAM_NAME) + .await + .expect("Failed to read param back"); + assert_eq!(1.0, response.value.parse::().unwrap()); + + // Confirm the parameter is in the list of parameters + let param_names = api + .get_param_names() + .await + .expect("Failed to get param names"); + assert!(param_names.names.contains(&PARAM_NAME.to_string())); + + // Confirm has_param sees it + assert!(api.has_param(PARAM_NAME).await.unwrap().exists); + + // Delete it! + api.delete_param(PARAM_NAME).await.unwrap(); + + // Confirm it is gone + assert!(!api.has_param(PARAM_NAME).await.unwrap().exists); + } + + #[test_log::test(tokio::test)] + async fn rosapi_message_details() { + let api = fixture_client().await; + let response = api.message_details("std_msgs/Header").await.unwrap(); + // Spot check we actually got some data back + assert!(response + .typedefs + .iter() + .any(|t| t.fieldtypes.contains(&"time".to_string()))); + } + + #[test_log::test(tokio::test)] + async fn rosapi_publishers() { + let api = fixture_client().await; + let response = api.publishers("/rosout").await.unwrap(); + assert!(response.publishers.iter().any(|p| p == "/rosapi")); + } + + #[test_log::test(tokio::test)] + async fn rosapi_service_providers() { + let api = fixture_client().await; + let response = api.service_providers("rosapi/ServiceHost").await.unwrap(); + assert!(response.providers.iter().any(|p| p == "/rosapi")); + } + + #[test_log::test(tokio::test)] + async fn rosapi_service_request_details() { + let api = fixture_client().await; + let response = api + .get_service_request_details("rosapi/SetParam") + .await + .unwrap(); + // Spot check we got some data back + assert!(response.typedefs[0].fieldnames.len() == 2); + } + + #[test_log::test(tokio::test)] + async fn rosapi_service_response_details() { + let api = fixture_client().await; + let response = api + .get_service_response_details("rosapi/GetParam") + .await + .unwrap(); + // Spot check we got some data back + assert_eq!(response.typedefs[0].fieldnames[0], "value"); + } + + #[test_log::test(tokio::test)] + async fn rosapi_get_service_type() { + let api = fixture_client().await; + let response = api.get_service_type("/rosapi/node_details").await.unwrap(); + assert_eq!(response.r#type, "rosapi/NodeDetails"); + } + + #[test_log::test(tokio::test)] + async fn rosapi_services() { + let api = fixture_client().await; + let response = api.get_services().await.unwrap(); + assert!(!response.services.is_empty()); + } +} diff --git a/roslibrust/src/rosbridge/integration_tests.rs b/roslibrust/src/rosbridge/integration_tests.rs index 6503319e..fd11c38b 100644 --- a/roslibrust/src/rosbridge/integration_tests.rs +++ b/roslibrust/src/rosbridge/integration_tests.rs @@ -172,62 +172,62 @@ mod integration_tests { }); } - // /// Tests that dropping a publisher correctly unadvertises - // #[test_log::test(tokio::test)] - // // This test is currently broken, it seems that rosbridge still sends the message regardless - // // 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) - // // 3. Drop the publisher, unadvertise should be sent - // // 4. Manually send a message without a publisher it should fail since topic was unadvertised - // const TOPIC: &str = "/unadvertise"; - // debug!("Start unadvertise test"); - - // let opt = ClientHandleOptions::new(LOCAL_WS).timeout(TIMEOUT); - - // let client = ClientHandle::new_with_options(opt).await?; - // let publisher = client.advertise(TOPIC).await?; - // debug!("Got publisher"); - - // let sub = client.subscribe::
(TOPIC).await?; - // debug!("Got subscriber"); - - // let msg = Header::default(); - // publisher.publish(msg).await?; - // timeout(TIMEOUT, sub.next()).await?; - - // debug!("Dropping publisher"); - // // drop subscriber so it doesn't keep topic open - // std::mem::drop(sub); - // // unadvertise should happen here - // std::mem::drop(publisher); - - // // Wait for drop to complete - // tokio::time::sleep(TIMEOUT).await; - - // let sub = client.subscribe::
(TOPIC).await?; - // // manually publishing using private api - // let msg = Header::default(); - // client.publish(TOPIC, msg).await?; - - // match timeout(TIMEOUT, sub.next()).await { - // Ok(_msg) => { - // anyhow::bail!("Received message after unadvertised!"); - // } - // Err(_e) => { - // // All good! Timeout should expire - // } - // } - // Ok(()) - // } + /// Tests that dropping a publisher correctly unadvertises + #[test_log::test(tokio::test)] + // This test is currently broken, it seems that rosbridge still sends the message regardless + // 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) + // 3. Drop the publisher, unadvertise should be sent + // 4. Manually send a message without a publisher it should fail since topic was unadvertised + const TOPIC: &str = "/unadvertise"; + debug!("Start unadvertise test"); + + let opt = ClientHandleOptions::new(LOCAL_WS).timeout(TIMEOUT); + + let client = ClientHandle::new_with_options(opt).await?; + let publisher = client.advertise(TOPIC).await?; + debug!("Got publisher"); + + let sub = client.subscribe::
(TOPIC).await?; + debug!("Got subscriber"); + + let msg = Header::default(); + publisher.publish(msg).await?; + timeout(TIMEOUT, sub.next()).await?; + + debug!("Dropping publisher"); + // drop subscriber so it doesn't keep topic open + std::mem::drop(sub); + // unadvertise should happen here + std::mem::drop(publisher); + + // Wait for drop to complete + tokio::time::sleep(TIMEOUT).await; + + let sub = client.subscribe::
(TOPIC).await?; + // manually publishing using private api + let msg = Header::default(); + client.publish(TOPIC, msg).await?; + + match timeout(TIMEOUT, sub.next()).await { + Ok(_msg) => { + anyhow::bail!("Received message after unadvertised!"); + } + Err(_e) => { + // All good! Timeout should expire + } + } + Ok(()) + } // This test currently doesn't work for ROS2, however all other service functionalities appear fine // It may be that ros2 prevents a "service_loop" where a node calls a service on itself? diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 0a8e01f1..ddebf8db 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -11,10 +11,7 @@ mod tests { async fn call_node_api_raw(uri: &str, endpoint: &str, args: Vec) -> String { let client = reqwest::Client::new(); let body = serde_xmlrpc::request_to_string(endpoint, args).unwrap(); - let response = timeout(TIMEOUT, client.post(uri).body(body).send()) - .await - .unwrap() - .unwrap(); + let response = client.post(uri).body(body).send().await.unwrap(); timeout(TIMEOUT, response.text()).await.unwrap().unwrap() } @@ -31,191 +28,125 @@ mod tests { value } - async fn test_with_watch_dog( - test: impl std::future::Future>>, - ) -> Result<(), Box> { - // Overall watchdog since I can't get these tests to timeout, but they take 20min in CI - // Need the task to fail so I can get any debug information out - // Can't get the failure to repro locally - let watchdog = async { - tokio::time::sleep(TIMEOUT * 5).await; - log::error!("Test watchdog tripped!"); - }; - tokio::select! { - _ = watchdog => { - simple_error::bail!("Bark!"); - } - _ = test => { - Ok(()) - } - } - } - #[test_log::test(tokio::test)] async fn verify_get_master_uri() -> Result<(), Box> { - test_with_watch_dog(async { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_master_uri"), - ) - .await - .unwrap() - .unwrap(); - log::info!("Got new handle"); - - let node_uri = timeout(TIMEOUT, node.get_client_uri()) - .await - .unwrap() - .unwrap(); - log::info!("Got uri"); - - // Note: doesn't need timeout as it is internally timeout()'d - let master_uri = call_node_api::( - &node_uri, - "getMasterUri", - vec!["/get_master_uri_test".into()], - ) - .await; - log::info!("Got master"); - assert_eq!(master_uri, "http://localhost:11311"); - Ok(()) - }) - .await + let node = + roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_master_uri").await?; + log::info!("Got new handle"); + + let node_uri = node.get_client_uri().await?; + log::info!("Got uri"); + + // Note: doesn't need timeout as it is internally timeout()'d + let master_uri = call_node_api::( + &node_uri, + "getMasterUri", + vec!["/get_master_uri_test".into()], + ) + .await; + log::info!("Got master"); + assert_eq!(master_uri, "http://localhost:11311"); + Ok(()) } #[test_log::test(tokio::test(flavor = "multi_thread"))] async fn verify_get_publications() -> Result<(), Box> { - test_with_watch_dog(async { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications"), - ) - .await??; - log::info!("Got new handle"); - - let node_uri = timeout(TIMEOUT, node.get_client_uri()).await??; - log::info!("Got uri"); - - // Note: timeout not needed as it is internally timeout'd - let publications = call_node_api::>( - &node_uri, - "getPublications", - vec!["/verify_get_publications".into()], - ) - .await; - assert_eq!(publications.len(), 0); - log::info!("Got publications"); - - let _publisher = timeout( - TIMEOUT, - node.advertise::("/test_topic", 1), - ) - .await??; - log::info!("advertised"); - - // Note: internally timeout()'d - let publications = timeout( - TIMEOUT, - call_node_api::>( - &node_uri, - "getPublications", - vec!["/verify_get_publications".into()], - ), - ) + let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications") .await?; - log::info!("Got post advertise publications"); - - assert_eq!(publications.len(), 1); - let (topic, topic_type) = publications - .iter() - .nth(0) - .ok_or(simple_error::SimpleError::new("wtf"))?; - assert_eq!(topic, "/test_topic"); - assert_eq!(topic_type, std_msgs::String::ROS_TYPE_NAME); - Ok(()) - }) - .await + log::info!("Got new handle"); + + let node_uri = node.get_client_uri().await?; + log::info!("Got uri"); + + // Note: timeout not needed as it is internally timeout'd + let publications = call_node_api::>( + &node_uri, + "getPublications", + vec!["/verify_get_publications".into()], + ) + .await; + assert_eq!(publications.len(), 0); + log::info!("Got publications"); + + let _publisher = node.advertise::("/test_topic", 1).await?; + log::info!("advertised"); + + // Note: internally timeout()'d + let publications = call_node_api::>( + &node_uri, + "getPublications", + vec!["/verify_get_publications".into()], + ) + .await; + log::info!("Got post advertise publications"); + + assert_eq!(publications.len(), 1); + let (topic, topic_type) = publications + .iter() + .nth(0) + .ok_or(simple_error::SimpleError::new("wtf"))?; + assert_eq!(topic, "/test_topic"); + assert_eq!(topic_type, std_msgs::String::ROS_TYPE_NAME); + Ok(()) } #[test_log::test(tokio::test)] async fn verify_shutdown() { - test_with_watch_dog(async { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_shutdown"), - ) - .await??; - log::info!("Got handle"); - let node_uri = timeout(TIMEOUT, node.get_client_uri()).await??; - assert!(node.is_ok()); - log::info!("Got uri"); - - // Note: internally timeout()'d - call_node_api::( - &node_uri, - "shutdown", - vec!["/verify_shutdown".into(), "".into()], - ) - .await; - log::info!("shutdown?"); - - tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; - - assert!(!node.is_ok()); - Ok(()) - }) + let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_shutdown") + .await + .unwrap(); + log::info!("Got handle"); + let node_uri = node.get_client_uri().await.unwrap(); + assert!(node.is_ok()); + log::info!("Got uri"); + + // Note: internally timeout()'d + call_node_api::( + &node_uri, + "shutdown", + vec!["/verify_shutdown".into(), "".into()], + ) .await; + log::info!("shutdown?"); + + tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; + + assert!(!node.is_ok()); } #[test_log::test(tokio::test)] async fn verify_request_topic() { - test_with_watch_dog(async { - let node = timeout( - TIMEOUT, - roslibrust::NodeHandle::new("http://localhost:11311", "verify_request_topic"), - ) + let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_request_topic") .await - .unwrap() .unwrap(); - log::info!("Got handle"); - let node_uri = timeout(TIMEOUT, node.get_client_uri()) - .await - .unwrap() - .unwrap(); - log::info!("Got uri {node_uri:?}"); - - let _publisher = timeout( - TIMEOUT, - node.advertise::("/test_topic", 1), - ) + log::info!("Got handle"); + let node_uri = node.get_client_uri().await.unwrap(); + log::info!("Got uri {node_uri:?}"); + + let _publisher = node + .advertise::("/test_topic", 1) .await - .unwrap() .unwrap(); - log::info!("Got publisher"); - - // Note: internally timeout()'d - let response = call_node_api_raw( - &node_uri, - "requestTopic", - vec![ - "verify_request_topic".into(), - "/test_topic".into(), - serde_xmlrpc::Value::Array(vec![serde_xmlrpc::Value::Array(vec![ - "TCPROS".into() - ])]), - ], - ) - .await; - log::info!("Got response"); - - let (code, _description, (protocol, host, port)): (i8, String, (String, String, u16)) = - serde_xmlrpc::response_from_str(&response).unwrap(); - assert_eq!(code, 1); - assert_eq!(protocol, "TCPROS"); - assert!(!host.is_empty()); - assert!(port != 0); - Ok(()) - }) + log::info!("Got publisher"); + + // Note: internally timeout()'d + let response = call_node_api_raw( + &node_uri, + "requestTopic", + vec![ + "verify_request_topic".into(), + "/test_topic".into(), + serde_xmlrpc::Value::Array(vec![serde_xmlrpc::Value::Array(vec!["TCPROS".into()])]), + ], + ) .await; + log::info!("Got response"); + + let (code, _description, (protocol, host, port)): (i8, String, (String, String, u16)) = + serde_xmlrpc::response_from_str(&response).unwrap(); + assert_eq!(code, 1); + assert_eq!(protocol, "TCPROS"); + assert!(!host.is_empty()); + assert!(port != 0); } } From de44534c2bc53293683271336177801db8aa17ca Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:42:55 -0600 Subject: [PATCH 24/34] Re-enabling rosapi --- CHANGELOG.md | 1 - roslibrust/Cargo.toml | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 82fb6420..1643cec7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,7 +30,6 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed - - Removed `rosapi` module and functionality. Will likely return in a separate crate in the future. - Removed `find_and_generate_ros_messages_relative_to_manifest_dir!` this proc_macro was changing the current working directory of the compilation job resulting in a variety of strange compilation behaviors. Build.rs scripts are recommended for use cases requiring fine grained control of message generation. - The function interface for top level generation functions in `roslibrust_codegen` have been changed to include the list of dependent diff --git a/roslibrust/Cargo.toml b/roslibrust/Cargo.toml index 7448d340..ea4943f0 100644 --- a/roslibrust/Cargo.toml +++ b/roslibrust/Cargo.toml @@ -56,6 +56,8 @@ simple-error = "0.3" default = [] # Note: all does not include running_bridge as that is only intended for CI all = [] +# Provides a rosapi rust interface +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 From b27a872a498403a1b333e93b8c6924fab732080f Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:48:30 -0600 Subject: [PATCH 25/34] Rosapi fix --- roslibrust/src/rosapi/mod.rs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/roslibrust/src/rosapi/mod.rs b/roslibrust/src/rosapi/mod.rs index 8e198208..a8daf3d0 100644 --- a/roslibrust/src/rosapi/mod.rs +++ b/roslibrust/src/rosapi/mod.rs @@ -6,10 +6,11 @@ use crate::{ClientHandle, RosLibRustResult}; use async_trait::async_trait; -// TODO Fix this... this sucks -roslibrust_codegen_macro::find_and_generate_ros_messages_relative_to_manifest_dir!( - "../assets/ros1_common_interfaces/rosapi" -); +// 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 +// We almost certainly need to generate and commit the resulting messages, or +// do some include_str!() hax to be able to ship these types with the crate... +roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces/rosapi"); /// 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. From 70d7a673d34bdd5b3ba251f8c630fb238561ed3a Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 14:51:09 -0600 Subject: [PATCH 26/34] Adding rosapi back in for ros1 integration tests, experimenting with removing --test-threads 1 --- .github/workflows/galactic.yml | 2 +- .github/workflows/humble.yml | 2 +- .github/workflows/noetic.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/galactic.yml b/.github/workflows/galactic.yml index 4e703f50..84318183 100644 --- a/.github/workflows/galactic.yml +++ b/.github/workflows/galactic.yml @@ -38,4 +38,4 @@ jobs: - name: Start rosbridge run: source /opt/ros/galactic/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 - name: Integration Tests - run: source /root/.cargo/env; cargo test --features ros2_test -- --test-threads 1 \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros2_test \ No newline at end of file diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index 53831e37..adff2bfa 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -38,4 +38,4 @@ jobs: - name: Start rosbridge run: source /opt/ros/humble/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 - name: Integration Tests - run: source /root/.cargo/env; cargo test --features ros2_test -- --test-threads 1 \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros2_test \ No newline at end of file diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 7ac7c833..9da92ec4 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 /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge -- --test-threads 1 \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge,rosapi \ No newline at end of file From e060f30c8122c6b8ee704c0e6043ed10066de062 Mon Sep 17 00:00:00 2001 From: carter Date: Sun, 22 Oct 2023 16:10:27 -0600 Subject: [PATCH 27/34] Turning single threaded execution back on --- .github/workflows/galactic.yml | 2 +- .github/workflows/humble.yml | 2 +- .github/workflows/noetic.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/galactic.yml b/.github/workflows/galactic.yml index 84318183..4e703f50 100644 --- a/.github/workflows/galactic.yml +++ b/.github/workflows/galactic.yml @@ -38,4 +38,4 @@ jobs: - name: Start rosbridge run: source /opt/ros/galactic/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 - name: Integration Tests - run: source /root/.cargo/env; cargo test --features ros2_test \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros2_test -- --test-threads 1 \ No newline at end of file diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index adff2bfa..53831e37 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -38,4 +38,4 @@ jobs: - name: Start rosbridge run: source /opt/ros/humble/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 - name: Integration Tests - run: source /root/.cargo/env; cargo test --features ros2_test \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros2_test -- --test-threads 1 \ No newline at end of file diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml index 9da92ec4..6110e7f6 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 /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge,rosapi \ No newline at end of file + run: source /root/.cargo/env; cargo test --features ros1_test,ros1,running_bridge,rosapi -- --test-threads 1 \ No newline at end of file From f1090e2b19ced751e5c0243212f35d17c3dab9bb Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 23 Oct 2023 17:05:40 -0600 Subject: [PATCH 28/34] Cleanups from MR review --- Cargo.lock | 1 - roslibrust/Cargo.toml | 1 - roslibrust/src/ros1/node.rs | 22 ++-------------------- roslibrust/tests/ros1_xmlrpc.rs | 12 +++--------- 4 files changed, 5 insertions(+), 31 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index c8914d93..83e7cd47 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1252,7 +1252,6 @@ dependencies = [ "serde_json", "serde_rosmsg", "serde_xmlrpc", - "simple-error", "simple_logger", "smart-default 0.6.0", "test-log", diff --git a/roslibrust/Cargo.toml b/roslibrust/Cargo.toml index ea4943f0..744e8a02 100644 --- a/roslibrust/Cargo.toml +++ b/roslibrust/Cargo.toml @@ -50,7 +50,6 @@ regex = { version = "1.9", optional = true } # Only used with native ros1 env_logger = "0.10" test-log = "0.2" simple_logger = "2.1.0" -simple-error = "0.3" [features] default = [] diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node.rs index 2e212d75..45709724 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node.rs @@ -119,16 +119,12 @@ impl NodeServerHandle { pub async fn get_publications( &self, ) -> Result, Box> { - log::debug!("get_publications reached"); let (sender, receiver) = oneshot::channel(); match self .node_server_sender .send(NodeMsg::GetPublications { reply: sender }) { - Ok(()) => { - log::debug!("Awaiting receiver"); - Ok(receiver.await.map_err(|err| Box::new(err))?) - } + Ok(()) => Ok(receiver.await.map_err(|err| Box::new(err))?), Err(e) => Err(Box::new(e)), } } @@ -169,11 +165,8 @@ impl NodeServerHandle { md5sum: T::MD5SUM.to_owned(), }) { Ok(()) => { - log::trace!("Recieved okay from node_server_send."); let received = receiver.await.map_err(|err| Box::new(err))?; - log::trace!("Awaiting receiver produced: {received:?}"); - Ok(received.map_err(|err| { - log::error!("Failed to register publisher: {err}"); + Ok(received.map_err(|_err| { Box::new(std::io::Error::from(std::io::ErrorKind::ConnectionAborted)) })?) } @@ -367,11 +360,9 @@ impl Node { msg_definition, md5sum, } => { - log::trace!("Got register publisher command from mpsc for {topic:?}"); let res = self .register_publisher(topic, &topic_type, queue_size, msg_definition, md5sum) .await; - log::trace!("Result of register_publisher: {res:?}"); match res { Ok(handle) => reply.send(Ok(handle)), Err(err) => reply.send(Err(err.to_string())), @@ -478,11 +469,8 @@ impl Node { msg_definition: String, md5sum: String, ) -> Result>, Box> { - log::trace!("Registering publisher for: {topic:?}"); - let existing_entry = { self.publishers.iter().find_map(|(key, value)| { - log::trace!("Found existing entry for: {topic:?}"); if key.as_str() == &topic { if value.topic_type() == topic_type { Some(Ok(value.get_sender())) @@ -500,7 +488,6 @@ impl Node { if let Some(handle) = existing_entry { Ok(handle?) } else { - log::trace!("Creating new entry for {topic:?}"); let channel = Publication::new( &self.node_name, false, @@ -516,12 +503,9 @@ impl Node { log::error!("Failed to create publishing channel: {err:?}"); err })?; - log::trace!("Created new publication for {topic:?}"); let handle = channel.get_sender(); self.publishers.insert(topic.clone(), channel); - log::trace!("Inserted new publsiher into dashmap"); let _current_subscribers = self.client.register_publisher(&topic, topic_type).await?; - log::trace!("Registered new publication for {topic:?}"); Ok(handle) } } @@ -565,12 +549,10 @@ impl NodeHandle { topic_name: &str, queue_size: usize, ) -> Result, Box> { - log::trace!("Advertising: {topic_name:?}"); let sender = self .inner .register_publisher::(topic_name, T::ROS_TYPE_NAME, queue_size) .await?; - log::trace!("Advertised: {topic_name:?}"); Ok(Publisher::new(topic_name, sender)) } diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index ddebf8db..6ed96066 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -3,16 +3,13 @@ mod tests { use roslibrust_codegen::RosMessageType; use serde::de::DeserializeOwned; use serde_xmlrpc::Value; - use tokio::time::timeout; - const TIMEOUT: tokio::time::Duration = tokio::time::Duration::from_millis(500); - roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); async fn call_node_api_raw(uri: &str, endpoint: &str, args: Vec) -> String { let client = reqwest::Client::new(); let body = serde_xmlrpc::request_to_string(endpoint, args).unwrap(); let response = client.post(uri).body(body).send().await.unwrap(); - timeout(TIMEOUT, response.text()).await.unwrap().unwrap() + response.text().await.unwrap() } async fn call_node_api(uri: &str, endpoint: &str, args: Vec) -> T { @@ -49,7 +46,7 @@ mod tests { Ok(()) } - #[test_log::test(tokio::test(flavor = "multi_thread"))] + #[test_log::test(tokio::test)] async fn verify_get_publications() -> Result<(), Box> { let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications") .await?; @@ -81,10 +78,7 @@ mod tests { log::info!("Got post advertise publications"); assert_eq!(publications.len(), 1); - let (topic, topic_type) = publications - .iter() - .nth(0) - .ok_or(simple_error::SimpleError::new("wtf"))?; + let (topic, topic_type) = publications.iter().nth(0).unwrap(); assert_eq!(topic, "/test_topic"); assert_eq!(topic_type, std_msgs::String::ROS_TYPE_NAME); Ok(()) From 62eb22ae4accf200467b6077e2eb774a264b394b Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 23 Oct 2023 10:32:37 -0600 Subject: [PATCH 29/34] Add iron CI --- .github/workflows/iron.yml | 37 ++++++++++++++++++++++++++++++++++++ README.md | 1 + docker/galactic/Dockerfile | 5 ----- docker/humble/Dockerfile | 5 ----- docker/humble/README.md | 10 +++++----- docker/iron/Dockerfile | 17 +++++++++++++++++ docker/iron/README.md | 10 ++++++++++ docker/iron/entrypoint.sh | 2 ++ docker/iron_compose.yaml | 7 +++++++ docker/noetic/Dockerfile | 4 ---- docker/noetic_cpp/Dockerfile | 4 ---- 11 files changed, 79 insertions(+), 23 deletions(-) create mode 100644 .github/workflows/iron.yml create mode 100644 docker/iron/Dockerfile create mode 100644 docker/iron/README.md create mode 100644 docker/iron/entrypoint.sh create mode 100644 docker/iron_compose.yaml diff --git a/.github/workflows/iron.yml b/.github/workflows/iron.yml new file mode 100644 index 00000000..95705087 --- /dev/null +++ b/.github/workflows/iron.yml @@ -0,0 +1,37 @@ +name: Iron + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +env: + HOME: /root + +jobs: + humble: + runs-on: ubuntu-latest + defaults: + run: + shell: bash + container: carter12s/roslibrust-ci-iron:latest + timeout-minutes: 20 + steps: + - name: Checkout repo + uses: actions/checkout@v3 + with: + submodules: 'true' + - name: Lint + run: source /root/.cargo/env; cargo fmt --all -- --check + - name: Build Main Lib + run: source /root/.cargo/env; cargo build + # This step is required to confirm feature combinations work, the main workspace build does all features + - name: Build Proc Macro + run: source /root/.cargo/env; cargo build -p roslibrust_codegen_macro + - name: Unit Tests + run: source /root/.cargo/env; cargo test + - name: Start rosbridge + run: source /opt/ros/humble/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 + - name: Integration Tests + run: source /root/.cargo/env; cargo test --features ros2_test -- --test-threads 1 \ No newline at end of file diff --git a/README.md b/README.md index 54523d19..f606040f 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,7 @@ [![Noetic](https://github.com/Carter12s/roslibrust/actions/workflows/noetic.yml/badge.svg)](https://github.com/Carter12s/roslibrust/actions/workflows/noetic.yml) [![Galactic](https://github.com/Carter12s/roslibrust/actions/workflows/galactic.yml/badge.svg)](https://github.com/Carter12s/roslibrust/actions/workflows/galactic.yml) [![Humble](https://github.com/Carter12s/roslibrust/actions/workflows/humble.yml/badge.svg)](https://github.com/Carter12s/roslibrust/actions/workflows/humble.yml) +[![Iron](https://github.com/Carter12s/roslibrust/actions/workflows/iron.yml/badge.svg)](https://github.com/Carter12s/roslibrust/actions/workflows/iron.yml) [![License:MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) This package aims to provide a convenient intermediary between ROS1's rosbridge and Rust similar to roslibpy and roslibjs. diff --git a/docker/galactic/Dockerfile b/docker/galactic/Dockerfile index bfb9ecab..7efd9e2a 100644 --- a/docker/galactic/Dockerfile +++ b/docker/galactic/Dockerfile @@ -4,7 +4,6 @@ LABEL maintainer="Carter Schultz " # Required by github CI for submodule support RUN apt update && apt install -y git -# Adapted from https://github.com/JoaquinGimenez1/docker-noetic-rosbridge/blob/main/Dockerfile RUN apt update && apt install -y ros-galactic-rosbridge-suite # Curl required to install rust, build-essential required to build quote & proc-macro2 @@ -12,10 +11,6 @@ RUN apt update && apt install -y --fix-missing curl build-essential # Install latest stable rust RUN curl https://sh.rustup.rs -sSf | sh -s -- -y -# Update Cargo Registry -# See this for why we do it so wierd https://github.com/rust-lang/cargo/issues/3377 -RUN /root/.cargo/bin/cargo install lazy_static; exit 0 - WORKDIR / COPY entrypoint.sh . RUN chmod +x entrypoint.sh diff --git a/docker/humble/Dockerfile b/docker/humble/Dockerfile index d52a0835..586e7551 100644 --- a/docker/humble/Dockerfile +++ b/docker/humble/Dockerfile @@ -4,7 +4,6 @@ LABEL maintainer="Carter Schultz " # Required by github CI for submodule support RUN apt update && apt install -y git -# Adapted from https://github.com/JoaquinGimenez1/docker-noetic-rosbridge/blob/main/Dockerfile RUN apt update && apt install -y ros-humble-rosbridge-suite # Curl required to install rust, build-essential required to build quote & proc-macro2 @@ -12,10 +11,6 @@ RUN apt update && apt install -y --fix-missing curl build-essential # Install latest stable rust RUN curl https://sh.rustup.rs -sSf | sh -s -- -y -# Update Cargo Registry -# See this for why we do it so wierd https://github.com/rust-lang/cargo/issues/3377 -RUN /root/.cargo/bin/cargo install lazy_static; exit 0 - WORKDIR / COPY entrypoint.sh . RUN chmod +x entrypoint.sh diff --git a/docker/humble/README.md b/docker/humble/README.md index ce8abbad..04044364 100644 --- a/docker/humble/README.md +++ b/docker/humble/README.md @@ -1,10 +1,10 @@ -# Galactic CI Docker Image -Produces a docker image including both galactic rosbridge and needed rust. +# Humble CI Docker Image +Produces a docker image including both humble rosbridge and needed rust. # Building / Publishing (currently only carter has access, need to fix) -- docker build -t carter12s/roslibrust-ci-galactic:latest . +- docker build -t carter12s/roslibrust-ci-humble:latest . - Maybe needed: docker login -- docker push carter12s/roslibrust-ci-galactic:latest +- docker push carter12s/roslibrust-ci-humble:latest For debug: -docker run -it carter12s/roslibrust-ci-galactic /bin/bash \ No newline at end of file +docker run -it carter12s/roslibrust-ci-humble /bin/bash \ No newline at end of file diff --git a/docker/iron/Dockerfile b/docker/iron/Dockerfile new file mode 100644 index 00000000..be37d179 --- /dev/null +++ b/docker/iron/Dockerfile @@ -0,0 +1,17 @@ +FROM ros:iron-ros-core +LABEL maintainer="Carter Schultz " + +# Required by github CI for submodule support +RUN apt update && apt install -y git + +RUN apt update && apt install -y ros-iron-rosbridge-suite + +# Curl required to install rust, build-essential required to build quote & proc-macro2 +RUN apt update && apt install -y --fix-missing curl build-essential +# Install latest stable rust +RUN curl https://sh.rustup.rs -sSf | sh -s -- -y + +WORKDIR / +COPY entrypoint.sh . +RUN chmod +x entrypoint.sh +ENTRYPOINT ["/entrypoint.sh"] diff --git a/docker/iron/README.md b/docker/iron/README.md new file mode 100644 index 00000000..f15bd5c8 --- /dev/null +++ b/docker/iron/README.md @@ -0,0 +1,10 @@ +# Iron CI Docker Image +Produces a docker image including both iron rosbridge and needed rust. + +# Building / Publishing (currently only carter has access, need to fix) +- docker build -t carter12s/roslibrust-ci-iron:latest . +- Maybe needed: docker login +- docker push carter12s/roslibrust-ci-iron:latest + +For debug: +docker run -it carter12s/roslibrust-ci-iron /bin/bash diff --git a/docker/iron/entrypoint.sh b/docker/iron/entrypoint.sh new file mode 100644 index 00000000..9d8ad4d3 --- /dev/null +++ b/docker/iron/entrypoint.sh @@ -0,0 +1,2 @@ +#!/bin/bash +exec "$@" diff --git a/docker/iron_compose.yaml b/docker/iron_compose.yaml new file mode 100644 index 00000000..45eb17ce --- /dev/null +++ b/docker/iron_compose.yaml @@ -0,0 +1,7 @@ +version: "3.9" +services: + rosbridge: + image: carter12s/roslibrust-ci-iron:latest + ports: + - "9090:9090" + command: bash -c "source /opt/ros/humble/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node --ros-args --log-level debug" \ No newline at end of file diff --git a/docker/noetic/Dockerfile b/docker/noetic/Dockerfile index c2ff582c..b570c5c9 100644 --- a/docker/noetic/Dockerfile +++ b/docker/noetic/Dockerfile @@ -9,10 +9,6 @@ RUN apt update && apt install -y --fix-missing curl build-essential # Install latest stable rust RUN curl https://sh.rustup.rs -sSf | sh -s -- -y -# Update Cargo Registry -# See this for why we do it so wierd https://github.com/rust-lang/cargo/issues/3377 -RUN /root/.cargo/bin/cargo install lazy_static; exit 0 - WORKDIR / COPY entrypoint.sh . RUN chmod +x entrypoint.sh diff --git a/docker/noetic_cpp/Dockerfile b/docker/noetic_cpp/Dockerfile index 24b69cbc..00212510 100644 --- a/docker/noetic_cpp/Dockerfile +++ b/docker/noetic_cpp/Dockerfile @@ -16,10 +16,6 @@ RUN apt update && apt install -y ros-noetic-ros-base # Install latest stable rust RUN curl https://sh.rustup.rs -sSf | sh -s -- -y -# Update Cargo Registry -# See this for why we do it so weird https://github.com/rust-lang/cargo/issues/3377 -RUN /root/.cargo/bin/cargo install lazy_static; exit 0 - WORKDIR / COPY entrypoint.sh . RUN chmod +x entrypoint.sh From 17951ea9c331aa1eb14c185d0321c96cfbd9d9a2 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 23 Oct 2023 10:38:59 -0600 Subject: [PATCH 30/34] Fix iron ci --- .github/workflows/iron.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/iron.yml b/.github/workflows/iron.yml index 95705087..e13488d6 100644 --- a/.github/workflows/iron.yml +++ b/.github/workflows/iron.yml @@ -32,6 +32,6 @@ jobs: - name: Unit Tests run: source /root/.cargo/env; cargo test - name: Start rosbridge - run: source /opt/ros/humble/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 + run: source /opt/ros/iron/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml & disown; ros2 run rosapi rosapi_node & sleep 1 - name: Integration Tests run: source /root/.cargo/env; cargo test --features ros2_test -- --test-threads 1 \ No newline at end of file From 925435242e7a81fad7b50a11405f0088dda6ff02 Mon Sep 17 00:00:00 2001 From: carter Date: Mon, 23 Oct 2023 19:56:59 -0600 Subject: [PATCH 31/34] Add in rust_log from #147 --- .github/workflows/iron.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/iron.yml b/.github/workflows/iron.yml index e13488d6..db003954 100644 --- a/.github/workflows/iron.yml +++ b/.github/workflows/iron.yml @@ -8,6 +8,8 @@ on: env: HOME: /root + # Coupled with our use of the test_log crate this should give us good CI output on failure + RUST_LOG: debug jobs: humble: From 28df7f603f878c76ce592b1778a7d256b9c449ba Mon Sep 17 00:00:00 2001 From: Shane Snover Date: Sat, 16 Dec 2023 13:41:49 -0700 Subject: [PATCH 32/34] Reorganize the code in node.rs into a couple separate files with different areas of concern to manage growing number of node actor methods --- CHANGELOG.md | 9 +- roslibrust/src/ros1/mod.rs | 4 - .../src/ros1/{node.rs => node/actor.rs} | 145 ++---------------- roslibrust/src/ros1/node/handle.rs | 60 ++++++++ roslibrust/src/ros1/node/mod.rs | 70 +++++++++ .../ros1/{xmlrpc_server.rs => node/xmlrpc.rs} | 2 +- 6 files changed, 148 insertions(+), 142 deletions(-) rename roslibrust/src/ros1/{node.rs => node/actor.rs} (77%) create mode 100644 roslibrust/src/ros1/node/handle.rs create mode 100644 roslibrust/src/ros1/node/mod.rs rename roslibrust/src/ros1/{xmlrpc_server.rs => node/xmlrpc.rs} (99%) diff --git a/CHANGELOG.md b/CHANGELOG.md index d291403d..6a5d1dfe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,16 +25,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added - The build.rs example in example_package now correctly informs cargo of filesystem dependencies -- The `advertise_serveice` method in `rosbridge/client.rs` now accepts closures +- The `advertise_service` method in `rosbridge/client.rs` now accepts closures ### Fixed ### Changed - - Removed `find_and_generate_ros_messages_relative_to_manifest_dir!` this proc_macro was changing the current working directory of the compilation job resulting in a variety of strange compilation behaviors. Build.rs scripts are recommended for use cases requiring fine - grained control of message generation. - - The function interface for top level generation functions in `roslibrust_codegen` have been changed to include the list of dependent -filesystem paths that should trigger re-running code generation. Note: new files added to the search paths will not be automatically detected. + - Removed `find_and_generate_ros_messages_relative_to_manifest_dir!` this proc_macro was changing the current working directory of the compilation job resulting in a variety of strange compilation behaviors. Build.rs scripts are recommended for use cases requiring fine grained control of message generation. + - The function interface for top level generation functions in `roslibrust_codegen` have been changed to include the list of dependent filesystem paths that should trigger re-running code generation. Note: new files added to the search paths will not be automatically detected. + - Refactor the `ros1::node` module into separate smaller pieces. This should be invisible externally (and no changes to examples were required). ## 0.8.0 - October 4th, 2023 diff --git a/roslibrust/src/ros1/mod.rs b/roslibrust/src/ros1/mod.rs index 3c15c71d..f8c2e514 100644 --- a/roslibrust/src/ros1/mod.rs +++ b/roslibrust/src/ros1/mod.rs @@ -4,10 +4,6 @@ mod master_client; pub use master_client::*; -/// [xmlrpc_server] module contains the xmlrpc server that a node must host -mod xmlrpc_server; -pub(crate) use xmlrpc_server::*; - mod names; /// [node] module contains the central Node and NodeHandle APIs diff --git a/roslibrust/src/ros1/node.rs b/roslibrust/src/ros1/node/actor.rs similarity index 77% rename from roslibrust/src/ros1/node.rs rename to roslibrust/src/ros1/node/actor.rs index 45709724..47276bfa 100644 --- a/roslibrust/src/ros1/node.rs +++ b/roslibrust/src/ros1/node/actor.rs @@ -1,28 +1,18 @@ -//! 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 super::{ - names::Name, - publisher::{Publication, Publisher}, - subscriber::{Subscriber, Subscription}, +use super::ProtocolParams; +use crate::{ + ros1::{ + names::Name, + node::{XmlRpcServer, XmlRpcServerHandle}, + publisher::Publication, + subscriber::Subscription, + }, + MasterClient, ServiceCallback, }; -use crate::{MasterClient, RosMasterError, ServiceCallback, XmlRpcServer, XmlRpcServerHandle}; use abort_on_drop::ChildTask; use roslibrust_codegen::RosMessageType; -use std::{ - collections::HashMap, - net::{IpAddr, Ipv4Addr}, - sync::Arc, -}; +use std::{collections::HashMap, net::Ipv4Addr, sync::Arc}; use tokio::sync::{broadcast, mpsc, oneshot}; -#[derive(Debug)] -pub struct ProtocolParams { - pub hostname: String, - pub protocol: String, - pub port: u16, -} - #[derive(Debug)] pub enum NodeMsg { GetMasterUri { @@ -68,7 +58,7 @@ pub enum NodeMsg { #[derive(Clone)] pub(crate) struct NodeServerHandle { - node_server_sender: mpsc::UnboundedSender, + pub(crate) node_server_sender: mpsc::UnboundedSender, // If this handle should keep the underlying node task alive it will hold an // Arc to the underlying node task. This is an option because internal handles // within the node shouldn't keep it alive (e.g. what we hand to xml server) @@ -228,7 +218,7 @@ impl NodeServerHandle { /// Represents a single "real" node, typically only one of these is expected per process /// but nothing should specifically prevent that. -pub struct Node { +pub(crate) struct Node { // The xmlrpc client this node uses to make requests to master client: MasterClient, // Server which handles updates from the rosmaster and other ROS nodes @@ -248,7 +238,7 @@ pub struct Node { } impl Node { - async fn new( + pub(crate) async fn new( master_uri: &str, hostname: &str, node_name: &str, @@ -510,112 +500,3 @@ impl Node { } } } - -/// 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. -#[derive(Clone)] -pub struct NodeHandle { - inner: NodeServerHandle, -} - -impl NodeHandle { - // TODO builder, result, better error type - /// Creates a new node connect and returns a handle to it - /// It is idiomatic to call this once per process and treat the created node as singleton. - /// The returned handle can be freely clone'd to create additional handles without creating additional connections. - pub async fn new( - master_uri: &str, - name: &str, - ) -> Result> { - // Follow ROS rules and determine our IP and hostname - let (addr, hostname) = determine_addr().await?; - - let node = Node::new(master_uri, &hostname, name, addr).await?; - let nh = NodeHandle { inner: node }; - - Ok(nh) - } - - pub fn is_ok(&self) -> bool { - !self.inner.node_server_sender.is_closed() - } - - pub async fn get_client_uri(&self) -> Result> { - self.inner.get_client_uri().await - } - - pub async fn advertise( - &self, - topic_name: &str, - queue_size: usize, - ) -> Result, Box> { - let sender = self - .inner - .register_publisher::(topic_name, T::ROS_TYPE_NAME, queue_size) - .await?; - Ok(Publisher::new(topic_name, sender)) - } - - pub async fn subscribe( - &self, - topic_name: &str, - queue_size: usize, - ) -> Result, Box> { - let receiver = self - .inner - .register_subscriber::(topic_name, queue_size) - .await?; - Ok(Subscriber::new(receiver)) - } -} - -// TODO at the end of the day I'd like to offer a builder pattern for configuration that allow manual setting of this or "ros idiomatic" behavior - Carter -/// Following ROS's idiomatic address rules uses ROS_HOSTNAME and ROS_IP to determine the address that server should be hosted at. -/// Returns both the resolved IpAddress of the host (used for actually opening the socket), and the String "hostname" which should -/// be used in the URI. -async fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { - // If ROS_IP is set that trumps anything else - if let Ok(ip_str) = std::env::var("ROS_IP") { - let ip = ip_str.parse().map_err(|e| { - RosMasterError::HostIpResolutionFailure(format!( - "ROS_IP environment variable did not parse to a valid IpAddr::V4: {e:?}" - )) - })?; - return Ok((ip, ip_str)); - } - // If ROS_HOSTNAME is set that is next highest precedent - if let Ok(name) = std::env::var("ROS_HOSTNAME") { - let ip = hostname_to_ipv4(&name).await?; - return Ok((ip, name)); - } - // If neither env var is set, use the computers "hostname" - let name = gethostname::gethostname(); - let name = name.into_string().map_err(|e| { - RosMasterError::HostIpResolutionFailure(format!("This host's hostname is a string that cannot be validly converted into a Rust type, and therefore we cannot convert it into an IpAddrv4: {e:?}")) - })?; - let ip = hostname_to_ipv4(&name).await?; - return Ok((ip, name)); -} - -/// Given a the name of a host use's std::net::ToSocketAddrs to perform a DNS lookup and return the resulting IP address. -/// This function is intended to be used to determine the correct IP host the socket for the xmlrpc server on. -async fn hostname_to_ipv4(name: &str) -> Result { - let name_with_port = &format!("{name}:0"); - let mut i = tokio::net::lookup_host(name_with_port).await.map_err(|e| { - RosMasterError::HostIpResolutionFailure(format!( - "Failure while attempting to lookup ROS_HOSTNAME: {e:?}" - )) - })?; - if let Some(addr) = i.next() { - match addr.ip() { - IpAddr::V4(ip) => Ok(ip), - IpAddr::V6(ip) => { - Err(RosMasterError::HostIpResolutionFailure(format!("ROS_HOSTNAME resolved to an IPv6 address which is not support by ROS/roslibrust: {ip:?}"))) - } - } - } else { - Err(RosMasterError::HostIpResolutionFailure(format!( - "ROS_HOSTNAME did not resolve any address: {name:?}" - ))) - } -} diff --git a/roslibrust/src/ros1/node/handle.rs b/roslibrust/src/ros1/node/handle.rs new file mode 100644 index 00000000..493ca3a4 --- /dev/null +++ b/roslibrust/src/ros1/node/handle.rs @@ -0,0 +1,60 @@ +use super::actor::{Node, NodeServerHandle}; +use crate::ros1::{publisher::Publisher, subscriber::Subscriber}; + +/// 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. +#[derive(Clone)] +pub struct NodeHandle { + inner: NodeServerHandle, +} + +impl NodeHandle { + // TODO builder, result, better error type + /// Creates a new node connect and returns a handle to it + /// It is idiomatic to call this once per process and treat the created node as singleton. + /// The returned handle can be freely clone'd to create additional handles without creating additional connections. + pub async fn new( + master_uri: &str, + name: &str, + ) -> Result> { + // Follow ROS rules and determine our IP and hostname + let (addr, hostname) = super::determine_addr().await?; + + let node = Node::new(master_uri, &hostname, name, addr).await?; + let nh = NodeHandle { inner: node }; + + Ok(nh) + } + + pub fn is_ok(&self) -> bool { + !self.inner.node_server_sender.is_closed() + } + + pub async fn get_client_uri(&self) -> Result> { + self.inner.get_client_uri().await + } + + pub async fn advertise( + &self, + topic_name: &str, + queue_size: usize, + ) -> Result, Box> { + let sender = self + .inner + .register_publisher::(topic_name, T::ROS_TYPE_NAME, queue_size) + .await?; + Ok(Publisher::new(topic_name, sender)) + } + + pub async fn subscribe( + &self, + topic_name: &str, + queue_size: usize, + ) -> Result, Box> { + let receiver = self + .inner + .register_subscriber::(topic_name, queue_size) + .await?; + Ok(Subscriber::new(receiver)) + } +} diff --git a/roslibrust/src/ros1/node/mod.rs b/roslibrust/src/ros1/node/mod.rs new file mode 100644 index 00000000..4fa4a1d3 --- /dev/null +++ b/roslibrust/src/ros1/node/mod.rs @@ -0,0 +1,70 @@ +//! 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::RosMasterError; +use std::net::{IpAddr, Ipv4Addr}; + +mod actor; +mod handle; +mod xmlrpc; +use actor::*; +pub use handle::NodeHandle; +use xmlrpc::*; + +#[derive(Debug)] +pub struct ProtocolParams { + pub hostname: String, + pub protocol: String, + pub port: u16, +} + +// TODO at the end of the day I'd like to offer a builder pattern for configuration that allow manual setting of this or "ros idiomatic" behavior - Carter +/// Following ROS's idiomatic address rules uses ROS_HOSTNAME and ROS_IP to determine the address that server should be hosted at. +/// Returns both the resolved IpAddress of the host (used for actually opening the socket), and the String "hostname" which should +/// be used in the URI. +async fn determine_addr() -> Result<(Ipv4Addr, String), RosMasterError> { + // If ROS_IP is set that trumps anything else + if let Ok(ip_str) = std::env::var("ROS_IP") { + let ip = ip_str.parse().map_err(|e| { + RosMasterError::HostIpResolutionFailure(format!( + "ROS_IP environment variable did not parse to a valid IpAddr::V4: {e:?}" + )) + })?; + return Ok((ip, ip_str)); + } + // If ROS_HOSTNAME is set that is next highest precedent + if let Ok(name) = std::env::var("ROS_HOSTNAME") { + let ip = hostname_to_ipv4(&name).await?; + return Ok((ip, name)); + } + // If neither env var is set, use the computers "hostname" + let name = gethostname::gethostname(); + let name = name.into_string().map_err(|e| { + RosMasterError::HostIpResolutionFailure(format!("This host's hostname is a string that cannot be validly converted into a Rust type, and therefore we cannot convert it into an IpAddrv4: {e:?}")) + })?; + let ip = hostname_to_ipv4(&name).await?; + return Ok((ip, name)); +} + +/// Given a the name of a host use's std::net::ToSocketAddrs to perform a DNS lookup and return the resulting IP address. +/// This function is intended to be used to determine the correct IP host the socket for the xmlrpc server on. +async fn hostname_to_ipv4(name: &str) -> Result { + let name_with_port = &format!("{name}:0"); + let mut i = tokio::net::lookup_host(name_with_port).await.map_err(|e| { + RosMasterError::HostIpResolutionFailure(format!( + "Failure while attempting to lookup ROS_HOSTNAME: {e:?}" + )) + })?; + if let Some(addr) = i.next() { + match addr.ip() { + IpAddr::V4(ip) => Ok(ip), + IpAddr::V6(ip) => { + Err(RosMasterError::HostIpResolutionFailure(format!("ROS_HOSTNAME resolved to an IPv6 address which is not support by ROS/roslibrust: {ip:?}"))) + } + } + } else { + Err(RosMasterError::HostIpResolutionFailure(format!( + "ROS_HOSTNAME did not resolve any address: {name:?}" + ))) + } +} diff --git a/roslibrust/src/ros1/xmlrpc_server.rs b/roslibrust/src/ros1/node/xmlrpc.rs similarity index 99% rename from roslibrust/src/ros1/xmlrpc_server.rs rename to roslibrust/src/ros1/node/xmlrpc.rs index ea3a3701..58015494 100644 --- a/roslibrust/src/ros1/xmlrpc_server.rs +++ b/roslibrust/src/ros1/node/xmlrpc.rs @@ -1,4 +1,4 @@ -use super::node::NodeServerHandle; +use super::NodeServerHandle; use abort_on_drop::ChildTask; use hyper::{Body, Response, StatusCode}; use log::*; From 2f5701f26e7774df85cba3c4866ff83d8a440325 Mon Sep 17 00:00:00 2001 From: Shane Snover Date: Tue, 19 Dec 2023 19:33:06 -0700 Subject: [PATCH 33/34] Moving refactoring changes to this earlier MR --- roslibrust/examples/native_ros1.rs | 2 +- roslibrust/examples/ros1_listener.rs | 2 +- roslibrust/examples/ros1_talker.rs | 2 +- roslibrust/src/lib.rs | 33 ++++++++++++++++++++++--- roslibrust/src/ros1/mod.rs | 2 ++ roslibrust/src/ros1/names.rs | 15 ++++++++--- roslibrust/src/ros1/node/actor.rs | 13 +++------- roslibrust/src/ros1/node/handle.rs | 2 +- roslibrust/src/ros1/node/mod.rs | 2 +- roslibrust/src/ros1/publisher.rs | 4 +-- roslibrust/src/ros1/subscriber.rs | 2 +- roslibrust/src/rosbridge/mod.rs | 37 ++-------------------------- 12 files changed, 57 insertions(+), 59 deletions(-) diff --git a/roslibrust/examples/native_ros1.rs b/roslibrust/examples/native_ros1.rs index ddceef8b..5e4e3673 100644 --- a/roslibrust/examples/native_ros1.rs +++ b/roslibrust/examples/native_ros1.rs @@ -5,7 +5,7 @@ #[cfg(feature = "ros1")] #[tokio::main] async fn main() -> Result<(), Box> { - use roslibrust::NodeHandle; + use roslibrust::ros1::NodeHandle; simple_logger::SimpleLogger::new() .with_level(log::LevelFilter::Debug) diff --git a/roslibrust/examples/ros1_listener.rs b/roslibrust/examples/ros1_listener.rs index 3d94b3e5..cdbc6826 100644 --- a/roslibrust/examples/ros1_listener.rs +++ b/roslibrust/examples/ros1_listener.rs @@ -3,7 +3,7 @@ roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_in #[cfg(feature = "ros1")] #[tokio::main] async fn main() -> Result<(), Box> { - use roslibrust::NodeHandle; + use roslibrust::ros1::NodeHandle; simple_logger::SimpleLogger::new() .with_level(log::LevelFilter::Debug) diff --git a/roslibrust/examples/ros1_talker.rs b/roslibrust/examples/ros1_talker.rs index 3d40d479..934a19cf 100644 --- a/roslibrust/examples/ros1_talker.rs +++ b/roslibrust/examples/ros1_talker.rs @@ -3,7 +3,7 @@ roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_in #[cfg(feature = "ros1")] #[tokio::main] async fn main() -> Result<(), Box> { - use roslibrust::NodeHandle; + use roslibrust::ros1::NodeHandle; simple_logger::SimpleLogger::new() .with_level(log::LevelFilter::Debug) diff --git a/roslibrust/src/lib.rs b/roslibrust/src/lib.rs index 8932461d..e3044431 100644 --- a/roslibrust/src/lib.rs +++ b/roslibrust/src/lib.rs @@ -105,6 +105,33 @@ pub use rosbridge::*; pub mod rosapi; #[cfg(feature = "ros1")] -mod ros1; -#[cfg(feature = "ros1")] -pub use ros1::*; +pub mod ros1; + +/// 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("Rosbridge server reported an error: {0}")] + ServerError(String), + #[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 RosLibRustResult = Result; diff --git a/roslibrust/src/ros1/mod.rs b/roslibrust/src/ros1/mod.rs index f8c2e514..2ae9fdb5 100644 --- a/roslibrust/src/ros1/mod.rs +++ b/roslibrust/src/ros1/mod.rs @@ -11,5 +11,7 @@ mod node; pub use node::*; mod publisher; +pub use publisher::Publisher; mod subscriber; +pub use subscriber::Subscriber; mod tcpros; diff --git a/roslibrust/src/ros1/names.rs b/roslibrust/src/ros1/names.rs index ebf9a120..d61a3d93 100644 --- a/roslibrust/src/ros1/names.rs +++ b/roslibrust/src/ros1/names.rs @@ -1,3 +1,6 @@ +use crate::{RosLibRustError, RosLibRustResult}; +use std::fmt::Display; + lazy_static::lazy_static! { static ref GRAPH_NAME_REGEX: regex::Regex = regex::Regex::new(r"^([/~a-zA-Z]){1}([a-zA-Z0-9_/])*([A-z0-9_])$").unwrap(); } @@ -8,12 +11,12 @@ pub struct Name { } impl Name { - pub fn new(name: impl Into) -> Option { + pub fn new(name: impl Into) -> RosLibRustResult { let name: String = name.into(); if is_valid(&name) { - Some(Self { inner: name }) + Ok(Self { inner: name }) } else { - None + Err(RosLibRustError::InvalidName(name)) } } @@ -54,6 +57,12 @@ fn is_valid(name: &str) -> bool { GRAPH_NAME_REGEX.is_match(name) } +impl Display for Name { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + self.inner.fmt(f) + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/roslibrust/src/ros1/node/actor.rs b/roslibrust/src/ros1/node/actor.rs index 47276bfa..c72258a3 100644 --- a/roslibrust/src/ros1/node/actor.rs +++ b/roslibrust/src/ros1/node/actor.rs @@ -5,8 +5,9 @@ use crate::{ node::{XmlRpcServer, XmlRpcServerHandle}, publisher::Publication, subscriber::Subscription, + MasterClient, }, - MasterClient, ServiceCallback, + ServiceCallback, }; use abort_on_drop::ChildTask; use roslibrust_codegen::RosMessageType; @@ -142,14 +143,13 @@ impl NodeServerHandle { pub async fn register_publisher( &self, topic: &str, - topic_type: &str, queue_size: usize, ) -> Result>, Box> { let (sender, receiver) = oneshot::channel(); match self.node_server_sender.send(NodeMsg::RegisterPublisher { reply: sender, topic: topic.to_owned(), - topic_type: topic_type.to_owned(), + topic_type: T::ROS_TYPE_NAME.to_owned(), queue_size, msg_definition: T::DEFINITION.to_owned(), md5sum: T::MD5SUM.to_owned(), @@ -254,12 +254,7 @@ impl Node { let xmlrpc_server = XmlRpcServer::new(addr, xml_server_handle)?; let client_uri = format!("http://{hostname}:{}", xmlrpc_server.port()); - if let None = Name::new(node_name) { - log::error!("Node name {node_name} is not valid"); - return Err(Box::new(std::io::Error::from( - std::io::ErrorKind::InvalidInput, - ))); - } + let _ = Name::new(node_name)?; let rosmaster_client = MasterClient::new(master_uri, client_uri, node_name).await?; let mut node = Self { diff --git a/roslibrust/src/ros1/node/handle.rs b/roslibrust/src/ros1/node/handle.rs index 493ca3a4..75888ec8 100644 --- a/roslibrust/src/ros1/node/handle.rs +++ b/roslibrust/src/ros1/node/handle.rs @@ -41,7 +41,7 @@ impl NodeHandle { ) -> Result, Box> { let sender = self .inner - .register_publisher::(topic_name, T::ROS_TYPE_NAME, queue_size) + .register_publisher::(topic_name, queue_size) .await?; Ok(Publisher::new(topic_name, sender)) } diff --git a/roslibrust/src/ros1/node/mod.rs b/roslibrust/src/ros1/node/mod.rs index 4fa4a1d3..1a167764 100644 --- a/roslibrust/src/ros1/node/mod.rs +++ b/roslibrust/src/ros1/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::RosMasterError; +use super::RosMasterError; use std::net::{IpAddr, Ipv4Addr}; mod actor; diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index bc1db0c6..b25abcca 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -1,6 +1,4 @@ -use crate::RosLibRustError; - -use super::tcpros::ConnectionHeader; +use crate::{ros1::tcpros::ConnectionHeader, RosLibRustError}; use abort_on_drop::ChildTask; use roslibrust_codegen::RosMessageType; use std::{ diff --git a/roslibrust/src/ros1/subscriber.rs b/roslibrust/src/ros1/subscriber.rs index 7d821a4d..7416dca1 100644 --- a/roslibrust/src/ros1/subscriber.rs +++ b/roslibrust/src/ros1/subscriber.rs @@ -1,4 +1,4 @@ -use super::tcpros::ConnectionHeader; +use crate::ros1::tcpros::ConnectionHeader; use abort_on_drop::ChildTask; use roslibrust_codegen::RosMessageType; use std::{marker::PhantomData, sync::Arc}; diff --git a/roslibrust/src/rosbridge/mod.rs b/roslibrust/src/rosbridge/mod.rs index 284b9dec..258c8602 100644 --- a/roslibrust/src/rosbridge/mod.rs +++ b/roslibrust/src/rosbridge/mod.rs @@ -30,46 +30,13 @@ mod topic_provider; mod comm; use futures_util::stream::{SplitSink, SplitStream}; -use log::*; use std::collections::HashMap; use tokio::net::TcpStream; use tokio_tungstenite::*; use tungstenite::Message; -/// 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(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("Rosbridge server reported an error: {0}")] - ServerError(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), -} - -/// Provides an implementation tranlating the underlying websocket error into our error type -impl From for RosLibRustError { - fn from(e: tokio_tungstenite::tungstenite::Error) -> Self { - // TODO we probably want to expand this type and do some matching here - RosLibRustError::CommFailure(e) - } -} - -/// 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; +// Doing this to maintain backwards compatibilities like `use roslibrust::rosbridge::RosLibRustError` +pub use super::{RosLibRustError, RosLibRustResult}; /// Used for type erasure of message type so that we can store arbitrary handles type Callback = Box; From dfcb4b790785169ebeb06ad6499513cd9d8144e2 Mon Sep 17 00:00:00 2001 From: Shane Snover Date: Tue, 19 Dec 2023 19:46:31 -0700 Subject: [PATCH 34/34] Fix some test imports --- roslibrust/src/ros1/master_client.rs | 2 +- roslibrust/tests/ros1_xmlrpc.rs | 11 +++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/roslibrust/src/ros1/master_client.rs b/roslibrust/src/ros1/master_client.rs index 4ceaa819..4a015880 100644 --- a/roslibrust/src/ros1/master_client.rs +++ b/roslibrust/src/ros1/master_client.rs @@ -361,7 +361,7 @@ impl MasterClient { #[cfg(test)] mod test { - use crate::{MasterClient, RosMasterError}; + use super::{MasterClient, RosMasterError}; const TEST_NODE_ID: &str = "/native_ros1_test"; diff --git a/roslibrust/tests/ros1_xmlrpc.rs b/roslibrust/tests/ros1_xmlrpc.rs index 6ed96066..54837788 100644 --- a/roslibrust/tests/ros1_xmlrpc.rs +++ b/roslibrust/tests/ros1_xmlrpc.rs @@ -1,5 +1,6 @@ #[cfg(all(feature = "ros1", feature = "ros1_test"))] mod tests { + use roslibrust::ros1::NodeHandle; use roslibrust_codegen::RosMessageType; use serde::de::DeserializeOwned; use serde_xmlrpc::Value; @@ -27,8 +28,7 @@ mod tests { #[test_log::test(tokio::test)] async fn verify_get_master_uri() -> Result<(), Box> { - let node = - roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_master_uri").await?; + let node = NodeHandle::new("http://localhost:11311", "verify_get_master_uri").await?; log::info!("Got new handle"); let node_uri = node.get_client_uri().await?; @@ -48,8 +48,7 @@ mod tests { #[test_log::test(tokio::test)] async fn verify_get_publications() -> Result<(), Box> { - let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_get_publications") - .await?; + let node = NodeHandle::new("http://localhost:11311", "verify_get_publications").await?; log::info!("Got new handle"); let node_uri = node.get_client_uri().await?; @@ -86,7 +85,7 @@ mod tests { #[test_log::test(tokio::test)] async fn verify_shutdown() { - let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_shutdown") + let node = NodeHandle::new("http://localhost:11311", "verify_shutdown") .await .unwrap(); log::info!("Got handle"); @@ -110,7 +109,7 @@ mod tests { #[test_log::test(tokio::test)] async fn verify_request_topic() { - let node = roslibrust::NodeHandle::new("http://localhost:11311", "verify_request_topic") + let node = NodeHandle::new("http://localhost:11311", "verify_request_topic") .await .unwrap(); log::info!("Got handle");