Replace location_monitor_node.py

src/sim/location_monitor/scripts/location_monitor_node.py

@@ -186,11 +186,11 @@ def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime):
     safedist = radius + SafeDist  # Radius/Size + Bot front Rad + safe dist
     rospy.loginfo("Safedist: " + str(safedist))
     logger.info("Safedist.: " + str(safedist))
-    lenght = len(cordlist)-1
-    rospy.loginfo("Distance: " + str(distance(cordlist[0], cordlist[1], l_x, l_y))) #Zenterpunkt
+    lenght = len(cordlist)
     dist = distance(cordlist[0], cordlist[1], l_x, l_y)
-    logger.info("[T:" + simtime + "] " + "Dist. To Bot Center: " + str(dist))
-    for i in range(lenght):
+    rospy.loginfo("Distance: " + str(dist)) #Zenterpunkt
+    logger.info("[T:" + simtime + "] " + "Dist. To Bot: " + str(dist))
+    for i in range(0,lenght,2):
      dist = distance(cordlist[i], cordlist[i+1], l_x, l_y)
      if dist < safedist:
          rospy.loginfo("Collision!")
@@ -213,9 +213,9 @@ def boxkdetection(size, cordlist, l_x, l_y, yaw_degrees, simtime):
         xR = l_x + size * math.cos(math.radians(yaw_degrees))
         yL = l_y + size * math.sin(math.radians(yaw_degrees))
 
-        lenght = len(cordlist) - 1
+        lenght = len(cordlist)
 
-        for i in range(lenght):
+        for i in range(0,lenght,2):
           lineA = ((cordlist[i], cordlist[i+1]), (l_x, l_y))
           lineB = ((l_x, l_y), (xR, yL))
 
@@ -237,7 +237,7 @@ def boxkdetection(size, cordlist, l_x, l_y, yaw_degrees, simtime):
 
           distult = distance(cordlist[i], cordlist[i+1], dynX, dynY)
           if WriteFlag == 0 and i == 0:
-              logger.info("[T:" + str(simtime) + "] " + "Dist. To Bot Center: " + str(distult))
+              logger.info("[T:" + str(simtime) + "] " + "Dist. To Bot: " + str(distult))
           rospy.loginfo("Dist to collision point: " + str(distult))
           if distult < safedist:
               rospy.loginfo("Collision!")