From 329d22eabab785d83c340f0ce77bab4c4c8ba320 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Mon, 13 Jan 2020 19:38:19 +0100
Subject: [PATCH] Replace location_monitor_node.py
---
.../scripts/location_monitor_node.py | 82 ++++++++++++++++---
1 file changed, 72 insertions(+), 10 deletions(-)
diff --git a/Simulation/location_monitor/scripts/location_monitor_node.py b/Simulation/location_monitor/scripts/location_monitor_node.py
index 6a7c336..043b419 100644
--- a/Simulation/location_monitor/scripts/location_monitor_node.py
+++ b/Simulation/location_monitor/scripts/location_monitor_node.py
@@ -2,25 +2,26 @@
import math
import rospy
+import re
from nav_msgs.msg import Odometry
from gazebo_msgs.srv import GetModelState
from gazebo_msgs.srv import GetWorldProperties
-#botx = 0
+TurtlebotBurger = 0.05
#boty = 0
class Block:
- def __init__(self, name, relative_entity_name):
+ def __init__(self, name, relative_entity_name, radius):
self._name = name
self._relative_entity_name = relative_entity_name
+ self._radius = radius
class Tutorial:
_blockListDict = {
- #'block_a': Block('ball' , 'link'),
-
+
}
def show_gazebo_models(self, botx, boty):
@@ -29,13 +30,20 @@ class Tutorial:
resp1 = get_world_properties()
i = 0
for model in resp1.model_names:
- if ("ball" in str(model)) or ("cylinder" in str(model)) or ("box" in str(model)):
+ if ("ball" in str(model)) or ("cylinder" in str(model)) or ("box" in str(model) or ("sphere" in str(model))):
+ matches = re.findall(r"[-+]?\d*\.\d+|\d+", str(model))
+ rad = 0
+ if matches:
+ if "box" in str(model):
+ rad = (matches[0])
+ rad = float(rad)/2.0
+ else:
+ rad = (matches[0])
s = 'block_' + str(i)
- self._blockListDict.update( {s : Block(model , 'link' )})
+ self._blockListDict.update( {s : Block(model , 'link' , rad)})
i = i+1
#print(block)
- #print(self._blockListDict)
@@ -52,9 +60,19 @@ class Tutorial:
print("X: " + str(l_x))
print("Y: " + str(l_y))
dist = distance(botx, boty, l_x, l_y)
- print("Dist: " + str(dist))
- if dist < 0.20:
- rospy.loginfo("Collision!")
+ safedist = 0
+ if block._radius != 0:
+ if "box" in str(blockName):
+ blockdetection(float(block._radius), botx, boty, l_x, l_y)
+ else:
+ safedist = float(block._radius) + TurtlebotBurger + 0.05 #Radius/Size + Bot front Rad + 0.05 safe dist
+ print("Safe " + str(safedist))
+ print("Dist: " + str(dist))
+ if dist < safedist:
+ rospy.loginfo("Collision!")
+ else:
+
+ print("Dist: " + str(dist))
except rospy.ServiceException as e:
rospy.loginfo("Get Model State service call failed: {0}".format(e))
@@ -70,6 +88,50 @@ def distance(x1, y1, x2, y2):
return dist
+def blockdetection(size, botx, boty, l_x, l_y):
+ print(str(size))
+ cornerdist = (size)/math.sin(math.radians(45))
+ safedist = TurtlebotBurger
+ #print("Safe " + str(safedist))
+ safedist2 = float(size) + TurtlebotBurger + 0.05
+ #print("Safe2 " + str(safedist2))
+
+ xUpR = l_x + cornerdist*math.cos(math.radians(45))
+ yUpR = l_y + cornerdist*math.sin(math.radians(45))
+ dist1 = distance(botx, boty, xUpR, yUpR)
+ #print("Dist1: " + str(dist1))
+ if dist1 < safedist:
+ rospy.loginfo("Collision!")
+
+ xUpL = l_x + cornerdist*math.cos(math.radians(135))
+ yUpL = l_y + cornerdist*math.sin(math.radians(135))
+ dist2 = distance(botx, boty, xUpL, yUpL)
+ #print("Dist2: " + str(dist2))
+ if dist2 < safedist:
+ rospy.loginfo("Collision!")
+
+ xLoL = l_x + cornerdist*math.cos(math.radians(225))
+ yLoL = l_y + cornerdist*math.sin(math.radians(225))
+ dist3 = distance(botx, boty, xLoL, yLoL)
+ #print("Dist3: " + str(dist3))
+ if dist3 < safedist:
+ rospy.loginfo("Collision!")
+
+ xLoR = l_x + cornerdist*math.cos(math.radians(315))
+ yLoR = l_y + cornerdist*math.sin(math.radians(315))
+ dist4 = distance(botx, boty, xLoR, yLoR)
+ #print("Dist4: " + str(dist4))
+ if dist4 < safedist:
+ rospy.loginfo("Collision!")
+
+ centerdist = distance(botx, boty, l_x, l_y)
+ #print("Center: " + str(centerdist))
+ if centerdist < safedist2:
+ rospy.loginfo("Collision!")
+
+
+
+
def callback(msg):
botx = msg.pose.pose.position.x
boty = msg.pose.pose.position.y
--
GitLab