Skip to content
Snippets Groups Projects
Commit cf95ba6f authored by Daniel Christoph's avatar Daniel Christoph
Browse files

Replace location_monitor_node.py

parent 329d22ea
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Simulation/location_monitor/scripts/location_monitor_node.py
+ 64
− 9
View file @ cf95ba6f
......@@ -3,13 +3,18 @@
import math
import rospy
import re
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
#boty = 0
WriteFlag = 0
now = datetime.now()
current_time = now.strftime("%H:%M:%S")
logtime = str(current_time) + "_log.txt"
f = open(logtime,"w+")
class Block:
def __init__(self, name, relative_entity_name, radius):
......@@ -28,6 +33,7 @@ class Tutorial:
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))):
......@@ -48,31 +54,47 @@ class Tutorial:
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):
blockdetection(float(block._radius), botx, boty, l_x, l_y)
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 dist < safedist:
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))
......@@ -88,11 +110,16 @@ def distance(x1, y1, x2, y2):
return dist
def blockdetection(size, botx, boty, l_x, l_y):
print(str(size))
def boxkdetection(size, botx, boty, l_x, l_y, simtime):
global f
global WriteFlag
cornerdist = (size)/math.sin(math.radians(45))
safedist = TurtlebotBurger
#print("Safe " + str(safedist))
if WriteFlag == 0:
f.write("Safedist.: " + str(safedist) + "\n")
safedist2 = float(size) + TurtlebotBurger + 0.05
#print("Safe2 " + str(safedist2))
......@@ -100,38 +127,66 @@ def blockdetection(size, botx, boty, l_x, l_y):
yUpR = l_y + cornerdist*math.sin(math.radians(45))
dist1 = distance(botx, boty, xUpR, yUpR)
#print("Dist1: " + str(dist1))
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 1: " + str(dist1) + "\n")
if dist1 < safedist:
rospy.loginfo("Collision!")
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Collision!")
f.close
WriteFlag = 1
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 WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 2: " + str(dist2) + "\n")
if dist2 < safedist:
rospy.loginfo("Collision!")
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Collision!")
f.close
WriteFlag = 1
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 WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 3: " + str(dist3) + "\n")
if dist3 < safedist:
rospy.loginfo("Collision!")
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Collision!")
f.close
WriteFlag = 1
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 WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Dist. Box Corner 4: " + str(dist4) + "\n")
if dist4 < safedist:
rospy.loginfo("Collision!")
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Collision!")
f.close
WriteFlag = 1
centerdist = distance(botx, boty, l_x, l_y)
#print("Center: " + str(centerdist))
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Dist. Box Center: " + str(centerdist) + "\n")
if centerdist < safedist2:
rospy.loginfo("Collision!")
if WriteFlag == 0:
f.write("[T:" + str(simtime) + "] " + "Collision!")
f.close
WriteFlag = 1
def callback(msg):
botx = msg.pose.pose.position.x
boty = msg.pose.pose.position.y
......@@ -139,7 +194,7 @@ def callback(msg):
print("BotY: " + str(boty))
tuto = Tutorial()
tuto.show_gazebo_models(botx, boty)
def main():
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment