diff --git a/.gitignore b/.gitignore index 96b8a87..d1d04ef 100644 --- a/.gitignore +++ b/.gitignore @@ -2,10 +2,4 @@ Cargo.lock -# code generation -msg_gen/src/introspection_functions.rs -msg_gen/src/msg_bindings.rs -msg_gen/src/msg_includes.h -rcl/src/rcl_bindings.rs -rcl/src/rcl_wrapper.h -src/generated_msgs.rs +.envrc diff --git a/Cargo.toml b/Cargo.toml index 802b397..ec80080 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "r2r" -version = "0.0.6" +version = "0.0.8" authors = ["Martin Dahl "] description = "Minimal ros2 bindings." license = "Apache-2.0/MIT" @@ -10,8 +10,7 @@ edition = "2018" [dependencies] serde = { version = "1.0.123", features = ["derive"] } serde_json = "1.0.62" -failure = "0.1.8" -failure_derive = "0.1.8" +thiserror = "1.0" lazy_static = "1.4.0" common = { path = "common", version = "0.0.3" } rcl = { path = "rcl", version = "0.0.3" } diff --git a/README.md b/README.md index 3134f5a..7e89787 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,10 @@ A couple of examples are included in examples/ . /opt/ros/foxy/setup.sh cargo build cargo run --example subscriber_with_thread +# In other shell +ros2 topic pub /hi std_msgs/msg/String "data: 'Hello, world!'" ``` + An example application can be found here , which can be installed by running cargo install --git https://github.com/sequenceplanner/r2r-echo. What works? diff --git a/build.rs b/build.rs index 71b517f..3f6864c 100644 --- a/build.rs +++ b/build.rs @@ -3,21 +3,29 @@ use msg_gen; use std::env; use std::fs::File; use std::io::Write; -use std::path::{Path,PathBuf}; +use std::path::{Path, PathBuf}; fn main() { common::print_cargo_watches(); let msg_list = if let Some(cmake_includes) = env::var("CMAKE_INCLUDE_DIRS").ok() { - let packages = cmake_includes.split(":").flat_map(|i| Path::new(i).parent()).collect::>(); + let packages = cmake_includes + .split(":") + .flat_map(|i| Path::new(i).parent()) + .collect::>(); let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default()); let deps = deps.split(":").collect::>(); let msgs = common::get_ros_msgs(&packages); - common::parse_msgs(&msgs).into_iter() - .filter(|msg| deps.contains(&msg.module.as_str())).collect::>() + common::parse_msgs(&msgs) + .into_iter() + .filter(|msg| deps.contains(&msg.module.as_str())) + .collect::>() } else { let ament_prefix_var = env::var("AMENT_PREFIX_PATH").expect("Source your ROS!"); - let paths = ament_prefix_var.split(":").map(|i| Path::new(i)).collect::>(); + let paths = ament_prefix_var + .split(":") + .map(|i| Path::new(i)) + .collect::>(); let msgs = common::get_ros_msgs(&paths); common::parse_msgs(&msgs) }; @@ -26,7 +34,11 @@ fn main() { let mut modules = String::new(); for (module, prefixes) in &msgs { - modules.push_str(&format!(r#"pub mod {module}{{include!(concat!(env!("OUT_DIR"), "/{module}.rs"));}}{lf}"#, module=module, lf="\n")); + modules.push_str(&format!( + r#"pub mod {module}{{include!(concat!(env!("OUT_DIR"), "/{module}.rs"));}}{lf}"#, + module = module, + lf = "\n" + )); let mut codegen = String::new(); for (prefix, msgs) in prefixes { diff --git a/common/src/lib.rs b/common/src/lib.rs index 324e74d..9e12e0a 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -14,7 +14,7 @@ pub fn print_cargo_watches() { pub struct RosMsg { pub module: String, // e.g. std_msgs pub prefix: String, // e.g. "msg" or "srv" - pub name: String, // e.g. "String" + pub name: String, // e.g. "String" } fn get_msgs_from_package(package: &Path) -> Vec { @@ -28,7 +28,6 @@ fn get_msgs_from_package(package: &Path) -> Vec { let mut msgs = vec![]; if let Ok(paths) = fs::read_dir(path) { - for path in paths { // println!("PATH Name: {}", path.unwrap().path().display()); @@ -45,14 +44,14 @@ fn get_msgs_from_package(package: &Path) -> Vec { lines.for_each(|l| { if l.starts_with("msg/") && (l.ends_with(".idl") || l.ends_with(".msg")) { if let Some(file_name_str) = file_name.to_str() { - let substr = &l[4..l.len()-4]; + let substr = &l[4..l.len() - 4]; let msg_name = format!("{}/msg/{}", file_name_str, substr); msgs.push(msg_name); } } if l.starts_with("srv/") && (l.ends_with(".idl") || l.ends_with(".srv")) { if let Some(file_name_str) = file_name.to_str() { - let substr = &l[4..l.len()-4]; + let substr = &l[4..l.len() - 4]; let srv_name = format!("{}/srv/{}", file_name_str, substr); msgs.push(srv_name); } @@ -60,9 +59,9 @@ fn get_msgs_from_package(package: &Path) -> Vec { if l.starts_with("action/") && (l.ends_with(".idl") || l.ends_with(".action")) { if let Some(file_name_str) = file_name.to_str() { let substr = if l.ends_with(".action") { - &l[7..l.len()-7] + &l[7..l.len() - 7] } else { - &l[7..l.len()-4] // .idl + &l[7..l.len() - 4] // .idl }; let action_name = format!("{}/action/{}", file_name_str, substr); println!("found action: {}", action_name); @@ -93,23 +92,38 @@ pub fn get_ros_msgs(paths: &[&Path]) -> Vec { } pub fn parse_msgs(msgs: &Vec) -> Vec { - let v: Vec> = msgs.iter().map(|l| l.split("/").into_iter().take(3).collect()).collect(); - let v: Vec<_> = v.iter().filter(|v|v.len() == 3). - map(|v| RosMsg { module: v[0].into(), prefix: v[1].into(), name: v[2].into()}).collect(); + let v: Vec> = msgs + .iter() + .map(|l| l.split("/").into_iter().take(3).collect()) + .collect(); + let v: Vec<_> = v + .iter() + .filter(|v| v.len() == 3) + .map(|v| RosMsg { + module: v[0].into(), + prefix: v[1].into(), + name: v[2].into(), + }) + .collect(); // hack because I don't have time to find out the root cause of this at the moment. // for some reason the library files generated to this are called // liblibstatistics_collector_test_msgs__..., but I don't know where test_msgs come from. // (this seems to be a useless package anyway) // also affects message generation below. - v.into_iter().filter(|v| v.module != "libstatistics_collector").collect() + v.into_iter() + .filter(|v| v.module != "libstatistics_collector") + .collect() } pub fn as_map(included_msgs: &[RosMsg]) -> HashMap<&str, HashMap<&str, Vec<&str>>> { let mut msgs = HashMap::new(); for msg in included_msgs { - msgs.entry(msg.module.as_str()).or_insert(HashMap::new()).entry(msg.prefix.as_str()). - or_insert(Vec::new()).push(msg.name.as_str()); + msgs.entry(msg.module.as_str()) + .or_insert(HashMap::new()) + .entry(msg.prefix.as_str()) + .or_insert(Vec::new()) + .push(msg.name.as_str()); } msgs } @@ -125,7 +139,7 @@ std_msgs/msg/Bool x/y std_msgs/msg/String "; - let msgs = msgs.lines().map(|l|l.to_string()).collect(); + let msgs = msgs.lines().map(|l| l.to_string()).collect(); let parsed = parse_msgs(&msgs); assert_eq!(parsed[0].module, "std_msgs"); assert_eq!(parsed[0].prefix, "msg"); @@ -142,15 +156,14 @@ std_msgs/msg/Bool x/y std_msgs/msg/String "; - let msgs: Vec = msgs.lines().map(|l|l.to_string()).collect(); + let msgs: Vec = msgs.lines().map(|l| l.to_string()).collect(); let parsed = parse_msgs(&msgs); let map = as_map(&parsed); assert_eq!(map.get("std_msgs").unwrap().get("msg").unwrap()[0], "Bool"); - assert_eq!(map.get("std_msgs").unwrap().get("msg").unwrap()[1], "String"); - - + assert_eq!( + map.get("std_msgs").unwrap().get("msg").unwrap()[1], + "String" + ); } - - } diff --git a/examples/client.rs b/examples/client.rs index 29aff6b..98300da 100644 --- a/examples/client.rs +++ b/examples/client.rs @@ -1,9 +1,8 @@ use r2r; -use failure::Error; use r2r::example_interfaces::srv::AddTwoInts; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "testnode", "")?; let client = node.create_client::("/add_two_ints")?; @@ -18,18 +17,18 @@ fn main() -> Result<(), Error> { let mut c = 0; loop { - let req = AddTwoInts::Request { a: 10*c, b: c }; + let req = AddTwoInts::Request { a: 10 * c, b: c }; let cb_req = req.clone(); - let cb = Box::new(move |r: AddTwoInts::Response| - println!("{} + {} = {}", cb_req.a, cb_req.b, r.sum)); + let cb = Box::new(move |r: AddTwoInts::Response| { + println!("{} + {} = {}", cb_req.a, cb_req.b, r.sum) + }); client.request(&req, cb)?; node.spin_once(std::time::Duration::from_millis(1000)); std::thread::sleep(std::time::Duration::from_millis(1000)); - c+=1; + c += 1; } - } diff --git a/examples/clock.rs b/examples/clock.rs index aaafcf6..1b64a81 100644 --- a/examples/clock.rs +++ b/examples/clock.rs @@ -1,8 +1,6 @@ use r2r; -use failure::Error; - -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { { let mut clock = r2r::Clock::create(r2r::ClockType::RosTime)?; let now = clock.get_now()?; diff --git a/examples/logging.rs b/examples/logging.rs index 0d7236e..64f15fe 100644 --- a/examples/logging.rs +++ b/examples/logging.rs @@ -1,11 +1,10 @@ use r2r; -use failure::Error; /// try to run like this /// cargo run --example logging -- --ros-args --log-level DEBUG /// The logs produced with the node logger should show up in /rosout -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { r2r::log_debug!("before_init", "debug msg"); let ctx = r2r::Context::create()?; let node = r2r::Node::create(ctx, "logger_node", "")?; @@ -17,8 +16,7 @@ fn main() -> Result<(), Error> { 0 => r2r::log_debug!(nl, "debug msg: {}", i as f64 / 2.5), 1 => r2r::log_info!(nl, "info msg {}", i % 2), 2 => r2r::log_warn!(nl, "warn msg {:?}", i.to_string()), - 3 => r2r::log_error!(nl, "error msg {:?}", - i.to_string().as_bytes()), + 3 => r2r::log_error!(nl, "error msg {:?}", i.to_string().as_bytes()), _ => r2r::log_fatal!(nl, "fatal msg {:#X}", i), } @@ -26,6 +24,6 @@ fn main() -> Result<(), Error> { // non-node logger only outputs to stdout r2r::log_debug!("other_logger", "i = {}", i); - i+=1; + i += 1; } } diff --git a/examples/parameters.rs b/examples/parameters.rs index d7bf5e5..688892a 100644 --- a/examples/parameters.rs +++ b/examples/parameters.rs @@ -1,19 +1,21 @@ use r2r; -use failure::Error; // try to run like this // cargo run --example parameters -- --ros-args -p param_key:=[hej,hopp] -p key2:=5.5 key2=true -r __ns:=/demo -r __node:=my_node -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let node = r2r::Node::create(ctx, "testnode", "")?; println!("node name: {}", node.name()?); - println!("node fully qualified name: {}", node.fully_qualified_name()?); + println!( + "node fully qualified name: {}", + node.fully_qualified_name()? + ); println!("node namespace: {}", node.namespace()?); println!("node parameters"); - node.params.iter().for_each(|(k,v)| { + node.params.iter().for_each(|(k, v)| { println!("{} - {:?}", k, v); }); diff --git a/examples/publish_complex_msgs.rs b/examples/publish_complex_msgs.rs index 2ac0df9..e08da94 100644 --- a/examples/publish_complex_msgs.rs +++ b/examples/publish_complex_msgs.rs @@ -1,10 +1,9 @@ use r2r; use r2r::builtin_interfaces::msg::Duration; -use r2r::trajectory_msgs::msg::*; use r2r::std_msgs::msg::Int32; -use failure::Error; +use r2r::trajectory_msgs::msg::*; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "testnode", "")?; let publisher = node.create_publisher::("/hej")?; @@ -12,13 +11,13 @@ fn main() -> Result<(), Error> { let mut c = 0; let mut positions: Vec = Vec::new(); - let cb = move |x:r2r::std_msgs::msg::String| { + let cb = move |x: r2r::std_msgs::msg::String| { println!("at count {} got: {}", c, x.data); c = c + 1; positions.push(c as f64); let to_send = JointTrajectoryPoint { positions: positions.clone(), - time_from_start : Duration { sec: c, nanosec: 0 }, + time_from_start: Duration { sec: c, nanosec: 0 }, ..Default::default() }; let mut native = r2r::WrappedNativeMsg::::new(); @@ -28,12 +27,12 @@ fn main() -> Result<(), Error> { publisher2.publish_native(&native).unwrap(); }; - let cb2 = move |x:JointTrajectoryPoint| { + let cb2 = move |x: JointTrajectoryPoint| { let serialized = serde_json::to_string(&x).unwrap(); println!("JTP serialized as: {}", serialized); }; - let cb3 = move |raw_c:&r2r::WrappedNativeMsg| { + let cb3 = move |raw_c: &r2r::WrappedNativeMsg| { println!("Raw c data: {:?}", raw_c.positions); }; diff --git a/examples/rostopic_echo.rs b/examples/rostopic_echo.rs index c0cf3c2..3dfd5db 100644 --- a/examples/rostopic_echo.rs +++ b/examples/rostopic_echo.rs @@ -1,10 +1,9 @@ use r2r; -use std::thread; -use std::env; use std::collections::HashMap; -use failure::Error; +use std::env; +use std::thread; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "echo", "")?; @@ -17,17 +16,19 @@ fn main() -> Result<(), Error> { while count < 50 { thread::sleep(std::time::Duration::from_millis(10)); nt = node.get_topic_names_and_types()?; - if nt.contains_key(topic) { break; } + if nt.contains_key(topic) { + break; + } count += 1; } - let type_name = nt.get(topic).and_then(|types|types.get(0)); + let type_name = nt.get(topic).and_then(|types| types.get(0)); let type_name = match type_name { Some(tn) => tn, None => { eprintln!("Could not determine the type for the passed topic"); return Ok(()); - }, + } }; println!("topic: {}, type: {}", topic, type_name); @@ -36,16 +37,14 @@ fn main() -> Result<(), Error> { let echo = &format!("{}_echo", topic); let echo_pub = node.create_publisher_untyped(echo, type_name)?; - let cb = move |msg: r2r::Result | { - match msg { - Ok(msg) => { - let s = serde_json::to_string_pretty(&msg).unwrap(); - println!("{}\n---\n", &s); - echo_pub.publish(msg).unwrap(); - } - Err(err) => { - println!("Could not parse msg. {}", err); - } + let cb = move |msg: r2r::Result| match msg { + Ok(msg) => { + let s = serde_json::to_string_pretty(&msg).unwrap(); + println!("{}\n---\n", &s); + echo_pub.publish(msg).unwrap(); + } + Err(err) => { + println!("Could not parse msg. {}", err); } }; diff --git a/examples/rostopic_list.rs b/examples/rostopic_list.rs index 8cd04e7..afa3e01 100644 --- a/examples/rostopic_list.rs +++ b/examples/rostopic_list.rs @@ -1,9 +1,8 @@ use r2r; use std::thread; use std::time::Duration; -use failure::Error; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let node = r2r::Node::create(ctx, "testnode", "")?; @@ -16,5 +15,5 @@ fn main() -> Result<(), Error> { println!("{}: {:?}", k, nt[k]); } thread::sleep(Duration::from_millis(500)); - }; + } } diff --git a/examples/service.rs b/examples/service.rs index 8d990a4..a9667b7 100644 --- a/examples/service.rs +++ b/examples/service.rs @@ -1,16 +1,14 @@ use r2r; -use failure::Error; - use r2r::example_interfaces::srv::AddTwoInts; fn handle_service(request: AddTwoInts::Request) -> AddTwoInts::Response { println!("request: {} + {}", request.a, request.b); AddTwoInts::Response { - sum: request.a + request.b + sum: request.a + request.b, } } -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "testnode", "")?; node.create_service::("/add_two_ints", Box::new(handle_service))?; diff --git a/examples/subscriber.rs b/examples/subscriber.rs index d21410a..e6ee96c 100644 --- a/examples/subscriber.rs +++ b/examples/subscriber.rs @@ -1,9 +1,8 @@ use r2r; use std::sync::mpsc; use std::thread; -use failure::Error; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let th = { diff --git a/examples/subscriber_with_thread.rs b/examples/subscriber_with_thread.rs index 151e2f5..7375d11 100644 --- a/examples/subscriber_with_thread.rs +++ b/examples/subscriber_with_thread.rs @@ -1,25 +1,28 @@ use std::sync::mpsc; use std::thread; -use failure::Error; use r2r; use r2r::std_msgs; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "testnode", "")?; - let publisher = node.create_publisher::("/hej")?; + let publisher = node.create_publisher::("/hello")?; let pubint = node.create_publisher::("/count")?; let (tx, rx) = mpsc::channel::(); thread::spawn(move || loop { - let msg = rx.recv().unwrap(); - let deserialized: std_msgs::msg::String = serde_json::from_str(&msg).unwrap(); - println!( - "received: {}, deserialized ros msg = {:?}", - msg, deserialized - ); + if let Ok(msg) = rx.recv() { + let deserialized: std_msgs::msg::String = serde_json::from_str(&msg).unwrap(); + println!( + "received: {}, deserialized ros msg = {:?}", + msg, deserialized + ); + } else { + println!("stopping"); + break; + } }); let mut c = 0; @@ -34,7 +37,7 @@ fn main() -> Result<(), Error> { pubint.publish(&to_send).unwrap(); }; - let _ws2 = node.subscribe("/hopp", Box::new(cb))?; + let _ws2 = node.subscribe("/hi", Box::new(cb))?; // run for 10 seconds let mut count = 0; diff --git a/examples/wall_timer.rs b/examples/wall_timer.rs index 34761bf..cbd4da9 100644 --- a/examples/wall_timer.rs +++ b/examples/wall_timer.rs @@ -1,14 +1,17 @@ use r2r; -use failure::Error; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Box> { let ctx = r2r::Context::create()?; let mut node = r2r::Node::create(ctx, "testnode", "")?; let mut x = 0; let cb = move |elapsed: std::time::Duration| { - println!("timer called ({}), {}us since last call", x, elapsed.as_micros()); - x+=1; + println!( + "timer called ({}), {}us since last call", + x, + elapsed.as_micros() + ); + x += 1; }; node.create_wall_timer(std::time::Duration::from_millis(2000), Box::new(cb))?; diff --git a/msg_gen/build.rs b/msg_gen/build.rs index 822620c..afd8f93 100644 --- a/msg_gen/build.rs +++ b/msg_gen/build.rs @@ -1,9 +1,9 @@ +use bindgen; +use common; use std::env; use std::fs::File; use std::io::Write; -use std::path::{Path,PathBuf}; -use bindgen; -use common; +use std::path::{Path, PathBuf}; fn main() { common::print_cargo_watches(); @@ -11,21 +11,30 @@ fn main() { let mut builder = bindgen::Builder::default(); let msg_list = if let Some(cmake_includes) = env::var("CMAKE_INCLUDE_DIRS").ok() { - let packages = cmake_includes.split(":").flat_map(|i| Path::new(i).parent()).collect::>(); + let packages = cmake_includes + .split(":") + .flat_map(|i| Path::new(i).parent()) + .collect::>(); for p in cmake_includes.split(":") { builder = builder.clang_arg(format!("-I{}", p)); } let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default()); let deps = deps.split(":").collect::>(); let msgs = common::get_ros_msgs(&packages); - common::parse_msgs(&msgs).into_iter() - .filter(|msg| deps.contains(&msg.module.as_str())).collect::>() + common::parse_msgs(&msgs) + .into_iter() + .filter(|msg| deps.contains(&msg.module.as_str())) + .collect::>() } else { let ament_prefix_var = env::var("AMENT_PREFIX_PATH").expect("Source your ROS!"); for p in ament_prefix_var.split(":") { builder = builder.clang_arg(format!("-I{}/include", p)); } - let paths = ament_prefix_var.split(":").map(|i| Path::new(i)).collect::>(); + let paths = ament_prefix_var + .split(":") + .map(|i| Path::new(i)) + .collect::>(); + let msgs = common::get_ros_msgs(&paths); common::parse_msgs(&msgs) }; @@ -78,8 +87,7 @@ fn main() { let val = &format!("unsafe {{ rosidl_typesupport_introspection_c__get_message_type_support_handle__{}__{}__{}_{}() }} as *const i32 as usize", &msg.module, &msg.prefix, &msg.name, s); introspecion_map.push_str(&format!("m.insert(\"{}\", {});\n", key, val)); } - } - else { + } else { let key = &format!("{}__{}__{}", &msg.module, &msg.prefix, &msg.name); let val = &format!("unsafe {{ rosidl_typesupport_introspection_c__get_message_type_support_handle__{}__{}__{}() }} as *const i32 as usize", &msg.module, &msg.prefix, &msg.name); introspecion_map.push_str(&format!("m.insert(\"{}\", {});\n", key, val)); @@ -100,7 +108,7 @@ fn main() { builder = builder .header(msg_includes_fn.to_str().unwrap()) .derive_copy(false) - // blacklist types that are handled by rcl bindings + // blacklist types that are handled by rcl bindings .blacklist_type("rosidl_message_type_support_t") .blacklist_type("rosidl_service_type_support_t") .blacklist_type("rosidl_runtime_c__String") diff --git a/msg_gen/src/lib.rs b/msg_gen/src/lib.rs index 16632f9..6c969a5 100644 --- a/msg_gen/src/lib.rs +++ b/msg_gen/src/lib.rs @@ -104,7 +104,7 @@ fn field_name(field_name: &str) -> String { pub fn generate_rust_service(module_: &str, prefix_: &str, name_: &str) -> String { format!( - " + " pub struct Service(); impl WrappedServiceTypeSupport for Service {{ type Request = Request; @@ -116,12 +116,14 @@ pub fn generate_rust_service(module_: &str, prefix_: &str, name_: &str) -> Strin }} }} - ", module_, prefix_, name_) + ", + module_, prefix_, name_ + ) } pub fn generate_rust_action(module_: &str, prefix_: &str, name_: &str) -> String { format!( - " + " pub struct Action(); impl WrappedActionTypeSupport for Action {{ type Goal = Goal; @@ -134,10 +136,11 @@ pub fn generate_rust_action(module_: &str, prefix_: &str, name_: &str) -> String }} }} - ", module_, prefix_, name_) + ", + module_, prefix_, name_ + ) } - // TODO: this is a terrible hack :) pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { let key = format!("{}__{}__{}", module_, prefix_, name_); @@ -157,8 +160,12 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { // same for actions with _Goal, _Result, _Feedback // TODO: refactor... let mut nn = name.splitn(2, "_"); - let _mod_name = nn.next().expect(&format!("malformed service name {}", name)); - let msg_name = nn.next().expect(&format!("malformed service name {}", name)); + let _mod_name = nn + .next() + .expect(&format!("malformed service name {}", name)); + let msg_name = nn + .next() + .expect(&format!("malformed service name {}", name)); name = msg_name.to_owned(); } @@ -282,14 +289,16 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { copy_to_native.push_str(&format!("assert_eq!(self.{field_name}.len(), {array_size}, \"Field {{}} is fixed size of {{}}!\", \"{field_name}\", {array_size});\n", field_name = field_name, array_size = member.array_size_)); if rust_field_type == "message" { copy_to_native.push_str(&format!("for (t,s) in msg.{field_name}.iter_mut().zip(&self.{field_name}) {{ s.copy_to_native(t);}}\n", field_name=field_name)); - } - else if rust_field_type == "std::string::String" { + } else if rust_field_type == "std::string::String" { copy_to_native.push_str(&format!("for (t,s) in msg.{field_name}.iter_mut().zip(&self.{field_name}) {{ t.assign(&s);}}\n", field_name=field_name)); } else { - copy_to_native.push_str(&format!("msg.{field_name}.copy_from_slice(&self.{field_name}[..{array_size}]);\n", field_name = field_name, array_size = member.array_size_)); + copy_to_native.push_str(&format!( + "msg.{field_name}.copy_from_slice(&self.{field_name}[..{array_size}]);\n", + field_name = field_name, + array_size = member.array_size_ + )); } - } - else if member.is_array_ && (member.array_size_ == 0 || member.is_upper_bound_) { + } else if member.is_array_ && (member.array_size_ == 0 || member.is_upper_bound_) { // these are __Sequence:s if rust_field_type == "message" { let (_, _, _, c_struct, _) = introspection(member.members_); @@ -359,7 +368,9 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { {msgname}::from_native(&msg_native) }} }} - ", msgname = name); + ", + msgname = name + ); let module_str = format!( " @@ -401,7 +412,9 @@ impl WrappedNativeMsgUntyped { let mut lines = String::new(); for msg in msgs { // for now don't generate untyped services or actions - if msg.prefix == "srv" || msg.prefix == "action" { continue; } + if msg.prefix == "srv" || msg.prefix == "action" { + continue; + } let typename = format!("{}/{}/{}", msg.module, msg.prefix, msg.name); let rustname = format!("{}::{}::{}", msg.module, msg.prefix, msg.name); diff --git a/rcl/build.rs b/rcl/build.rs index 745adca..debb137 100644 --- a/rcl/build.rs +++ b/rcl/build.rs @@ -1,9 +1,9 @@ extern crate bindgen; -use std::env; -use std::path::{Path,PathBuf}; -use itertools::Itertools; use common; +use itertools::Itertools; +use std::env; +use std::path::{Path, PathBuf}; fn main() { common::print_cargo_watches(); @@ -28,7 +28,9 @@ fn main() { builder = builder.clang_arg(clang_arg); } - env::var("CMAKE_LIBRARIES").unwrap_or(String::new()).split(":") + env::var("CMAKE_LIBRARIES") + .unwrap_or(String::new()) + .split(":") .into_iter() .filter(|s| s.contains(".so") || s.contains(".dylib")) .flat_map(|l| Path::new(l).parent().and_then(|p| p.to_str())) @@ -46,10 +48,12 @@ fn main() { for ament_prefix_path in ament_prefix_var.split(":") { builder = builder.clang_arg(format!("-I{}/include", ament_prefix_path)); - println!("added include search dir: {}" , format!("-I{}/include", ament_prefix_path)); + println!( + "added include search dir: {}", + format!("-I{}/include", ament_prefix_path) + ); println!("cargo:rustc-link-search=native={}/lib", ament_prefix_path); } - } println!("cargo:rustc-link-lib=dylib=rcl"); @@ -62,7 +66,8 @@ fn main() { let bindings = builder .no_debug("_OSUnaligned.*") - .generate().expect("Unable to generate bindings"); + .generate() + .expect("Unable to generate bindings"); let out_path = PathBuf::from(env::var("OUT_DIR").unwrap()); bindings diff --git a/rcl/src/lib.rs b/rcl/src/lib.rs index c651cca..cea8d12 100644 --- a/rcl/src/lib.rs +++ b/rcl/src/lib.rs @@ -17,24 +17,20 @@ impl Default for rmw_message_info_t { impl Default for rmw_qos_profile_t { fn default() -> Self { let mut profile: rmw_qos_profile_t = unsafe { std::mem::zeroed() }; - profile.history = - rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT; + profile.history = rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT; profile.depth = 10; profile.reliability = rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; - profile.durability = - rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT; + profile.durability = rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT; profile.avoid_ros_namespace_conventions = false; profile.deadline = rmw_time_t { sec: 0, nsec: 0 }; profile.lifespan = rmw_time_t { sec: 0, nsec: 0 }; - profile.liveliness = - rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + profile.liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; profile.liveliness_lease_duration = rmw_time_t { sec: 0, nsec: 0 }; profile } } - // special treatment to convert to/from rust strings. // ros strings are owned by ros, assignment is a copy impl rosidl_runtime_c__String { @@ -72,8 +68,12 @@ impl rosidl_runtime_c__U16String { impl rosidl_runtime_c__U16String__Sequence { pub fn update(&mut self, values: &[String]) { - unsafe { rosidl_runtime_c__U16String__Sequence__fini(self as *mut _); } - unsafe { rosidl_runtime_c__U16String__Sequence__init(self as *mut _, values.len()); } + unsafe { + rosidl_runtime_c__U16String__Sequence__fini(self as *mut _); + } + unsafe { + rosidl_runtime_c__U16String__Sequence__init(self as *mut _, values.len()); + } let strs = unsafe { std::slice::from_raw_parts_mut(self.data, values.len()) }; for (target, source) in strs.iter_mut().zip(values) { target.assign(&source); @@ -92,8 +92,12 @@ impl rosidl_runtime_c__U16String__Sequence { impl rosidl_runtime_c__String__Sequence { pub fn update(&mut self, values: &[String]) { - unsafe { rosidl_runtime_c__String__Sequence__fini(self as *mut _); } - unsafe { rosidl_runtime_c__String__Sequence__init(self as *mut _, values.len()); } + unsafe { + rosidl_runtime_c__String__Sequence__fini(self as *mut _); + } + unsafe { + rosidl_runtime_c__String__Sequence__init(self as *mut _, values.len()); + } let strs = unsafe { std::slice::from_raw_parts_mut(self.data, values.len()) }; for (target, source) in strs.iter_mut().zip(values) { target.assign(&source); @@ -130,7 +134,7 @@ macro_rules! primitive_sequence { } } } - } + }; } primitive_sequence!(rosidl_runtime_c__float32, f32); diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..2e97035 --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,4 @@ +hard_tabs = false +tab_spaces = 4 +reorder_imports = true +newline_style = "Unix" diff --git a/src/error.rs b/src/error.rs index a2be80c..3082221 100644 --- a/src/error.rs +++ b/src/error.rs @@ -1,84 +1,85 @@ #![allow(non_camel_case_types)] use rcl::*; +use thiserror::Error; // TODO -#[derive(Debug, Fail)] +#[derive(Error, Debug)] pub enum Error { // Copied from the generated bindgen - #[fail(display = "RCL_RET_OK")] + #[error("RCL_RET_OK")] RCL_RET_OK, - #[fail(display = "RCL_RET_ERROR")] + #[error("RCL_RET_ERROR")] RCL_RET_ERROR, - #[fail(display = "RCL_RET_TIMEOUT")] + #[error("RCL_RET_TIMEOUT")] RCL_RET_TIMEOUT, - #[fail(display = "RCL_RET_BAD_ALLOC")] + #[error("RCL_RET_BAD_ALLOC")] RCL_RET_BAD_ALLOC, - #[fail(display = "RCL_RET_INVALID_ARGUMENT")] + #[error("RCL_RET_INVALID_ARGUMENT")] RCL_RET_INVALID_ARGUMENT, - #[fail(display = "RCL_RET_UNSUPPORTED")] + #[error("RCL_RET_UNSUPPORTED")] RCL_RET_UNSUPPORTED, - #[fail(display = "RCL_RET_ALREADY_INIT")] + #[error("RCL_RET_ALREADY_INIT")] RCL_RET_ALREADY_INIT, - #[fail(display = "RCL_RET_NOT_INIT")] + #[error("RCL_RET_NOT_INIT")] RCL_RET_NOT_INIT, - #[fail(display = "RCL_RET_MISMATCHED_RMW_ID")] + #[error("RCL_RET_MISMATCHED_RMW_ID")] RCL_RET_MISMATCHED_RMW_ID, - #[fail(display = "RCL_RET_TOPIC_NAME_INVALID")] + #[error("RCL_RET_TOPIC_NAME_INVALID")] RCL_RET_TOPIC_NAME_INVALID, - #[fail(display = "RCL_RET_SERVICE_NAME_INVALID")] + #[error("RCL_RET_SERVICE_NAME_INVALID")] RCL_RET_SERVICE_NAME_INVALID, - #[fail(display = "RCL_RET_UNKNOWN_SUBSTITUTION")] + #[error("RCL_RET_UNKNOWN_SUBSTITUTION")] RCL_RET_UNKNOWN_SUBSTITUTION, - #[fail(display = "RCL_RET_ALREADY_SHUTDOWN")] + #[error("RCL_RET_ALREADY_SHUTDOWN")] RCL_RET_ALREADY_SHUTDOWN, - #[fail(display = "RCL_RET_NODE_INVALID")] + #[error("RCL_RET_NODE_INVALID")] RCL_RET_NODE_INVALID, - #[fail(display = "RCL_RET_NODE_INVALID_NAME")] + #[error("RCL_RET_NODE_INVALID_NAME")] RCL_RET_NODE_INVALID_NAME, - #[fail(display = "RCL_RET_NODE_INVALID_NAMESPACE")] + #[error("RCL_RET_NODE_INVALID_NAMESPACE")] RCL_RET_NODE_INVALID_NAMESPACE, - #[fail(display = "RCL_RET_PUBLISHER_INVALID")] + #[error("RCL_RET_PUBLISHER_INVALID")] RCL_RET_PUBLISHER_INVALID, - #[fail(display = "RCL_RET_SUBSCRIPTION_INVALID")] + #[error("RCL_RET_SUBSCRIPTION_INVALID")] RCL_RET_SUBSCRIPTION_INVALID, - #[fail(display = "RCL_RET_SUBSCRIPTION_TAKE_FAILED")] + #[error("RCL_RET_SUBSCRIPTION_TAKE_FAILED")] RCL_RET_SUBSCRIPTION_TAKE_FAILED, - #[fail(display = "RCL_RET_CLIENT_INVALID")] + #[error("RCL_RET_CLIENT_INVALID")] RCL_RET_CLIENT_INVALID, - #[fail(display = "RCL_RET_CLIENT_TAKE_FAILED")] + #[error("RCL_RET_CLIENT_TAKE_FAILED")] RCL_RET_CLIENT_TAKE_FAILED, - #[fail(display = "RCL_RET_SERVICE_INVALID")] + #[error("RCL_RET_SERVICE_INVALID")] RCL_RET_SERVICE_INVALID, - #[fail(display = "RCL_RET_SERVICE_TAKE_FAILED")] + #[error("RCL_RET_SERVICE_TAKE_FAILED")] RCL_RET_SERVICE_TAKE_FAILED, - #[fail(display = "RCL_RET_TIMER_INVALID")] + #[error("RCL_RET_TIMER_INVALID")] RCL_RET_TIMER_INVALID, - #[fail(display = "RCL_RET_TIMER_CANCELED")] + #[error("RCL_RET_TIMER_CANCELED")] RCL_RET_TIMER_CANCELED, - #[fail(display = "RCL_RET_WAIT_SET_INVALID")] + #[error("RCL_RET_WAIT_SET_INVALID")] RCL_RET_WAIT_SET_INVALID, - #[fail(display = "RCL_RET_WAIT_SET_EMPTY")] + #[error("RCL_RET_WAIT_SET_EMPTY")] RCL_RET_WAIT_SET_EMPTY, - #[fail(display = "RCL_RET_WAIT_SET_FULL")] + #[error("RCL_RET_WAIT_SET_FULL")] RCL_RET_WAIT_SET_FULL, - #[fail(display = "RCL_RET_INVALID_REMAP_RULE")] + #[error("RCL_RET_INVALID_REMAP_RULE")] RCL_RET_INVALID_REMAP_RULE, - #[fail(display = "RCL_RET_WRONG_LEXEME")] + #[error("RCL_RET_WRONG_LEXEME")] RCL_RET_WRONG_LEXEME, - #[fail(display = "RCL_RET_INVALID_PARAM_RULE")] + #[error("RCL_RET_INVALID_PARAM_RULE")] RCL_RET_INVALID_PARAM_RULE, - #[fail(display = "RCL_RET_INVALID_LOG_LEVEL_RULE")] + #[error("RCL_RET_INVALID_LOG_LEVEL_RULE")] RCL_RET_INVALID_LOG_LEVEL_RULE, - #[fail(display = "RCL_RET_EVENT_INVALID")] + #[error("RCL_RET_EVENT_INVALID")] RCL_RET_EVENT_INVALID, - #[fail(display = "RCL_RET_EVENT_TAKE_FAILED")] + #[error("RCL_RET_EVENT_TAKE_FAILED")] RCL_RET_EVENT_TAKE_FAILED, // Our own errors - #[fail(display = "No typesupport build for the message type: {}", msgtype)] + #[error("No typesupport built for the message type: {}", msgtype)] InvalidMessageType { msgtype: String }, - #[fail(display = "Serde error: {}", err)] + #[error("Serde error: {}", err)] SerdeError { err: String }, } diff --git a/src/lib.rs b/src/lib.rs index c632eb7..e1d13f5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,14 +1,16 @@ include!(concat!(env!("OUT_DIR"), "/_r2r_generated_msgs.rs")); -include!(concat!(env!("OUT_DIR"), "/_r2r_generated_untyped_helper.rs")); +include!(concat!( + env!("OUT_DIR"), + "/_r2r_generated_untyped_helper.rs" +)); -#[macro_use] extern crate failure_derive; use serde::{Deserialize, Serialize}; -use std::ffi::{CString,CStr}; -use std::mem::MaybeUninit; +use std::collections::HashMap; +use std::ffi::{CStr, CString}; use std::marker::PhantomData; +use std::mem::MaybeUninit; use std::ops::{Deref, DerefMut}; use std::time::Duration; -use std::collections::HashMap; use msg_gen::*; use rcl::*; @@ -46,7 +48,6 @@ pub trait WrappedActionTypeSupport { fn get_ts() -> &'static rosidl_action_type_support_t; } - #[derive(Debug)] pub struct WrappedNativeMsg where @@ -60,27 +61,33 @@ pub struct WrappedNativeMsgUntyped { ts: &'static rosidl_message_type_support_t, msg: *mut std::os::raw::c_void, destroy: fn(*mut std::os::raw::c_void), - msg_to_json: fn(native: *const std::os::raw::c_void) -> - std::result::Result, - msg_from_json: fn(native: *mut std::os::raw::c_void, json: serde_json::Value) -> - std::result::Result<(), serde_json::error::Error>, + msg_to_json: fn( + native: *const std::os::raw::c_void, + ) -> std::result::Result, + msg_from_json: fn( + native: *mut std::os::raw::c_void, + json: serde_json::Value, + ) -> std::result::Result<(), serde_json::error::Error>, } impl WrappedNativeMsgUntyped { - fn new() -> Self where T: WrappedTypesupport { - let destroy = | native: *mut std::os::raw::c_void | { + fn new() -> Self + where + T: WrappedTypesupport, + { + let destroy = |native: *mut std::os::raw::c_void| { let native_msg = native as *mut T::CStruct; T::destroy_msg(native_msg); }; - let msg_to_json = | native: *const std::os::raw::c_void | { + let msg_to_json = |native: *const std::os::raw::c_void| { let msg = unsafe { T::from_native(&*(native as *const T::CStruct)) }; serde_json::to_value(&msg) }; - let msg_from_json = | native: *mut std::os::raw::c_void, json: serde_json::Value | { - serde_json::from_value(json).map(|msg: T| { - unsafe { msg.copy_to_native(&mut *(native as *mut T::CStruct)); } + let msg_from_json = |native: *mut std::os::raw::c_void, json: serde_json::Value| { + serde_json::from_value(json).map(|msg: T| unsafe { + msg.copy_to_native(&mut *(native as *mut T::CStruct)); }) }; @@ -95,12 +102,15 @@ impl WrappedNativeMsgUntyped { fn to_json(&self) -> Result { let json = (self.msg_to_json)(self.msg); - json.map_err(|serde_err|Error::SerdeError { err: serde_err.to_string() }) + json.map_err(|serde_err| Error::SerdeError { + err: serde_err.to_string(), + }) } fn from_json(&mut self, json: serde_json::Value) -> Result<()> { - (self.msg_from_json)(self.msg, json). - map_err(|serde_err|Error::SerdeError { err: serde_err.to_string() }) + (self.msg_from_json)(self.msg, json).map_err(|serde_err| Error::SerdeError { + err: serde_err.to_string(), + }) } pub fn void_ptr(&self) -> *const std::os::raw::c_void { @@ -253,8 +263,7 @@ where } } -impl Sub for WrappedSubUntyped -{ +impl Sub for WrappedSubUntyped { fn handle(&self) -> &rcl_subscription_t { &self.rcl_handle } @@ -275,7 +284,6 @@ impl Sub for WrappedSubUntyped } } - // services struct WrappedService where @@ -317,7 +325,11 @@ where let response = (self.callback)(request); let mut native_response = WrappedNativeMsg::::from(&response); let res = unsafe { - rcl_send_response(&self.rcl_handle, &mut self.rcl_request, native_response.void_ptr_mut()) + rcl_send_response( + &self.rcl_handle, + &mut self.rcl_request, + native_response.void_ptr_mut(), + ) }; // TODO @@ -379,10 +391,16 @@ where } else { // I don't think this should be able to occur? Let's panic so we // find out... - let we_have: String = self.callbacks.iter() + let we_have: String = self + .callbacks + .iter() .map(|(id, _)| id.to_string()) - .collect::>().join(","); - eprintln!("no such req id: {}, we have [{}], ignoring", req_id, we_have); + .collect::>() + .join(","); + eprintln!( + "no such req id: {}, we have [{}], ignoring", + req_id, we_have + ); } } @@ -447,10 +465,9 @@ where client_: Weak>>, } - #[derive(Debug, Clone)] pub struct Context { - context_handle: Arc>>, + context_handle: Arc>, } // Not 100% about this one. From our end the context is rarely used @@ -463,47 +480,76 @@ impl Context { pub fn create() -> Result { let mut ctx: Box = unsafe { Box::new(rcl_get_zero_initialized_context()) }; // argc/v - let args = std::env::args().map(|arg| CString::new(arg).unwrap() ).collect::>(); - let mut c_args = args.iter().map(|arg| arg.as_ptr()).collect::>(); + let args = std::env::args() + .map(|arg| CString::new(arg).unwrap()) + .collect::>(); + let mut c_args = args + .iter() + .map(|arg| arg.as_ptr()) + .collect::>(); c_args.push(std::ptr::null()); let is_valid = unsafe { let allocator = rcutils_get_default_allocator(); let mut init_options = rcl_get_zero_initialized_init_options(); rcl_init_options_init(&mut init_options, allocator); - rcl_init((c_args.len() - 1) as ::std::os::raw::c_int, c_args.as_ptr(), &init_options, ctx.as_mut()); + rcl_init( + (c_args.len() - 1) as ::std::os::raw::c_int, + c_args.as_ptr(), + &init_options, + ctx.as_mut(), + ); rcl_init_options_fini(&mut init_options as *mut _); rcl_context_is_valid(ctx.as_mut()) }; let logging_ok = unsafe { let _guard = log_guard(); - let ret = rcl_logging_configure(&ctx.as_ref().global_arguments, - &rcutils_get_default_allocator()); + let ret = rcl_logging_configure( + &ctx.as_ref().global_arguments, + &rcutils_get_default_allocator(), + ); ret == RCL_RET_OK as i32 }; if is_valid && logging_ok { Ok(Context { - context_handle: Arc::new(Mutex::new(ctx)), + context_handle: Arc::new(Mutex::new(ContextHandle(ctx))), }) } else { Err(Error::RCL_RET_ERROR) // TODO } } + + pub fn is_valid(&self) -> bool { + let mut ctx = self.context_handle.lock().unwrap(); + unsafe { rcl_context_is_valid(ctx.as_mut()) } + } } -#[derive(Debug, Clone)] -pub struct ContextHandle(Arc>>); +#[derive(Debug)] +pub struct ContextHandle(Box); + +impl Deref for ContextHandle { + type Target = Box; + + fn deref<'a>(&'a self) -> &'a Box { + &self.0 + } +} + +impl DerefMut for ContextHandle { + fn deref_mut<'a>(&'a mut self) -> &'a mut Box { + &mut self.0 + } +} impl Drop for ContextHandle { fn drop(&mut self) { - println!("DROPPING CONTEXT HANDLE!"); - let mut ctx_handle = self.0.lock().unwrap(); // TODO: error handling? atleast probably need rcl_reset_error unsafe { - rcl::rcl_shutdown(ctx_handle.as_mut()); - rcl::rcl_context_fini(ctx_handle.as_mut()); + rcl::rcl_shutdown(self.0.as_mut()); + rcl::rcl_context_fini(self.0.as_mut()); } } } @@ -536,50 +582,46 @@ impl ParameterValue { ParameterValue::String(string) } else if v.byte_array_value != std::ptr::null_mut() { let vals = unsafe { - std::slice::from_raw_parts( - (*v.byte_array_value).values, - (*v.byte_array_value).size) + std::slice::from_raw_parts((*v.byte_array_value).values, (*v.byte_array_value).size) }; ParameterValue::ByteArray(vals.iter().cloned().collect()) - } - else if v.bool_array_value != std::ptr::null_mut() { + } else if v.bool_array_value != std::ptr::null_mut() { let vals = unsafe { - std::slice::from_raw_parts( - (*v.bool_array_value).values, - (*v.bool_array_value).size) + std::slice::from_raw_parts((*v.bool_array_value).values, (*v.bool_array_value).size) }; ParameterValue::BoolArray(vals.iter().cloned().collect()) - } - else if v.integer_array_value != std::ptr::null_mut() { + } else if v.integer_array_value != std::ptr::null_mut() { let vals = unsafe { std::slice::from_raw_parts( (*v.integer_array_value).values, - (*v.integer_array_value).size) + (*v.integer_array_value).size, + ) }; ParameterValue::IntegerArray(vals.iter().cloned().collect()) - } - else if v.double_array_value != std::ptr::null_mut() { + } else if v.double_array_value != std::ptr::null_mut() { let vals = unsafe { std::slice::from_raw_parts( (*v.double_array_value).values, - (*v.double_array_value).size) + (*v.double_array_value).size, + ) }; ParameterValue::DoubleArray(vals.iter().cloned().collect()) - } - else if v.string_array_value != std::ptr::null_mut() { + } else if v.string_array_value != std::ptr::null_mut() { let vals = unsafe { std::slice::from_raw_parts( (*v.string_array_value).data, - (*v.string_array_value).size) + (*v.string_array_value).size, + ) }; - let s = vals.iter().map(|cs| { - let s = unsafe { CStr::from_ptr(*cs) }; - s.to_str().unwrap_or("").to_owned() - }).collect(); + let s = vals + .iter() + .map(|cs| { + let s = unsafe { CStr::from_ptr(*cs) }; + s.to_str().unwrap_or("").to_owned() + }) + .collect(); ParameterValue::StringArray(s) - } - - else { + } else { ParameterValue::NotSet } } @@ -633,9 +675,8 @@ impl Node { let ctx = self.context.context_handle.lock().unwrap(); let mut params: Box<*mut rcl_params_t> = Box::new(std::ptr::null_mut()); - let ret = unsafe { - rcl_arguments_get_param_overrides(&ctx.global_arguments, params.as_mut()) - }; + let ret = + unsafe { rcl_arguments_get_param_overrides(&ctx.global_arguments, params.as_mut()) }; if ret != RCL_RET_OK as i32 { eprintln!("could not read parameters: {}", ret); return Err(Error::from_rcl_error(ret)); @@ -647,12 +688,16 @@ impl Node { let node_names = unsafe { std::slice::from_raw_parts( - (*(*params.as_ref())).node_names, (*(*params.as_ref())).num_nodes) + (*(*params.as_ref())).node_names, + (*(*params.as_ref())).num_nodes, + ) }; let node_params = unsafe { std::slice::from_raw_parts( - (*(*params.as_ref())).params, (*(*params.as_ref())).num_nodes) + (*(*params.as_ref())).params, + (*(*params.as_ref())).num_nodes, + ) }; let qualified_name = self.fully_qualified_name()?; @@ -665,25 +710,22 @@ impl Node { // This is copied from rclcpp, but there is a comment there suggesting // that more wildcards will be added in the future. Take note and mimic // their behavior. - if !( - node_name == "/**" || - node_name == "**" || - qualified_name == node_name || - name == node_name - ) { + if !(node_name == "/**" + || node_name == "**" + || qualified_name == node_name + || name == node_name) + { continue; } // make key value pairs. - let param_names = unsafe { - std::slice::from_raw_parts(np.parameter_names,np.num_params) - }; + let param_names = + unsafe { std::slice::from_raw_parts(np.parameter_names, np.num_params) }; - let param_values = unsafe { - std::slice::from_raw_parts(np.parameter_values,np.num_params) - }; + let param_values = + unsafe { std::slice::from_raw_parts(np.parameter_values, np.num_params) }; - for (s,v) in param_names.iter().zip(param_values) { + for (s, v) in param_names.iter().zip(param_values) { let s = unsafe { CStr::from_ptr(*s) }; let key = s.to_str().unwrap_or(""); let val = ParameterValue::from_rcl(&*v); @@ -691,11 +733,10 @@ impl Node { } } - unsafe { rcl_yaml_node_struct_fini(*params) } ; + unsafe { rcl_yaml_node_struct_fini(*params) }; Ok(()) } - pub fn create(ctx: Context, name: &str, namespace: &str) -> Result { // cleanup default options. let (res, node_handle) = { @@ -737,9 +778,13 @@ impl Node { } } - fn create_subscription_helper(&mut self, topic: &str, ts: *const rosidl_message_type_support_t) -> Result { + fn create_subscription_helper( + &mut self, + topic: &str, + ts: *const rosidl_message_type_support_t, + ) -> Result { let mut subscription_handle = unsafe { rcl_get_zero_initialized_subscription() }; - let topic_c_string = CString::new(topic).map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; + let topic_c_string = CString::new(topic).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; let result = unsafe { let mut subscription_options = rcl_subscription_get_default_options(); @@ -814,17 +859,24 @@ impl Node { Ok(self.subs.last().unwrap().handle()) // hmm... } - pub fn create_service_helper(&mut self, service_name: &str, - service_ts: *const rosidl_service_type_support_t) - -> Result { + pub fn create_service_helper( + &mut self, + service_name: &str, + service_ts: *const rosidl_service_type_support_t, + ) -> Result { let mut service_handle = unsafe { rcl_get_zero_initialized_service() }; - let service_name_c_string = CString::new(service_name) - .map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; + let service_name_c_string = + CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; let result = unsafe { let service_options = rcl_service_get_default_options(); - rcl_service_init(&mut service_handle, self.node_handle.as_mut(), - service_ts, service_name_c_string.as_ptr(), &service_options) + rcl_service_init( + &mut service_handle, + self.node_handle.as_mut(), + service_ts, + service_name_c_string.as_ptr(), + &service_options, + ) }; if result == RCL_RET_OK as i32 { Ok(service_handle) @@ -856,18 +908,24 @@ impl Node { Ok(self.services.last().unwrap().handle()) // hmm... } - - pub fn create_client_helper(&mut self, service_name: &str, - service_ts: *const rosidl_service_type_support_t) - -> Result { + pub fn create_client_helper( + &mut self, + service_name: &str, + service_ts: *const rosidl_service_type_support_t, + ) -> Result { let mut client_handle = unsafe { rcl_get_zero_initialized_client() }; - let service_name_c_string = CString::new(service_name) - .map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; + let service_name_c_string = + CString::new(service_name).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; let result = unsafe { let client_options = rcl_client_get_default_options(); - rcl_client_init(&mut client_handle, self.node_handle.as_mut(), - service_ts, service_name_c_string.as_ptr(), &client_options) + rcl_client_init( + &mut client_handle, + self.node_handle.as_mut(), + service_ts, + service_name_c_string.as_ptr(), + &client_options, + ) }; if result == RCL_RET_OK as i32 { Ok(client_handle) @@ -876,16 +934,13 @@ impl Node { } } - pub fn create_client( - &mut self, - service_name: &str, - ) -> Result> + pub fn create_client(&mut self, service_name: &str) -> Result> where T: WrappedServiceTypeSupport, { let client_handle = self.create_client_helper(service_name, T::get_ts())?; let cloned_ch = rcl_client_t { - impl_: client_handle.impl_ + impl_: client_handle.impl_, }; let ws = WrappedClient:: { rcl_handle: cloned_ch, @@ -900,21 +955,22 @@ impl Node { let arc = Arc::new(Mutex::new(ws)); let client_ = Arc::downgrade(&arc); self.clients.push((client_handle, arc)); - let c = Client { - client_ - }; + let c = Client { client_ }; Ok(c) } - pub fn service_available(&self, client: &Client) -> Result { - let client = client.client_.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?; + pub fn service_available( + &self, + client: &Client, + ) -> Result { + let client = client + .client_ + .upgrade() + .ok_or(Error::RCL_RET_CLIENT_INVALID)?; let client = client.lock().unwrap(); let mut avail = false; let result = unsafe { - rcl_service_server_is_available( - self.node_handle.as_ref(), - client.handle(), - &mut avail) + rcl_service_server_is_available(self.node_handle.as_ref(), client.handle(), &mut avail) }; if result == RCL_RET_OK as i32 { @@ -925,11 +981,13 @@ impl Node { } } - pub fn create_publisher_helper(&mut self, topic: &str, - typesupport: *const rosidl_message_type_support_t) -> Result { + pub fn create_publisher_helper( + &mut self, + topic: &str, + typesupport: *const rosidl_message_type_support_t, + ) -> Result { let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() }; - let topic_c_string = CString::new(topic) - .map_err(|_|Error::RCL_RET_INVALID_ARGUMENT)?; + let topic_c_string = CString::new(topic).map_err(|_| Error::RCL_RET_INVALID_ARGUMENT)?; let result = unsafe { let mut publisher_options = rcl_publisher_get_default_options(); @@ -949,7 +1007,6 @@ impl Node { } } - pub fn create_publisher(&mut self, topic: &str) -> Result> where T: WrappedTypesupport, @@ -963,7 +1020,11 @@ impl Node { Ok(p) } - pub fn create_publisher_untyped(&mut self, topic: &str, topic_type: &str) -> Result { + pub fn create_publisher_untyped( + &mut self, + topic: &str, + topic_type: &str, + ) -> Result { let dummy = WrappedNativeMsgUntyped::new_from(topic_type)?; // TODO, get ts without allocating msg let publisher_handle = self.create_publisher_helper(topic, dummy.ts)?; @@ -1030,9 +1091,7 @@ impl Node { } } - let ret = unsafe { - rcl_wait(&mut ws, timeout) - }; + let ret = unsafe { rcl_wait(&mut ws, timeout) }; if ret == RCL_RET_TIMEOUT as i32 { unsafe { @@ -1056,21 +1115,19 @@ impl Node { } } - let ws_timers = - unsafe { std::slice::from_raw_parts(ws.timers, ws.size_of_timers) }; + let ws_timers = unsafe { std::slice::from_raw_parts(ws.timers, ws.size_of_timers) }; assert_eq!(ws_timers.len(), self.timers.len()); for (s, ws_s) in self.timers.iter_mut().zip(ws_timers) { if ws_s != &std::ptr::null() { let mut is_ready = false; - let ret = unsafe { - rcl_timer_is_ready(&s.timer_handle, &mut is_ready) - }; + let ret = unsafe { rcl_timer_is_ready(&s.timer_handle, &mut is_ready) }; if ret == RCL_RET_OK as i32 { if is_ready { let mut nanos = 0i64; // todo: error handling - let ret = unsafe { rcl_timer_get_time_since_last_call(&s.timer_handle, - &mut nanos) }; + let ret = unsafe { + rcl_timer_get_time_since_last_call(&s.timer_handle, &mut nanos) + }; if ret == RCL_RET_OK as i32 { let ret = unsafe { rcl_timer_call(&mut s.timer_handle) }; if ret == RCL_RET_OK as i32 { @@ -1082,8 +1139,7 @@ impl Node { } } - let ws_clients = - unsafe { std::slice::from_raw_parts(ws.clients, ws.size_of_clients) }; + let ws_clients = unsafe { std::slice::from_raw_parts(ws.clients, ws.size_of_clients) }; assert_eq!(ws_clients.len(), self.clients.len()); for ((_, s), ws_s) in self.clients.iter_mut().zip(ws_clients) { if ws_s != &std::ptr::null() { @@ -1097,8 +1153,7 @@ impl Node { } } - let ws_services = - unsafe { std::slice::from_raw_parts(ws.services, ws.size_of_services) }; + let ws_services = unsafe { std::slice::from_raw_parts(ws.services, ws.size_of_services) }; assert_eq!(ws_services.len(), self.services.len()); for (s, ws_s) in self.services.iter_mut().zip(ws_services) { if ws_s != &std::ptr::null() { @@ -1116,11 +1171,15 @@ impl Node { } } - pub fn get_topic_names_and_types(&self) -> Result>> { let mut tnat = unsafe { rmw_get_zero_initialized_names_and_types() }; let ret = unsafe { - rcl_get_topic_names_and_types(self.node_handle.as_ref(), &mut rcutils_get_default_allocator(), false, &mut tnat) + rcl_get_topic_names_and_types( + self.node_handle.as_ref(), + &mut rcutils_get_default_allocator(), + false, + &mut tnat, + ) }; if ret != RCL_RET_OK as i32 { eprintln!("could not get topic names and types {}", ret); @@ -1131,27 +1190,35 @@ impl Node { let types = unsafe { std::slice::from_raw_parts(tnat.types, tnat.names.size) }; let mut res = HashMap::new(); - for (n,t) in names.iter().zip(types) { - let topic_name = unsafe {CStr::from_ptr(*n).to_str().unwrap().to_owned() }; + for (n, t) in names.iter().zip(types) { + let topic_name = unsafe { CStr::from_ptr(*n).to_str().unwrap().to_owned() }; let topic_types = unsafe { std::slice::from_raw_parts(t, t.size) }; let topic_types: Vec = unsafe { - topic_types.iter().map(|t| CStr::from_ptr(*((*t).data)).to_str().unwrap().to_owned()).collect() + topic_types + .iter() + .map(|t| CStr::from_ptr(*((*t).data)).to_str().unwrap().to_owned()) + .collect() }; res.insert(topic_name, topic_types); } - unsafe { rmw_names_and_types_fini(&mut tnat); } // TODO: check return value + unsafe { + rmw_names_and_types_fini(&mut tnat); + } // TODO: check return value Ok(res) } pub fn create_wall_timer( &mut self, period: Duration, - callback: Box ()>) -> Result<&Timer> { - + callback: Box ()>, + ) -> Result<&Timer> { let mut clock_handle = MaybeUninit::::uninit(); let ret = unsafe { - rcl_steady_clock_init(clock_handle.as_mut_ptr(), &mut rcutils_get_default_allocator()) + rcl_steady_clock_init( + clock_handle.as_mut_ptr(), + &mut rcutils_get_default_allocator(), + ) }; if ret != RCL_RET_OK as i32 { eprintln!("could not create steady clock: {}", ret); @@ -1164,9 +1231,14 @@ impl Node { { let mut ctx = self.context.context_handle.lock().unwrap(); let ret = unsafe { - rcl_timer_init(&mut timer_handle, clock_handle.as_mut(), - ctx.as_mut(), period.as_nanos() as i64, - None, rcutils_get_default_allocator()) + rcl_timer_init( + &mut timer_handle, + clock_handle.as_mut(), + ctx.as_mut(), + period.as_nanos() as i64, + None, + rcutils_get_default_allocator(), + ) }; if ret != RCL_RET_OK as i32 { @@ -1175,10 +1247,14 @@ impl Node { } } - let timer = Timer { timer_handle, clock_handle, callback }; + let timer = Timer { + timer_handle, + clock_handle, + callback, + }; self.timers.push(timer); - Ok(&self.timers[self.timers.len()-1]) + Ok(&self.timers[self.timers.len() - 1]) } pub fn logger<'a>(&'a self) -> &'a str { @@ -1189,7 +1265,6 @@ impl Node { let s = unsafe { CStr::from_ptr(ptr) }; s.to_str().unwrap_or("") } - } pub struct Timer { @@ -1252,7 +1327,10 @@ where T: WrappedTypesupport, { // upgrade to actual ref. if still alive - let publisher = self.handle.upgrade().ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; + let publisher = self + .handle + .upgrade() + .ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; // copy rust msg to native and publish it let native_msg: WrappedNativeMsg = WrappedNativeMsg::::from(msg); let result = unsafe { @@ -1276,7 +1354,10 @@ where T: WrappedTypesupport, { // upgrade to actual ref. if still alive - let publisher = self.handle.upgrade().ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; + let publisher = self + .handle + .upgrade() + .ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; let result = unsafe { rcl_publish(publisher.as_ref(), msg.void_ptr(), std::ptr::null_mut()) }; @@ -1289,7 +1370,6 @@ where } } - impl Client where T: WrappedServiceTypeSupport, @@ -1299,18 +1379,16 @@ where T: WrappedServiceTypeSupport, { // upgrade to actual ref. if still alive - let client = self.client_.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?; + let client = self + .client_ + .upgrade() + .ok_or(Error::RCL_RET_CLIENT_INVALID)?; let mut client = client.lock().unwrap(); // copy rust msg to native and publish it let native_msg: WrappedNativeMsg = WrappedNativeMsg::::from(msg); let mut seq_no = 0i64; - let result = unsafe { - rcl_send_request( - &client.rcl_handle, - native_msg.void_ptr(), - &mut seq_no, - ) - }; + let result = + unsafe { rcl_send_request(&client.rcl_handle, native_msg.void_ptr(), &mut seq_no) }; if result == RCL_RET_OK as i32 { client.callbacks.push((seq_no, cb)); @@ -1322,11 +1400,13 @@ where } } - impl PublisherUntyped { pub fn publish(&self, msg: serde_json::Value) -> Result<()> { // upgrade to actual ref. if still alive - let publisher = self.handle.upgrade().ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; + let publisher = self + .handle + .upgrade() + .ok_or(Error::RCL_RET_PUBLISHER_INVALID)?; let mut native_msg = WrappedNativeMsgUntyped::new_from(&self.type_)?; native_msg.from_json(msg)?; @@ -1375,7 +1455,11 @@ impl Clock { let rcl_ct = Clock::clock_type_to_rcl(&ct); let ret = unsafe { - rcl_clock_init(rcl_ct, clock_handle.as_mut_ptr(), &mut rcutils_get_default_allocator()) + rcl_clock_init( + rcl_ct, + clock_handle.as_mut_ptr(), + &mut rcutils_get_default_allocator(), + ) }; if ret != RCL_RET_OK as i32 { eprintln!("could not create {:?} clock: {}", ct, ret); @@ -1392,10 +1476,7 @@ impl Clock { return Err(Error::from_rcl_error(RCL_RET_INVALID_ARGUMENT as i32)); } let mut tp: rcutils_time_point_value_t = 0; - let ret = unsafe { - rcl_clock_get_now(&mut *self.clock_handle, - &mut tp) - }; + let ret = unsafe { rcl_clock_get_now(&mut *self.clock_handle, &mut tp) }; if ret != RCL_RET_OK as i32 { eprintln!("could not create steady clock: {}", ret); @@ -1410,10 +1491,7 @@ impl Clock { pub fn to_builtin_time(d: &Duration) -> builtin_interfaces::msg::Time { let sec = d.as_secs() as i32; let nanosec = d.subsec_nanos(); - builtin_interfaces::msg::Time { - sec, - nanosec, - } + builtin_interfaces::msg::Time { sec, nanosec } } } @@ -1429,6 +1507,18 @@ impl Drop for Clock { mod tests { use super::*; + #[test] + fn test_context_drop() -> () { + { + let ctx = Context::create().unwrap(); + assert!(ctx.is_valid()); + } + { + let ctx = Context::create().unwrap(); + assert!(ctx.is_valid()); + } + } + #[test] fn test_ros_str() -> () { let hej = "hej hopp"; @@ -1582,12 +1672,14 @@ mod tests { msg.positions.push(34.0); let json = serde_json::to_value(msg.clone()).unwrap(); - let mut native = WrappedNativeMsgUntyped::new_from("trajectory_msgs/msg/JointTrajectoryPoint").unwrap(); + let mut native = + WrappedNativeMsgUntyped::new_from("trajectory_msgs/msg/JointTrajectoryPoint").unwrap(); native.from_json(json.clone()).unwrap(); let json2 = native.to_json().unwrap(); assert_eq!(json, json2); - let msg2: trajectory_msgs::msg::JointTrajectoryPoint = serde_json::from_value(json2).unwrap(); + let msg2: trajectory_msgs::msg::JointTrajectoryPoint = + serde_json::from_value(json2).unwrap(); assert_eq!(msg, msg2); } @@ -1596,9 +1688,7 @@ mod tests { fn test_test_msgs_array() -> () { let mut msg = test_msgs::msg::Arrays::default(); println!("msg: {:?}", msg.string_values); - msg.string_values = vec![ - "hej".to_string(), "hopp".to_string(), "stropp".to_string() - ]; + msg.string_values = vec!["hej".to_string(), "hopp".to_string(), "stropp".to_string()]; let msg_native = WrappedNativeMsg::::from(&msg); let msg2 = test_msgs::msg::Arrays::from_native(&msg_native); @@ -1612,7 +1702,7 @@ mod tests { fn test_test_msgs_array_too_few_elems() -> () { let mut msg = test_msgs::msg::Arrays::default(); println!("msg: {:?}", msg.string_values); - msg.string_values = vec![ "hej".to_string(), "hopp".to_string() ]; + msg.string_values = vec!["hej".to_string(), "hopp".to_string()]; let _msg_native = WrappedNativeMsg::::from(&msg); } @@ -1659,14 +1749,14 @@ mod tests { assert_eq!(goal, goal2); let mut res = Fibonacci::Result::default(); - res.sequence = vec![1,2,3]; + res.sequence = vec![1, 2, 3]; let rn = WrappedNativeMsg::<_>::from(&res); let res2 = Fibonacci::Result::from_native(&rn); println!("res2 {:?}", res2); assert_eq!(res, res2); let mut fb = Fibonacci::Feedback::default(); - fb.sequence = vec![4,3,6]; + fb.sequence = vec![4, 3, 6]; let fbn = WrappedNativeMsg::<_>::from(&fb); let fb2 = Fibonacci::Feedback::from_native(&fbn); println!("feedback2 {:?}", fb2); diff --git a/src/utils.rs b/src/utils.rs index 7b1baea..5bac86e 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,6 +1,6 @@ -use std::ffi::{CString}; -use std::sync::{Mutex,MutexGuard}; use rcl::*; +use std::ffi::CString; +use std::sync::{Mutex, MutexGuard}; use lazy_static::lazy_static; @@ -37,11 +37,13 @@ pub fn log(msg: &str, logger_name: &str, file: &str, line: u32, severity: LogSev let message = CString::new(msg).unwrap(); let severity = severity.to_native(); unsafe { - rcutils_log(&location, - severity as i32, - logger_name.as_ptr(), - format.as_ptr(), - message.as_ptr()); + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format.as_ptr(), + message.as_ptr(), + ); } } @@ -52,7 +54,7 @@ pub enum LogSeverity { Info, Warn, Error, - Fatal + Fatal, } impl LogSeverity { @@ -74,8 +76,14 @@ impl LogSeverity { #[macro_export] macro_rules! __impl_log { ($logger_name:expr, $msg:expr, $file:expr, $line:expr, $severity:expr) => {{ - $crate::log(&std::fmt::format($msg), $logger_name, $file, $line, $severity); - }} + $crate::log( + &std::fmt::format($msg), + $logger_name, + $file, + $line, + $severity, + ); + }}; } #[macro_export] @@ -118,7 +126,6 @@ macro_rules! log_fatal { }} } - #[test] fn test_log() { log_debug!("log_test", "debug msg"); diff --git a/tests/threads.rs b/tests/threads.rs index b293a80..ad47e6b 100644 --- a/tests/threads.rs +++ b/tests/threads.rs @@ -1,12 +1,11 @@ use std::thread; use std::time::Duration; -use failure::Error; use r2r; #[test] // Let's create and drop a lot of node and publishers for a while to see that we can cope. -fn doesnt_crash() -> Result<(), Error> { +fn doesnt_crash() -> Result<(), Box> { // a global shared context. let ctx = r2r::Context::create()?;