Replace location_monitor_node.py

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))