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
+
+