Replace location_monitor_node.py

src/sim/location_monitor/scripts/location_monitor_node.py

@@ -20,11 +20,12 @@ from gazebo_msgs.srv import GetModelState
 from gazebo_msgs.srv import GetWorldProperties
 
 BotDist = float(sys.argv[1])#Abstand vom BotKoordinatenursprung zum Kollisions Rand
-BotSides = float(sys.argv[2])#Distanz Zur Ecke.      Waffle:0.07 Burger:0
-BotSideRad = float(sys.argv[3])#Ausrichtungs Radius. Waffle:68  Burger:0
-SafeDist = float(sys.argv[4])#Sicherheitsabstand
-DestinationX = float(sys.argv[5])#Ziellinie
-Timeout = float(sys.argv[6])#timeout
+BotHeight = float(sys.argv[2])#Hoehe des Bots        Waffle:0.14 Burger:0.19
+BotSides = float(sys.argv[3])#Distanz Zur Ecke.      Waffle:0.07 Burger:0
+BotSideRad = float(sys.argv[4])#Ausrichtungs Radius. Waffle:68  Burger:0
+SafeDist = float(sys.argv[5])#Sicherheitsabstand
+DestinationX = float(sys.argv[6])#Ziellinie
+Timeout = float(sys.argv[7])#timeout
 yawBot = 0.0
 WriteFlag = 0
 
@@ -181,6 +182,7 @@ def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime):
     global WriteFlag
     global logger
     global SafeDist
+    global BotHeight
     safedist = radius + SafeDist  # Radius/Size + Bot front Rad + safe dist
     rospy.loginfo("Safedist: " + str(SafeDist))
     if WriteFlag == 0:
@@ -194,15 +196,30 @@ def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime):
 
     temp = min(distList)
     #print(distList)
-    if WriteFlag == 0:
-        logger.info("[T:" + str(simtime) + "] " + "Dist. To nearest Bot Collision point: " + str(temp-radius))
-    rospy.loginfo("Dist to collision point: " + str(temp-radius))
-    if temp < safedist:
-        rospy.loginfo("Collision!")
-        if WriteFlag == 0:
-            logger.info("[T:" + str(simtime) + "] " + "Collision!")
-            os._exit(1)
 
+    if radius > BotHeight:
+       both = BotHeight
+       h = radius-both
+       f = temp
+       g = pow(h,2) + pow(f,2)
+       g = math.sqrt(g)
+       if WriteFlag == 0:
+           logger.info("[T:" + str(simtime) + "] " + "Dist. To nearest Bot Collision point: " + str(g - radius))
+       rospy.loginfo("Dist to collision point: " + str(g - radius))
+       if temp < safedist and g < safedist:
+           rospy.loginfo("Collision!")
+           if WriteFlag == 0:
+               logger.info("[T:" + str(simtime) + "] " + "Collision!")
+               os._exit(1)
+    else:
+        if WriteFlag == 0:
+            logger.info("[T:" + str(simtime) + "] " + "Dist. To nearest Bot Collision point: " + str(temp - radius))
+        rospy.loginfo("Dist to collision point: " + str(temp - radius))
+        if temp < safedist:
+            rospy.loginfo("Collision!")
+            if WriteFlag == 0:
+                logger.info("[T:" + str(simtime) + "] " + "Collision!")
+                os._exit(1)
 
 def boxkdetection(size, cordlist, l_x, l_y, yaw_degrees, simtime):
     global WriteFlag