Replace location_monitor_node.py

Simulation/location_monitor/scripts/location_monitor_node.py

@@ -3,6 +3,8 @@
 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
@@ -26,7 +28,7 @@ class Block:
 class Tutorial:
 
     _blockListDict = {
-        
+
     }
 
     def show_gazebo_models(self, botx, boty):
@@ -77,7 +79,7 @@ class Tutorial:
                    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 
+                      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")
@@ -113,86 +115,111 @@ def distance(x1, y1, x2, y2):
 def boxkdetection(size, botx, boty, l_x, l_y, simtime):
  global f
  global WriteFlag
- if (size*2.0) > 0.5:
+ if (size*2.0) > 111:
    print("Box to big, > 0.5")
    if WriteFlag == 0:
       f.write("[T:" + str(simtime) + "] " + "Box Invalid (size > 0.5)" + "\n")
       f.close
  else:
-  print("SIZEBOX" + str(size))
+  #print("SIZEBOX" + str(size))
   cornerdist = (size)/math.sin(math.radians(45))
   safedist = TurtlebotBurger
-  #print("Safe " + str(safedist))
-  
+
+
+
+  safedist2 = float(size) + TurtlebotBurger + 0.02
   if WriteFlag == 0:
      f.write("Safedist.: " + str(safedist) + "\n")
-  
-  safedist2 = float(size) + TurtlebotBurger + 0.05
+
+  #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)
-  #print("Dist1: " + str(dist1))
-  if WriteFlag == 0:
-     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 1: " + str(dist1) + "\n")
-  if dist1 < safedist:
-     rospy.loginfo("Collision!")
-     if WriteFlag == 0:
-       f.write("[T:" + str(simtime) + "] " + "Collision!")
-       f.close
-       WriteFlag = 1
-
-  xUpL = l_x + cornerdist*math.cos(math.radians(135))
-  yUpL = l_y + cornerdist*math.sin(math.radians(135))
+
+  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 WriteFlag == 0:
-     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 2: " + str(dist2) + "\n")
-  if dist2 < safedist:
-     rospy.loginfo("Collision!")
-     if WriteFlag == 0:
-       f.write("[T:" + str(simtime) + "] " + "Collision!")
-       f.close
-       WriteFlag = 1
-  
-  xLoL = l_x + cornerdist*math.cos(math.radians(225))
-  yLoL = l_y + cornerdist*math.sin(math.radians(225))
+
+  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 WriteFlag == 0:
-     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 3: " + str(dist3) + "\n")
-  if dist3 < safedist:
-     rospy.loginfo("Collision!")
-     if WriteFlag == 0:
-       f.write("[T:" + str(simtime) + "] " + "Collision!")
-       f.close
-       WriteFlag = 1
-
-  xLoR = l_x + cornerdist*math.cos(math.radians(315))
-  yLoR = l_y + cornerdist*math.sin(math.radians(315))
+
+  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 WriteFlag == 0:
-     f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 4: " + str(dist4) + "\n")
-  if dist4 < safedist:
-     rospy.loginfo("Collision!")
-     if WriteFlag == 0:
-       f.write("[T:" + str(simtime) + "] " + "Collision!")
-       f.close
-       WriteFlag = 1
 
   centerdist = distance(botx, boty, l_x, l_y)
-  #print("Center: " + str(centerdist))
-  if WriteFlag == 0:
-     f.write("[T:" + str(simtime) + "] " + "Dist. Box Center: " + str(centerdist) + "\n")
-  if centerdist < safedist2:
-     rospy.loginfo("Collision!")
-     if WriteFlag == 0:
-       f.write("[T:" + str(simtime) + "] " + "Collision!")
-       f.close
-       WriteFlag = 1
+
+
+
+  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)
+  angle1 = angle
+  if angle > 45:
+      angle = 90 - angle;
+  print("Angle: " + str(angle))
+  dis = (size)/math.cos(math.radians(angle))
+  print("Dids: " + str(dis))
+
+  if botx < l_x and boty > l_y:
+      angle1 = 180-angle
+  if botx < l_x and boty < l_y:
+      angle1 = 270 - 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))
+
+  distult = distance(botx, boty, dynX, dynY)
+  #if WriteFlag == 0:
+     #f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 1: " + str(dist1) + "\n")
+  print(distult)
+  if distult < safedist:
+      rospy.loginfo("Collision!")
+      if WriteFlag == 0:
+          f.write("[T:" + str(simtime) + "] " + "Collision!")
+          f.close
+          WriteFlag = 1
+
+
+  distList = [dist2,dist3,dist4]
+  min_dist = dist1
+  coll = 0
+  for i in distList:
+      if i < min_dist:
+          min_dist = i
+          coll = distList.index(i) + 1
+  #print(coll)
+
+
     
+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)
+    #if ang > 45:
+     #   ang = 90 - ang;
+
+    return ang
+
 
 def callback(msg):
     botx = msg.pose.pose.position.x