mirror
/
r2r
mirror of https://github.com/sequenceplanner/r2r.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

apply rustfmt

This commit is contained in:
Martin Dahl 2021-05-15 11:04:32 +02:00
parent 3ae62f172a
commit 6b26328d65
17 changed files with 408 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
build.rs
View File

@ -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::<Vec<_>>();
let packages = cmake_includes
.split(":")
.flat_map(|i| Path::new(i).parent())
.collect::<Vec<_>>();
let deps = env::var("CMAKE_IDL_PACKAGES").unwrap_or(String::default());
let deps = deps.split(":").collect::<Vec<_>>();
let msgs = common::get_ros_msgs(&packages);
common::parse_msgs(&msgs).into_iter()
.filter(|msg| deps.contains(&msg.module.as_str())).collect::<Vec<_>>()
common::parse_msgs(&msgs)
.into_iter()
.filter(|msg| deps.contains(&msg.module.as_str()))
.collect::<Vec<_>>()
} 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::<Vec<_>>();
let paths = ament_prefix_var
.split(":")
.map(|i| Path::new(i))
.collect::<Vec<_>>();
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 {

47
common/src/lib.rs
View File

@ -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<String> {
@ -28,7 +28,6 @@ fn get_msgs_from_package(package: &Path) -> Vec<String> {
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<String> {
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);
}
@ -81,23 +80,38 @@ pub fn get_ros_msgs(paths: &[&Path]) -> Vec<String> {
}
pub fn parse_msgs(msgs: &Vec<String>) -> Vec<RosMsg> {
let v: Vec<Vec<&str>> = 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<Vec<&str>> = 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
}
@ -113,7 +127,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");
@ -130,15 +144,14 @@ std_msgs/msg/Bool
x/y
std_msgs/msg/String
";
let msgs: Vec<String> = msgs.lines().map(|l|l.to_string()).collect();
let msgs: Vec<String> = 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"
);
}
}

10
examples/client.rs
View File

@ -17,18 +17,18 @@ fn main() -> Result<(), Box<dyn std::error::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;
}
}

5
examples/logging.rs
View File

@ -16,8 +16,7 @@ fn main() -> Result<(), Box<dyn std::error::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),
}
@ -25,6 +24,6 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
// non-node logger only outputs to stdout
r2r::log_debug!("other_logger", "i = {}", i);
i+=1;
i += 1;
}
}

7
examples/parameters.rs
View File

@ -8,11 +8,14 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
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);
});

10
examples/publish_complex_msgs.rs
View File

@ -1,7 +1,7 @@
use r2r;
use r2r::builtin_interfaces::msg::Duration;
use r2r::trajectory_msgs::msg::*;
use r2r::std_msgs::msg::Int32;
use r2r::trajectory_msgs::msg::*;
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
@ -11,13 +11,13 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let mut c = 0;
let mut positions: Vec<f64> = 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::<Int32>::new();
@ -27,12 +27,12 @@ fn main() -> Result<(), Box<dyn std::error::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<JointTrajectoryPoint>| {
let cb3 = move |raw_c: &r2r::WrappedNativeMsg<JointTrajectoryPoint>| {
println!("Raw c data: {:?}", raw_c.positions);
};

30
examples/rostopic_echo.rs
View File

@ -1,7 +1,7 @@
use r2r;
use std::thread;
use std::env;
use std::collections::HashMap;
use std::env;
use std::thread;
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
@ -16,17 +16,19 @@ fn main() -> Result<(), Box<dyn std::error::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);
@ -35,16 +37,14 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let echo = &format!("{}_echo", topic);
let echo_pub = node.create_publisher_untyped(echo, type_name)?;
let cb = move |msg: r2r::Result<serde_json::Value> | {
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<serde_json::Value>| 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);
}
};

2
examples/rostopic_list.rs
View File

@ -15,5 +15,5 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
println!("{}: {:?}", k, nt[k]);
}
thread::sleep(Duration::from_millis(500));
};
}
}

2
examples/service.rs
View File

@ -4,7 +4,7 @@ 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,
}
}

8
examples/wall_timer.rs
View File

@ -6,8 +6,12 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
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))?;

24
msg_gen/build.rs
View File

@ -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,22 +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::<Vec<_>>();
let packages = cmake_includes
.split(":")
.flat_map(|i| Path::new(i).parent())
.collect::<Vec<_>>();
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::<Vec<_>>();
let msgs = common::get_ros_msgs(&packages);
common::parse_msgs(&msgs).into_iter()
.filter(|msg| deps.contains(&msg.module.as_str())).collect::<Vec<_>>()
common::parse_msgs(&msgs)
.into_iter()
.filter(|msg| deps.contains(&msg.module.as_str()))
.collect::<Vec<_>>()
} 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::<Vec<_>>();
let paths = ament_prefix_var
.split(":")
.map(|i| Path::new(i))
.collect::<Vec<_>>();
let msgs = common::get_ros_msgs(&paths);
common::parse_msgs(&msgs)
};
@ -94,7 +102,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")

35
msg_gen/src/lib.rs
View File

@ -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,10 +116,11 @@ pub fn generate_rust_service(module_: &str, prefix_: &str, name_: &str) -> Strin
}}
}}
", 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_);
@ -137,8 +138,12 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String {
// for srv, the message name is both the service name and _Request or _Respone
// we only want to keep the last part.
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();
}
@ -262,14 +267,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_);
@ -339,7 +346,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!(
"
@ -381,7 +390,9 @@ impl WrappedNativeMsgUntyped {
let mut lines = String::new();
for msg in msgs {
// for now don't generate untyped services
if msg.prefix == "srv" { continue; }
if msg.prefix == "srv" {
continue;
}
let typename = format!("{}/{}/{}", msg.module, msg.prefix, msg.name);
let rustname = format!("{}::{}::{}", msg.module, msg.prefix, msg.name);

19
rcl/build.rs
View File

@ -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

28
rcl/src/lib.rs
View File

@ -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);

4
rustfmt.toml Normal file
View File

@ -0,0 +1,4 @@
hard_tabs = false
tab_spaces = 4
reorder_imports = true
newline_style = "Unix"

400
src/lib.rs
View File

@ -1,13 +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"
));
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::*;
@ -37,7 +40,6 @@ pub trait WrappedServiceTypeSupport {
fn get_ts() -> &'static rosidl_service_type_support_t;
}
#[derive(Debug)]
pub struct WrappedNativeMsg<T>
where
@ -51,27 +53,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<serde_json::Value, serde_json::error::Error>,
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<serde_json::Value, serde_json::error::Error>,
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<T>() -> Self where T: WrappedTypesupport {
let destroy = | native: *mut std::os::raw::c_void | {
fn new<T>() -> 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));
})
};
@ -86,12 +94,15 @@ impl WrappedNativeMsgUntyped {
fn to_json(&self) -> Result<serde_json::Value> {
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 {
@ -244,8 +255,7 @@ where
}
}
impl Sub for WrappedSubUntyped
{
impl Sub for WrappedSubUntyped {
fn handle(&self) -> &rcl_subscription_t {
&self.rcl_handle
}
@ -266,7 +276,6 @@ impl Sub for WrappedSubUntyped
}
}
// services
struct WrappedService<T>
where
@ -308,7 +317,11 @@ where
let response = (self.callback)(request);
let mut native_response = WrappedNativeMsg::<T::Response>::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
@ -370,10 +383,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::<Vec<_>>().join(",");
eprintln!("no such req id: {}, we have [{}], ignoring", req_id, we_have);
.collect::<Vec<_>>()
.join(",");
eprintln!(
"no such req id: {}, we have [{}], ignoring",
req_id, we_have
);
}
}
@ -438,7 +457,6 @@ where
client_: Weak<Mutex<WrappedClient<T>>>,
}
#[derive(Debug, Clone)]
pub struct Context {
context_handle: Arc<Mutex<ContextHandle>>,
@ -454,23 +472,35 @@ impl Context {
pub fn create() -> Result<Context> {
let mut ctx: Box<rcl_context_t> = unsafe { Box::new(rcl_get_zero_initialized_context()) };
// argc/v
let args = std::env::args().map(|arg| CString::new(arg).unwrap() ).collect::<Vec<CString>>();
let mut c_args = args.iter().map(|arg| arg.as_ptr()).collect::<Vec<*const ::std::os::raw::c_char>>();
let args = std::env::args()
.map(|arg| CString::new(arg).unwrap())
.collect::<Vec<CString>>();
let mut c_args = args
.iter()
.map(|arg| arg.as_ptr())
.collect::<Vec<*const ::std::os::raw::c_char>>();
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
};
@ -544,50 +574,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
}
}
@ -641,9 +667,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));
@ -655,12 +680,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()?;
@ -673,25 +702,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);
@ -699,11 +725,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<Node> {
// cleanup default options.
let (res, node_handle) = {
@ -745,9 +770,13 @@ impl Node {
}
}
fn create_subscription_helper(&mut self, topic: &str, ts: *const rosidl_message_type_support_t) -> Result<rcl_subscription_t> {
fn create_subscription_helper(
&mut self,
topic: &str,
ts: *const rosidl_message_type_support_t,
) -> Result<rcl_subscription_t> {
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();
@ -822,17 +851,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<rcl_service_t> {
pub fn create_service_helper(
&mut self,
service_name: &str,
service_ts: *const rosidl_service_type_support_t,
) -> Result<rcl_service_t> {
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)
@ -864,18 +900,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<rcl_client_t> {
pub fn create_client_helper(
&mut self,
service_name: &str,
service_ts: *const rosidl_service_type_support_t,
) -> Result<rcl_client_t> {
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)
@ -884,16 +926,13 @@ impl Node {
}
}
pub fn create_client<T: 'static>(
&mut self,
service_name: &str,
) -> Result<Client<T>>
pub fn create_client<T: 'static>(&mut self, service_name: &str) -> Result<Client<T>>
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::<T> {
rcl_handle: cloned_ch,
@ -908,21 +947,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<T: WrappedServiceTypeSupport>(&self, client: &Client<T>) -> Result<bool> {
let client = client.client_.upgrade().ok_or(Error::RCL_RET_CLIENT_INVALID)?;
pub fn service_available<T: WrappedServiceTypeSupport>(
&self,
client: &Client<T>,
) -> Result<bool> {
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 {
@ -933,11 +973,13 @@ impl Node {
}
}
pub fn create_publisher_helper(&mut self, topic: &str,
typesupport: *const rosidl_message_type_support_t) -> Result<rcl_publisher_t> {
pub fn create_publisher_helper(
&mut self,
topic: &str,
typesupport: *const rosidl_message_type_support_t,
) -> Result<rcl_publisher_t> {
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();
@ -957,7 +999,6 @@ impl Node {
}
}
pub fn create_publisher<T>(&mut self, topic: &str) -> Result<Publisher<T>>
where
T: WrappedTypesupport,
@ -971,7 +1012,11 @@ impl Node {
Ok(p)
}
pub fn create_publisher_untyped(&mut self, topic: &str, topic_type: &str) -> Result<PublisherUntyped> {
pub fn create_publisher_untyped(
&mut self,
topic: &str,
topic_type: &str,
) -> Result<PublisherUntyped> {
let dummy = WrappedNativeMsgUntyped::new_from(topic_type)?; // TODO, get ts without allocating msg
let publisher_handle = self.create_publisher_helper(topic, dummy.ts)?;
@ -1038,9 +1083,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 {
@ -1064,21 +1107,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 {
@ -1090,8 +1131,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() {
@ -1105,8 +1145,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() {
@ -1124,11 +1163,15 @@ impl Node {
}
}
pub fn get_topic_names_and_types(&self) -> Result<HashMap<String, Vec<String>>> {
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);
@ -1139,27 +1182,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<String> = 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<dyn FnMut(Duration) -> ()>) -> Result<&Timer> {
callback: Box<dyn FnMut(Duration) -> ()>,
) -> Result<&Timer> {
let mut clock_handle = MaybeUninit::<rcl_clock_t>::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);
@ -1172,9 +1223,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 {
@ -1183,10 +1239,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 {
@ -1197,7 +1257,6 @@ impl Node {
let s = unsafe { CStr::from_ptr(ptr) };
s.to_str().unwrap_or("")
}
}
pub struct Timer {
@ -1260,7 +1319,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<T> = WrappedNativeMsg::<T>::from(msg);
let result = unsafe {
@ -1284,7 +1346,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()) };
@ -1297,7 +1362,6 @@ where
}
}
impl<T> Client<T>
where
T: WrappedServiceTypeSupport,
@ -1307,18 +1371,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<T::Request> = WrappedNativeMsg::<T::Request>::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));
@ -1330,11 +1392,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)?;
@ -1383,7 +1447,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);
@ -1400,10 +1468,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);
@ -1418,10 +1483,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 }
}
}
@ -1602,12 +1664,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);
}
@ -1616,9 +1680,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::<test_msgs::msg::Arrays>::from(&msg);
let msg2 = test_msgs::msg::Arrays::from_native(&msg_native);
@ -1632,7 +1694,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::<test_msgs::msg::Arrays>::from(&msg);
}

29
src/utils.rs
View File

@ -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");