diff --git a/Simulation/location_monitor/scripts/location_monitor_node.py b/Simulation/location_monitor/scripts/location_monitor_node.py deleted file mode 100644 index bb4a6cfa1701f91a2ba0b387b441f9ee6cad135d..0000000000000000000000000000000000000000 --- a/Simulation/location_monitor/scripts/location_monitor_node.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -import math -import rospy -from nav_msgs.msg import Odometry -from gazebo_msgs.srv import GetModelState - - -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): - - try: - model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) - for block in self._blockListDict.itervalues(): - blockName = str(block._name) - resp_coordinates = model_coordinates(blockName, block._relative_entity_name) - l_x = resp_coordinates.pose.position.x - l_y = resp_coordinates.pose.position.y - dist = distance(botx, boty, l_x, l_y) - print(dist) - if dist < 0.20: - rospy.loginfo("Collision!") - - - print(blockName) - print("X: " + str(l_x)) - print("Y: " + str(l_y)) - dist = 0 - - except rospy.ServiceException as e: - rospy.loginfo("Get Model State service call failed: {0}".format(e)) - - -def distance(x1, y1, x2, y2): - dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) - 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() - - - -def main(): - rospy.init_node('location_monitor') - rospy.Subscriber("/odom", Odometry, callback) - rospy.spin() - -if __name__ == '__main__': - main()