use std::{convert::TryInto, time::Duration};
use chrono::{DateTime, Utc};
use ros2_client::*;
pub fn main() {
log4rs::init_file("log4rs.yaml", Default::default()).unwrap();
let context = Context::new().unwrap();
let mut node = context
.new_node(
NodeName::new("/", "time_broadcaster").unwrap(),
NodeOptions::new()
.enable_rosout(true)
.declare_parameter("my_param", ParameterValue::String("foo".to_owned()))
.parameter_validator(Box::new(|name, value| match name {
"my_param" => match value {
ParameterValue::String(s) if s.len() == 3 => Ok(()),
_ => Err("my_param must be a string of 3 chars".to_string()),
},
_ => Ok(()),
}))
.parameter_set_action(Box::new(|name, value| {
println!("Setting {}={:?}", name, value);
Ok(())
})),
)
.unwrap();
let clock_publisher = node
.create_publisher::<builtin_interfaces::Time>(
&node
.create_topic(
&Name::new("/", "clock").unwrap(),
MessageTypeName::new("builtin_interfaces", "Time"),
&DEFAULT_PUBLISHER_QOS,
)
.unwrap(),
None,
)
.unwrap();
smol::spawn(node.spinner().unwrap().spin()).detach();
let sim_time_tick = Duration::from_millis(1000);
let real_time_tick = Duration::from_millis(2000);
let mut sim_time = node.time_now();
smol::block_on(async move {
loop {
println!("tick {:?}", DateTime::<Utc>::from(sim_time));
clock_publisher.publish(sim_time.into()).unwrap();
sim_time = sim_time + sim_time_tick.try_into().unwrap();
smol::Timer::after(real_time_tick).await;
}
});
}