Started adding support for arrays and nested messages. More derives!
Serde derives should maybe be a feature that can be toggled.
This commit is contained in:
Martin Dahl
parent
2ebeaf8682
commit
b789ee9e36
|
|
@ -11,10 +11,14 @@ edition = "2018"
|
|||
headers = ["rcl/headers", "msg_gen/headers"]
|
||||
|
||||
[dependencies]
|
||||
serde = { version = "1.0", features = ["derive"] }
|
||||
common = { path = "common", version = "0.0.1" }
|
||||
rcl = { path = "rcl", version = "0.0.1" }
|
||||
msg_gen = { path = "msg_gen", version = "0.0.1" }
|
||||
|
||||
[dev-dependencies]
|
||||
serde_json = "1.0"
|
||||
|
||||
[build-dependencies]
|
||||
common = { path = "common", version = "0.0.1" }
|
||||
msg_gen = { path = "msg_gen", version = "0.0.1" }
|
||||
|
|
|
|||
|
|
@ -0,0 +1,50 @@
|
|||
use r2r::*;
|
||||
use builtin_interfaces::msg::Duration;
|
||||
use trajectory_msgs::msg::*;
|
||||
|
||||
fn main() -> Result<(), ()> {
|
||||
let mut ctx = rcl_create_context()?;
|
||||
let mut node = rcl_create_node(&mut ctx, "testnode", "")?;
|
||||
let publisher = rcl_create_publisher::<JointTrajectoryPoint>(&mut node, "/hej")?;
|
||||
|
||||
let mut c = 0;
|
||||
let cb = move |x:std_msgs::msg::String| {
|
||||
println!("at count {} got: {}", c, x.data);
|
||||
c = c + 1;
|
||||
let positions: Vec<f64> = vec!(94.2 * c as f64);
|
||||
let to_send = JointTrajectoryPoint {
|
||||
positions,
|
||||
time_from_start : Duration { sec: c, nanosec: 0 },
|
||||
..Default::default()
|
||||
};
|
||||
publish(&publisher, &to_send).unwrap();
|
||||
};
|
||||
|
||||
let cb2 = move |x:JointTrajectoryPoint| {
|
||||
let serialized = serde_json::to_string(&x).unwrap();
|
||||
println!("JTP serialized as: {}", serialized);
|
||||
};
|
||||
|
||||
let sub1 = rcl_create_subscription(&mut node, "/hopp", Box::new(cb))?;
|
||||
let sub2 = rcl_create_subscription(&mut node, "/hej", Box::new(cb2))?;
|
||||
|
||||
// TODO: group subscriptions in a "node" struct
|
||||
let mut subst: Vec<Box<Sub>> = vec![Box::new(sub1), Box::new(sub2)];
|
||||
|
||||
// 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(())
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -5,16 +5,17 @@ use r2r::*;
|
|||
|
||||
fn main() -> Result<(), ()> {
|
||||
let mut ctx = rcl_create_context()?;
|
||||
let mut node = rcl_create_node("qqq", "", &mut ctx)?;
|
||||
let mut node = rcl_create_node(&mut ctx, "qqq", "")?;
|
||||
|
||||
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>();
|
||||
let (tx, rx) = mpsc::channel::<String>();
|
||||
thread::spawn(move|| {
|
||||
loop {
|
||||
let msg = rx.recv().unwrap();
|
||||
println!("other thread received: {}", msg.data);
|
||||
let deserialized: std_msgs::msg::String = serde_json::from_str(&msg).unwrap();
|
||||
println!("received: {}, deserialized ros msg = {:?}", msg, deserialized);
|
||||
}
|
||||
});
|
||||
|
||||
|
|
@ -22,7 +23,8 @@ fn main() -> Result<(), ()> {
|
|||
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 serialized = serde_json::to_string(&x).unwrap();
|
||||
tx.send(serialized).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 };
|
||||
|
|
|
|||
|
|
@ -16,3 +16,4 @@ rcl = { path = "../rcl", version = "0.0.1" }
|
|||
bindgen = "0.50.0"
|
||||
rcl = { path = "../rcl", version = "0.0.1" }
|
||||
common = { path = "../common", version = "0.0.1" }
|
||||
heck = "0.3.1"
|
||||
|
|
|
|||
|
|
@ -6,6 +6,8 @@ use std::path::PathBuf;
|
|||
use common::*;
|
||||
|
||||
fn main() {
|
||||
println!("cargo:rerun-if-changed=../");
|
||||
|
||||
let msgs = read_file("../msgs.txt").unwrap();
|
||||
let msg_list = parse_msgs(&msgs);
|
||||
let msg_map = as_map(&msg_list);
|
||||
|
|
@ -24,12 +26,17 @@ fn main() {
|
|||
|
||||
for msg in msg_list {
|
||||
|
||||
// I *think* these are always just lowercase
|
||||
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));
|
||||
// filename is certainly CamelCase -> snake_case. convert
|
||||
use heck::SnakeCase;
|
||||
let include_filename = msg.name.to_snake_case();
|
||||
|
||||
includes.push_str(&format!("#include <{}/{}/{}.h>\n", &module, &prefix, &include_filename));
|
||||
includes.push_str(&format!("#include <{}/{}/{}__rosidl_typesupport_introspection_c.h>\n", &module, &prefix, &include_filename));
|
||||
|
||||
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);
|
||||
|
|
@ -52,6 +59,7 @@ fn main() {
|
|||
// blacklist types that are handled by rcl bindings
|
||||
.blacklist_type("rosidl_message_type_support_t")
|
||||
.blacklist_type("rosidl_generator_c__String")
|
||||
.blacklist_type("rosidl_generator_c__double__Sequence") // etc...
|
||||
.default_enum_style(bindgen::EnumVariation::Rust { non_exhaustive: false } );
|
||||
|
||||
let ament_prefix_var_name = "AMENT_PREFIX_PATH";
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ use std::ffi::CStr;
|
|||
// float64[] accelerations
|
||||
// float64[] effort
|
||||
// builtin_interfaces/Duration time_from_start
|
||||
// martin@martin-XPS-15-9550 ~ $ ros2 msg show builtin_interfaces/Duration
|
||||
// martin@martin-XPS-15-9550 ~ $ ros2 msg show builtin_interfaces/msg/Duration
|
||||
// int32 sec
|
||||
// uint32 nanosec
|
||||
//
|
||||
|
|
@ -57,14 +57,19 @@ fn field_type(t: u8) -> String {
|
|||
// 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_UINT32 as u8) { "u32".to_owned() }
|
||||
else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_INT32 as u8) { "i32".to_owned() }
|
||||
else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_FLOAT as u8) { "f32".to_owned() }
|
||||
else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_DOUBLE as u8) { "f64".to_owned() }
|
||||
else if t == (rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE as u8) { "message".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 = INTROSPECTION_FNS.get(key.as_str()).expect(&format!("code generation error: {}", name));
|
||||
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;
|
||||
|
|
@ -84,7 +89,21 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
|
|||
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);
|
||||
let rust_field_type = if rust_field_type == "message" {
|
||||
// perform a hack!
|
||||
let ts = (*member).members_;
|
||||
let members = (*ts).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] );
|
||||
format!("{module}::{prefix}::{msgname}", module = module, prefix=prefix, msgname = name)
|
||||
} else { rust_field_type };
|
||||
let s = if is_array {
|
||||
format!("pub {}: Vec<{}>,\n",field_name, rust_field_type)
|
||||
} else {
|
||||
format!("pub {}: {},\n",field_name, rust_field_type)
|
||||
};
|
||||
fields.push_str(&s);
|
||||
}
|
||||
|
||||
|
|
@ -102,10 +121,23 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
|
|||
|
||||
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 if rust_field_type == "message" {
|
||||
// perform a hack!
|
||||
let ts = (*member).members_;
|
||||
let members = (*ts).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] );
|
||||
from_native.push_str(&format!("{field_name}: {module}::{prefix}::{msgname}::from_native(&msg.{field_name}),", field_name = field_name, module = module, prefix=prefix, msgname = name));
|
||||
} else {
|
||||
if is_array {
|
||||
from_native.push_str(&format!("{field_name}: msg.{field_name}.to_vec(),\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();
|
||||
|
|
@ -121,10 +153,16 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
|
|||
// 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 if rust_field_type == "message" {
|
||||
copy_to_native.push_str(&format!("self.{field_name}.copy_to_native(&mut msg.{field_name});", field_name = field_name));
|
||||
} else {
|
||||
if is_array {
|
||||
copy_to_native.push_str(&format!("msg.{field_name}.update(&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
|
||||
|
|
@ -143,7 +181,7 @@ pub fn generate_rust_msg(module: &str, prefix: &str, name: &str) -> String {
|
|||
}}\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)]
|
||||
#[derive(Clone,Debug,Default,PartialEq,Serialize,Deserialize)]
|
||||
pub struct {msgname} {{\n
|
||||
{fields}
|
||||
}}\n
|
||||
|
|
|
|||
2
msgs.txt
2
msgs.txt
|
|
@ -1,3 +1,5 @@
|
|||
std_msgs/msg/Bool
|
||||
std_msgs/msg/String
|
||||
std_msgs/msg/Int32
|
||||
builtin_interfaces/msg/Duration
|
||||
trajectory_msgs/msg/JointTrajectoryPoint
|
||||
|
|
|
|||
|
|
@ -4,6 +4,8 @@ use std::env;
|
|||
use std::path::PathBuf;
|
||||
|
||||
fn main() {
|
||||
println!("cargo:rerun-if-changed=../");
|
||||
|
||||
let headers_enabled = env::var_os("CARGO_FEATURE_HEADERS").is_some();
|
||||
let mut builder = bindgen::Builder::default()
|
||||
.header("src/rcl_wrapper.h")
|
||||
|
|
|
|||
|
|
@ -55,3 +55,31 @@ impl Default for rmw_qos_profile_t {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
// conversions from/to vectors
|
||||
// macro:ify this increase maintainability
|
||||
|
||||
impl rosidl_generator_c__double__Sequence {
|
||||
pub fn update(&mut self, values: &[f64]) {
|
||||
// crash here?
|
||||
unsafe { rosidl_generator_c__float64__Sequence__fini(self as *mut _); }
|
||||
unsafe { rosidl_generator_c__float64__Sequence__init(self as *mut _, values.len()); }
|
||||
unsafe { std::ptr::copy(values.as_ptr(), self.data, values.len()); }
|
||||
}
|
||||
|
||||
pub fn to_vec(&self) -> Vec<f64> {
|
||||
let mut dst = Vec::with_capacity(self.size);
|
||||
unsafe { dst.set_len(self.size); }
|
||||
unsafe { std::ptr::copy(self.data, dst.as_mut_ptr(), self.size); }
|
||||
dst
|
||||
}
|
||||
|
||||
// dont think we need fini? surely messages call fini on all their fields...?
|
||||
//
|
||||
// extern "C" {
|
||||
// pub fn rosidl_generator_c__float64__Sequence__fini(
|
||||
// sequence: *mut rosidl_generator_c__double__Sequence,
|
||||
// );
|
||||
// }
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@
|
|||
#include <rosidl_generator_c/string.h>
|
||||
#include <rosidl_generator_c/string_functions.h>
|
||||
|
||||
#include <rosidl_generator_c/primitives_sequence.h>
|
||||
#include <rosidl_generator_c/primitives_sequence_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>
|
||||
|
|
|
|||
33
src/lib.rs
33
src/lib.rs
|
|
@ -5,6 +5,7 @@ use rcl::*;
|
|||
|
||||
use std::ffi::CString;
|
||||
use std::ops::{Deref, DerefMut};
|
||||
use serde::{Serialize, Deserialize};
|
||||
|
||||
pub trait WrappedTypesupport {
|
||||
type CStruct;
|
||||
|
|
@ -132,9 +133,9 @@ pub fn rcl_create_context() -> Result<rcl_context_t, ()> {
|
|||
}
|
||||
|
||||
pub fn rcl_create_node(
|
||||
ctx: &mut rcl_context_t,
|
||||
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();
|
||||
|
|
@ -290,6 +291,10 @@ pub fn rcl_take_subst(
|
|||
}
|
||||
}
|
||||
|
||||
unsafe {
|
||||
rcl_wait_set_fini(&mut ws);
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
|
|
@ -363,4 +368,30 @@ mod tests {
|
|||
assert_eq!(msg,msg2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_float_sequence() -> () {
|
||||
use trajectory_msgs::msg::*;
|
||||
let native = WrappedNativeMsg::<JointTrajectoryPoint>::new();
|
||||
let mut msg = JointTrajectoryPoint::from_native(&native);
|
||||
msg.positions.push(39.0);
|
||||
msg.positions.push(34.0);
|
||||
let new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
|
||||
let new_msg = JointTrajectoryPoint::from_native(&new_native);
|
||||
println!("{:#?}", new_msg);
|
||||
assert_eq!(msg,new_msg);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_deault() -> () {
|
||||
use trajectory_msgs::msg::*;
|
||||
let mut msg: JointTrajectoryPoint = Default::default();
|
||||
msg.positions.push(39.0);
|
||||
msg.positions.push(34.0);
|
||||
let mut new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
|
||||
unsafe { *((*new_native).positions.data) = 88.9 };
|
||||
let new_msg = JointTrajectoryPoint::from_native(&new_native);
|
||||
println!("{:#?}", new_msg);
|
||||
assert_ne!(msg,new_msg);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue