From 01cc6ca79b08a77bd4938230d3fa1c7e81303c54 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Sun, 26 Jan 2020 15:01:49 +0100
Subject: [PATCH] Update
src/sim/location_monitor/scripts/location_monitor_node.py
---
.../scripts/location_monitor_node.py | 234 ++++++++++++++++++
1 file changed, 234 insertions(+)
create mode 100644 src/sim/location_monitor/scripts/location_monitor_node.py
diff --git a/src/sim/location_monitor/scripts/location_monitor_node.py b/src/sim/location_monitor/scripts/location_monitor_node.py
new file mode 100644
index 0000000..f05bdda
--- /dev/null
+++ b/src/sim/location_monitor/scripts/location_monitor_node.py
@@ -0,0 +1,234 @@
+#!/usr/bin/env python
+
+import math
+import rospy
+import re
+import array
+import numpy as np
+from datetime import datetime
+from nav_msgs.msg import Odometry
+from gazebo_msgs.srv import GetModelState
+from gazebo_msgs.srv import GetWorldProperties
+
+
+TurtlebotBurger = 0.05
+WriteFlag = 0
+now = datetime.now()
+current_time = now.strftime("%H:%M:%S")
+logtime = str(current_time) + "_log.txt"
+f = open(logtime,"w+")
+# y
+# ^
+# |
+# | ------- Box muss im rechtem Winkel zur x-Achse sein!
+# | | |
+# | | |
+# | -------
+# |
+# ---------------->x
+
+
+
+class Block:
+ def __init__(self, name, relative_entity_name, radius):
+ self._name = name
+ self._relative_entity_name = relative_entity_name
+ self._radius = radius
+
+
+class Tutorial:
+
+ _blockListDict = {
+
+ }
+
+ def show_gazebo_models(self, botx, boty):
+ get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
+ #object_list = list()
+ resp1 = get_world_properties()
+ #print(str(resp1.sim_time))
+ i = 0
+ for model in resp1.model_names:
+ 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' , rad)})
+ i = i+1
+ #print(block)
+ try:
+ global f
+ global WriteFlag
+ model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+ for block in self._blockListDict.itervalues():
+ blockName = str(block._name)
+ if blockName in resp1.model_names:
+ resp_coordinates = model_coordinates(blockName, block._relative_entity_name)
+ l_x = resp_coordinates.pose.position.x
+ l_y = resp_coordinates.pose.position.y
+ print(blockName)
+ if WriteFlag == 0:
+ f.write(str(blockName) + "\n")
+ print("X: " + str(l_x))
+ print("Y: " + str(l_y))
+ if WriteFlag == 0:
+ f.write("[T:" + str(resp1.sim_time) + "] " + "Bot X: " + str(l_x) + "\n")
+ f.write("[T:" + str(resp1.sim_time) + "] " + "Bot Y: " + str(l_y) + "\n")
+ dist = distance(botx, boty, l_x, l_y)
+ safedist = 0
+ if block._radius != 0:
+ if "box" in str(blockName):
+ boxkdetection(float(block._radius), botx, boty, l_x, l_y, resp1.sim_time)
+ else:
+ safedist = float(block._radius) + TurtlebotBurger + 0.05 #Radius/Size + Bot front Rad + 0.05 safe dist
+ print("Safe " + str(safedist))
+ if WriteFlag == 0:
+ f.write("Safedist.: " + str(safedist) + "\n")
+ print("Dist: " + str(dist))
+ if WriteFlag == 0:
+ f.write("[T:" + str(resp1.sim_time) + "] " + "Dist. To Collision Point: " + str(dist) + "\n")
+ if dist < safedist:
+ rospy.loginfo("Collision!")
+ if WriteFlag == 0:
+ f.write("[T:" + str(resp1.sim_time) + "] " + "Collision!")
+ f.close
+ WriteFlag = 1
+ else:
+
+ print("Dist: " + str(dist))
+ if WriteFlag == 0:
+ f.write("[T:" + str(resp1.sim_time) + "] " + "Dist: " + str(dist) + "\n")
+
+ except rospy.ServiceException as e:
+ rospy.loginfo("Get Model State service call failed: {0}".format(e))
+
+
+def distance(x1, y1, x2, y2):
+ #print("bX1: " + str(x1))
+ #print("bY1: " + str(y1))
+ #print("X2: " + str(x2))
+ #print("Y2: " + str(y2))
+ dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
+ #print(dist)
+ return dist
+
+
+def boxkdetection(size, botx, boty, l_x, l_y, simtime):
+ global f
+ global WriteFlag
+ if (size*2.0) > 5.0:
+ print("Box to big, > 5m")
+ if WriteFlag == 0:
+ f.write("[T:" + str(simtime) + "] " + "Box Invalid (size > 5m)" + "\n")
+ f.close
+ else:
+ #print("SIZEBOX" + str(size))
+ #cornerdist = (size)/math.sin(math.radians(45))
+ safedist = TurtlebotBurger + 0.05
+ #safedist2 = float(size) + TurtlebotBurger + 0.02
+ if WriteFlag == 0:
+ f.write("Safedist.: " + str(safedist) + "\n")
+ #print("Safe " + str(safedist))
+ #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)
+
+ #xUpL = l_x + cornerdist * math.cos(math.radians(135))
+ #yUpL = l_y + cornerdist * math.sin(math.radians(135))
+ #dist2 = distance(botx, boty, xUpL, yUpL)
+
+ #xLoL = l_x + cornerdist * math.cos(math.radians(225))
+ #yLoL = l_y + cornerdist * math.sin(math.radians(225))
+ #dist3 = distance(botx, boty, xLoL, yLoL)
+
+ #xLoR = l_x + cornerdist * math.cos(math.radians(315))
+ #yLoR = l_y + cornerdist * math.sin(math.radians(315))
+ #dist4 = distance(botx, boty, xLoR, yLoR)
+
+ #centerdist = distance(botx, boty, l_x, l_y)
+ xR = l_x + size * math.cos(math.radians(0))
+ yL = l_y + size * math.sin(math.radians(0))
+
+ lineA = ((botx,boty), (l_x, l_y))
+ lineB = ((l_x,l_y), (xR,yL))
+
+ angle = angle_between(lineA,lineB)
+ disangle = angle
+ angle1 = angle
+ if angle > 45:
+ disangle = 90 - angle;
+ #print("Angle: " + str(disangle))
+ dis = (size)/math.cos(math.radians(disangle))
+ #print("Dist from center do edge: " + str(dis))
+
+ if botx < l_x and boty > l_y:
+ angle1 = 180 - angle
+ if botx < l_x and boty < l_y:
+ angle1 = 180 + angle
+ if botx > l_x and boty < l_y:
+ angle1 = 360-angle
+ #print("Angle1: " + str(angle1))
+ dynX = l_x + dis * math.cos(math.radians(angle1))
+ dynY = l_y + dis * math.sin(math.radians(angle1))
+ #print("DynX : " + str(dynX))
+ #print("DynY : " + str(dynY))
+ distult = distance(botx, boty, dynX, dynY)
+ if WriteFlag == 0:
+ f.write("[T:" + str(simtime) + "] " + "Dist to collision point: " + str(distult) + "\n")
+ print("Dist to collision point: " + str(distult))
+ if distult < safedist:
+ rospy.loginfo("Collision!")
+ if WriteFlag == 0:
+ f.write("[T:" + str(simtime) + "] " + "Collision!")
+ f.close
+ WriteFlag = 1
+
+
+
+def slope(x1, y1, x2, y2): # Line slope given two points:
+ return (y2-y1)/(x2-x1)
+
+def angle(s1, s2):
+ return math.degrees(math.atan((s2-s1)/(1+(s2*s1))))
+
+def angle_between(lineA, lineB):
+ slope1 = slope(lineA[0][0], lineA[0][1], lineA[1][0], lineA[1][1])
+ slope2 = slope(lineB[0][0], lineB[0][1], lineB[1][0], lineB[1][1])
+
+ ang = angle(slope1, slope2)
+ if ang < 0:
+ ang = ang *(-1.0)
+
+ return ang
+
+
+def callback(msg):
+ global WriteFlag
+ botx = msg.pose.pose.position.x
+ boty = msg.pose.pose.position.y
+ print("BotX: " + str(botx))
+ print("BotY: " + str(boty))
+ if botx > 0 and boty > 0:
+ tuto = Tutorial()
+ tuto.show_gazebo_models(botx, boty)
+ else:
+ rospy.loginfo("Bot out of tracking Range!((x,y)<0)")
+ if WriteFlag == 0:
+ f.write("Bot out of tracking Range!((x,y)<0)")
+ f.close
+
+def main():
+ rospy.init_node('location_monitor')
+ rospy.Subscriber("/odom", Odometry, callback)
+ rospy.spin()
+
+if __name__ == '__main__':
+ main()
--
GitLab