I am pretty new to python.Ι have got a script that detects aruco markers. I am trying to create a list that stores the IDs that are returned from the detect_aruco
function and add any new IDs that are detected. Basically the function detect_aruco
is called which returns an array of IDs and a array with their coordinates. When an aruco is detected, I want check the list to determine if i have encountered it's id before and if not store it to the list and print it's coordinates. Any ideas on how it can be done?
#!/usr/bin/env python
import rospy
import numpy as np
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2.aruco as aruco
class image_convert_pub:
def __init__(self):
self.image_pub = rospy.Publisher("/detected_markers",Image, queue_size=1)
self.id_pub = rospy.Publisher("/arudo_ID", String, queue_size=1)
self.bridge = CvBridge()
#self.image_sub = rospy.Subscriber("/usb_cam/image_raw/", Image, self.callback)
self.image_sub = rospy.Subscriber('/camera/image_raw', Image, self.callback)
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
markers_img,corners_list,ids_list = self.detect_aruco(cv_image)
if ids_list is None:
self.id_pub.publish(ids_list)
else:
ids_str = ''.join(str(e) for e in ids_list)
self.id_pub.publish(ids_str)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(markers_img, "bgr8"))
except CvBridgeError as e:
print(e)
def detect_aruco(self,img):
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)
parameters = aruco.DetectorParameters_create()
corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
output = aruco.drawDetectedMarkers(img, corners, ids) # detect the sruco markers and display its aruco id.
return output,corners,ids
def main():
print("Initializing ROS-node")
rospy.init_node('detect_markers', anonymous=True)
print("Bring the aruco-ID in front of camera")
ic = image_convert_pub()
rospy.spin()
if __name__ == '__main__':
main()
It could be simpler to use dictionary {id: corners, other_id: other_corners, ...}
You could create empty dictionary self.encountered = {}
in __init__
And later you could use zip(ids_list, corners_list)
to create pairs (id, corners)
and check id
in self.encountered
and add new self.encountered[id] = corners
def __init__(self):
# ... code ...
self.encountered = {}
def callback(self, data):
# ... code ...
markers_img, corners_list, ids_list = self.detect_aruco(cv_image)
for id_, corners in zip(ids_list, corners_list):
if id_ not in self.encountered:
self.encountered[id_] = corners
print('new:', id_, corners)
# ... code ...
The technical post webpages of this site follow the CC BY-SA 4.0 protocol. If you need to reprint, please indicate the site URL or the original address.Any question please contact:yoyou2525@163.com.