Implement pixel-based surface and publisher to ROS image

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 && \

src/draw_svg/package.xml

@@ -15,6 +15,9 @@
   <depend>tf2_ros</depend>
   <depend>python3-lxml</depend>
   <depend>python3-tk</depend>
+  <depend>python-imaging</depend>
+  <depend>python-numpy</depend>
+  <depend>python3-pil.imagetk</depend>
   <depend>xarm_description</depend>
   <depend>xarm_moveit_config</depend>
   <!-- <depend>moveit_ros_planning_interface</depend> -->

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.point_queue = point_queue
-        self.canvas = tk.Canvas(self, width=400, height=400)
+        self.image_queue = image_queue
-        self.canvas.pack(side="top", fill="both", expand=True)
+        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 = tk.Canvas(self,width=self.width,height=self.height)
-        self.canvas.create_oval(x-r, y-r, x+r, y+r, **kwargs)
+        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'''
+        '''Read queue and draw circle every 10 ms'''
-        if self.queue.empty():
+        self.counter = self.counter + self.frame_delay
-            self.after(10, lambda: self.read_queue())
+        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))
+        while not self.point_queue.empty():
-        r = 5
+            p = self.point_queue.get()
-        x = translate(p.x, -1.0, 0.5, 0, 400)
+            #x = translate(p.x, -1.0, 0.5, 0, self.width)
-        y = translate(p.y, 0.5, -1.0, 0, 400)
+            #y = translate(p.y, 0.5, -1.0, 0, self.height)
-        self.create_circle(x,y,r, fill="red")
+            x = translate(p.x, -1.0, 0.5, 0, self.width)
-        self.after(10, lambda: self.read_queue())
+            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()
+    node = constructor(queue)
-
-    log_pen = LogPen(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)

src/draw_svg/worlds/draw_svg_world.sdf

@@ -78,7 +78,7 @@
          </link>
     </model>
     <model name="drawing_surface">
-      <pose>-0.14 -0.3 1.0 0 0 0</pose>
+      <pose>-0.14 -0.3 0.5 0 0 0</pose>
       <static>true</static>
       <link name="drawing_surface_link">
         <collision name="drawing_surface_collision">
@@ -90,17 +90,32 @@
           </geometry>
         </collision>
         <visual name="drawing_surface_visual">
+          <origin xyz="0 0 0" rpy="0 0 0" />
           <geometry>
-            <plane>
+             <plane>
-              <normal>0 0 1</normal>
+               <normal>0 0 1</normal>
-              <size>0.28 0.21</size>
+               <size>0.28 0.21</size>
-            </plane>
+             </plane>
-          </geometry>
+           </geometry>
-          <material>
+      <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_video.cpp -->
-            <diffuse>0.8 0.8 0.8 1</diffuse>
+      <!--  https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/worlds/gazebo_ros_video_demo.world -->
-            <specular>0.8 0.8 0.8 1</specular>
+        <!--         TODO: note docs outdated: https://classic.gazebosim.org/tutorials?tut=ros_gzplugins -->
-          </material>
+          <plugin name="drawing_surface" filename="libgazebo_ros_video.so">
+            <!-- <ros>
+                 <remapping>~/image_raw:=/camera1/image_raw</remapping>
+                 </ros> -->
+            <height>400</height>
+            <width>400</width>
+          </plugin>
         </visual>
+        <!-- <inertial>
+             <origin xyz="0 0 1" rpy="0 0 0"/>
+             <mass value="1"/>
+             <inertia
+             ixx="1.0" ixy="0.0" ixz="0.0"
+             iyy="1.0" iyz="0.0"
+             izz="1.0"/>
+             </inertial> -->
       </link>
     </model>
     <!-- <include>