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 0000000000000000000000000000000000000000..ced2a83c77dfd29e90a02664f151cee7d7bcd69b --- /dev/null +++ b/Simulation/location_monitor/scripts/location_monitor_node.py @@ -0,0 +1,90 @@ +#!/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'), + #'block_b': Block('ball_clone' , 'link'), + #'block_c': Block('ball_clone_0' , 'link'), + #'block_d': Block('ball_clone_1' , '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 str(model) != "turtlebot3_burger" and str(model) != "turtlebot3_waffle" and str(model) != "ground_plane": + 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) + 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()