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..019ef44ec759baa68b38e477c4b7a8a21e6d487a --- /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()