From 37b28a0acaee335f1f6fe9506354ee1886d75407 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Thu, 26 Mar 2020 19:04:19 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 43 +++++++++++++------
1 file changed, 30 insertions(+), 13 deletions(-)
diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py
index f1d5d96..d0a18eb 100755
--- a/src/sim/location_monitor/scripts/location_monitor_node.py
+++ b/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
--
GitLab