nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implement pixel-based surface and publisher to ROS image

Browse Source
This commit is contained in:
Nicolas Hiillos
2022-11-15 20:38:38 +02:00
parent c318751cdb
commit 896838eac5
4 changed files with 139 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/draw_svg/package.xml
View File

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

134
src/draw_svg/src/py/drawing_surface.py
View File

@@ -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)

35
src/draw_svg/worlds/draw_svg_world.sdf
View File

@@ -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>
<normal>0 0 1</normal>
<size>0.28 0.21</size>
</plane>
</geometry>
<material>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
<plane>
<normal>0 0 1</normal>
<size>0.28 0.21</size>
</plane>
</geometry>
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_video.cpp -->
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/worlds/gazebo_ros_video_demo.world -->
<!-- TODO: note docs outdated: https://classic.gazebosim.org/tutorials?tut=ros_gzplugins -->
<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>