51 lines
1.5 KiB
Rust
51 lines
1.5 KiB
Rust
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(())
|
|
|
|
}
|
|
|