diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py index f93868e749b5a99816aeb9d33f0df0772b24990d..05dd65b6a5b30c9bb9413fb8752fd2ccddd27ead 100644 --- a/src/sim/location_monitor/scripts/location_monitor_node.py +++ b/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!")