Update Simulation/location_monitor/scripts/location_monitor_node.py

Simulation/location_monitor/scripts/location_monitor_node.py

+#!/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()