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

Merge branch 'master' into actions

This commit is contained in:
Martin Dahl 2021-05-15 11:24:09 +02:00
parent bbfaf8141a 6b26328d65
commit afb62321e5
25 changed files with 523 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.gitignore vendored
View File

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

5
Cargo.toml
View File

@ -1,7 +1,7 @@
[package]
name = "r2r"
version = "0.0.6"
version = "0.0.8"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
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" }

3
README.md
View File

@ -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 <https://github.com/sequenceplanner/r2r-echo>, which can be installed by running cargo install --git https://github.com/sequenceplanner/r2r-echo.
What works?

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 {

51
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);
}
@ -60,9 +59,9 @@ fn get_msgs_from_package(package: &Path) -> Vec<String> {
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<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
}
@ -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<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"
);
}
}

13
examples/client.rs
View File

@ -1,9 +1,8 @@
use r2r;
use failure::Error;
use r2r::example_interfaces::srv::AddTwoInts;
fn main() -> Result<(), Error> {
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let client = node.create_client::<AddTwoInts::Service>("/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;
}
}

4
examples/clock.rs
View File

@ -1,8 +1,6 @@
use r2r;
use failure::Error;
fn main() -> Result<(), Error> {
fn main() -> Result<(), Box<dyn std::error::Error>> {
{
let mut clock = r2r::Clock::create(r2r::ClockType::RosTime)?;
let now = clock.get_now()?;

8
examples/logging.rs
View File

@ -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<dyn std::error::Error>> {
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;
}
}

10
examples/parameters.rs
View File

@ -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<dyn std::error::Error>> {
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);
});

13
examples/publish_complex_msgs.rs
View File

@ -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<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let publisher = node.create_publisher::<JointTrajectoryPoint>("/hej")?;
@ -12,13 +11,13 @@ fn main() -> Result<(), 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();
@ -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<JointTrajectoryPoint>| {
let cb3 = move |raw_c: &r2r::WrappedNativeMsg<JointTrajectoryPoint>| {
println!("Raw c data: {:?}", raw_c.positions);
};

33
examples/rostopic_echo.rs
View File

@ -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<dyn std::error::Error>> {
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<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);
}
};

5
examples/rostopic_list.rs
View File

@ -1,9 +1,8 @@
use r2r;
use std::thread;
use std::time::Duration;
use failure::Error;
fn main() -> Result<(), Error> {
fn main() -> Result<(), Box<dyn std::error::Error>> {
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));
};
}
}

6
examples/service.rs
View File

@ -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<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
node.create_service::<AddTwoInts::Service>("/add_two_ints", Box::new(handle_service))?;

3
examples/subscriber.rs
View File

@ -1,9 +1,8 @@
use r2r;
use std::sync::mpsc;
use std::thread;
use failure::Error;
fn main() -> Result<(), Error> {
fn main() -> Result<(), Box<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let th = {

23
examples/subscriber_with_thread.rs
View File

@ -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<dyn std::error::Error>> {
let ctx = r2r::Context::create()?;
let mut node = r2r::Node::create(ctx, "testnode", "")?;
let publisher = node.create_publisher::<std_msgs::msg::String>("/hej")?;
let publisher = node.create_publisher::<std_msgs::msg::String>("/hello")?;
let pubint = node.create_publisher::<std_msgs::msg::Int32>("/count")?;
let (tx, rx) = mpsc::channel::<String>();
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;

11
examples/wall_timer.rs
View File

@ -1,14 +1,17 @@
use r2r;
use failure::Error;
fn main() -> Result<(), Error> {
fn main() -> Result<(), Box<dyn std::error::Error>> {
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))?;

28
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,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::<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)
};
@ -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")

41
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,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);

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"

75
src/error.rs
View File

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

450
src/lib.rs
View File

@ -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<T>
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<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));
})
};
@ -95,12 +102,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 {
@ -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<T>
where
@ -317,7 +325,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
@ -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::<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
);
}
}
@ -447,10 +465,9 @@ where
client_: Weak<Mutex<WrappedClient<T>>>,
}
#[derive(Debug, Clone)]
pub struct Context {
context_handle: Arc<Mutex<Box<rcl_context_t>>>,
context_handle: Arc<Mutex<ContextHandle>>,
}
// Not 100% about this one. From our end the context is rarely used
@ -463,47 +480,76 @@ 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
};
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<Mutex<Box<rcl_context_t>>>);
#[derive(Debug)]
pub struct ContextHandle(Box<rcl_context_t>);
impl Deref for ContextHandle {
type Target = Box<rcl_context_t>;
fn deref<'a>(&'a self) -> &'a Box<rcl_context_t> {
&self.0
}
}
impl DerefMut for ContextHandle {
fn deref_mut<'a>(&'a mut self) -> &'a mut Box<rcl_context_t> {
&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<Node> {
// 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<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();
@ -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<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)
@ -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<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)
@ -876,16 +934,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,
@ -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<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 {
@ -925,11 +981,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();
@ -949,7 +1007,6 @@ impl Node {
}
}
pub fn create_publisher<T>(&mut self, topic: &str) -> Result<Publisher<T>>
where
T: WrappedTypesupport,
@ -963,7 +1020,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)?;
@ -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<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);
@ -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<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);
@ -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<T> = WrappedNativeMsg::<T>::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<T> Client<T>
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<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));
@ -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::<test_msgs::msg::Arrays>::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::<test_msgs::msg::Arrays>::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);

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

3
tests/threads.rs
View File

@ -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<dyn std::error::Error>> {
// a global shared context.
let ctx = r2r::Context::create()?;