From 37e15b5af1672d853214ac77ce8ca74cb1df6977 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Sun, 1 Mar 2020 19:23:53 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 29 +++++++++++++++++++
1 file changed, 29 insertions(+)
diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py
index 850fd41..2fa66a6 100644
--- a/src/sim/location_monitor/scripts/location_monitor_node.py
+++ b/src/sim/location_monitor/scripts/location_monitor_node.py
@@ -28,6 +28,12 @@ Timeout = float(sys.argv[6])#timeout
yawBot = 0.0
WriteFlag = 0
+SaveBotX = 0.0
+SaveBotY = 0.0
+SaveSimtime = 0
+SimTime = 0
+
+
logger = logging.getLogger("EventLog")
logger.setLevel(logging.INFO)
@@ -68,8 +74,29 @@ class Detection:
global DestinationX
global WriteFlag
global logger
+ global SimTime
+ global SaveSimtime
+ global SaveBotX
+ global SaveBotY
+
get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
resp1 = get_world_properties()
+ SimTime = resp1.sim_time
+
+ check = 0
+
+ if SaveBotX != float("{0:.2f}".format(cordlist[0])) or SaveBotY != float("{0:.2f}".format(cordlist[1])):
+ SaveBotX = float("{0:.2f}".format(cordlist[0]))
+ SaveBotY = float("{0:.2f}".format(cordlist[1]))
+ SaveSimtime = SimTime
+ check = 1
+
+ #print(str(check))
+ if check == 0 and ((SimTime-SaveSimtime)>5.0) and SaveSimtime >0.0 and SimTime >0.0:
+ logger.info("[T:" + str(resp1.sim_time) + "] " + "Bot did not move for 5 seconds!")
+ os._exit(1)
+
+
if resp1.sim_time >= Timeout:
rospy.loginfo("Simulation Timeout!")
if WriteFlag == 0:
@@ -253,9 +280,11 @@ def callback(msg):
global yawBot
global BotSides
global BotSideRad
+
get_rotation(msg)
botx = msg.pose.pose.position.x
boty = msg.pose.pose.position.y
+
botx = botx + BotDist * math.cos(math.radians(yawBot))
boty = boty + BotDist * math.sin(math.radians(yawBot))
rospy.loginfo("BotX: " + str(botx))
--
GitLab