From 3c230c7da02609c3d8ba8ac9372177b48e57ca63 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Thu, 23 Mar 2023 12:13:38 +0200
Subject: [PATCH] Improve virtual surface

---
 .../worlds/virtual_surface_world.sdf          | 63 ++++++++++++-------
 .../src/py/drawing_surface.py                 | 47 +++++++++-----
 2 files changed, 70 insertions(+), 40 deletions(-)

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 @@
 <?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">
-      <engine>
-        <filename>ignition-physics-dartsim-plugin</filename>
-      </engine>
-      <dart>
-        <collision_detector>bullet</collision_detector>
-      </dart>
-    </plugin>
-    <physics name="physics_config" type="dart">
-      <max_step_size>0.005</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
+    <!-- <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+         <engine>
+         <filename>ignition-physics-dartsim-plugin</filename>
+         </engine>
+         <dart>
+         <collision_detector>bullet</collision_detector>
+         </dart>
+         </plugin>
+         <physics name="physics_config" type="dart">
+         <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>
-    <scene>
-      <ambient>0.8 0.8 0.8</ambient>
-      <grid>false</grid>
-    </scene>
-
+    <!-- <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>
-
+    <!-- <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>
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)
             #<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")