diff --git a/src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf b/src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf
index b1017f1..395ca8c 100644
--- a/src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf
+++ b/src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf
@@ -1,34 +1,43 @@
+
-
-
- ignition-physics-dartsim-plugin
-
-
- bullet
-
-
-
- 0.005
- 1.0
+
+
+ 0.001
+ 1
+ 1000
+
+ 0 0 0
-
-
-
- 0.8 0.8 0.8
- false
-
-
+
-
-
-
+
@@ -105,8 +114,11 @@
~/image_raw:=/camera1/image_raw
-->
- 2480
- 3508
+
+
+ 1654
+ 2339
+
diff --git a/src/virtual_drawing_surface/src/py/drawing_surface.py b/src/virtual_drawing_surface/src/py/drawing_surface.py
index c76712e..21fc886 100755
--- a/src/virtual_drawing_surface/src/py/drawing_surface.py
+++ b/src/virtual_drawing_surface/src/py/drawing_surface.py
@@ -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)
#-0.1485 -0.3 0.5 0 0 0
#0.297 0.21
- 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")