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

Improve virtual surface

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-23 12:13:38 +02:00
parent e4c10b23a7
commit 3c230c7da0
2 changed files with 70 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf
View File

@@ -1,10 +1,11 @@
<?xml version="1.0"?>
<!-- <sdf version="1.9"> -->
<!-- <sdf version="1.7"> -->
<sdf version="1.7">
<world name="virtual_surface_world">
<!-- Physics -->
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
<!-- <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
<engine>
<filename>ignition-physics-dartsim-plugin</filename>
</engine>
@@ -16,19 +17,27 @@
<max_step_size>0.005</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
-->
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<!-- (2022-12-20) humble: change gravity from [0 0 -9.81] to [0 0 0] -->
<gravity>0 0 0</gravity>
</physics>
<!-- Scene -->
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
<!-- <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<scene>
<ambient>0.8 0.8 0.8</ambient>
<grid>false</grid>
</scene>
-->
<!-- User Commands (transform control) -->
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
<!-- <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
</plugin>
-->
<!-- -->
<!-- Illumination -->
@@ -105,8 +114,11 @@
<remapping>~/image_raw:=/camera1/image_raw</remapping>
</ros> -->
<!-- 300dpi A4 paper is 2480x3508 px -->
<height>2480</height>
<width>3508</width>
<!-- <height>2480</height>
<width>3508</width> -->
<!-- 200dpi A4 paper is 1654x2339 px -->
<height>1654</height>
<width>2339</width>
</plugin>
</visual>
<!-- <inertial>
@@ -166,5 +178,10 @@
<publish_nested_model_pose>false</publish_nested_model_pose>
</plugin> -->
<!-- <include>
<uri>model://table</uri>
<name>table</name>
<pose>0.0 -0.84 0 0 0 0</pose>
</include> -->
</world>
</sdf>

47
src/virtual_drawing_surface/src/py/drawing_surface.py
View File

@@ -20,14 +20,21 @@ from PIL import ImageTk
from PIL import Image as PImage
import numpy as np
def bound(val, lim1, lim2):
minval = min(lim1,lim2)
maxval = max(lim1,lim2)
val = max(minval, val)
val = min(maxval, val)
return val
def translate(val, lmin, lmax, rmin, rmax):
val = max(lmin, val)
val = min(lmax, val)
#val = bound(val, lmin, lmax)
lspan = lmax - lmin
rspan = rmax - rmin
val = float(val - lmin) / float(lspan)
return rmin + (val * rspan)
val = rmin + (val * rspan)
val = bound(val,rmin, rmax)
return val
class DrawingApp(tk.Tk):
def __init__(self, point_queue, image_queue):
@@ -37,12 +44,15 @@ class DrawingApp(tk.Tk):
self.image_queue = image_queue
#300dpi A4 paper is 2480x3508 px
self.width = 3508
self.height = 2480
#self.width = 3508
#self.height = 2480
#200dpi A4 paper is 1654x2339 px -->
self.width = 1654
self.height = 2339
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.window_update_delay = 500 #ms
self.counter = 0
self.read_queue()
@@ -63,14 +73,12 @@ class DrawingApp(tk.Tk):
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
self.arr[x+1,y,1] = 0
self.arr[x+1,y,2] = 0
self.arr[x+1,y+1,1] = 0
self.arr[x+1,y+1,2] = 0
self.arr[x,y+1,1] = 0
self.arr[x,y+1,2] = 0
r = 4 # radius
for xp in range(max(0, x-r), min(self.width-1, x+r)):
for yp in range(max(0, y-r), min(self.height-1, y+r)):
self.arr[xp,yp,0] = 0 #red
self.arr[xp,yp,1] = 0 #green
self.arr[xp,yp,2] = 0 #blue
#print("Set x:{} y:{} to:{}".format(x,y,255))
def draw_window(self):
@@ -93,11 +101,16 @@ class DrawingApp(tk.Tk):
#y = translate(p.y, 0.5, -1.0, 0, self.height)
#<pose>-0.1485 -0.3 0.5 0 0 0</pose>
#<size>0.297 0.21</size>
x = translate(p.x, -0.1485, 0.1485, 0, self.width)
y = translate(p.y, -0.3, -0.09, 0, self.height)
#x = translate(p.x, -0.1485, 0.1485, 0, self.width)
#y = translate(p.y, -0.51, -0.3, 0, self.height)
x = int(translate(p.y, -0.5, 0.5, 0, self.width))
y = int(translate(p.x, -0.3485, 0.1, 0, self.height))
self.draw(int(x),int(y),0)
#x = bound(self.width - x, 0, self.width)
#y = bound(self.height - y, 0, self.height)
self.draw(x, y, 0)
if self.counter >= self.window_update_delay:
#print("Redraw")