Replace location_monitor_node.py

Simulation/location_monitor/scripts/location_monitor_node.py

@@ -2,25 +2,26 @@
 
 import math
 import rospy
+import re
 from nav_msgs.msg import Odometry  
 from gazebo_msgs.srv import GetModelState
 from gazebo_msgs.srv import GetWorldProperties 
 
 
-#botx = 0
+TurtlebotBurger = 0.05
 #boty = 0
 
 class Block:
-    def __init__(self, name, relative_entity_name):
+    def __init__(self, name, relative_entity_name, radius):
         self._name = name
         self._relative_entity_name = relative_entity_name
+        self._radius = radius
 
 
 class Tutorial:
 
     _blockListDict = {
-          #'block_a': Block('ball' , 'link'),
-
+        
     }
 
     def show_gazebo_models(self, botx, boty):
@@ -29,13 +30,20 @@ class Tutorial:
       resp1 = get_world_properties()
       i = 0
       for model in resp1.model_names:
-        if ("ball" in str(model)) or ("cylinder" in str(model)) or ("box" in str(model)):
+        if ("ball" in str(model)) or ("cylinder" in str(model)) or ("box" in str(model) or ("sphere" in str(model))):
+           matches = re.findall(r"[-+]?\d*\.\d+|\d+", str(model))
+           rad = 0
+           if matches:
+             if "box" in str(model):
+              rad = (matches[0])
+              rad = float(rad)/2.0
+             else:
+                rad = (matches[0])
            s = 'block_' + str(i)
-           self._blockListDict.update( {s : Block(model , 'link' )})
+           self._blockListDict.update( {s : Block(model , 'link' , rad)})
            i = i+1
            #print(block)
       
-      #print(self._blockListDict) 
   
 
 
@@ -52,9 +60,19 @@ class Tutorial:
                  print("X: " + str(l_x))
                  print("Y: " + str(l_y))
                  dist = distance(botx, boty, l_x, l_y)
-                 print("Dist: " + str(dist))
-                 if dist < 0.20:
-                    rospy.loginfo("Collision!")
+                 safedist = 0
+                 if block._radius != 0:
+                   if "box" in str(blockName):
+                       blockdetection(float(block._radius), botx, boty, l_x, l_y)
+                   else: 
+                      safedist = float(block._radius) + TurtlebotBurger + 0.05 #Radius/Size + Bot front Rad + 0.05 safe dist 
+                      print("Safe " + str(safedist))
+                      print("Dist: " + str(dist))
+                      if dist < safedist:
+                         rospy.loginfo("Collision!")
+                 else:
+                   
+                   print("Dist: " + str(dist))
 
       except rospy.ServiceException as e:
              rospy.loginfo("Get Model State service call failed:  {0}".format(e))
@@ -70,6 +88,50 @@ def distance(x1, y1, x2, y2):
     return dist
 
 
+def blockdetection(size, botx, boty, l_x, l_y):
+  print(str(size))
+  cornerdist = (size)/math.sin(math.radians(45))
+  safedist = TurtlebotBurger
+  #print("Safe " + str(safedist))
+  safedist2 = float(size) + TurtlebotBurger + 0.05
+  #print("Safe2 " + str(safedist2))
+
+  xUpR = l_x + cornerdist*math.cos(math.radians(45))
+  yUpR = l_y + cornerdist*math.sin(math.radians(45))
+  dist1 = distance(botx, boty, xUpR, yUpR)
+  #print("Dist1: " + str(dist1))
+  if dist1 < safedist:
+     rospy.loginfo("Collision!")
+
+  xUpL = l_x + cornerdist*math.cos(math.radians(135))
+  yUpL = l_y + cornerdist*math.sin(math.radians(135))
+  dist2 = distance(botx, boty, xUpL, yUpL)
+  #print("Dist2: " + str(dist2))
+  if dist2 < safedist:
+     rospy.loginfo("Collision!")
+  
+  xLoL = l_x + cornerdist*math.cos(math.radians(225))
+  yLoL = l_y + cornerdist*math.sin(math.radians(225))
+  dist3 = distance(botx, boty, xLoL, yLoL)
+  #print("Dist3: " + str(dist3))
+  if dist3 < safedist:
+     rospy.loginfo("Collision!")
+
+  xLoR = l_x + cornerdist*math.cos(math.radians(315))
+  yLoR = l_y + cornerdist*math.sin(math.radians(315))
+  dist4 = distance(botx, boty, xLoR, yLoR)
+  #print("Dist4: " + str(dist4))
+  if dist4 < safedist:
+     rospy.loginfo("Collision!")
+
+  centerdist = distance(botx, boty, l_x, l_y)
+  #print("Center: " + str(centerdist))
+  if centerdist < safedist2:
+     rospy.loginfo("Collision!")
+    
+    
+
+
 def callback(msg):
     botx = msg.pose.pose.position.x
     boty = msg.pose.pose.position.y