58 lines
1.8 KiB
Rust
58 lines
1.8 KiB
Rust
use r2r;
|
|
use r2r::builtin_interfaces::msg::Duration;
|
|
use r2r::trajectory_msgs::msg::*;
|
|
use r2r::std_msgs::msg::Int32;
|
|
use failure::Error;
|
|
|
|
fn main() -> Result<(), Error> {
|
|
let ctx = r2r::Context::create()?;
|
|
let mut node = r2r::Node::create(ctx, "testnode", "")?;
|
|
let publisher = node.create_publisher::<JointTrajectoryPoint>("/hej")?;
|
|
let publisher2 = node.create_publisher::<Int32>("/native_count")?;
|
|
|
|
let mut c = 0;
|
|
let mut positions: Vec<f64> = Vec::new();
|
|
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 },
|
|
..Default::default()
|
|
};
|
|
let mut native = r2r::WrappedNativeMsg::<Int32>::new();
|
|
native.data = c;
|
|
|
|
publisher.publish(&to_send).unwrap();
|
|
publisher2.publish_native(&native).unwrap();
|
|
};
|
|
|
|
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>| {
|
|
println!("Raw c data: {:?}", raw_c.positions);
|
|
};
|
|
|
|
let _sub1 = node.subscribe("/hopp", Box::new(cb))?;
|
|
let _sub2 = node.subscribe("/hej", Box::new(cb2))?;
|
|
let _sub3 = node.subscribe_native("/hej", Box::new(cb3))?;
|
|
|
|
// TODO: group subscriptions in a "node" struct
|
|
//let mut subs: Vec<Box<Sub>> = vec![Box::new(sub1), Box::new(sub2), Box::new(sub3)];
|
|
|
|
// run for 10 seconds
|
|
let mut count = 0;
|
|
while count < 100 {
|
|
node.spin_once(std::time::Duration::from_millis(100));
|
|
count += 1;
|
|
}
|
|
|
|
println!("All done!");
|
|
|
|
Ok(())
|
|
}
|