Update src/sim/location_monitor/scripts/location_monitor_node.py

src/sim/location_monitor/scripts/location_monitor_node.py

+#!/usr/bin/env python
+
+import math
+import rospy
+import re
+import array
+import numpy as np
+from datetime import datetime
+from nav_msgs.msg import Odometry  
+from gazebo_msgs.srv import GetModelState
+from gazebo_msgs.srv import GetWorldProperties 
+
+
+TurtlebotBurger = 0.05
+WriteFlag = 0
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S")
+logtime = str(current_time) + "_log.txt"
+f = open(logtime,"w+")
+# y
+# ^
+# |
+# |  -------          Box muss im rechtem Winkel zur x-Achse sein!
+# |  |     |
+# |  |     |
+# |  -------
+# |
+# ---------------->x
+
+
+
+class Block:
+    def __init__(self, name, relative_entity_name, radius):
+        self._name = name
+        self._relative_entity_name = relative_entity_name
+        self._radius = radius
+
+
+class Tutorial:
+
+    _blockListDict = {
+
+    }
+
+    def show_gazebo_models(self, botx, boty):
+      get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
+      #object_list = list()
+      resp1 = get_world_properties()
+      #print(str(resp1.sim_time))
+      i = 0
+      for model in resp1.model_names:
+        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' , rad)})
+           i = i+1
+           #print(block)
+      try:
+            global f 
+            global WriteFlag
+            model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+            for block in self._blockListDict.itervalues():
+                blockName = str(block._name)
+                if blockName in resp1.model_names:
+                 resp_coordinates = model_coordinates(blockName, block._relative_entity_name)
+                 l_x = resp_coordinates.pose.position.x
+                 l_y = resp_coordinates.pose.position.y
+                 print(blockName)
+                 if WriteFlag == 0:
+                  f.write(str(blockName) + "\n")
+                 print("X: " + str(l_x))
+                 print("Y: " + str(l_y))
+                 if WriteFlag == 0:
+                  f.write("[T:" + str(resp1.sim_time) + "] " + "Bot X: " + str(l_x) + "\n")
+                  f.write("[T:" + str(resp1.sim_time) + "] " + "Bot Y: " + str(l_y) + "\n")                
+                 dist = distance(botx, boty, l_x, l_y)
+                 safedist = 0
+                 if block._radius != 0:
+                   if "box" in str(blockName):
+                       boxkdetection(float(block._radius), botx, boty, l_x, l_y, resp1.sim_time)
+                   else: 
+                      safedist = float(block._radius) + TurtlebotBurger + 0.05 #Radius/Size + Bot front Rad + 0.05 safe dist
+                      print("Safe " + str(safedist))
+                      if WriteFlag == 0:
+                       f.write("Safedist.: " + str(safedist) + "\n")
+                      print("Dist: " + str(dist))
+                      if WriteFlag == 0:
+                       f.write("[T:" + str(resp1.sim_time) + "] " + "Dist. To Collision Point: " + str(dist) + "\n")
+                      if dist < safedist:                        
+                         rospy.loginfo("Collision!")
+                         if WriteFlag == 0:
+                           f.write("[T:" + str(resp1.sim_time) + "] " + "Collision!")
+                           f.close
+                           WriteFlag = 1
+                 else:
+                   
+                   print("Dist: " + str(dist))
+                   if WriteFlag == 0:
+                    f.write("[T:" + str(resp1.sim_time) + "] " + "Dist: " + str(dist) + "\n")
+
+      except rospy.ServiceException as e:
+             rospy.loginfo("Get Model State service call failed:  {0}".format(e))
+
+
+def distance(x1, y1, x2, y2):
+    #print("bX1: " + str(x1))
+    #print("bY1: " + str(y1))
+    #print("X2: " + str(x2)) 
+    #print("Y2: " + str(y2))
+    dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
+    #print(dist)
+    return dist
+
+
+def boxkdetection(size, botx, boty, l_x, l_y, simtime):
+ global f
+ global WriteFlag
+ if (size*2.0) > 5.0:
+   print("Box to big, > 5m")
+   if WriteFlag == 0:
+      f.write("[T:" + str(simtime) + "] " + "Box Invalid (size > 5m)" + "\n")
+      f.close
+ else:
+  #print("SIZEBOX" + str(size))
+  #cornerdist = (size)/math.sin(math.radians(45))
+  safedist = TurtlebotBurger + 0.05
+  #safedist2 = float(size) + TurtlebotBurger + 0.02
+  if WriteFlag == 0:
+     f.write("Safedist.: " + str(safedist) + "\n")
+  #print("Safe " + str(safedist))
+  #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)
+
+  #xUpL = l_x + cornerdist * math.cos(math.radians(135))
+  #yUpL = l_y + cornerdist * math.sin(math.radians(135))
+  #dist2 = distance(botx, boty, xUpL, yUpL)
+
+  #xLoL = l_x + cornerdist * math.cos(math.radians(225))
+  #yLoL = l_y + cornerdist * math.sin(math.radians(225))
+  #dist3 = distance(botx, boty, xLoL, yLoL)
+
+  #xLoR = l_x + cornerdist * math.cos(math.radians(315))
+  #yLoR = l_y + cornerdist * math.sin(math.radians(315))
+  #dist4 = distance(botx, boty, xLoR, yLoR)
+
+  #centerdist = distance(botx, boty, l_x, l_y)
+  xR = l_x + size * math.cos(math.radians(0))
+  yL = l_y + size * math.sin(math.radians(0))
+
+  lineA = ((botx,boty), (l_x, l_y))
+  lineB = ((l_x,l_y), (xR,yL))
+
+  angle = angle_between(lineA,lineB)
+  disangle = angle
+  angle1 = angle
+  if angle > 45:
+      disangle = 90 - angle;
+  #print("Angle: " + str(disangle))
+  dis = (size)/math.cos(math.radians(disangle))
+  #print("Dist from center do edge: " + str(dis))
+
+  if botx < l_x and boty > l_y:
+      angle1 = 180 - angle
+  if botx < l_x and boty < l_y:
+      angle1 = 180 + angle
+  if botx > l_x and boty < l_y:
+      angle1 = 360-angle
+  #print("Angle1: " + str(angle1))
+  dynX = l_x + dis * math.cos(math.radians(angle1))
+  dynY = l_y + dis * math.sin(math.radians(angle1))
+  #print("DynX : " + str(dynX))
+  #print("DynY : " + str(dynY))
+  distult = distance(botx, boty, dynX, dynY)
+  if WriteFlag == 0:
+     f.write("[T:" + str(simtime) + "] " + "Dist to collision point: " + str(distult) + "\n")
+  print("Dist to collision point: " + str(distult))
+  if distult < safedist:
+      rospy.loginfo("Collision!")
+      if WriteFlag == 0:
+          f.write("[T:" + str(simtime) + "] " + "Collision!")
+          f.close
+          WriteFlag = 1
+
+
+    
+def slope(x1, y1, x2, y2): # Line slope given two points:
+    return (y2-y1)/(x2-x1)
+
+def angle(s1, s2):
+    return math.degrees(math.atan((s2-s1)/(1+(s2*s1))))
+
+def angle_between(lineA, lineB):
+    slope1 = slope(lineA[0][0], lineA[0][1], lineA[1][0], lineA[1][1])
+    slope2 = slope(lineB[0][0], lineB[0][1], lineB[1][0], lineB[1][1])
+
+    ang = angle(slope1, slope2)
+    if ang < 0:
+        ang = ang *(-1.0)
+
+    return ang
+
+
+def callback(msg):
+    global WriteFlag
+    botx = msg.pose.pose.position.x
+    boty = msg.pose.pose.position.y  
+    print("BotX: " + str(botx))
+    print("BotY: " + str(boty))
+    if botx > 0 and boty > 0:
+       tuto = Tutorial()
+       tuto.show_gazebo_models(botx, boty)
+    else:
+        rospy.loginfo("Bot out of tracking Range!((x,y)<0)")
+        if WriteFlag == 0:
+            f.write("Bot out of tracking Range!((x,y)<0)")
+            f.close
+
+def main():
+    rospy.init_node('location_monitor')
+    rospy.Subscriber("/odom", Odometry, callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+   main()