diff --git a/examples/turtlesim_rs/Cargo.toml b/examples/turtlesim_rs/Cargo.toml new file mode 100644 index 000000000..5c54fa73e --- /dev/null +++ b/examples/turtlesim_rs/Cargo.toml @@ -0,0 +1,44 @@ +[package] +name = "turtlesim_rs" +version = "0.1.0" +edition = "2021" + +[dependencies] +eframe = "0.27.0" +egui_extras = { version = "0.27.0", features = ["all_loaders"]} +tiny-skia = "0.11.4" +rand = "0.8.5" +termion = "1.5" + +[[bin]] +name = "turtlesim_node" +path = "src/turtlesim.rs" + +[[bin]] +name = "turtle_teleop_key" +path = "tutorials/turtle_teleop_key.rs" + +[[bin]] +name = "mimic" +path = "tutorials/mimic.rs" + +[dependencies.rclrs] +version = "0.4" + +[dependencies.std_msgs] +version = "*" + +[dependencies.std_srvs] +version = "*" + +[dependencies.geometry_msgs] +version = "*" + +[dependencies.turtlesim_rs_msgs] +version = "0.1.0" + +[dependencies.rosidl_runtime_rs] +version = "0.4" + + + diff --git a/examples/turtlesim_rs/images/box-turtle.png b/examples/turtlesim_rs/images/box-turtle.png new file mode 100644 index 000000000..6584fd27e Binary files /dev/null and b/examples/turtlesim_rs/images/box-turtle.png differ diff --git a/examples/turtlesim_rs/images/diamondback.png b/examples/turtlesim_rs/images/diamondback.png new file mode 100644 index 000000000..b3d07054f Binary files /dev/null and b/examples/turtlesim_rs/images/diamondback.png differ diff --git a/examples/turtlesim_rs/images/electric.png b/examples/turtlesim_rs/images/electric.png new file mode 100644 index 000000000..e5bb4fccc Binary files /dev/null and b/examples/turtlesim_rs/images/electric.png differ diff --git a/examples/turtlesim_rs/images/fuerte.png b/examples/turtlesim_rs/images/fuerte.png new file mode 100644 index 000000000..b633f4d44 Binary files /dev/null and b/examples/turtlesim_rs/images/fuerte.png differ diff --git a/examples/turtlesim_rs/images/groovy.png b/examples/turtlesim_rs/images/groovy.png new file mode 100644 index 000000000..e6932521a Binary files /dev/null and b/examples/turtlesim_rs/images/groovy.png differ diff --git a/examples/turtlesim_rs/images/hydro.png b/examples/turtlesim_rs/images/hydro.png new file mode 100644 index 000000000..58868fd1f Binary files /dev/null and b/examples/turtlesim_rs/images/hydro.png differ diff --git a/examples/turtlesim_rs/images/hydro.svg b/examples/turtlesim_rs/images/hydro.svg new file mode 100644 index 000000000..21f53ca4f --- /dev/null +++ b/examples/turtlesim_rs/images/hydro.svg @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/turtlesim_rs/images/indigo.png b/examples/turtlesim_rs/images/indigo.png new file mode 100644 index 000000000..d57670ace Binary files /dev/null and b/examples/turtlesim_rs/images/indigo.png differ diff --git a/examples/turtlesim_rs/images/indigo.svg b/examples/turtlesim_rs/images/indigo.svg new file mode 100644 index 000000000..ce1f01c4d --- /dev/null +++ b/examples/turtlesim_rs/images/indigo.svg @@ -0,0 +1,691 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/turtlesim_rs/images/jade.png b/examples/turtlesim_rs/images/jade.png new file mode 100644 index 000000000..f9029198b Binary files /dev/null and b/examples/turtlesim_rs/images/jade.png differ diff --git a/examples/turtlesim_rs/images/kinetic.png b/examples/turtlesim_rs/images/kinetic.png new file mode 100644 index 000000000..ab8a3b1f9 Binary files /dev/null and b/examples/turtlesim_rs/images/kinetic.png differ diff --git a/examples/turtlesim_rs/images/kinetic.svg b/examples/turtlesim_rs/images/kinetic.svg new file mode 100644 index 000000000..ad78b79f6 --- /dev/null +++ b/examples/turtlesim_rs/images/kinetic.svg @@ -0,0 +1,137 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/examples/turtlesim_rs/images/lunar.png b/examples/turtlesim_rs/images/lunar.png new file mode 100644 index 000000000..a18f76639 Binary files /dev/null and b/examples/turtlesim_rs/images/lunar.png differ diff --git a/examples/turtlesim_rs/images/lunar.svg b/examples/turtlesim_rs/images/lunar.svg new file mode 100644 index 000000000..0835959a0 --- /dev/null +++ b/examples/turtlesim_rs/images/lunar.svg @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/turtlesim_rs/images/melodic.png b/examples/turtlesim_rs/images/melodic.png new file mode 100644 index 000000000..cab240854 Binary files /dev/null and b/examples/turtlesim_rs/images/melodic.png differ diff --git a/examples/turtlesim_rs/images/robot-turtle.png b/examples/turtlesim_rs/images/robot-turtle.png new file mode 100644 index 000000000..126533b1f Binary files /dev/null and b/examples/turtlesim_rs/images/robot-turtle.png differ diff --git a/examples/turtlesim_rs/images/sea-turtle.png b/examples/turtlesim_rs/images/sea-turtle.png new file mode 100644 index 000000000..1a4829497 Binary files /dev/null and b/examples/turtlesim_rs/images/sea-turtle.png differ diff --git a/examples/turtlesim_rs/images/turtle.png b/examples/turtlesim_rs/images/turtle.png new file mode 100644 index 000000000..da835ad42 Binary files /dev/null and b/examples/turtlesim_rs/images/turtle.png differ diff --git a/examples/turtlesim_rs/package.xml b/examples/turtlesim_rs/package.xml new file mode 100644 index 000000000..2d1970865 --- /dev/null +++ b/examples/turtlesim_rs/package.xml @@ -0,0 +1,24 @@ + + turtlesim_rs + 0.1.0 + + turtlesim_rs is a ROS2 package implemented in Rust, designed to teach ROS concepts and serve as + an educational tool for developing ROS packages in Rust. + + + user + Apache License 2.0 + Abdelrahman osama + + rclrs + std_msgs + std_srvs + geometry_msgs + turtlesim_rs_msgs + + rosidl_runtime_rs + + + ament_cargo + + diff --git a/examples/turtlesim_rs/src/lib.rs b/examples/turtlesim_rs/src/lib.rs new file mode 100644 index 000000000..de5080c83 --- /dev/null +++ b/examples/turtlesim_rs/src/lib.rs @@ -0,0 +1,2 @@ +pub mod turtle; +pub mod turtle_frame; diff --git a/examples/turtlesim_rs/src/turtle.rs b/examples/turtlesim_rs/src/turtle.rs new file mode 100644 index 000000000..5e07ce9bf --- /dev/null +++ b/examples/turtlesim_rs/src/turtle.rs @@ -0,0 +1,346 @@ +use eframe::egui::{Image, Pos2, Rect, Ui, Vec2}; +use rclrs::{Node, Time, QOS_PROFILE_DEFAULT}; +use std::sync::mpsc::{channel, Receiver}; +use std::time; +use std::{ + f32::consts::{FRAC_PI_2, PI}, + sync::{Arc, Mutex}, +}; +use tiny_skia::{LineCap, Paint, PathBuilder, Pixmap, Stroke, Transform}; + +use crate::turtle_frame::{TURTLE_IMG_HEIGHT, TURTLE_IMG_WIDTH}; + +const DEFAULT_PEN_R: u8 = 0xb3; +const DEFAULT_PEN_G: u8 = 0xb8; +const DEFAULT_PEN_B: u8 = 0xff; +const DEFAULT_PEN_ALPHA: u8 = 255; +const DEFAULT_STROKE_WIDTH: f32 = 3.0; + +enum TurtleSrvs { + SetPen(u8, u8, u8, u8, u8), + TeleportAbsolute(f32, f32, f32), + TeleportRelative(f32, f32), +} + +pub struct TurtleVel { + lin_vel: f64, + ang_vel: f64, + last_command_time: Time, +} + +pub struct Pen<'a> { + paint: Paint<'a>, + stroke: Stroke, +} + +#[allow(unused)] +pub struct Turtle<'a> { + node: Arc, + image: Image<'a>, + pos: Pos2, + orient: f32, + meter: f32, + + pen_: Pen<'a>, + pen_on: bool, + + turtle_vel: Arc>, + + srv_rx: Receiver, + + velocity_sub: Arc>, + pose_pub: Arc>, + color_pub: Arc>, + set_pen_srv: Arc>, + teleport_absolute_srv: Arc>, + teleport_relative_srv: Arc>, +} + +impl<'a> Turtle<'a> { + pub fn new(node: Arc, real_name: &str, image: Image<'a>, pos: Pos2, orient: f32) -> Self { + let meter = TURTLE_IMG_HEIGHT; + + let turtle_vel = Arc::new(Mutex::new(TurtleVel { + lin_vel: 0.0, + ang_vel: 0.0, + last_command_time: node.get_clock().now(), + })); + + let turtle_vel_clone = Arc::clone(&turtle_vel); + let node_clone = Arc::clone(&node); + + let velocity_sub = node + .create_subscription( + &(real_name.to_owned() + "/cmd_vel"), + QOS_PROFILE_DEFAULT, + move |msg: geometry_msgs::msg::Twist| { + let mut vel = turtle_vel_clone.lock().unwrap(); + vel.lin_vel = msg.linear.x; + vel.ang_vel = msg.angular.z; + vel.last_command_time = node_clone.get_clock().now(); + }, + ) + .unwrap(); + + let pose_pub = node + .create_publisher(&(real_name.to_owned() + "/pose"), QOS_PROFILE_DEFAULT) + .unwrap(); + + let color_pub = node + .create_publisher( + &(real_name.to_owned() + "/color_sensor"), + QOS_PROFILE_DEFAULT, + ) + .unwrap(); + + let (srv_tx, srv_rx) = channel(); + + let set_pen_srv_tx = srv_tx.clone(); + let set_pen_srv = node + .create_service( + &(real_name.to_owned() + "/set_pen"), + move |_, srv: turtlesim_rs_msgs::srv::SetPen_Request| { + let (r, g, b, width, off) = (srv.r, srv.g, srv.b, srv.width, srv.off); + set_pen_srv_tx + .send(TurtleSrvs::SetPen(r, g, b, width, off)) + .unwrap(); + turtlesim_rs_msgs::srv::SetPen_Response::default() + }, + ) + .unwrap(); + + let teleport_absolute_srv_tx = srv_tx.clone(); + let teleport_absolute_srv = node + .create_service( + &(real_name.to_owned() + "/teleport_absolute"), + move |_, srv: turtlesim_rs_msgs::srv::TeleportAbsolute_Request| { + let x = srv.x; + let y = srv.y; + let theta = srv.theta; + teleport_absolute_srv_tx + .send(TurtleSrvs::TeleportAbsolute(x, y, theta)) + .unwrap(); + turtlesim_rs_msgs::srv::TeleportAbsolute_Response::default() + }, + ) + .unwrap(); + + let teleport_relative_srv_tx = srv_tx.clone(); + let teleport_relative_srv = node + .create_service( + &(real_name.to_owned() + "/teleport_relative"), + move |_, srv: turtlesim_rs_msgs::srv::TeleportRelative_Request| { + let linear = srv.linear; + let angular = srv.angular; + teleport_relative_srv_tx + .send(TurtleSrvs::TeleportRelative(linear, angular)) + .unwrap(); + turtlesim_rs_msgs::srv::TeleportRelative_Response::default() + }, + ) + .unwrap(); + + let stroke = Stroke { + width: DEFAULT_STROKE_WIDTH, + line_cap: LineCap::Round, + ..Default::default() + }; + + let mut paint = Paint::default(); + paint.set_color_rgba8( + DEFAULT_PEN_R, + DEFAULT_PEN_G, + DEFAULT_PEN_B, + DEFAULT_PEN_ALPHA, + ); + paint.anti_alias = true; + + let pen_ = Pen { paint, stroke }; + let pen_on = true; + + Turtle { + node, + image, + pos, + orient, + meter, + + pen_, + pen_on, + + turtle_vel, + + srv_rx, + + velocity_sub, + pose_pub, + color_pub, + set_pen_srv, + teleport_absolute_srv, + teleport_relative_srv, + } + } + + fn rotate_image(&mut self) { + let image = self.image.clone(); + self.image = image.rotate(-self.orient + FRAC_PI_2, Vec2::splat(0.5)); + } + + pub fn update( + &mut self, + dt: f64, + path_image: &mut Pixmap, + canvas_width: f32, + canvas_height: f32, + ) -> bool { + let mut modified = false; + + let old_orient = self.orient; + let old_pos = self.pos; + + modified |= self.handle_service_requests(path_image, old_pos, canvas_height); + + let mut turtle_vel = self.turtle_vel.lock().unwrap(); + let is_old_command = self + .node + .get_clock() + .now() + .compare_with(&turtle_vel.last_command_time, |now_ns, command_ns| { + let diff_ns = (now_ns - command_ns) as u64; + time::Duration::from_nanos(diff_ns) > time::Duration::from_secs(1) + }) + .unwrap(); + + if is_old_command { + turtle_vel.lin_vel = 0.0; + turtle_vel.ang_vel = 0.0; + } + + let lin_vel_ = turtle_vel.lin_vel; + let ang_vel_ = turtle_vel.ang_vel; + + drop(turtle_vel); + + self.orient += (ang_vel_ * dt) as f32; + // Keep orient between -pi and +pi + self.orient -= 2.0 * PI * ((self.orient + PI) / (2.0 * PI)).floor(); + + self.pos.x += self.orient.cos() * (lin_vel_ * dt) as f32; + self.pos.y += -self.orient.sin() * (lin_vel_ * dt) as f32; + + // Clamp to screen size + if self.pos.x < 0.0 + || self.pos.x > canvas_width + || self.pos.y < 0.0 + || self.pos.y > canvas_height + { + println!( + "Oh no! I hit the wall! (Clamping from [x={}, y={}])", + self.pos.x, self.pos.y + ); + } + + self.pos.x = f32::min(f32::max(self.pos.x, 0.0), canvas_width); + self.pos.y = f32::min(f32::max(self.pos.y, 0.0), canvas_height); + + let pose_msg = turtlesim_rs_msgs::msg::Pose { + x: self.pos.x, + y: canvas_height - self.pos.y, + theta: self.orient, + linear_velocity: lin_vel_ as f32, + angular_velocity: ang_vel_ as f32, + }; + + self.pose_pub.publish(pose_msg).unwrap(); + + let pixel_color = path_image.pixel( + (self.pos.x * self.meter) as u32, + (self.pos.y * self.meter) as u32, + ); + + if let Some(color) = pixel_color { + let color_msg = turtlesim_rs_msgs::msg::Color { + r: color.red(), + g: color.green(), + b: color.blue(), + }; + self.color_pub.publish(color_msg).unwrap(); + } + + if self.orient != old_orient { + modified = true; + self.rotate_image(); + } + + if self.pos != old_pos { + modified = true; + + if self.pen_on { + self.draw_line_on_path_image(path_image, old_pos, self.pos); + } + } + + modified + } + + fn handle_service_requests( + &mut self, + path_image: &mut Pixmap, + old_pos: Pos2, + canvas_height: f32, + ) -> bool { + let mut modified = false; + + for srvs in self.srv_rx.try_iter() { + match srvs { + TurtleSrvs::SetPen(r, g, b, width, off) => { + self.pen_.paint.set_color_rgba8(r, g, b, 255); + self.pen_.stroke.width = width as f32; + self.pen_on = off == 0; + } + TurtleSrvs::TeleportAbsolute(x, y, theta) => { + self.pos.x = x; + self.pos.y = canvas_height - y; + self.orient = theta; + self.draw_line_on_path_image(path_image, old_pos, self.pos); + modified = true; + } + TurtleSrvs::TeleportRelative(linear, angular) => { + self.orient += angular; + self.pos.x += self.orient.cos() * linear; + self.pos.y += -self.orient.sin() * linear; + self.draw_line_on_path_image(path_image, old_pos, self.pos); + modified = true; + } + } + } + + modified + } + + pub fn paint(&self, ui: &mut Ui) { + let top_left_pos = Pos2 { + x: self.pos.x * self.meter - TURTLE_IMG_WIDTH / 2.0, + y: self.pos.y * self.meter - TURTLE_IMG_HEIGHT / 2.0, + }; + let image_rect = + Rect::from_min_size(top_left_pos, Vec2::new(TURTLE_IMG_WIDTH, TURTLE_IMG_HEIGHT)); + self.image.paint_at(ui, image_rect); + } + + fn draw_line_on_path_image(&self, path_image: &mut Pixmap, pos1: Pos2, pos2: Pos2) { + let mut path_builder = PathBuilder::new(); + path_builder.move_to(pos1.x * self.meter, pos1.y * self.meter); + path_builder.line_to(pos2.x * self.meter, pos2.y * self.meter); + + if let Some(path) = path_builder.finish() { + path_image.stroke_path( + &path, + &self.pen_.paint, + &self.pen_.stroke, + Transform::identity(), + None, + ); + } + } +} diff --git a/examples/turtlesim_rs/src/turtle_frame.rs b/examples/turtlesim_rs/src/turtle_frame.rs new file mode 100644 index 000000000..054d7e7ea --- /dev/null +++ b/examples/turtlesim_rs/src/turtle_frame.rs @@ -0,0 +1,383 @@ +use crate::turtle::Turtle; +use core::time; +use eframe::egui::{self, ColorImage, TextureOptions}; +use eframe::egui::{Image, Ui, Vec2}; +use std::collections::BTreeMap; +use std::f32::consts::FRAC_PI_2; +use std::sync::mpsc::{channel, Receiver, Sender}; +use std::sync::Arc; +use std::{env, thread}; +use tiny_skia::{Color, Pixmap}; + +pub const FRAME_WIDTH: u32 = 500; +pub const FRAME_HEIGHT: u32 = 500; + +pub const TURTLE_IMG_WIDTH: f32 = 45.0; +pub const TURTLE_IMG_HEIGHT: f32 = 45.0; + +const BACKGROUND_R: u8 = 69; +const BACKGROUND_G: u8 = 86; +const BACKGROUND_B: u8 = 255; +const BACKGROUND_ALPHA: u8 = 255; + +pub const UPDATE_INTERVAL_MS: u64 = 16; + +enum ServiceMsg { + Clear, + Reset, + Kill(String), + Spawn(f32, f32, f32, String, Sender), +} + +#[allow(unused)] +pub struct TurtleFrame<'a> { + ctx: egui::Context, + image_handle: egui::TextureHandle, + turtle_images: Vec>, + path_image: Pixmap, + + turtles: BTreeMap>, + id_count: u32, + frame_count: u64, + + meter: f32, + width_in_meters: f32, + height_in_meters: f32, + + context: rclrs::Context, + nh: Arc, + last_turtle_update: rclrs::Time, + + bg_r_param: rclrs::OptionalParameter, + bg_g_param: rclrs::OptionalParameter, + bg_b_param: rclrs::OptionalParameter, + + srv_rx: Receiver, + + clear_srv: Arc>, + kill_srv: Arc>, + reset_srv: Arc>, + spawn_srv: Arc>, +} + +impl<'a> TurtleFrame<'a> { + pub fn new(ctx: egui::Context) -> Self { + let mut turtle_images = vec![]; + load_turtle_images(&mut turtle_images); + + let turtles = BTreeMap::new(); + let frame_count = 0; + let id_count = 0; + + let meter = TURTLE_IMG_HEIGHT; + let width_in_meters = (FRAME_WIDTH as f32 - 1.0) / meter; + let height_in_meters = (FRAME_HEIGHT as f32 - 1.0) / meter; + let context = rclrs::Context::new(env::args()).unwrap(); + + let nh = rclrs::create_node(&context, "turtlesim_rs").unwrap(); + println!("Starting turtlesim_rs with node name {}", nh.name()); + + let last_turtle_update = nh.get_clock().now(); + + let bg_r_param = nh + .declare_parameter("background_r") + .default(BACKGROUND_R as i64) + .optional() + .unwrap(); + + let bg_g_param = nh + .declare_parameter("background_g") + .default(BACKGROUND_G as i64) + .optional() + .unwrap(); + + let bg_b_param = nh + .declare_parameter("background_b") + .default(BACKGROUND_B as i64) + .optional() + .unwrap(); + + let bg_r = bg_r_param.get().unwrap() as u8; + let bg_g = bg_g_param.get().unwrap() as u8; + let bg_b = bg_b_param.get().unwrap() as u8; + + let mut path_image = Pixmap::new(FRAME_WIDTH, FRAME_HEIGHT).unwrap(); + path_image.fill(Color::from_rgba8(bg_r, bg_g, bg_b, BACKGROUND_ALPHA)); + + let color_image = ColorImage::from_rgba_unmultiplied( + [FRAME_WIDTH as usize, FRAME_HEIGHT as usize], + path_image.data(), + ); + + let image_handle = ctx.load_texture("background", color_image, Default::default()); + + let (srv_tx, srv_rx) = channel(); + + let clear_srv_tx = srv_tx.clone(); + let clear_srv = nh + .create_service("clear", move |_, _| { + clear_srv_tx.send(ServiceMsg::Clear).unwrap(); + std_srvs::srv::Empty_Response::default() + }) + .unwrap(); + + let kill_srv_tx = srv_tx.clone(); + let kill_srv = nh + .create_service( + "kill", + move |_, req: turtlesim_rs_msgs::srv::Kill_Request| { + let turtle_name = req.name; + kill_srv_tx.send(ServiceMsg::Kill(turtle_name)).unwrap(); + turtlesim_rs_msgs::srv::Kill_Response::default() + }, + ) + .unwrap(); + + let reset_srv_tx = srv_tx.clone(); + let reset_srv = nh + .create_service("reset", move |_, _| { + reset_srv_tx.send(ServiceMsg::Reset).unwrap(); + std_srvs::srv::Empty_Response::default() + }) + .unwrap(); + + let spawn_srv_tx = srv_tx.clone(); + let spawn_srv = nh + .create_service( + "spawn", + move |_, req: turtlesim_rs_msgs::srv::Spawn_Request| { + let (name_tx, name_rx) = channel(); + let (x, y, theta, turtle_name) = (req.x, req.y, req.theta, req.name); + + spawn_srv_tx + .send(ServiceMsg::Spawn(x, y, theta, turtle_name, name_tx)) + .unwrap(); + let turtle_realname = name_rx.recv().unwrap(); + + turtlesim_rs_msgs::srv::Spawn_Response { + name: turtle_realname, + } + }, + ) + .unwrap(); + + let nh_weak = Arc::downgrade(&nh); + thread::spawn(move || loop { + std::thread::sleep(time::Duration::from_millis(UPDATE_INTERVAL_MS / 2)); + if let Some(nh_clone) = nh_weak.upgrade() { + let _ = rclrs::spin_once(nh_clone, Some(time::Duration::ZERO)); + } else { + break; + } + }); + + TurtleFrame { + ctx, + image_handle, + turtle_images, + path_image, + + turtles, + id_count, + frame_count, + + context, + nh, + last_turtle_update, + + meter, + width_in_meters, + height_in_meters, + + bg_r_param, + bg_g_param, + bg_b_param, + + srv_rx, + + clear_srv, + kill_srv, + reset_srv, + spawn_srv, + } + } + + pub fn get_frame_center(&self) -> (f32, f32) { + (self.width_in_meters / 2.0, self.height_in_meters / 2.0) + } + pub fn spawn(&mut self, name: &str, x: f32, y: f32, angle: f32) -> String { + let rand_usize = rand::random::() % self.turtle_images.len(); + let turtle_img = self.turtle_images[rand_usize].clone(); + self.spawn_internal(name, x, y, angle, turtle_img) + } + + fn spawn_internal( + &mut self, + name: &str, + x: f32, + y: f32, + angle: f32, + image: egui::Image<'a>, + ) -> String { + let mut real_name = name.to_owned(); + if name.is_empty() { + self.id_count += 1; + let mut new_name = format!("turtle{}", self.id_count); + + while self.has_turtle(&new_name) { + self.id_count += 1; + new_name = format!("turtle{}", self.id_count); + } + real_name = new_name; + } else if self.has_turtle(name) { + return String::new(); + } + + let turtle = Turtle::new( + self.nh.clone(), + &real_name.clone(), + image, + egui::Pos2::new(x, self.height_in_meters - y), + angle, + ); + self.turtles.insert(real_name.clone(), turtle); + + self.ctx.request_repaint(); + println!( + "Spawning turtle [{}] at x=[{}], y=[{}], theta=[{}]", + real_name, x, y, angle + ); + + real_name + } + + pub fn has_turtle(&self, name: &str) -> bool { + self.turtles.contains_key(name) + } + + pub fn update(&mut self, ui: &mut Ui) { + self.image_handle + .set(self.get_color_image(), TextureOptions::default()); + ui.image((self.image_handle.id(), self.image_handle.size_vec2())); + for turtle in self.turtles.values() { + turtle.paint(ui); + } + } + + pub fn update_turtles(&mut self) { + let mut modified = false; + + for turtle in self.turtles.values_mut() { + modified |= turtle.update( + UPDATE_INTERVAL_MS as f64 * 0.001, + &mut self.path_image, + self.width_in_meters, + self.height_in_meters, + ); + } + + if modified { + self.ctx.request_repaint(); + } + + self.frame_count += 1; + } + + pub fn handle_service_requests(&mut self) { + let service_requests = self.srv_rx.try_iter().collect::>(); + + if service_requests.is_empty() { + return; + } + + for srv_req in service_requests { + match srv_req { + ServiceMsg::Clear => self.clear_callback(), + ServiceMsg::Reset => self.reset_callback(), + ServiceMsg::Kill(turtle_name) => self.kill_callback(turtle_name), + ServiceMsg::Spawn(x, y, theta, turtle_name, name_tx) => { + self.spawn_callback(x, y, theta, turtle_name, name_tx) + } + } + } + + self.ctx.request_repaint(); + } + + fn get_color_image(&self) -> ColorImage { + ColorImage::from_rgba_unmultiplied( + [FRAME_WIDTH as usize, FRAME_HEIGHT as usize], + self.path_image.data(), + ) + } + + fn clear_callback(&mut self) { + let bg_r = self.bg_r_param.get().unwrap() as u8; + let bg_g = self.bg_g_param.get().unwrap() as u8; + let bg_b = self.bg_b_param.get().unwrap() as u8; + + self.path_image + .fill(Color::from_rgba8(bg_r, bg_g, bg_b, BACKGROUND_ALPHA)) + } + + fn reset_callback(&mut self) { + self.turtles.clear(); + self.id_count = 0; + self.spawn( + "", + self.width_in_meters / 2.0, + self.height_in_meters / 2.0, + 0.0, + ); + self.clear_callback(); + } + + fn kill_callback(&mut self, turtle_name: String) { + let has_turtle = self.has_turtle(&turtle_name); + if has_turtle { + self.turtles.remove(&turtle_name); + } else { + println!("Tried to kill turtle {}, which does not exist", turtle_name); + } + } + + fn spawn_callback( + &mut self, + x: f32, + y: f32, + theta: f32, + turtle_name: String, + name_tx: Sender, + ) { + let turtle_realname = self.spawn(&turtle_name, x, y, theta); + name_tx.send(turtle_realname).unwrap(); + } +} + +fn load_turtle_images(turtle_images: &mut Vec>) { + let turtles = vec![ + egui::include_image!("../images/box-turtle.png"), + egui::include_image!("../images/robot-turtle.png"), + egui::include_image!("../images/sea-turtle.png"), + egui::include_image!("../images/diamondback.png"), + egui::include_image!("../images/electric.png"), + egui::include_image!("../images/fuerte.png"), + egui::include_image!("../images/groovy.png"), + egui::include_image!("../images/hydro.svg"), + egui::include_image!("../images/indigo.svg"), + egui::include_image!("../images/jade.png"), + egui::include_image!("../images/kinetic.png"), + egui::include_image!("../images/lunar.png"), + egui::include_image!("../images/melodic.png"), + ]; + + turtle_images.reserve(turtles.len()); + for img in turtles { + let image = egui::Image::new(img); + turtle_images.push( + image + .max_size(Vec2::new(TURTLE_IMG_WIDTH, TURTLE_IMG_HEIGHT)) + .rotate(FRAC_PI_2, Vec2::splat(0.5)), + ); + } +} diff --git a/examples/turtlesim_rs/src/turtlesim.rs b/examples/turtlesim_rs/src/turtlesim.rs new file mode 100644 index 000000000..a6aadeac4 --- /dev/null +++ b/examples/turtlesim_rs/src/turtlesim.rs @@ -0,0 +1,70 @@ +use core::time; +use eframe::egui::{self, CentralPanel, Frame, ViewportBuilder}; +use std::sync::{Arc, Mutex}; +use std::thread; + +use egui_extras::install_image_loaders; +use turtlesim_rs::turtle_frame::{TurtleFrame, FRAME_HEIGHT, FRAME_WIDTH, UPDATE_INTERVAL_MS}; + +fn main() { + let viewport = ViewportBuilder::default() + .with_resizable(false) + .with_inner_size((FRAME_WIDTH as f32, FRAME_HEIGHT as f32)); + + let native_options = eframe::NativeOptions { + viewport, + ..Default::default() + }; + + let _ = eframe::run_native( + "TurtleSim_rs", + native_options, + Box::new(|cc| { + install_image_loaders(&cc.egui_ctx); + Box::new(MyEguiApp::new(cc)) + }), + ); +} + +struct MyEguiApp { + turtle_frame: Arc>>, +} + +impl MyEguiApp { + fn new(cc: &eframe::CreationContext<'_>) -> Self { + let mut turtle_frame = TurtleFrame::new(cc.egui_ctx.clone()); + + let (x, y) = turtle_frame.get_frame_center(); + let theta = 0.0; + let turtle_name = ""; + turtle_frame.spawn(turtle_name, x, y, theta); + + let turtle_frame = Arc::new(Mutex::new(turtle_frame)); + + let turtle_frame_weak = Arc::downgrade(&turtle_frame); + + thread::spawn(move || loop { + std::thread::sleep(time::Duration::from_millis(UPDATE_INTERVAL_MS)); + if let Some(turtle_frame_clone) = turtle_frame_weak.upgrade() { + let mut frame = turtle_frame_clone.lock().unwrap(); + frame.update_turtles(); + frame.handle_service_requests(); + } else { + break; + } + }); + + MyEguiApp { turtle_frame } + } +} + +impl eframe::App for MyEguiApp { + fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) { + CentralPanel::default() + .frame(Frame::default()) + .show(ctx, |ui| { + let mut frame = self.turtle_frame.lock().unwrap(); + frame.update(ui) + }); + } +} diff --git a/examples/turtlesim_rs/tutorials/mimic.rs b/examples/turtlesim_rs/tutorials/mimic.rs new file mode 100755 index 000000000..60486b556 --- /dev/null +++ b/examples/turtlesim_rs/tutorials/mimic.rs @@ -0,0 +1,51 @@ +use rclrs::{Context, Node, Publisher, Subscription, QOS_PROFILE_DEFAULT}; +use std::env; +use std::sync::Arc; + +#[allow(unused)] +struct Mimic { + output_nh: Arc, + input_nh: Arc, + twist_pub: Arc>, + pose_sub: Arc>, +} + +impl Mimic { + fn new() -> Self { + let context = Context::new(env::args()).unwrap(); + let output_nh = rclrs::create_node(&context, "output").unwrap(); + + let twist_pub = output_nh + .create_publisher("/output/cmd_vel", QOS_PROFILE_DEFAULT) + .unwrap(); + + let input_nh = rclrs::create_node(&context, "input").unwrap(); + + let twist_pub_clone = Arc::clone(&twist_pub); + let pose_sub = input_nh + .create_subscription( + "/input/pose", + QOS_PROFILE_DEFAULT, + move |pose_msg: turtlesim_rs_msgs::msg::Pose| { + let mut twist_msg = geometry_msgs::msg::Twist::default(); + twist_msg.linear.x = pose_msg.linear_velocity as f64; + twist_msg.angular.z = pose_msg.angular_velocity as f64; + twist_pub_clone.publish(twist_msg).unwrap(); + }, + ) + .unwrap(); + + Self { + output_nh, + input_nh, + twist_pub, + pose_sub, + } + } +} + +fn main() -> Result<(), rclrs::RclrsError> { + let mimic = Mimic::new(); + rclrs::spin(mimic.input_nh.clone())?; + Ok(()) +} diff --git a/examples/turtlesim_rs/tutorials/turtle_teleop_key.rs b/examples/turtlesim_rs/tutorials/turtle_teleop_key.rs new file mode 100644 index 000000000..4b6502ce1 --- /dev/null +++ b/examples/turtlesim_rs/tutorials/turtle_teleop_key.rs @@ -0,0 +1,97 @@ +use rclrs::{Node, Publisher}; +use std::env; +use std::io; +use std::sync::Arc; +use termion::event::Key; +use termion::input::TermRead; +use termion::raw::IntoRawMode; + +struct TeleopTurtle { + _nh: Arc, + linear: f64, + angular: f64, + l_scale: f64, + a_scale: f64, + twist_pub: Arc>, +} + +impl TeleopTurtle { + pub fn new(context: &rclrs::Context) -> Self { + let _nh = rclrs::create_node(context, "teleop_turtle").unwrap(); + + let l_scale_param = _nh + .declare_parameter("scale_linear") + .default(2.0) + .optional() + .unwrap(); + + let a_scale_param = _nh + .declare_parameter("scale_angular") + .default(2.0) + .optional() + .unwrap(); + + let l_scale = l_scale_param.get().unwrap(); + let a_scale = a_scale_param.get().unwrap(); + + let twist_pub = _nh + .create_publisher("/turtle1/cmd_vel", rclrs::QOS_PROFILE_DEFAULT) + .unwrap(); + + Self { + _nh, + linear: 0.0, + angular: 0.0, + l_scale, + a_scale, + twist_pub, + } + } + + pub fn key_loop(&mut self) { + println!("Reading from keyboard"); + println!("---------------------------"); + println!("Use arrow keys to move the turtle."); + println!("'q' to quit."); + + let _stdout = io::stdout().into_raw_mode().unwrap(); + let stdin = io::stdin(); + for key in stdin.keys() { + self.linear = 0.0; + self.angular = 0.0; + + match key.unwrap() { + Key::Left => { + self.angular = 1.0; + } + Key::Right => { + self.angular = -1.0; + } + Key::Up => { + self.linear = 1.0; + } + Key::Down => { + self.linear = -1.0; + } + Key::Char('q') | Key::Ctrl('c') => { + break; + } + _ => {} + } + let mut twist_msg = geometry_msgs::msg::Twist::default(); + twist_msg.angular.z = self.angular * self.a_scale; + twist_msg.linear.x = self.linear * self.l_scale; + self.twist_pub.publish(twist_msg).unwrap(); + } + } +} + +fn main() -> Result<(), rclrs::RclrsError> { + let context = rclrs::Context::new(env::args()).unwrap(); + + let mut teleop_turtle = TeleopTurtle::new(&context); + + teleop_turtle.key_loop(); + + Ok(()) +} diff --git a/examples/turtlesim_rs_msgs/CMakeLists.txt b/examples/turtlesim_rs_msgs/CMakeLists.txt new file mode 100644 index 000000000..644a612d2 --- /dev/null +++ b/examples/turtlesim_rs_msgs/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) + +project(turtlesim_rs_msgs) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +set(msg_files + "msg/Color.msg" + "msg/Pose.msg" +) + +set(srv_files + "srv/Kill.srv" + "srv/SetPen.srv" + "srv/Spawn.srv" + "srv/TeleportAbsolute.srv" + "srv/TeleportRelative.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/examples/turtlesim_rs_msgs/msg/Color.msg b/examples/turtlesim_rs_msgs/msg/Color.msg new file mode 100644 index 000000000..c0af95aab --- /dev/null +++ b/examples/turtlesim_rs_msgs/msg/Color.msg @@ -0,0 +1,3 @@ +uint8 r +uint8 g +uint8 b diff --git a/examples/turtlesim_rs_msgs/msg/Pose.msg b/examples/turtlesim_rs_msgs/msg/Pose.msg new file mode 100644 index 000000000..c1d03a375 --- /dev/null +++ b/examples/turtlesim_rs_msgs/msg/Pose.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 theta + +float32 linear_velocity +float32 angular_velocity \ No newline at end of file diff --git a/examples/turtlesim_rs_msgs/package.xml b/examples/turtlesim_rs_msgs/package.xml new file mode 100644 index 000000000..b0574d042 --- /dev/null +++ b/examples/turtlesim_rs_msgs/package.xml @@ -0,0 +1,26 @@ + + turtlesim_rs_msgs + 0.1.0 + Package containing message and service definitions for the turtlesim_rs package. + user + + Apache License 2.0 + Abdelrahman osama + + rclrs + std_msgs + + ament_cmake + rosidl_default_generators + rosidl_generator_rs + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/examples/turtlesim_rs_msgs/srv/Kill.srv b/examples/turtlesim_rs_msgs/srv/Kill.srv new file mode 100644 index 000000000..1da96270a --- /dev/null +++ b/examples/turtlesim_rs_msgs/srv/Kill.srv @@ -0,0 +1,2 @@ +string name +--- \ No newline at end of file diff --git a/examples/turtlesim_rs_msgs/srv/SetPen.srv b/examples/turtlesim_rs_msgs/srv/SetPen.srv new file mode 100644 index 000000000..a1b3d9cc9 --- /dev/null +++ b/examples/turtlesim_rs_msgs/srv/SetPen.srv @@ -0,0 +1,6 @@ +uint8 r +uint8 g +uint8 b +uint8 width +uint8 off +--- diff --git a/examples/turtlesim_rs_msgs/srv/Spawn.srv b/examples/turtlesim_rs_msgs/srv/Spawn.srv new file mode 100644 index 000000000..b8eeaeee0 --- /dev/null +++ b/examples/turtlesim_rs_msgs/srv/Spawn.srv @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 theta +string name # Optional. A unique name will be created and returned if this is empty +--- +string name \ No newline at end of file diff --git a/examples/turtlesim_rs_msgs/srv/TeleportAbsolute.srv b/examples/turtlesim_rs_msgs/srv/TeleportAbsolute.srv new file mode 100644 index 000000000..0dc51b99a --- /dev/null +++ b/examples/turtlesim_rs_msgs/srv/TeleportAbsolute.srv @@ -0,0 +1,4 @@ +float32 x +float32 y +float32 theta +--- diff --git a/examples/turtlesim_rs_msgs/srv/TeleportRelative.srv b/examples/turtlesim_rs_msgs/srv/TeleportRelative.srv new file mode 100644 index 000000000..842dcb1e2 --- /dev/null +++ b/examples/turtlesim_rs_msgs/srv/TeleportRelative.srv @@ -0,0 +1,3 @@ +float32 linear +float32 angular +---