diff --git a/examples/message_demo/Cargo.toml b/examples/message_demo/Cargo.toml new file mode 100644 index 0000000..7599608 --- /dev/null +++ b/examples/message_demo/Cargo.toml @@ -0,0 +1,25 @@ +[package] +name = "examples_rclrs_message_demo" +version = "0.4.1" +authors = ["Nikolai Morin "] +edition = "2021" + +[[bin]] +name = "message_demo" +path = "src/message_demo.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} + +[dependencies.rclrs] +version = "0.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[dependencies.rclrs_example_msgs] +version = "0.4" +features = ["serde"] + +[dependencies.serde_json] +version = "1.0" diff --git a/examples/message_demo/package.xml b/examples/message_demo/package.xml new file mode 100644 index 0000000..60519f4 --- /dev/null +++ b/examples/message_demo/package.xml @@ -0,0 +1,23 @@ + + + + examples_rclrs_message_demo + 0.4.1 + Package containing an example of message-related functionality in rclrs. + Nikolai Morin + Apache License 2.0 + + rclrs + rosidl_runtime_rs + rclrs_example_msgs + + rclrs + rosidl_runtime_rs + rclrs_example_msgs + + + ament_cargo + + diff --git a/examples/message_demo/src/message_demo.rs b/examples/message_demo/src/message_demo.rs new file mode 100644 index 0000000..d1fcea1 --- /dev/null +++ b/examples/message_demo/src/message_demo.rs @@ -0,0 +1,185 @@ +use std::{convert::TryInto, env, sync::Arc}; + +use anyhow::{Error, Result}; +use rosidl_runtime_rs::{seq, BoundedSequence, Message, Sequence}; + +fn check_default_values() { + let msg = rclrs_example_msgs::msg::rmw::VariousTypes::default(); + assert!(msg.bool_member); + assert_eq!(msg.int8_member, 1i8); + assert_eq!(msg.uint8_member, 2u8); + assert_eq!(msg.byte_member, 3u8); + assert_eq!(msg.float32_member, 1e-2f32); + assert_eq!(msg.float_array, [1.0, 2.0, 3.0]); + assert_eq!(msg.float_seq_bounded, seq![3 # 4.0, 5.0]); + assert_eq!(msg.float_seq_unbounded, seq![6.0]); + assert_eq!(msg.string_member.to_string(), "Χαίρετε 你好"); + assert_eq!(msg.wstring_member.to_string(), "αντίο σου 再见"); + assert_eq!(msg.bounded_string_member.to_string(), "aou"); + assert_eq!(msg.bounded_wstring_member.to_string(), "äöü"); + assert_eq!( + msg.string_array.clone().map(|s| s.to_string()), + ["R", "O", "S", "2"].map(String::from) + ); + assert_eq!( + msg.string_seq_bounded, + seq![4 # "R".into(), "O".into(), "S".into(), "2".into()] + ); + assert_eq!( + msg.string_seq_unbounded, + seq!["R".into(), "O".into(), "S".into(), "2".into()] + ); + assert_eq!( + msg.bounded_string_array.clone().map(|s| s.to_string()), + ["R", "O", "S", "2"].map(String::from) + ); + assert_eq!( + msg.bounded_string_seq_bounded, + ["R", "O", "S", "2"] + .into_iter() + .map(|s| s.try_into().unwrap()) + .collect() + ); + assert_eq!( + msg.bounded_string_seq_unbounded, + ["R", "O", "S", "2"] + .into_iter() + .map(|s| s.try_into().unwrap()) + .collect() + ); + assert_eq!(msg.nested_member.effect.to_string(), "discombobulate"); + assert_eq!( + msg.nested_array, + [msg.nested_member.clone(), msg.nested_member.clone()] + ); + assert_eq!(msg.nested_seq_bounded, seq![3 #]); + assert_eq!(msg.nested_seq_unbounded, seq![]); + + // The default instance for the idiomatic type also has the defaults set + let idiomatic_msg = rclrs_example_msgs::msg::VariousTypes::default(); + assert_eq!( + rclrs_example_msgs::msg::VariousTypes::into_rmw_message(std::borrow::Cow::Owned( + idiomatic_msg + )) + .into_owned(), + msg + ); +} + +fn check_default_idl_values() { + let idiomatic_msg = rclrs_example_msgs::msg::MyMessage::default(); + let rmw_msg = rclrs_example_msgs::msg::rmw::MyMessage::default(); + + assert_eq!(idiomatic_msg.wchar_value, 0u16); + assert_eq!(rmw_msg.wchar_value, 0u16); +} + +fn demonstrate_printing() { + let default_msg = rclrs_example_msgs::msg::VariousTypes::default(); + println!("================== Compact debug representation =================="); + println!("{:?}", default_msg); + println!("================== Pretty debug representation ==================="); + println!("{:#?}", default_msg); + // The RMW-native message type has the same output + let default_rmw_msg = rclrs_example_msgs::msg::rmw::VariousTypes::default(); + assert_eq!( + format!("{:?}", default_msg), + format!("{:?}", default_rmw_msg) + ); + assert_eq!( + format!("{:#?}", default_msg), + format!("{:#?}", default_rmw_msg) + ); +} + +fn demonstrate_serde() -> Result<(), Error> { + // When the serde feature is turned on, messages are able to be serialized + // to and deserialized from a variety of formats. Here JSON is used as an + // example. + // Works with RMW-native and idiomatic messages. + let idiomatic_msg = rclrs_example_msgs::msg::VariousTypes::default(); + let rmw_msg = rclrs_example_msgs::msg::rmw::VariousTypes::default(); + println!("================= JSON serialization with Serde =================="); + let idiomatic_serialized = serde_json::to_string_pretty(&idiomatic_msg)?; + let rmw_serialized = serde_json::to_string_pretty(&rmw_msg)?; + assert_eq!(idiomatic_serialized, rmw_serialized); + println!("{}", rmw_serialized); + let idiomatic_deserialized = serde_json::from_str(&idiomatic_serialized)?; + let rmw_deserialized = serde_json::from_str(&rmw_serialized)?; + assert_eq!(idiomatic_msg, idiomatic_deserialized); + assert_eq!(rmw_msg, rmw_deserialized); + Ok(()) +} + +fn demonstrate_sequences() { + // Convenient creation of (bounded) sequences with the seq! macro + // This one has three items and a length bound of 5 + let mut float_seq_bounded = seq![5 # 1.0, 2.0, 3.0]; + // Sequences and bounded sequences have iter(), iter_mut(), and into_iter() + float_seq_bounded + .iter_mut() + .for_each(|n: &mut f32| *n += 1.0); + let float_vec_1: Vec<_> = float_seq_bounded.iter().copied().collect(); + let float_vec_2: Vec<_> = float_seq_bounded.into_iter().collect(); + assert_eq!(float_vec_1, float_vec_2); + // Sequences also implement FromIterator. + let mut int_seq_unbounded: Sequence = [42; 4].into_iter().collect(); + // Bounded sequences will ignore remaining items once the length bound is reached + let mut int_seq_bounded: BoundedSequence = [42; 4].into_iter().collect(); + // Sequences deref to slices + int_seq_bounded[2] = 24; + assert_eq!(int_seq_bounded.last(), Some(&24)); + int_seq_unbounded[2..].copy_from_slice(&int_seq_bounded[1..]); + // New sequences will contain default values – and 0 for primitive types + let seq_with_default_values = Sequence::::new(1); + assert_eq!(seq_with_default_values[0].effect, "discombobulate".into()); +} + +fn demonstrate_pubsub() -> Result<(), Error> { + println!("================== Interoperability demo =================="); + // Demonstrate interoperability between idiomatic and RMW-native message types + let context = rclrs::Context::new(env::args())?; + let node = rclrs::create_node(&context, "message_demo")?; + + let idiomatic_publisher = node.create_publisher::( + "topic", + rclrs::QOS_PROFILE_DEFAULT, + )?; + let direct_publisher = node.create_publisher::( + "topic", + rclrs::QOS_PROFILE_DEFAULT, + )?; + + let _idiomatic_subscription = node + .create_subscription::( + "topic", + rclrs::QOS_PROFILE_DEFAULT, + move |_msg: rclrs_example_msgs::msg::VariousTypes| println!("Got idiomatic message!"), + )?; + let _direct_subscription = node + .create_subscription::( + "topic", + rclrs::QOS_PROFILE_DEFAULT, + move |_msg: rclrs_example_msgs::msg::rmw::VariousTypes| { + println!("Got RMW-native message!") + }, + )?; + println!("Sending idiomatic message."); + idiomatic_publisher.publish(rclrs_example_msgs::msg::VariousTypes::default())?; + rclrs::spin_once(Arc::clone(&node), None)?; + println!("Sending RMW-native message."); + direct_publisher.publish(rclrs_example_msgs::msg::rmw::VariousTypes::default())?; + rclrs::spin_once(Arc::clone(&node), None)?; + + Ok(()) +} + +fn main() -> Result<(), Error> { + check_default_values(); + check_default_idl_values(); + demonstrate_printing(); + demonstrate_serde()?; + demonstrate_sequences(); + demonstrate_pubsub()?; + Ok(()) +} diff --git a/examples/minimal_client_service/Cargo.toml b/examples/minimal_client_service/Cargo.toml new file mode 100644 index 0000000..618b002 --- /dev/null +++ b/examples/minimal_client_service/Cargo.toml @@ -0,0 +1,30 @@ +[package] +name = "examples_rclrs_minimal_client_service" +version = "0.4.1" +authors = ["Esteve Fernandez "] +edition = "2021" + +[[bin]] +name = "minimal_client" +path = "src/minimal_client.rs" + +[[bin]] +name = "minimal_client_async" +path = "src/minimal_client_async.rs" + +[[bin]] +name = "minimal_service" +path = "src/minimal_service.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} +tokio = { version = "1", features = ["macros", "rt", "rt-multi-thread", "time"] } + +[dependencies.rclrs] +version = "0.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[dependencies.example_interfaces] +version = "*" diff --git a/examples/minimal_client_service/package.xml b/examples/minimal_client_service/package.xml new file mode 100644 index 0000000..dd4e41b --- /dev/null +++ b/examples/minimal_client_service/package.xml @@ -0,0 +1,23 @@ + + + + examples_rclrs_minimal_client_service + 0.4.1 + Package containing an example of the client-service mechanism in rclrs. + Esteve Fernandez + Apache License 2.0 + + example_interfaces + rclrs + rosidl_runtime_rs + + example_interfaces + rclrs + rosidl_runtime_rs + + + ament_cargo + + diff --git a/examples/minimal_client_service/src/minimal_client.rs b/examples/minimal_client_service/src/minimal_client.rs new file mode 100644 index 0000000..915541d --- /dev/null +++ b/examples/minimal_client_service/src/minimal_client.rs @@ -0,0 +1,34 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_client")?; + + let client = node.create_client::("add_two_ints")?; + + let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; + + println!("Starting client"); + + while !client.service_is_ready()? { + std::thread::sleep(std::time::Duration::from_millis(10)); + } + + client.async_send_request_with_callback( + &request, + move |response: example_interfaces::srv::AddTwoInts_Response| { + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum + ); + }, + )?; + + std::thread::sleep(std::time::Duration::from_millis(500)); + + println!("Waiting for response"); + rclrs::spin(node).map_err(|err| err.into()) +} diff --git a/examples/minimal_client_service/src/minimal_client_async.rs b/examples/minimal_client_service/src/minimal_client_async.rs new file mode 100644 index 0000000..0eeb87f --- /dev/null +++ b/examples/minimal_client_service/src/minimal_client_async.rs @@ -0,0 +1,35 @@ +use std::env; + +use anyhow::{Error, Result}; + +#[tokio::main] +async fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_client")?; + + let client = node.create_client::("add_two_ints")?; + + println!("Starting client"); + + while !client.service_is_ready()? { + std::thread::sleep(std::time::Duration::from_millis(10)); + } + + let request = example_interfaces::srv::AddTwoInts_Request { a: 41, b: 1 }; + + let future = client.call_async(&request); + + println!("Waiting for response"); + + let rclrs_spin = tokio::task::spawn_blocking(move || rclrs::spin(node)); + + let response = future.await?; + println!( + "Result of {} + {} is: {}", + request.a, request.b, response.sum + ); + + rclrs_spin.await.ok(); + Ok(()) +} diff --git a/examples/minimal_client_service/src/minimal_service.rs b/examples/minimal_client_service/src/minimal_service.rs new file mode 100644 index 0000000..b4149c8 --- /dev/null +++ b/examples/minimal_client_service/src/minimal_service.rs @@ -0,0 +1,25 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn handle_service( + _request_header: &rclrs::rmw_request_id_t, + request: example_interfaces::srv::AddTwoInts_Request, +) -> example_interfaces::srv::AddTwoInts_Response { + println!("request: {} + {}", request.a, request.b); + example_interfaces::srv::AddTwoInts_Response { + sum: request.a + request.b, + } +} + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_service")?; + + let _server = node + .create_service::("add_two_ints", handle_service)?; + + println!("Starting server"); + rclrs::spin(node).map_err(|err| err.into()) +} diff --git a/examples/minimal_pub_sub/Cargo.toml b/examples/minimal_pub_sub/Cargo.toml new file mode 100644 index 0000000..52c4f05 --- /dev/null +++ b/examples/minimal_pub_sub/Cargo.toml @@ -0,0 +1,41 @@ +[package] +name = "examples_rclrs_minimal_pub_sub" +version = "0.4.1" +# This project is not military-sponsored, Jacob's employment contract just requires him to use this email address +authors = ["Esteve Fernandez ", "Nikolai Morin ", "Jacob Hassold "] +edition = "2021" + +[[bin]] +name = "minimal_subscriber" +path = "src/minimal_subscriber.rs" + +[[bin]] +name = "minimal_publisher" +path = "src/minimal_publisher.rs" + +[[bin]] +name = "minimal_two_nodes" +path = "src/minimal_two_nodes.rs" + +[[bin]] +name = "zero_copy_subscriber" +path = "src/zero_copy_subscriber.rs" + +[[bin]] +name = "zero_copy_publisher" +path = "src/zero_copy_publisher.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} + +[dependencies.rclrs] +version = "0.4" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + +[dependencies.std_msgs] +version = "*" + +[package.metadata.ros] +install_to_share = ["launch"] diff --git a/examples/minimal_pub_sub/launch/minimal_pub_sub.launch.xml b/examples/minimal_pub_sub/launch/minimal_pub_sub.launch.xml new file mode 100644 index 0000000..6d0513b --- /dev/null +++ b/examples/minimal_pub_sub/launch/minimal_pub_sub.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/examples/minimal_pub_sub/package.xml b/examples/minimal_pub_sub/package.xml new file mode 100644 index 0000000..da3e760 --- /dev/null +++ b/examples/minimal_pub_sub/package.xml @@ -0,0 +1,26 @@ + + + + examples_rclrs_minimal_pub_sub + 0.4.1 + Package containing an example of the publish-subscribe mechanism in rclrs. + Esteve Fernandez + Nikolai Morin + + Jacob Hassold + Apache License 2.0 + + rclrs + rosidl_runtime_rs + std_msgs + + rclrs + rosidl_runtime_rs + std_msgs + + + ament_cargo + + diff --git a/examples/minimal_pub_sub/src/minimal_publisher.rs b/examples/minimal_pub_sub/src/minimal_publisher.rs new file mode 100644 index 0000000..7200869 --- /dev/null +++ b/examples/minimal_pub_sub/src/minimal_publisher.rs @@ -0,0 +1,25 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_publisher")?; + + let publisher = + node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + + let mut message = std_msgs::msg::String::default(); + + let mut publish_count: u32 = 1; + + while context.ok() { + message.data = format!("Hello, world! {}", publish_count); + println!("Publishing: [{}]", message.data); + publisher.publish(&message)?; + publish_count += 1; + std::thread::sleep(std::time::Duration::from_millis(500)); + } + Ok(()) +} diff --git a/examples/minimal_pub_sub/src/minimal_subscriber.rs b/examples/minimal_pub_sub/src/minimal_subscriber.rs new file mode 100644 index 0000000..ebc5fc1 --- /dev/null +++ b/examples/minimal_pub_sub/src/minimal_subscriber.rs @@ -0,0 +1,23 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_subscriber")?; + + let mut num_messages: usize = 0; + + let _subscription = node.create_subscription::( + "topic", + rclrs::QOS_PROFILE_DEFAULT, + move |msg: std_msgs::msg::String| { + num_messages += 1; + println!("I heard: '{}'", msg.data); + println!("(Got {} messages so far)", num_messages); + }, + )?; + + rclrs::spin(node).map_err(|err| err.into()) +} diff --git a/examples/minimal_pub_sub/src/minimal_two_nodes.rs b/examples/minimal_pub_sub/src/minimal_two_nodes.rs new file mode 100644 index 0000000..fb03574 --- /dev/null +++ b/examples/minimal_pub_sub/src/minimal_two_nodes.rs @@ -0,0 +1,81 @@ +use std::{ + env, + sync::{ + atomic::{AtomicU32, Ordering}, + Arc, Mutex, + }, +}; + +use anyhow::{Error, Result}; + +struct MinimalSubscriber { + num_messages: AtomicU32, + node: Arc, + subscription: Mutex>>>, +} + +impl MinimalSubscriber { + pub fn new(name: &str, topic: &str) -> Result, rclrs::RclrsError> { + let context = rclrs::Context::new(env::args())?; + let node = rclrs::create_node(&context, name)?; + let minimal_subscriber = Arc::new(MinimalSubscriber { + num_messages: 0.into(), + node, + subscription: None.into(), + }); + + let minimal_subscriber_aux = Arc::clone(&minimal_subscriber); + let subscription = minimal_subscriber + .node + .create_subscription::( + topic, + rclrs::QOS_PROFILE_DEFAULT, + move |msg: std_msgs::msg::String| { + minimal_subscriber_aux.callback(msg); + }, + )?; + *minimal_subscriber.subscription.lock().unwrap() = Some(subscription); + Ok(minimal_subscriber) + } + + fn callback(&self, msg: std_msgs::msg::String) { + self.num_messages.fetch_add(1, Ordering::SeqCst); + println!("[{}] I heard: '{}'", self.node.name(), msg.data); + println!( + "[{}] (Got {} messages so far)", + self.node.name(), + self.num_messages.load(Ordering::SeqCst) + ); + } +} + +fn main() -> Result<(), Error> { + let publisher_context = rclrs::Context::new(env::args())?; + let publisher_node = rclrs::create_node(&publisher_context, "minimal_publisher")?; + + let subscriber_node_one = MinimalSubscriber::new("minimal_subscriber_one", "topic")?; + let subscriber_node_two = MinimalSubscriber::new("minimal_subscriber_two", "topic")?; + + let publisher = publisher_node + .create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + + std::thread::spawn(move || -> Result<(), rclrs::RclrsError> { + let mut message = std_msgs::msg::String::default(); + let mut publish_count: u32 = 1; + loop { + message.data = format!("Hello, world! {}", publish_count); + println!("Publishing: [{}]", message.data); + publisher.publish(&message)?; + publish_count += 1; + std::thread::sleep(std::time::Duration::from_millis(500)); + } + }); + + let executor = rclrs::SingleThreadedExecutor::new(); + + executor.add_node(&publisher_node)?; + executor.add_node(&subscriber_node_one.node)?; + executor.add_node(&subscriber_node_two.node)?; + + executor.spin().map_err(|err| err.into()) +} diff --git a/examples/minimal_pub_sub/src/zero_copy_publisher.rs b/examples/minimal_pub_sub/src/zero_copy_publisher.rs new file mode 100644 index 0000000..5e73b5d --- /dev/null +++ b/examples/minimal_pub_sub/src/zero_copy_publisher.rs @@ -0,0 +1,24 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_publisher")?; + + let publisher = + node.create_publisher::("topic", rclrs::QOS_PROFILE_DEFAULT)?; + + let mut publish_count: u32 = 1; + + while context.ok() { + let mut message = publisher.borrow_loaned_message()?; + message.data = publish_count; + println!("Publishing: {}", message.data); + message.publish()?; + publish_count += 1; + std::thread::sleep(std::time::Duration::from_millis(500)); + } + Ok(()) +} diff --git a/examples/minimal_pub_sub/src/zero_copy_subscriber.rs b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs new file mode 100644 index 0000000..9551dba --- /dev/null +++ b/examples/minimal_pub_sub/src/zero_copy_subscriber.rs @@ -0,0 +1,23 @@ +use std::env; + +use anyhow::{Error, Result}; + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "minimal_subscriber")?; + + let mut num_messages: usize = 0; + + let _subscription = node.create_subscription::( + "topic", + rclrs::QOS_PROFILE_DEFAULT, + move |msg: rclrs::ReadOnlyLoanedMessage<'_, std_msgs::msg::UInt32>| { + num_messages += 1; + println!("I heard: '{}'", msg.data); + println!("(Got {} messages so far)", num_messages); + }, + )?; + + rclrs::spin(node).map_err(|err| err.into()) +} diff --git a/examples/rclrs_example_msgs/CMakeLists.txt b/examples/rclrs_example_msgs/CMakeLists.txt new file mode 100644 index 0000000..f5f76cc --- /dev/null +++ b/examples/rclrs_example_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclrs_example_msgs) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +set(msg_files + "msg/NestedType.msg" + "msg/VariousTypes.msg" + "msg/MyMessage.idl" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/examples/rclrs_example_msgs/msg/MyMessage.idl b/examples/rclrs_example_msgs/msg/MyMessage.idl new file mode 100644 index 0000000..d33f9bb --- /dev/null +++ b/examples/rclrs_example_msgs/msg/MyMessage.idl @@ -0,0 +1,86 @@ +// Taken and slightly adapted from +// https://github.com/ros2/rosidl/blob/iron/rosidl_parser/test/msg/MyMessage.idl + +module rclrs_example_msgs { + module msg { + module MyMessage_Constants { + const short SHORT_CONSTANT = -23; + const unsigned long UNSIGNED_LONG_CONSTANT = 42; + const float FLOAT_CONSTANT = 1.25; + const boolean BOOLEAN_CONSTANT = TRUE; + const string STRING_CONSTANT = "string_value"; + const wstring WSTRING_CONSTANT = "wstring_value_\u2122"; + const string EMPTY_STRING_CONSTANT = ""; + }; + + @verbatim ( language="comment", text="Documentation of MyMessage." "Adjacent string literal." ) + @transfer_mode(SHMEM_REF) + struct MyMessage { + short short_value, short_value2; + @default ( value=123 ) + unsigned short unsigned_short_value; + @key + @range ( min=-10, max=10 ) + long long_value; + @verbatim (language="comment", text="") + unsigned long unsigned_long_value; + long long long_long_value; + unsigned long long unsigned_long_long_value; + float float_value; + double double_value; +// long double long_double_value; + char char_value; + wchar wchar_value; + boolean boolean_value; + octet octet_value; + int8 int8_value; + uint8 uint8_value; + int16 int16_value; + uint16 uint16_value; + int32 int32_value; + uint32 uint32_value; + int64 int64_value; + uint64 uint64_value; + string string_value; + string<5> bounded_string_value; + wstring wstring_value; + wstring<23> bounded_wstring_value; +// wstring constant_bounded_wstring_value; + sequence unbounded_short_values; + sequence bounded_short_values; + sequence> unbounded_values_of_bounded_strings; + sequence, 4> bounded_values_of_bounded_strings; + short array_short_values[23]; + + // Tests of the floating point parser (7.2.6.4) + @default ( value=1.9e10 ) + float int_and_frac_with_positive_scientific; + @default ( value=1.9e+10 ) + float int_and_frac_with_explicit_positive_scientific; + @default ( value=1.1e-10) + float int_and_frac_with_negative_scientific; + @default ( value=0.00009 ) + float int_and_frac; + @default ( value = 1. ) + float int_with_empty_frac; + @default ( value = .1 ) + float frac_only; + @default ( value=9e05 ) + float int_with_positive_scientific; + @default ( value=9e+05 ) + float int_with_explicit_positive_scientific; + @default ( value=9e-05 ) + float int_with_negative_scientific; + + // Tests of the fixed point parser (7.2.6.5) + @default ( value=8.7d ) + float fixed_int_and_frac; + @default ( value=4.d ) + float fixed_int_with_dot_only; + @default ( value=.3d ) + float fixed_frac_only; + @default ( value=7d ) + float fixed_int_only; + }; + }; +}; diff --git a/examples/rclrs_example_msgs/msg/NestedType.msg b/examples/rclrs_example_msgs/msg/NestedType.msg new file mode 100644 index 0000000..471740a --- /dev/null +++ b/examples/rclrs_example_msgs/msg/NestedType.msg @@ -0,0 +1 @@ +string effect "discombobulate" \ No newline at end of file diff --git a/examples/rclrs_example_msgs/msg/VariousTypes.msg b/examples/rclrs_example_msgs/msg/VariousTypes.msg new file mode 100644 index 0000000..ec990f0 --- /dev/null +++ b/examples/rclrs_example_msgs/msg/VariousTypes.msg @@ -0,0 +1,41 @@ +# Primitive types +bool bool_member true +int8 int8_member 1 +uint8 uint8_member 2 +byte byte_member 3 +float32 float32_member 1e-2 + +# Array/sequence of primitive type +float32[3] float_array [1.0, 2.0, 3.0] +float32[<=3] float_seq_bounded [4.0, 5.0] +float32[] float_seq_unbounded [6.0] + +# String types +string string_member "Χαίρετε 你好" +wstring wstring_member "αντίο σου 再见" +string<=3 bounded_string_member "aou" +wstring<=3 bounded_wstring_member "äöü" + +# Array/sequence of string type +string[4] string_array ["R", "O", "S", "2"] +string[<=4] string_seq_bounded ["R", "O", "S", "2"] +string[] string_seq_unbounded ["R", "O", "S", "2"] +string<=1[4] bounded_string_array ["R", "O", "S", "2"] +string<=1[<=4] bounded_string_seq_bounded ["R", "O", "S", "2"] +string<=1[] bounded_string_seq_unbounded ["R", "O", "S", "2"] + +# Nested type +NestedType nested_member + +# Array/sequence of nested type +NestedType[2] nested_array +NestedType[] nested_seq_unbounded +NestedType[<=3] nested_seq_bounded + + +# binary, hexadecimal and octal constants are also possible +int8 TWO_PLUS_TWO = 5 +# Only unbounded strings are possible +string PASSWORD = "hunter2" +# As determined by Edward J. Goodwin +float32 PI = 3.0 diff --git a/examples/rclrs_example_msgs/package.xml b/examples/rclrs_example_msgs/package.xml new file mode 100644 index 0000000..a667786 --- /dev/null +++ b/examples/rclrs_example_msgs/package.xml @@ -0,0 +1,22 @@ + + + + rclrs_example_msgs + 0.4.1 + A package containing some example message definitions. + Nikolai Morin + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/examples/rust_pubsub/Cargo.toml b/examples/rust_pubsub/Cargo.toml new file mode 100644 index 0000000..4876675 --- /dev/null +++ b/examples/rust_pubsub/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "rust_pubsub" +version = "0.1.0" +edition = "2021" + +[[bin]] +name="simple_publisher" +path="src/simple_publisher.rs" + +[[bin]] +name="simple_subscriber" +path="src/simple_subscriber.rs" + +[dependencies] +rclrs = "*" +std_msgs = "*" diff --git a/examples/rust_pubsub/package.xml b/examples/rust_pubsub/package.xml new file mode 100644 index 0000000..f988ccf --- /dev/null +++ b/examples/rust_pubsub/package.xml @@ -0,0 +1,14 @@ + + rust_pubsub + 0.0.0 + TODO: Package description. + user + TODO: License declaration. + + rclrs + std_msgs + + + ament_cargo + + diff --git a/examples/rust_pubsub/src/simple_publisher.rs b/examples/rust_pubsub/src/simple_publisher.rs new file mode 100644 index 0000000..98d0e0f --- /dev/null +++ b/examples/rust_pubsub/src/simple_publisher.rs @@ -0,0 +1,36 @@ +use rclrs::{create_node, Context, Node, Publisher, RclrsError, QOS_PROFILE_DEFAULT}; +use std::{sync::Arc, thread, time::Duration}; +use std_msgs::msg::String as StringMsg; +struct SimplePublisherNode { + node: Arc, + _publisher: Arc>, +} +impl SimplePublisherNode { + fn new(context: &Context) -> Result { + let node = create_node(context, "simple_publisher").unwrap(); + let _publisher = node + .create_publisher("publish_hello", QOS_PROFILE_DEFAULT) + .unwrap(); + Ok(Self { node, _publisher }) + } + + fn publish_data(&self, increment: i32) -> Result { + let msg: StringMsg = StringMsg { + data: format!("Hello World {}", increment), + }; + self._publisher.publish(msg).unwrap(); + Ok(increment + 1_i32) + } +} + +fn main() -> Result<(), RclrsError> { + let context = Context::new(std::env::args()).unwrap(); + let publisher = Arc::new(SimplePublisherNode::new(&context).unwrap()); + let publisher_other_thread = Arc::clone(&publisher); + let mut count: i32 = 0; + thread::spawn(move || loop { + thread::sleep(Duration::from_millis(1000)); + count = publisher_other_thread.publish_data(count).unwrap(); + }); + rclrs::spin(publisher.node.clone()) +} diff --git a/examples/rust_pubsub/src/simple_subscriber.rs b/examples/rust_pubsub/src/simple_subscriber.rs new file mode 100644 index 0000000..a0d02bb --- /dev/null +++ b/examples/rust_pubsub/src/simple_subscriber.rs @@ -0,0 +1,52 @@ +use rclrs::{create_node, Context, Node, RclrsError, Subscription, QOS_PROFILE_DEFAULT}; +use std::{ + env, + sync::{Arc, Mutex}, + thread, + time::Duration, +}; +use std_msgs::msg::String as StringMsg; +pub struct SimpleSubscriptionNode { + node: Arc, + _subscriber: Arc>, + data: Arc>>, +} +impl SimpleSubscriptionNode { + fn new(context: &Context) -> Result { + let node = create_node(context, "simple_subscription").unwrap(); + let data: Arc>> = Arc::new(Mutex::new(None)); + let data_mut: Arc>> = Arc::clone(&data); + let _subscriber = node + .create_subscription::( + "publish_hello", + QOS_PROFILE_DEFAULT, + move |msg: StringMsg| { + *data_mut.lock().unwrap() = Some(msg); + }, + ) + .unwrap(); + Ok(Self { + node, + _subscriber, + data, + }) + } + fn data_callback(&self) -> Result<(), RclrsError> { + if let Some(data) = self.data.lock().unwrap().as_ref() { + println!("{}", data.data); + } else { + println!("No message available yet."); + } + Ok(()) + } +} +fn main() -> Result<(), RclrsError> { + let context = Context::new(env::args()).unwrap(); + let subscription = Arc::new(SimpleSubscriptionNode::new(&context).unwrap()); + let subscription_other_thread = Arc::clone(&subscription); + thread::spawn(move || loop { + thread::sleep(Duration::from_millis(1000)); + subscription_other_thread.data_callback().unwrap() + }); + rclrs::spin(subscription.node.clone()) +}