From 1dd694c9e459d5f2330f0092415baabc84ef6283 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Thu, 26 Mar 2020 22:24:27 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 51 +++++++++++--------
1 file changed, 30 insertions(+), 21 deletions(-)
diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py
index daf5310..23d3dcc 100755
--- a/src/sim/location_monitor/scripts/location_monitor_node.py
+++ b/src/sim/location_monitor/scripts/location_monitor_node.py
@@ -20,9 +20,9 @@ from gazebo_msgs.srv import GetModelState
from gazebo_msgs.srv import GetWorldProperties
BotDist = float(sys.argv[1])#Abstand vom BotKoordinatenursprung zum Kollisions Rand
-BotHeight = float(sys.argv[2])#Hoehe des Bots Waffle:0.14 Burger:0.19
-BotSides = float(sys.argv[3])#Distanz Zur Ecke. Waffle:0.12 Burger:0
-BotSideRad = float(sys.argv[4])#Ausrichtungs Radius. Waffle:55 Burger:0
+BotHeight = float(sys.argv[2])#Hoehe des Bots
+BotSides = float(sys.argv[3])#Distanz Zur Ecke.
+BotSideRad = float(sys.argv[4])#Ausrichtungs Radius.
SafeDist = float(sys.argv[5])#Sicherheitsabstand
DestinationX = float(sys.argv[6])#Ziellinie
Timeout = float(sys.argv[7])#timeout
@@ -161,7 +161,7 @@ class Detection:
yaw_degrees += 360.0
boxkdetection(float(block._radius), cordlist, l_x, l_y, yaw_degrees, resp1.sim_time)
else: #if "sphere" , "cylinder" , "ball" etc.
- objectWithRadiusDetection(float(block._radius), cordlist, l_x, l_y, str(resp1.sim_time))
+ objectWithRadiusDetection(float(block._radius), cordlist, l_x, l_y, str(resp1.sim_time), blockName)
else:
if WriteFlag == 0:
logger.info("[T:" + str(resp1.sim_time) + "] " + "Can't detect " + blockName)
@@ -178,7 +178,7 @@ def distance(x1, y1, x2, y2):
dist = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
return dist
-def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime):
+def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime, blockname):
global WriteFlag
global logger
global SafeDist
@@ -196,22 +196,31 @@ def objectWithRadiusDetection(radius, cordlist, l_x, l_y, simtime):
temp = min(distList)
#print(distList)
-
- 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 "ball" in blockname or "sphere" in blockname:
+ 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)
+ else:#cylinder
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))
--
GitLab