#![deny(clippy::all)] #[cfg(not(unix))] fn main() { println!("This example only works on a unix based system"); } use std::time::Duration; #[cfg(unix)] use termion::raw::*; #[allow(unused_imports)] use log::{debug, error, info, warn}; use serde::{Deserialize, Serialize}; use mio_06::{Events, Poll, PollOpt, Ready, Token}; use mio_extras::channel as mio_channel; use rustdds::{ policy::*, ros2::{NodeOptions, RosParticipant}, *, }; #[cfg(unix)] use ui::{RosCommand, UiController}; // modules #[cfg(unix)] mod ui; const TURTLE_CMD_VEL_READER_TOKEN: Token = Token(1); const ROS2_COMMAND_TOKEN: Token = Token(2); const TURTLE_POSE_READER_TOKEN: Token = Token(3); // This corresponds to ROS2 message type // https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/Twist.msg // // The struct definition must have a layout corresponding to the // ROS2 msg definition to get compatible serialization. #[derive(Debug, Clone, Serialize, Deserialize)] pub struct Twist { pub linear: Vector3, pub angular: Vector3, } // https://docs.ros2.org/foxy/api/turtlesim/msg/Pose.html #[derive(Debug, Clone, Serialize, Deserialize)] pub struct Pose { pub x: f32, pub y: f32, pub theta: f32, pub linear_velocity: f32, pub angular_velocity: f32, } // This corresponds to ROS2 message type // https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/Vector3.msg #[derive(Debug, Clone, Serialize, Deserialize)] pub struct Vector3 { pub x: f64, pub y: f64, pub z: f64, } impl Vector3 { pub const ZERO: Vector3 = Vector3 { x: 0.0, y: 0.0, z: 0.0, }; } #[cfg(unix)] fn main() { // Here is a fixed path, so this example must be started from // RustDDS main directory log4rs::init_file("examples/turtle_teleop/log4rs.yaml", Default::default()).unwrap(); let (command_sender, command_receiver) = mio_channel::sync_channel::(10); let (readback_sender, readback_receiver) = mio_channel::sync_channel(10); let (pose_sender, pose_receiver) = mio_channel::sync_channel(10); // For some strange reason the ROS2 messaging event loop is in a separate thread // and we talk to it using (mio) mpsc channels. let jhandle = std::thread::Builder::new() .name("ros2_loop".into()) .spawn(move || ros2_loop(command_receiver, readback_sender, pose_sender)) .unwrap(); // From termion docs: // "A terminal restorer, which keeps the previous state of the terminal, // and restores it, when dropped. // Restoring will entirely bring back the old TTY state." // So the point of _stdout_restorer is that it will restore the TTY back to // its original cooked mode when the variable is dropped. let _stdout_restorer = std::io::stdout().into_raw_mode().unwrap(); // UI loop, which is in the main thread let mut main_control = UiController::new( std::io::stdout(), command_sender, readback_receiver, pose_receiver, ); main_control.start(); jhandle.join().unwrap(); // wait until threads exit. // need to wait a bit for cleanup, because drop is not waited for join std::thread::sleep(Duration::from_millis(10)); } #[cfg(unix)] fn ros2_loop( command_receiver: mio_channel::Receiver, readback_sender: mio_channel::SyncSender, pose_sender: mio_channel::SyncSender, ) { info!("ros2_loop"); let qos: QosPolicies = { QosPolicyBuilder::new() .durability(Durability::Volatile) .liveliness(Liveliness::Automatic { lease_duration: rustdds::Duration::INFINITE, }) .reliability(Reliability::Reliable { max_blocking_time: rustdds::Duration::from_millis(100), }) .history(History::KeepLast { depth: 10 }) .build() }; let mut ros_participant = RosParticipant::new().unwrap(); let mut ros_node = ros_participant .new_ros_node( "turtle_teleop", // name "/ros2_demo", // namespace NodeOptions::new(false), // enable rosout ) .unwrap(); let turtle_cmd_vel_topic = ros_node .create_ros_topic( "/turtle1/cmd_vel", String::from("geometry_msgs::msg::dds_::Twist_"), &qos, TopicKind::NoKey, ) .unwrap(); // The point here is to publish Twist for the turtle let turtle_cmd_vel_writer = ros_node .create_ros_no_key_publisher::>(&turtle_cmd_vel_topic, None) .unwrap(); // But here is how to read it also, if anyone is interested. // This should show what is the turtle command in case someone else is // also issuing commands, i.e. there are two turtle controllers running. let mut turtle_cmd_vel_reader = ros_node .create_ros_no_key_subscriber::>(&turtle_cmd_vel_topic, None) .unwrap(); let turtle_pose_topic = ros_node .create_ros_topic( "/turtle1/pose", String::from("turtlesim::msg::dds_::Pose_"), &qos, TopicKind::NoKey, ) .unwrap(); let mut turtle_pose_reader = ros_node .create_ros_no_key_subscriber::>(&turtle_pose_topic, None) .unwrap(); let poll = Poll::new().unwrap(); poll .register( &command_receiver, ROS2_COMMAND_TOKEN, Ready::readable(), PollOpt::edge(), ) .unwrap(); poll .register( &turtle_cmd_vel_reader, TURTLE_CMD_VEL_READER_TOKEN, Ready::readable(), PollOpt::edge(), ) .unwrap(); poll .register( &turtle_pose_reader, TURTLE_POSE_READER_TOKEN, Ready::readable(), PollOpt::edge(), ) .unwrap(); info!("Entering event_loop"); 'event_loop: loop { let mut events = Events::with_capacity(100); poll.poll(&mut events, None).unwrap(); for event in events.iter() { match event.token() { ROS2_COMMAND_TOKEN => { while let Ok(command) = command_receiver.try_recv() { match command { RosCommand::StopEventLoop => { info!("Stopping main event loop"); ros_participant.clear(); break 'event_loop; } RosCommand::TurtleCmdVel { twist } => { match turtle_cmd_vel_writer.write(twist.clone(), None) { Ok(_) => { info!("Wrote to ROS2 {:?}", twist); } Err(e) => { error!("Failed to write to turtle writer. {e:?}"); ros_node.clear_node(); return; } } } }; } } TURTLE_CMD_VEL_READER_TOKEN => { while let Ok(Some(twist)) = turtle_cmd_vel_reader.take_next_sample() { readback_sender.send(twist.value().clone()).unwrap(); } } TURTLE_POSE_READER_TOKEN => { while let Ok(Some(pose)) = turtle_pose_reader.take_next_sample() { pose_sender.send(pose.value().clone()).unwrap(); } } _ => { error!("Unknown poll token {:?}", event.token()) } } // match } // for } }