Add benchmarks for Subscriber and Publisher

.github/workflows/rust.yml

@@ -83,6 +83,6 @@ jobs:
         for path in $(colcon list | awk '$3 == "(ament_cargo)" { print $2 }'); do
         cd $path
         echo "Checking $path"
-        cargo clippy -- -D warnings
+        cargo clippy --all-targets --all-features -- -D warnings
         cd -
-        done
+        done

rclrs/Cargo.toml

@@ -16,10 +16,18 @@ core-error = "0.0.0"
 parking_lot = {version = "0.11.2", optional = true}
 spin = "0.9.2"
 downcast = "0.10.0"
+criterion = "0.3.5"
+
+[[bench]]
+name = "node"
+harness = false
 
 [dependencies.rosidl_runtime_rs]
 version = "*"
 
+[dev-dependencies.std_msgs]
+version = "*"
+
 [build-dependencies]
 bindgen = "0.59.1"
 

rclrs/benches/node.rs

@@ -0,0 +1,190 @@
+use std::env;
+
+use criterion::{Criterion, criterion_group, criterion_main, SamplingMode};
+use cstr_core::CString;
+use std_msgs;
+
+use rclrs::{Context, Publisher, QOS_PROFILE_DEFAULT, RclReturnCode, Subscription};
+
+fn default_context() -> Context {
+    let args: Vec<CString> = env::args()
+        .filter_map(|arg| CString::new(arg).ok())
+        .collect();
+
+    Context::default(args)
+}
+
+fn bench_publisher(c: &mut Criterion) -> Result<(), RclReturnCode> {
+    let mut group = c.benchmark_group("Publisher");
+    group.sampling_mode(SamplingMode::Auto);
+
+    let context = default_context();
+    let node = context.create_node("bench_publisher")?;
+
+
+    // Benchmark publisher creation
+    group.bench_function("Create publisher", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            let _temp_publisher =
+                Publisher::<std_msgs::msg::String>::new(&node, "test", QOS_PROFILE_DEFAULT)?;
+            Ok(())
+        })
+    });
+
+    let mut message = std_msgs::msg::String {
+        data: "Hello world!".to_owned(),
+    };
+    let mut publish_count: u32 = 1;
+    let publisher = Publisher::<std_msgs::msg::String>::new(&node, "test", QOS_PROFILE_DEFAULT)?;
+
+    // Benchmark publish simple string 100 times
+    group.bench_function("Publish", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            // Publish 100 times
+            publish_count = 0;
+            while context.ok()? {
+                message.data = format!("Hello, world! {}", publish_count);
+                publisher.publish(&message)?;
+                publish_count += 1;
+                if publish_count >= 100 {
+                    break;
+                }
+            }
+            Ok(())
+        })
+    });
+
+    group.finish();
+    Ok(())
+}
+
+fn bench_node_creation(c: &mut Criterion) -> Result<(), RclReturnCode> {
+    let mut group = c.benchmark_group("Node");
+    group.sampling_mode(SamplingMode::Auto);
+
+    // Benchmark publisher creation
+    group.bench_function("Create node", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            let context = default_context();
+            let _node = context.create_node("node_test")?;
+            Ok(())
+        })
+    });
+
+
+    group.finish();
+    Ok(())
+}
+
+fn bench_context_creation(c: &mut Criterion) -> Result<(), RclReturnCode> {
+    let mut group = c.benchmark_group("Context");
+    group.sampling_mode(SamplingMode::Auto);
+
+    // Benchmark publisher creation
+    group.bench_function("Create context", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            let _context = default_context();
+            Ok(())
+        })
+    });
+
+
+    group.finish();
+    Ok(())
+}
+
+fn bench_subscriber(c: &mut Criterion) -> Result<(), RclReturnCode> {
+    let mut group = c.benchmark_group("Subscriber");
+    group.sampling_mode(SamplingMode::Auto);
+
+    let context = default_context();
+    let mut node_sub = context.create_node("bench_subscriber")?;
+    let node_pub = context.create_node("bench_publisher")?;
+
+    // Benchmark subscriber creation
+    group.bench_function("Create subscriber", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            let _temp_subscriber = Subscription::<std_msgs::msg::String>::new(
+                &node_pub,
+                "test",
+                QOS_PROFILE_DEFAULT,
+                move |msg: &std_msgs::msg::String| {
+                    let _temp_msg = msg;
+                },
+            )?;
+            Ok(())
+        })
+    });
+
+    let mut message = std_msgs::msg::String {
+        data: "Hello world!".to_owned(),
+    };
+    let mut num_messages_received: usize = 0;
+    let _subscriber = node_sub
+        .create_subscription::<std_msgs::msg::String, _>(
+            "test",
+            QOS_PROFILE_DEFAULT,
+            move |_msg: &std_msgs::msg::String| {
+                num_messages_received = &num_messages_received + 1;
+            },
+        )?;
+
+    let publisher = Publisher::<std_msgs::msg::String>::new(&node_pub, "test", QOS_PROFILE_DEFAULT)?;
+
+
+    let mut publish_count: u32 = 1;
+    // Benchmark publish and receive simple string 100 times
+    group.bench_function("Publish/Receive String", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            // Publish 100 times
+            publish_count = 0;
+            while context.ok()? {
+                message.data = format!("Hello, world! {}", publish_count);
+                publisher.publish(&message)?;
+                publish_count += 1;
+                if publish_count >= 100 {
+                    break;
+                }
+                let _ = rclrs::spin_once(&node_sub, 500);
+            }
+            Ok(())
+        })
+    });
+    num_messages_received = 0;
+    let _subscriber = node_sub
+        .create_subscription::<std_msgs::msg::Int32, _>(
+            "test_int",
+            QOS_PROFILE_DEFAULT,
+            move |_msg: &std_msgs::msg::Int32| {
+                num_messages_received = &num_messages_received + 1;
+            },
+        )?;
+
+    let publisher = Publisher::<std_msgs::msg::Int32>::new(&node_pub, "test_int", QOS_PROFILE_DEFAULT)?;
+
+    let mut message = std_msgs::msg::Int32 {
+        data: 0,
+    };
+
+    // Benchmark publish and receive trivially copyable object 100 times
+    group.bench_function("Publish/Receive i32", |b| {
+        b.iter(|| -> Result<(), RclReturnCode>  {
+            // Publish 100 times
+            publish_count = 0;
+            while context.ok()? {
+                message.data = 42;
+                publisher.publish(&message)?;
+                publish_count += 1;
+                if publish_count >= 100 {
+                    break;
+                }
+                let _ = rclrs::spin_once(&node_sub, 500);
+            }
+            Ok(())
+        })
+    });
+    group.finish();
+    Ok(())
+}
+criterion_group!(benches, bench_publisher, bench_subscriber, bench_node_creation, bench_context_creation);
+criterion_main!(benches);

rclrs/package.xml

@@ -13,6 +13,8 @@
   <build_depend>rosidl_runtime_rs</build_depend>
   <build_depend>rcl</build_depend>
 
+  <test_depend>std_msgs</test_depend>
+
   <export>
     <build_type>ament_cargo</build_type>
   </export>

rclrs/src/context.rs

@@ -11,6 +11,7 @@ use spin::{Mutex, MutexGuard};
 #[cfg(feature = "std")]
 use parking_lot::{Mutex, MutexGuard};
 
+#[derive(Debug)]
 pub(crate) struct ContextHandle(Mutex<rcl_context_t>);
 
 impl ContextHandle {
@@ -31,6 +32,7 @@ impl Drop for ContextHandle {
     }
 }
 
+#[derive(Debug)]
 pub struct Context {
     pub(crate) handle: Arc<ContextHandle>,
 }
@@ -76,3 +78,63 @@ impl Context {
         Node::new(node_name, self)
     }
 }
+
+#[cfg(test)]
+mod tests {
+
+    use super::*;
+    use std::{env, println};
+
+    fn default_context() -> Context {
+        let args: Vec<CString> = env::args()
+            .filter_map(|arg| CString::new(arg).ok())
+            .collect();
+        println!("<test_create_context> Context args: {:?}", args);
+        Context::default(args)
+    }
+
+    #[test]
+    fn test_create_context() {
+        // If the context fails to be created, this will cause a panic
+        let created_context = default_context();
+        println!(
+            "<test_create_context> Created Context: {:?}",
+            created_context
+        );
+    }
+
+    #[test]
+    fn test_context_ok() {
+        // If the context fails to be created, this will cause a panic
+        let created_context = default_context();
+        let ctxt_ok = created_context.ok();
+        match ctxt_ok {
+            Ok(is_ok) => assert!(is_ok),
+            Err(err_code) => panic!(
+                "<test_context_ok> RCL Error occured during test: {:?}",
+                err_code
+            ),
+        }
+    }
+
+    #[test]
+    fn test_create_node() -> Result<(), RclReturnCode> {
+        // If the context fails to be created, this will cause a panic
+        let created_context = default_context();
+        created_context.create_node("Bob").map(|_x| ())
+    }
+
+    #[test]
+    fn text_context_init() {
+        // If the context fails to be created, this will cause a panic
+        let args: Vec<CString> = env::args()
+            .filter_map(|arg| CString::new(arg).ok())
+            .collect();
+        let context = Context {
+            handle: Arc::new(ContextHandle(Mutex::new(unsafe {
+                rcl_get_zero_initialized_context()
+            }))),
+        };
+        context.init(args).unwrap();
+    }
+}

rclrs/src/lib.rs

@@ -118,3 +118,32 @@ pub fn spin_once(node: &Node, timeout: i64) -> Result<(), WaitSetErrorResponse>
 
     Ok(())
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use alloc::vec::Vec;
+    use cstr_core::CString;
+    use std::{env, println};
+
+    #[test]
+    fn test_spin_once() -> Result<(), WaitSetErrorResponse> {
+        let args: Vec<CString> = env::args()
+            .filter_map(|arg| CString::new(arg).ok())
+            .collect();
+        let context = Context::default(args);
+        let mut subscriber_node = context.create_node("minimal_subscriber")?;
+        let mut num_messages: usize = 0;
+        let _subscription = subscriber_node.create_subscription::<std_msgs::msg::String, _>(
+            "topic",
+            QOS_PROFILE_DEFAULT,
+            move |msg: &std_msgs::msg::String| {
+                println!("I heard: '{}'", msg.data);
+                num_messages += 1;
+                println!("(Got {} messages so far)", num_messages);
+            },
+        )?;
+
+        spin_once(&subscriber_node, 500)
+    }
+}

rclrs/src/node/mod.rs

@@ -111,3 +111,54 @@ impl Node {
         Ok(subscription)
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::{Context, Node, QOS_PROFILE_DEFAULT};
+    use std::{env, println};
+    use std_msgs;
+
+    fn default_context() -> Context {
+        let args: Vec<CString> = env::args()
+            .filter_map(|arg| CString::new(arg).ok())
+            .collect();
+        println!("<test_rclrs_mod> Context args: {:?}", args);
+        Context::default(args)
+    }
+
+    #[test]
+    fn test_new_with_namespace() -> Result<(), RclReturnCode> {
+        let context = default_context();
+        Node::new_with_namespace("Bob", "Testing", &context).map(|_x| ())
+    }
+
+    #[test]
+    fn test_new() -> Result<(), RclReturnCode> {
+        let context = default_context();
+        Node::new("Bob", &context).map(|_x| ())
+    }
+
+    #[test]
+    fn test_create_subscription() -> Result<(), RclReturnCode> {
+        let context = default_context();
+        let mut node = context.create_node("test_create_subscription")?;
+        let _subscription = node.create_subscription::<std_msgs::msg::String, _>(
+            "topic",
+            QOS_PROFILE_DEFAULT,
+            move |msg: &std_msgs::msg::String| {
+                println!("Got message: '{}'", msg.data);
+            },
+        )?;
+        Ok(())
+    }
+
+    #[test]
+    fn test_create_publisher() -> Result<(), RclReturnCode> {
+        let context = default_context();
+        let node = context.create_node("test_create_publisher")?;
+        let _publisher =
+            node.create_publisher::<std_msgs::msg::String>("topic", QOS_PROFILE_DEFAULT)?;
+        Ok(())
+    }
+}