From 896838eac5d7291312e1daf1daa447157dd177a5 Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Tue, 15 Nov 2022 20:38:38 +0200 Subject: [PATCH] Implement pixel-based surface and publisher to ROS image --- Dockerfile | 4 + src/draw_svg/package.xml | 3 + src/draw_svg/src/py/drawing_surface.py | 134 ++++++++++++++++++++----- src/draw_svg/worlds/draw_svg_world.sdf | 35 +++++-- 4 files changed, 139 insertions(+), 37 deletions(-) diff --git a/Dockerfile b/Dockerfile index b5888bc..014f430 100644 --- a/Dockerfile +++ b/Dockerfile @@ -20,6 +20,10 @@ RUN apt-get update && \ ignition-${IGNITION_VERSION} && \ rm -rf /var/lib/apt/lists/* +### Install extra dependencies +RUN apt-get update && \ + apt-get install -yq python3-pil.imagetk + ### Import and install dependencies, then build these dependencies (not ign_moveit2_examples yet) COPY ./drawing_robot_ros2.repos ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos RUN vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos && \ diff --git a/src/draw_svg/package.xml b/src/draw_svg/package.xml index a92ee98..810008b 100644 --- a/src/draw_svg/package.xml +++ b/src/draw_svg/package.xml @@ -15,6 +15,9 @@ tf2_ros python3-lxml python3-tk + python-imaging + python-numpy + python3-pil.imagetk xarm_description xarm_moveit_config diff --git a/src/draw_svg/src/py/drawing_surface.py b/src/draw_svg/src/py/drawing_surface.py index 4effce4..7d1e3ae 100755 --- a/src/draw_svg/src/py/drawing_surface.py +++ b/src/draw_svg/src/py/drawing_surface.py @@ -4,6 +4,7 @@ import rclpy from geometry_msgs.msg import Pose, PoseWithCovariance, Point from nav_msgs.msg import Odometry +from sensor_msgs.msg import Image as SensorImage from pymoveit2 import MoveIt2 from robots import lite6 from rclpy.callback_groups import ReentrantCallbackGroup @@ -17,6 +18,11 @@ import math import threading +from PIL import ImageTk +from PIL import Image as PImage +import numpy as np + + def translate(val, lmin, lmax, rmin, rmax): lspan = lmax - lmin rspan = rmax - rmin @@ -24,34 +30,78 @@ def translate(val, lmin, lmax, rmin, rmax): return rmin + (val * rspan) class DrawingApp(tk.Tk): - def __init__(self, queue): + def __init__(self, point_queue, image_queue): tk.Tk.__init__(self) - self.queue = queue - self.canvas = tk.Canvas(self, width=400, height=400) - self.canvas.pack(side="top", fill="both", expand=True) + self.point_queue = point_queue + self.image_queue = image_queue + self.width = 400 + self.height = 400 + self.img = PImage.new("RGB", (self.width, self.height), (255, 255, 255)) + self.arr = np.array(self.img) + self.frame_delay = 1 #ms + self.window_update_delay = 300 #ms + self.counter = 0 self.read_queue() - def create_circle(self, x, y, r, **kwargs): - self.canvas.create_oval(x-r, y-r, x+r, y+r, **kwargs) + self.canvas = tk.Canvas(self,width=self.width,height=self.height) + self.tk_image = ImageTk.PhotoImage(self.img) + self.canvas.create_image(self.width/2,self.height/2,image=self.tk_image) + self.canvas.pack(side='top', expand=True, fill='both') + self.draw_window() + + def draw(self,x,y,z): + # putpixel is too slow + #self.img.putpixel((int(x), int(y)), (255, 0, 0)) + self.arr[x,y,1] = 0 + self.arr[x,y,2] = 0 + #print("Set x:{} y:{} to:{}".format(x,y,255)) + + def draw_window(self): + self.img = PImage.fromarray(self.arr) + self.tk_image = ImageTk.PhotoImage(self.img) + self.canvas.create_image(self.width/2,self.height/2,image=self.tk_image) + self.image_queue.put(self.get_image_message()) + #self.after(self.window_update_delay, lambda: self.draw_window()) def read_queue(self): - '''Read queue and draw circle every 100 ms''' - if self.queue.empty(): - self.after(10, lambda: self.read_queue()) + '''Read queue and draw circle every 10 ms''' + self.counter = self.counter + self.frame_delay + if self.point_queue.empty(): + self.after(self.frame_delay, lambda: self.read_queue()) return - p = self.queue.get() - #print("last_point->x:{} y:{} z:{}".format(p.x, p.y, p.z)) - r = 5 - x = translate(p.x, -1.0, 0.5, 0, 400) - y = translate(p.y, 0.5, -1.0, 0, 400) - self.create_circle(x,y,r, fill="red") - self.after(10, lambda: self.read_queue()) + + while not self.point_queue.empty(): + p = self.point_queue.get() + #x = translate(p.x, -1.0, 0.5, 0, self.width) + #y = translate(p.y, 0.5, -1.0, 0, self.height) + x = translate(p.x, -1.0, 0.5, 0, self.width) + y = translate(p.y, -1.0, 0.5, 0, self.height) + self.draw(int(x),int(y),0) + + if self.counter >= self.window_update_delay: + #print("Redraw") + self.counter = 0 + self.draw_window() + + self.after(self.frame_delay, lambda: self.read_queue()) + + # https://stackoverflow.com/questions/64373334/how-can-i-publish-pil-image-binary-through-ros-without-opencv + def get_image_message(self): + msg = SensorImage() + #msg.header.stamp = rospy.Time.now() + msg.height = self.img.height + msg.width = self.img.width + msg.encoding = "rgb8" + msg.is_bigendian = False + msg.step = 3 * self.img.width + msg.data = np.array(self.img).tobytes() + return msg class LogPen(Node): - def __init__(self, queue=Queue()): + def __init__(self, point_queue=Queue()): - self.queue = queue + self.queue = point_queue super().__init__("log_pen") # Create callback group that allows execution of callbacks in parallel without restrictions @@ -72,9 +122,34 @@ class LogPen(Node): Log position of pen """ p = msg.pose.pose.position - self.get_logger().info("x:{} y:{} z:{}".format(p.x, p.y, p.z)) + #self.get_logger().info("x:{} y:{} z:{}".format(p.x, p.y, p.z)) self.queue.put(p) +class PublishImage(Node): + def __init__(self, image_queue=Queue()): + + self.image_queue = image_queue + super().__init__("publish_image") + + self.publisher_ = self.create_publisher(SensorImage, '/drawing_surface/image_raw', 10) + timer_period = 0.0002 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.get_logger().info("Initialization successful.") + + def timer_callback(self): + """ + Output image from queue + """ + if self.image_queue.empty(): + return + + img = self.image_queue.get() + #self.get_logger().info("Publishing image {}x{}".format(img.width,img.height)) + + self.publisher_.publish(img) + + + def main(args=None): rclpy.init(args=args) @@ -88,27 +163,32 @@ def main(args=None): rclpy.shutdown() exit(0) -def start_log_thread(queue=Queue()): +def start_node_thread(constructor, queue=Queue()): - rclpy.init() - - log_pen = LogPen(queue) + node = constructor(queue) executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(log_pen) + executor.add_node(node) executor.spin() if __name__ == "__main__": - q = Queue() + point_queue = Queue() + image_queue = Queue() + + rclpy.init() global app - app = DrawingApp(q) + app = DrawingApp(point_queue, image_queue) global log_thread - log_thread = threading.Thread(target=start_log_thread, args=[q]) + log_thread = threading.Thread(target=start_node_thread, args=[LogPen, point_queue]) log_thread.start() + global image_thread + image_thread = threading.Thread(target=start_node_thread, args=[PublishImage, image_queue]) + image_thread.start() + app.mainloop() rclpy.shutdown() exit(0) diff --git a/src/draw_svg/worlds/draw_svg_world.sdf b/src/draw_svg/worlds/draw_svg_world.sdf index 1579a30..3de14ed 100644 --- a/src/draw_svg/worlds/draw_svg_world.sdf +++ b/src/draw_svg/worlds/draw_svg_world.sdf @@ -78,7 +78,7 @@ - -0.14 -0.3 1.0 0 0 0 + -0.14 -0.3 0.5 0 0 0 true @@ -90,17 +90,32 @@ + - - 0 0 1 - 0.28 0.21 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - + + 0 0 1 + 0.28 0.21 + + + + + + + + 400 + 400 + +