From b5f93230a3f73e185cc53b713967332d82ad1e86 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Fri, 28 Feb 2020 19:59:05 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 14 ++++++++------
1 file changed, 8 insertions(+), 6 deletions(-)
diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py
index 2997903..850fd41 100644
--- a/src/sim/location_monitor/scripts/location_monitor_node.py
+++ b/src/sim/location_monitor/scripts/location_monitor_node.py
@@ -158,13 +158,13 @@ def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime):
global SafeDist
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)))
+ rospy.loginfo("Distance: " + str(distance(cordlist[0], cordlist[1], l_x, l_y))) #Zenterpunkt
+ 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):
dist = distance(cordlist[i], cordlist[i+1], l_x, l_y)
- if WriteFlag == 0:
- logger.info("Safedist.: " + str(safedist))
- logger.info("[T:" + simtime + "] " + "Dist. To Collision Point: " + str(dist))
if dist < safedist:
rospy.loginfo("Collision!")
if WriteFlag == 0:
@@ -209,8 +209,8 @@ def boxkdetection(size, cordlist, l_x, l_y, yaw_degrees, simtime):
dynY = l_y + dis * math.sin(math.radians(angle1))
distult = distance(cordlist[i], cordlist[i+1], dynX, dynY)
- if WriteFlag == 0:
- logger.info("[T:" + str(simtime) + "] " + "Dist to collision point: " + str(distult))
+ if WriteFlag == 0 and i == 0:
+ logger.info("[T:" + str(simtime) + "] " + "Dist. To Bot Center: " + str(distult))
rospy.loginfo("Dist to collision point: " + str(distult))
if distult < safedist:
rospy.loginfo("Collision!")
@@ -270,6 +270,8 @@ def callback(msg):
cordlist.append(botx + BotSides * math.cos(math.radians(yawBot-BotSideRad)))
cordlist.append(boty + BotSides * math.sin(math.radians(yawBot-BotSideRad)))
+
+
detect.show_gazebo_models(cordlist)
gc.collect()
--
GitLab