Replace location_monitor_node.py

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))