From fc4425ee9dd7f1f0a76a382191f825acfb5c3d2d Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Wed, 22 Jan 2020 18:14:55 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 110 +++++++++---------
1 file changed, 52 insertions(+), 58 deletions(-)
diff --git a/Simulation/location_monitor/scripts/location_monitor_node.py b/Simulation/location_monitor/scripts/location_monitor_node.py
index 99303c0..44bf840 100644
--- a/Simulation/location_monitor/scripts/location_monitor_node.py
+++ b/Simulation/location_monitor/scripts/location_monitor_node.py
@@ -17,6 +17,17 @@ 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):
@@ -51,10 +62,6 @@ class Tutorial:
self._blockListDict.update( {s : Block(model , 'link' , rad)})
i = i+1
#print(block)
-
-
-
-
try:
global f
global WriteFlag
@@ -115,45 +122,38 @@ def distance(x1, y1, x2, y2):
def boxkdetection(size, botx, boty, l_x, l_y, simtime):
global f
global WriteFlag
- if (size*2.0) > 111:
- print("Box to big, > 0.5")
+ if (size*2.0) > 5.0:
+ print("Box to big, > 5m")
if WriteFlag == 0:
- f.write("[T:" + str(simtime) + "] " + "Box Invalid (size > 0.5)" + "\n")
+ 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
-
-
-
- safedist2 = float(size) + TurtlebotBurger + 0.02
+ #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)
+ #xUpR = l_x + cornerdist*math.cos(math.radians(45))
+ #yUpR = l_y + cornerdist*math.sin(math.radians(45))
+ #dist1 = distance(botx, boty, xUpR, yUpR)
- 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)
+ #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))
@@ -161,28 +161,29 @@ def boxkdetection(size, botx, boty, l_x, l_y, simtime):
lineB = ((l_x,l_y), (xR,yL))
angle = angle_between(lineA,lineB)
+ disangle = angle
angle1 = angle
if angle > 45:
- angle = 90 - angle;
- print("Angle: " + str(angle))
- dis = (size)/math.cos(math.radians(angle))
- print("Dids: " + str(dis))
+ 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
+ angle1 = 180 - angle
if botx < l_x and boty < l_y:
- angle1 = 270 - angle
+ angle1 = 180 + angle
if botx > l_x and boty < l_y:
angle1 = 360-angle
-
- print("Angle1: " + str(angle1))
+ #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. Box Corner 1: " + str(dist1) + "\n")
- print(distult)
+ 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:
@@ -191,16 +192,6 @@ def boxkdetection(size, botx, boty, l_x, l_y, simtime):
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)
@@ -215,21 +206,24 @@ def angle_between(lineA, lineB):
ang = angle(slope1, slope2)
if ang < 0:
ang = ang *(-1.0)
- #if ang > 45:
- # ang = 90 - ang;
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))
- tuto = Tutorial()
- tuto.show_gazebo_models(botx, 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("[T:" + str(simtime) + "] " + "Bot out of tracking Range!((x,y)<0)")
+ f.close
def main():
rospy.init_node('location_monitor')
--
GitLab