From 6151ce445f448b00e94fa54d4e3513fce6b4bf64 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Mon, 6 Jan 2020 20:17:59 +0100
Subject: [PATCH] Update
Simulation/location_monitor/scripts/location_monitor_node.py
---
.../scripts/location_monitor_node.py | 89 +++++++++++++++++++
1 file changed, 89 insertions(+)
create mode 100644 Simulation/location_monitor/scripts/location_monitor_node.py
diff --git a/Simulation/location_monitor/scripts/location_monitor_node.py b/Simulation/location_monitor/scripts/location_monitor_node.py
new file mode 100644
index 0000000..6a7c336
--- /dev/null
+++ b/Simulation/location_monitor/scripts/location_monitor_node.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+
+import math
+import rospy
+from nav_msgs.msg import Odometry
+from gazebo_msgs.srv import GetModelState
+from gazebo_msgs.srv import GetWorldProperties
+
+
+#botx = 0
+#boty = 0
+
+class Block:
+ def __init__(self, name, relative_entity_name):
+ self._name = name
+ self._relative_entity_name = relative_entity_name
+
+
+class Tutorial:
+
+ _blockListDict = {
+ #'block_a': Block('ball' , 'link'),
+
+ }
+
+ def show_gazebo_models(self, botx, boty):
+ get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
+ object_list = list()
+ 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)):
+ s = 'block_' + str(i)
+ self._blockListDict.update( {s : Block(model , 'link' )})
+ i = i+1
+ #print(block)
+
+ #print(self._blockListDict)
+
+
+
+ try:
+ 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)
+ 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!")
+
+ 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 callback(msg):
+ botx = msg.pose.pose.position.x
+ boty = msg.pose.pose.position.y
+ print("BotX: " + str(botx))
+ print("BotY: " + str(boty))
+ tuto = Tutorial()
+ tuto.show_gazebo_models(botx, boty)
+
+
+
+def main():
+ rospy.init_node('location_monitor')
+ rospy.Subscriber("/odom", Odometry, callback)
+ rospy.spin()
+
+if __name__ == '__main__':
+ main()
--
GitLab