From ba8836193544c0faa832e56f2fcfef36f9bada3a Mon Sep 17 00:00:00 2001
From: Daniel Christoph <daniel.christoph@informatik.hu-berlin.de>
Date: Sat, 14 Dec 2019 17:39:24 +0100
Subject: [PATCH] Update
Simulation/location_monitor/scripts/location_monitor_node.py
---
.../scripts/location_monitor_node.py | 37 +++++++++++++++++++
1 file changed, 37 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..019ef44
--- /dev/null
+++ b/Simulation/location_monitor/scripts/location_monitor_node.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+
+import math
+import rospy
+from nav_msgs.msg import Odometry
+
+ob = []
+
+
+
+
+
+
+def distance(x1, y1, x2, y2):
+ xd = x1 - x2
+ yd = y1 - y2
+ return math.sqrt(xd*xd + yd*yd)
+
+
+def callback(msg):
+ x = msg.pose.pose.position.x
+ y = msg.pose.pose.position.y
+
+
+ for l_x, l_y in ob:
+ dist = distance(x, y, l_x, l_y)
+ if dist < 0.20:
+ rospy.loginfo("Collision!")
+
+
+def main():
+ rospy.init_node('location_monitor')
+ rospy.Subscriber("/odom", Odometry, callback)
+ rospy.spin()
+
+if __name__ == '__main__':
+ main()
--
GitLab