diff --git a/Simulation/location_monitor/scripts/location_monitor_node.py b/Simulation/location_monitor/scripts/location_monitor_node.py
index d6431c71c754a848c78bbd1f71036b548a226dcb..99303c0f60d16d179a0ae027fe24c432562c6f4b 100644
--- a/Simulation/location_monitor/scripts/location_monitor_node.py
+++ b/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