From 8dd41833498c899152c5962ddfad133b479bcac3 Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Mon, 6 Jan 2020 20:16:31 +0100
Subject: [PATCH] Deleted
Simulation/location_monitor/scripts/location_monitor_node.py
---
.../scripts/location_monitor_node.py | 90 -------------------
1 file changed, 90 deletions(-)
delete 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
deleted file mode 100644
index ced2a83..0000000
--- a/Simulation/location_monitor/scripts/location_monitor_node.py
+++ /dev/null
@@ -1,90 +0,0 @@
-#!/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()
--
GitLab