From cf95ba6fd18adaef5fa51289ced1db254d6ab57c Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Tue, 14 Jan 2020 20:13:36 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 73 ++++++++++++++++---
1 file changed, 64 insertions(+), 9 deletions(-)
diff --git a/Simulation/location_monitor/scripts/location_monitor_node.py b/Simulation/location_monitor/scripts/location_monitor_node.py
index 043b419..3f52696 100644
--- a/Simulation/location_monitor/scripts/location_monitor_node.py
+++ b/Simulation/location_monitor/scripts/location_monitor_node.py
@@ -3,13 +3,18 @@
import math
import rospy
import re
+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
-#boty = 0
+WriteFlag = 0
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S")
+logtime = str(current_time) + "_log.txt"
+f = open(logtime,"w+")
class Block:
def __init__(self, name, relative_entity_name, radius):
@@ -28,6 +33,7 @@ class Tutorial:
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))):
@@ -48,31 +54,47 @@ class Tutorial:
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):
- blockdetection(float(block._radius), botx, boty, l_x, l_y)
+ 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 dist < safedist:
+ 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))
@@ -88,11 +110,16 @@ def distance(x1, y1, x2, y2):
return dist
-def blockdetection(size, botx, boty, l_x, l_y):
- print(str(size))
+def boxkdetection(size, botx, boty, l_x, l_y, simtime):
+ global f
+ global WriteFlag
cornerdist = (size)/math.sin(math.radians(45))
safedist = TurtlebotBurger
#print("Safe " + str(safedist))
+
+ if WriteFlag == 0:
+ f.write("Safedist.: " + str(safedist) + "\n")
+
safedist2 = float(size) + TurtlebotBurger + 0.05
#print("Safe2 " + str(safedist2))
@@ -100,38 +127,66 @@ def blockdetection(size, botx, boty, l_x, l_y):
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))
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))
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))
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
-
def callback(msg):
botx = msg.pose.pose.position.x
boty = msg.pose.pose.position.y
@@ -139,7 +194,7 @@ def callback(msg):
print("BotY: " + str(boty))
tuto = Tutorial()
tuto.show_gazebo_models(botx, boty)
-
+
def main():
--
GitLab