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

Initial experiments with bindings and msg generation.

This commit is contained in:
Martin Dahl 2019-08-19 20:39:08 +02:00
commit 2ebeaf8682
15 changed files with 961 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
.gitignore vendored Normal file
View File

@ -0,0 +1,11 @@
/target/
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

22
Cargo.toml Normal file
View File

@ -0,0 +1,22 @@
[package]
name = "r2r"
version = "0.0.1"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
description = "Minimal ros2 bindings."
license = "Apache-2.0/MIT"
edition = "2018"
[features]
headers = ["rcl/headers", "msg_gen/headers"]
[dependencies]
common = { path = "common", version = "0.0.1" }
rcl = { path = "rcl", version = "0.0.1" }
msg_gen = { path = "msg_gen", version = "0.0.1" }
[build-dependencies]
common = { path = "common", version = "0.0.1" }
msg_gen = { path = "msg_gen", version = "0.0.1" }
[workspace]

40
build.rs Normal file
View File

@ -0,0 +1,40 @@
use msg_gen::*;
use common::*;
fn main() {
let msgs = read_file("./msgs.txt").unwrap();
let msgs = parse_msgs(&msgs);
let msgs = as_map(&msgs);
let mut codegen = String::new();
for (module, prefixes) in &msgs {
println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_c", module);
println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_introspection_c", module);
println!("cargo:rustc-link-lib=dylib={}__rosidl_generator_c", module);
codegen.push_str(&format!("pub mod {} {{\n", module));
for (prefix, msgs) in prefixes {
codegen.push_str(&format!(" pub mod {} {{\n", prefix));
codegen.push_str(" use super::super::*;\n");
for msg in msgs {
let module_lc = String::from(*module).to_lowercase();
let prefix_lc = String::from(*prefix).to_lowercase();
let name_lc = String::from(*msg).to_lowercase();
codegen.push_str(&generate_rust_msg(&module_lc, &prefix_lc, &name_lc));
}
codegen.push_str(" }\n");
}
codegen.push_str("}\n");
}
use std::io::Write;
use std::fs::File;
let mut f = File::create("src/generated_msgs.rs").unwrap();
write!(f, "{}", codegen).unwrap();
}

9
common/Cargo.toml Normal file
View File

@ -0,0 +1,9 @@
[package]
name = "common"
version = "0.0.1"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
description = "Minimal ros2 bindings."
license = "Apache-2.0/MIT"
edition = "2018"

74
common/src/lib.rs Normal file
View File

@ -0,0 +1,74 @@
use std::collections::HashMap;
#[derive(Debug)]
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 fn parse_msgs(msgs: &str) -> Vec<RosMsg> {
let v: Vec<Vec<&str>> = msgs.lines().map(|l| l.split("/").into_iter().take(3).collect()).collect();
v.iter().filter(|v|v.len() == 3).
map(|v| RosMsg { module: v[0].into(), prefix: v[1].into(), name: v[2].into()}).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
}
use std::io::{self, Read};
use std::fs::File;
pub fn read_file(filename: &str) -> io::Result<String> {
let mut file = File::open(filename)?;
let mut s = String::new();
file.read_to_string(&mut s)?;
Ok(s)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_parse_msgs() -> () {
let msgs = "
std_msgs/msg/Bool
x/y
std_msgs/msg/String
";
let parsed = parse_msgs(msgs);
assert_eq!(parsed[0].module, "std_msgs");
assert_eq!(parsed[0].prefix, "msg");
assert_eq!(parsed[0].name, "Bool");
assert_eq!(parsed[1].module, "std_msgs");
assert_eq!(parsed[1].prefix, "msg");
assert_eq!(parsed[1].name, "String");
}
#[test]
fn test_as_map() -> () {
let msgs = "
std_msgs/msg/Bool
x/y
std_msgs/msg/String
";
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");
}
}

53
examples/subscriber_with_thread.rs Normal file
View File

@ -0,0 +1,53 @@
use std::thread;
use std::sync::mpsc;
use r2r::*;
fn main() -> Result<(), ()> {
let mut ctx = rcl_create_context()?;
let mut node = rcl_create_node("qqq", "", &mut ctx)?;
let publisher = rcl_create_publisher::<std_msgs::msg::String>(&mut node, "/hej")?;
let pubint = rcl_create_publisher::<std_msgs::msg::Int32>(&mut node, "/count")?;
let (tx, rx) = mpsc::channel::<std_msgs::msg::String>();
thread::spawn(move|| {
loop {
let msg = rx.recv().unwrap();
println!("other thread received: {}", msg.data);
}
});
let mut c = 0;
let cb = move |x:std_msgs::msg::String| {
let to_send = format!("at count {} got: {}", c, x.data);
c = c + 1;
tx.send(x.clone()).unwrap(); // pass msg on to other thread for printing
let to_send = std_msgs::msg::String { data: to_send };
publish(&publisher, &to_send).unwrap();
let to_send = std_msgs::msg::Int32 { data: c };
publish(&pubint, &to_send).unwrap();
};
let ws2 = rcl_create_subscription(&mut node, "/hopp", Box::new(cb))?;
// TODO: group subscriptions in a "node" struct
let mut subst: Vec<Box<Sub>> = vec![Box::new(ws2)];
// run for 10 seconds
let mut count = 0;
while count < 100 {
let timeout = 100 * 1000 * 1000; // 0.1 sec
rcl_take_subst(&mut ctx, &mut subst, timeout)?;
count = count + 1;
}
// TODO: crashes here. maybe because pub and sub are not cleaned up
rcl_destroy_node(&mut node);
rcl_destroy_ctx(&mut ctx);
Ok(())
}

18
msg_gen/Cargo.toml Normal file
View File

@ -0,0 +1,18 @@
[package]
name = "msg_gen"
version = "0.0.1"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
[features]
headers = []
[dependencies]
lazy_static = "1.3.0"
libc = "0.2.0"
rcl = { path = "../rcl", version = "0.0.1" }
[build-dependencies]
bindgen = "0.50.0"
rcl = { path = "../rcl", version = "0.0.1" }
common = { path = "../common", version = "0.0.1" }

76
msg_gen/build.rs Normal file
View File

@ -0,0 +1,76 @@
extern crate bindgen;
use std::env;
use std::path::PathBuf;
use common::*;
fn main() {
let msgs = read_file("../msgs.txt").unwrap();
let msg_list = parse_msgs(&msgs);
let msg_map = as_map(&msg_list);
for module in msg_map.keys() {
println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_c", module);
println!("cargo:rustc-link-lib=dylib={}__rosidl_typesupport_introspection_c", module);
println!("cargo:rustc-link-lib=dylib={}__rosidl_generator_c", module);
}
let mut includes = String::new();
let mut introspecion_map = String::from("\
lazy_static! { \n
static ref INTROSPECTION_FNS: HashMap<&'static str, usize> = {\n
let mut m = HashMap::new();\n");
for msg in msg_list {
let module = msg.module.to_lowercase();
let prefix = msg.prefix.to_lowercase();
let name = msg.name.to_lowercase();
includes.push_str(&format!("#include <{}/{}/{}.h>\n", &module, &prefix, &name));
includes.push_str(&format!("#include <{}/{}/{}__rosidl_typesupport_introspection_c.h>\n", &module, &prefix, &name));
let key = &format!("{}__{}__{}", module, prefix, 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));
}
introspecion_map.push_str("m \n }; }\n\n");
use std::io::Write;
use std::fs::File;
let mut f = File::create("src/msg_includes.h").unwrap();
write!(f, "{}", includes).unwrap();
let mut f = File::create("src/introspection_functions.rs").unwrap();
write!(f, "{}", introspecion_map).unwrap();
let headers_enabled = env::var_os("CARGO_FEATURE_HEADERS").is_some();
let mut builder = bindgen::Builder::default()
.header("src/msg_includes.h")
.derive_copy(false)
// blacklist types that are handled by rcl bindings
.blacklist_type("rosidl_message_type_support_t")
.blacklist_type("rosidl_generator_c__String")
.default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } );
let ament_prefix_var_name = "AMENT_PREFIX_PATH";
let ament_prefix_var = env::var(ament_prefix_var_name).expect("Source your ROS!");
for ament_prefix_path in ament_prefix_var.split(":") {
builder = builder.clang_arg(format!("-I{}/include", ament_prefix_path));
println!("cargo:rustc-link-search=native={}/lib", ament_prefix_path);
}
// bindgen takes time so we dont want to do it always... must be a better way
if headers_enabled {
let bindings = builder.generate().expect("Unable to generate bindings");
let out_path = PathBuf::from(".");
bindings
.write_to_file(out_path.join("src/msg_bindings.rs"))
.expect("Couldn't write bindings!");
}
// assert!(false);
}

168
msg_gen/src/lib.rs Normal file
View File

@ -0,0 +1,168 @@
#![allow(non_upper_case_globals)]
#![allow(non_camel_case_types)]
#![allow(non_snake_case)]
#![allow(improper_ctypes)]
#![allow(dead_code)]
include!("./msg_bindings.rs");
include!("./introspection_functions.rs");
#[macro_use]
extern crate lazy_static;
use std::collections::HashMap;
use rcl::*;
use std::ffi::CStr;
// Try to make these work, containing a nested msg with arrays.
//
// martin@martin-XPS-15-9550 ~ $ ros2 msg show trajectory_msgs/msg/JointTrajectoryPoint
// # Each trajectory point specifies either positions[, velocities[, accelerations]]
// # or positions[, effort] for the trajectory to be executed.
// # All specified values are in the same order as the joint names in JointTrajectory.msg.
//
// float64[] positions
// float64[] velocities
// float64[] accelerations
// float64[] effort
// builtin_interfaces/Duration time_from_start
// martin@martin-XPS-15-9550 ~ $ ros2 msg show builtin_interfaces/Duration
// int32 sec
// uint32 nanosec
//
//
fn field_type(t: u8) -> String {
// rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT = 1,
// rosidl_typesupport_introspection_c__ROS_TYPE_DOUBLE = 2,
// rosidl_typesupport_introspection_c__ROS_TYPE_LONG_DOUBLE = 3,
// rosidl_typesupport_introspection_c__ROS_TYPE_CHAR = 4,
// rosidl_typesupport_introspection_c__ROS_TYPE_WCHAR = 5,
// rosidl_typesupport_introspection_c__ROS_TYPE_BOOLEAN = 6,
// rosidl_typesupport_introspection_c__ROS_TYPE_OCTET = 7,
// rosidl_typesupport_introspection_c__ROS_TYPE_UINT8 = 8,
// rosidl_typesupport_introspection_c__ROS_TYPE_INT8 = 9,
// rosidl_typesupport_introspection_c__ROS_TYPE_UINT16 = 10,
// rosidl_typesupport_introspection_c__ROS_TYPE_INT16 = 11,
// rosidl_typesupport_introspection_c__ROS_TYPE_UINT32 = 12,
// rosidl_typesupport_introspection_c__ROS_TYPE_INT32 = 13,
// rosidl_typesupport_introspection_c__ROS_TYPE_UINT64 = 14,
// rosidl_typesupport_introspection_c__ROS_TYPE_INT64 = 15,
// rosidl_typesupport_introspection_c__ROS_TYPE_STRING = 16,
// rosidl_typesupport_introspection_c__ROS_TYPE_WSTRING = 17,
// rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE = 18,
// todo: add these as needed...
if t == (rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8) { "std::string::String".to_owned() }
else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_BOOLEAN as u8) { "bool".to_owned() }
else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT32 as u8) { "i32".to_owned() }
else { panic!("ros native type not implemented: {}", t); }
}
// TODO: this is a terrible hack :)
pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
let key = format!("{}__{}__{}", module, prefix, name);
let ptr = INTROSPECTION_FNS.get(key.as_str()).expect("code generation error");
let ptr = *ptr as *const i32 as *const rosidl_message_type_support_t;
unsafe {
let members = (*ptr).data as *const rosidl_typesupport_introspection_c__MessageMembers;
let namespace = CStr::from_ptr((*members).message_namespace_).to_str().unwrap();
let name = CStr::from_ptr((*members).message_name_).to_str().unwrap();
let nn: Vec<&str> = namespace.split("__").into_iter().take(2).collect();
let (module, prefix) = ( nn[0], nn[1] );
let c_struct = format!("{module}__{prefix}__{msgname}", module = module, prefix=prefix, msgname = name);
let memberslice = std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize);
let mut fields = String::new();
for member in memberslice {
let field_name = CStr::from_ptr((*member).name_).to_str().unwrap();
let type_id = (*member).type_id_;
let is_array = (*member).is_array_; // TODO: use
let rust_field_type = field_type(type_id);
let s = format!("pub {}: {},\n",field_name, rust_field_type);
fields.push_str(&s);
}
let mut from_native = String::new();
from_native.push_str(&format!("fn from_native(msg: &Self::CStruct) -> {} {{\n", name));
from_native.push_str(&format!(" {} {{\n", name));
for member in memberslice {
let field_name = CStr::from_ptr((*member).name_).to_str().unwrap();
let type_id = (*member).type_id_;
let is_array = (*member).is_array_; // TODO: use
let rust_field_type = field_type(type_id);
if rust_field_type == "std::string::String" {
from_native.push_str(&format!("{field_name}: msg.{field_name}.to_str().to_owned(),\n", field_name = field_name));
} else {
from_native.push_str(&format!("{field_name}: msg.{field_name},\n", field_name = field_name));
}
}
from_native.push_str(" }\n }\n");
let mut copy_to_native = String::new();
copy_to_native.push_str("fn copy_to_native(&self, msg: &mut Self::CStruct) {");
for member in memberslice {
let field_name = CStr::from_ptr((*member).name_).to_str().unwrap();
let type_id = (*member).type_id_;
let is_array = (*member).is_array_; // TODO: use
let rust_field_type = field_type(type_id);
// handle other special cases...
if rust_field_type == "std::string::String" {
copy_to_native.push_str(&format!("msg.{field_name}.assign(&self.{field_name});\n", field_name = field_name));
} else {
copy_to_native.push_str(&format!("msg.{field_name} = self.{field_name};\n", field_name = field_name));
}
}
copy_to_native.push_str("}\n");
let typesupport = format!("impl WrappedTypesupport for {msgname} {{ \n
type CStruct = {c_struct}; \n\n
fn get_ts() -> &'static rosidl_message_type_support_t {{ \n
unsafe {{ &*rosidl_typesupport_c__get_message_type_support_handle__{c_struct}() }}
}}\n
fn create_msg() -> *mut {c_struct} {{\n
unsafe {{ {c_struct}__create() }}\n
}}\n
fn destroy_msg(msg: *mut {c_struct}) -> () {{\n
unsafe {{ {c_struct}__destroy(msg) }};\n
}}\n
{from_native}\n\n
{copy_to_native}\n\n
}}\n", msgname = name, c_struct = &c_struct, from_native=from_native, copy_to_native=copy_to_native);
let module_str = format!("
#[derive(Clone,Debug,PartialEq)]
pub struct {msgname} {{\n
{fields}
}}\n
{typesupport}\n\n
", msgname = name, fields = fields, typesupport = typesupport);
module_str
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_fn_ptrs() -> () {
assert_eq!(generate_rust_msg("std_msgs", "msg", "string"), "hej");
}
}

3
msgs.txt Normal file
View File

@ -0,0 +1,3 @@
std_msgs/msg/Bool
std_msgs/msg/String
std_msgs/msg/Int32

14
rcl/Cargo.toml Normal file
View File

@ -0,0 +1,14 @@
[package]
name = "rcl"
version = "0.0.1"
authors = ["Martin Dahl <martin.dahl@gmail.com>"]
edition = "2018"
[features]
headers = []
[dependencies]
libc = "0.2.0"
[build-dependencies]
bindgen = "0.50.0"

38
rcl/build.rs Normal file
View File

@ -0,0 +1,38 @@
extern crate bindgen;
use std::env;
use std::path::PathBuf;
fn main() {
let headers_enabled = env::var_os("CARGO_FEATURE_HEADERS").is_some();
let mut builder = bindgen::Builder::default()
.header("src/rcl_wrapper.h")
.derive_copy(false)
.default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } );
let ament_prefix_var_name = "AMENT_PREFIX_PATH";
let ament_prefix_var = env::var(ament_prefix_var_name).expect("Source your ROS!");
for ament_prefix_path in ament_prefix_var.split(":") {
builder = builder.clang_arg(format!("-I{}/include", ament_prefix_path));
println!("cargo:rustc-link-search=native={}/lib", ament_prefix_path);
}
println!("cargo:rustc-link-lib=dylib=rcl");
println!("cargo:rustc-link-lib=dylib=rcl_logging_noop");
println!("cargo:rustc-link-lib=dylib=rcutils");
println!("cargo:rustc-link-lib=dylib=rmw");
println!("cargo:rustc-link-lib=dylib=rmw_implementation");
println!("cargo:rustc-link-lib=dylib=rosidl_typesupport_c");
println!("cargo:rustc-link-lib=dylib=rosidl_generator_c");
// bindgen takes time so we dont want to do it always... must be a better way
if headers_enabled {
let bindings = builder.generate().expect("Unable to generate bindings");
let out_path = PathBuf::from(".");
bindings
.write_to_file(out_path.join("src/rcl_bindings.rs"))
.expect("Couldn't write bindings!");
}
}

57
rcl/src/lib.rs Normal file
View File

@ -0,0 +1,57 @@
#![allow(non_upper_case_globals)]
#![allow(non_camel_case_types)]
#![allow(non_snake_case)]
#![allow(improper_ctypes)]
#![allow(dead_code)]
pub mod rcl_bindings;
pub use rcl_bindings::*;
use std::ffi::CStr;
use std::ffi::CString;
// special treatment to convert to/from rust strings.
// ros strings are owned by ros, assignment is a copy
impl rosidl_generator_c__String {
pub fn to_str(&self) -> &str {
let s = unsafe { CStr::from_ptr(self.data as *mut i8) };
s.to_str().unwrap_or("")
}
pub fn assign(&mut self, other: &str) -> () {
let q = CString::new(other).unwrap();
let to_send_ptr = q.as_ptr() as *const i8;
unsafe {
rosidl_generator_c__String__assign(self as *mut _, to_send_ptr);
}
}
}
impl Default for rmw_message_info_t {
fn default() -> Self {
rmw_message_info_t {
publisher_gid: rmw_gid_t {
implementation_identifier: std::ptr::null(),
data: [0; 24],
},
from_intra_process: false,
}
}
}
impl Default for rmw_qos_profile_t {
fn default() -> Self {
rmw_qos_profile_t {
history: rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT,
depth: 10,
reliability: rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
durability: rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
avoid_ros_namespace_conventions: false,
deadline: rmw_time_t { sec: 0, nsec: 0 },
lifespan: rmw_time_t { sec: 0, nsec: 0 },
liveliness: rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
liveliness_lease_duration: rmw_time_t { sec: 0, nsec: 0 },
}
}
}

12
rcl/src/rcl_wrapper.h Normal file
View File

@ -0,0 +1,12 @@
// ros client library
#include <rcl/rcl.h>
#include <rcutils/error_handling.h>
// low level msg type handling
#include <rosidl_generator_c/string.h>
#include <rosidl_generator_c/string_functions.h>
#include <rosidl_generator_c/message_type_support_struct.h>
#include <rosidl_typesupport_introspection_c/message_introspection.h>
#include <rosidl_typesupport_introspection_c/field_types.h>

366
src/lib.rs Normal file
View File

@ -0,0 +1,366 @@
include!("./generated_msgs.rs");
use msg_gen::*;
use rcl::*;
use std::ffi::CString;
use std::ops::{Deref, DerefMut};
pub trait WrappedTypesupport {
type CStruct;
fn get_ts() -> &'static rosidl_message_type_support_t;
fn create_msg() -> *mut Self::CStruct;
fn destroy_msg(msg: *mut Self::CStruct);
fn from_native(msg: &Self::CStruct) -> Self;
fn copy_to_native(&self, msg: &mut Self::CStruct);
}
#[derive(Debug)]
pub struct WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
pub msg: *mut T::CStruct,
}
impl<T> WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
pub fn new() -> Self {
WrappedNativeMsg {
msg: T::create_msg(),
}
}
pub fn from(msg: &T) -> Self {
let mut native_msg = Self::new();
msg.copy_to_native(&mut native_msg);
native_msg
}
pub fn void_ptr(&self) -> *const std::os::raw::c_void {
self.msg as *const _ as *const std::os::raw::c_void
}
pub fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void {
self.msg as *mut _ as *mut std::os::raw::c_void
}
}
impl<T> Drop for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
fn drop(&mut self) {
T::destroy_msg(self.msg);
}
}
impl<T> Deref for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
type Target = T::CStruct;
fn deref(&self) -> &Self::Target {
unsafe { &(*self.msg) }
}
}
impl<T> DerefMut for WrappedNativeMsg<T>
where
T: WrappedTypesupport,
{
fn deref_mut(&mut self) -> &mut Self::Target {
unsafe { &mut (*self.msg) }
}
}
pub trait Sub {
fn handle(&self) -> &rcl_subscription_t;
fn run_cb(&mut self) -> ();
fn rcl_msg(&mut self) -> *mut std::os::raw::c_void;
}
pub struct WrappedSubT<T>
where
T: WrappedTypesupport,
{
pub rcl_handle: rcl_subscription_t,
pub callback: Box<dyn FnMut(T) -> ()>,
pub rcl_msg: WrappedNativeMsg<T>,
}
impl<T> Sub for WrappedSubT<T>
where
T: WrappedTypesupport,
{
fn handle(&self) -> &rcl_subscription_t {
&self.rcl_handle
}
fn rcl_msg(&mut self) -> *mut std::os::raw::c_void {
self.rcl_msg.void_ptr_mut()
}
fn run_cb(&mut self) -> () {
// copy native msg to rust type and run callback
let msg = T::from_native(&self.rcl_msg);
(self.callback)(msg);
}
}
pub fn rcl_create_context() -> Result<rcl_context_t, ()> {
let mut ctx = unsafe { rcl_get_zero_initialized_context() };
let isok = 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(0, std::ptr::null(), &init_options, &mut ctx);
let isok = rcl_context_is_valid(&mut ctx);
rcl_init_options_fini(&mut init_options as *mut _);
isok
};
if isok {
Ok(ctx)
} else {
Err(())
}
}
pub fn rcl_create_node(
name: &str,
namespace: &str,
ctx: &mut rcl_context_t,
) -> Result<rcl_node_t, ()> {
let c_node_name = CString::new(name).unwrap();
let c_node_ns = CString::new(namespace).unwrap();
let mut node_handle = unsafe { rcl_get_zero_initialized_node() };
let nr = unsafe {
let node_options = rcl_node_get_default_options();
rcl_node_init(
&mut node_handle as *mut _,
c_node_name.as_ptr(),
c_node_ns.as_ptr(),
ctx,
&node_options as *const _,
)
};
if nr == RCL_RET_OK as i32 {
Ok(node_handle)
} else {
eprintln!("{}", nr);
Err(())
}
}
pub fn rcl_destroy_node(node: &mut rcl_node_t) {
unsafe {
rcl_node_fini(node);
}
}
pub fn rcl_destroy_ctx(ctx: &mut rcl_context_t) {
unsafe {
rcl_shutdown(ctx);
}
}
pub fn rcl_create_publisher<T>(node: &mut rcl_node_t, topic: &str) -> Result<rcl_publisher_t, ()>
where
T: WrappedTypesupport,
{
let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() };
let topic_c_string = CString::new(topic).unwrap();
let result = unsafe {
let mut publisher_options = rcl_publisher_get_default_options();
publisher_options.qos = rmw_qos_profile_t::default();
rcl_publisher_init(
&mut publisher_handle,
node,
T::get_ts(),
topic_c_string.as_ptr(),
&publisher_options,
)
};
if result == RCL_RET_OK as i32 {
Ok(publisher_handle)
} else {
eprintln!("{}", result);
Err(())
}
}
pub fn publish<T>(publisher: &rcl_publisher_t, msg: &T) -> Result<(), ()>
where
T: WrappedTypesupport,
{
// copy rust msg to native and publish it
let native_msg: WrappedNativeMsg<T> = WrappedNativeMsg::<T>::from(msg);
let result = unsafe { rcl_publish(publisher, native_msg.void_ptr(), std::ptr::null_mut()) };
if result == RCL_RET_OK as i32 {
Ok(())
} else {
eprintln!("{}", result);
Err(())
}
}
pub fn rcl_create_subscription<T>(
node: &mut rcl_node_t,
topic: &str,
callback: Box<dyn FnMut(T) -> ()>,
) -> Result<WrappedSubT<T>, ()>
where
T: WrappedTypesupport,
{
let mut subscription_handle = unsafe { rcl_get_zero_initialized_subscription() };
let topic_c_string = CString::new(topic).unwrap();
let result = unsafe {
let mut subscription_options = rcl_subscription_get_default_options();
subscription_options.qos = rmw_qos_profile_t::default();
rcl_subscription_init(
&mut subscription_handle,
node,
T::get_ts(),
topic_c_string.as_ptr(),
&subscription_options,
)
};
if result == RCL_RET_OK as i32 {
let wrapped_sub = WrappedSubT {
rcl_handle: subscription_handle,
rcl_msg: WrappedNativeMsg::<T>::new(),
callback: callback,
};
Ok(wrapped_sub)
} else {
eprintln!("{}", result);
Err(())
}
}
pub fn rcl_take_subst(
ctx: &mut rcl_context_t,
subs: &mut Vec<Box<dyn Sub>>,
timeout: i64,
) -> Result<(), ()> {
let mut ws = unsafe { rcl_get_zero_initialized_wait_set() };
unsafe {
rcl_wait_set_init(
&mut ws,
subs.len(),
0,
0,
0,
0,
0,
ctx,
rcutils_get_default_allocator(),
);
rcl_wait_set_clear(&mut ws);
}
for s in subs.iter() {
unsafe {
rcl_wait_set_add_subscription(&mut ws, s.handle(), std::ptr::null_mut());
}
}
unsafe {
rcl_wait(&mut ws, timeout);
}
for s in subs {
let mut msg_info = rmw_message_info_t::default();
let ret = unsafe { rcl_take(s.handle(), s.rcl_msg(), &mut msg_info, std::ptr::null_mut()) };
// fresh message, run cb
if ret == RCL_RET_OK as i32 {
s.run_cb();
}
}
Ok(())
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_ros_str() -> () {
let hej = "hej hopp";
let mut msg = WrappedNativeMsg::<std_msgs::msg::String>::new();
msg.data.assign(hej);
assert_eq!(msg.data.to_str(), hej);
}
#[test]
fn test_copy_fields() -> () {
let msg_orig = std_msgs::msg::String { data: "hej".into() };
let rosmsg = WrappedNativeMsg::<std_msgs::msg::String>::from(&msg_orig);
let msg2 = std_msgs::msg::String::from_native(&rosmsg);
assert_eq!(msg_orig, msg2);
}
#[test]
fn test_introspection_string() -> () {
unsafe {
use std::ffi::CStr;
let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__std_msgs__msg__String();
let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
println!("{:#?}", *members);
assert_eq!((*members).member_count_, 1);
let s = CStr::from_ptr((*members).message_namespace_)
.to_str()
.unwrap();
assert_eq!(s, "std_msgs__msg");
let s = CStr::from_ptr((*members).message_name_).to_str().unwrap();
assert_eq!(s, "String");
let member = (*members).members_;
println!("member: {:#?}", *member);
let field_name = CStr::from_ptr((*member).name_).to_str().unwrap();
let type_id = (*member).type_id_;
let is_array = (*member).is_array_;
assert_eq!(field_name, "data");
assert_eq!(
type_id,
rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8
);
assert_eq!(is_array, false);
}
}
#[test]
fn test_generation_string_use() -> () {
let msg = std_msgs::msg::String { data: "hej".into() };
let msg2 = msg.clone();
let msg_native = WrappedNativeMsg::<std_msgs::msg::String>::from(&msg2);
let msg2 = std_msgs::msg::String::from_native(&msg_native);
assert_eq!(msg, msg2)
}
#[test]
fn test_generation_bool_use() -> () {
let msg = std_msgs::msg::Bool { data: true };
let msg_native = WrappedNativeMsg::<std_msgs::msg::Bool>::from(&msg);
let msg2 = std_msgs::msg::Bool::from_native(&msg_native);
assert_eq!(msg,msg2);
}
}