36 lines
1.1 KiB
Rust
36 lines
1.1 KiB
Rust
use r2r;
|
|
use r2r::builtin_interfaces::msg::Duration;
|
|
use r2r::std_msgs::msg::Int32;
|
|
use r2r::trajectory_msgs::msg::*;
|
|
|
|
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>("/jtp")?;
|
|
let publisher2 = node.create_publisher::<Int32>("/native_count")?;
|
|
|
|
// run for 10 seconds
|
|
let mut count = 0;
|
|
let mut positions: Vec<f64> = Vec::new();
|
|
while count < 100 {
|
|
positions.push(count as f64);
|
|
let to_send = JointTrajectoryPoint {
|
|
positions: positions.clone(),
|
|
time_from_start: Duration { sec: count, nanosec: 0 },
|
|
..Default::default()
|
|
};
|
|
let mut native = r2r::WrappedNativeMsg::<Int32>::new();
|
|
native.data = count;
|
|
|
|
publisher.publish(&to_send).unwrap();
|
|
publisher2.publish_native(&native).unwrap();
|
|
|
|
std::thread::sleep(std::time::Duration::from_millis(100));
|
|
count += 1;
|
|
}
|
|
|
|
println!("All done!");
|
|
|
|
Ok(())
|
|
}
|