forked from sangwansangwan/ROS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Localization.py
37 lines (24 loc) · 955 Bytes
/
Localization.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#!/usr/bin/env python
import rospy
import roslib
import tf
from geometry_msgs.msg import PoseArray
#Defining a class
class Marker_detect():
def __init__(self):
rospy.init_node('marker_detection',anonymous=False) # initializing a ros node with name marker_detection
self.whycon_marker = {} # Declaring dictionaries
rospy.Subscriber('/whycon/poses',PoseArray,self.whycon_data) # Subscribing to topic
# Callback for /whycon/poses
# Please fill in the function
def whycon_data(self,msg):
#Getting markers position with round-off upto 4 digits
roundoff=4
for count in range(len(msg.poses)):
self.whycon_marker[count]=[round(msg.poses[count].position.x,roundoff),round(msg.poses[count].position.y,roundoff),round(msg.poses[count].position.z,roundoff)]
# Printing the detected markers on terminal
print(self.whycon_marker)
if __name__=="__main__":
marker = Marker_detect()
while not rospy.is_shutdown():
rospy.spin()