From baa7cd0c1a3e6a0a0581c3659b66275705382810 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Mon, 9 Mar 2020 22:01:38 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 14 +++++++-------
1 file changed, 7 insertions(+), 7 deletions(-)
diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py
index f93868e..05dd65b 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!")
--
GitLab