Run Script to Track Aruco

#1

I seem to be having some trouble with this video.

At the 2:00 minute mark, running the “rosrun gazebo_drone track_aruco.py” gives me a huge error in red text (see image). It seems to be telling me that line 84 in my track_aruco.py code isn’t recognizing newimg_pub.

Similarly, when launching RQT, and switching from /camera/color/image_raw to /camera/color/image_new on gazebo, the entire RQT box just shuts down (sometimes, all of Gazebo shuts down too), and the message in the RQT terminal pops up.

I’m assuming there is something wrong with my track_aruco.py code, but I’m not positive.

rqt%20issue
rqt issue.jpg1119×595 41.5 KB
py%20issue
track_aruco.py issue.jpg1192×639 121 KB

#2

#!/usr/bin/python

#######IMPORTS########

import rospy
from sensor_msgs.msg import Image
import cv2
import cv2.aruco as aruco
import sys
import time
import math
import numpy as np
import ros_numpy as rnp

#######VARIABLES

newing_pub = rospy.Publisher(’./camera/color/image_new’, Image, queue_size=10)

id_to_find = 72 ##arucoID
marker_size = 20 ### CM

aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
parameters = aruco.DetectorParameters_create()

horizontal_res = 640
vertical_res = 480

horizontal_fov = 62.2 * (math.pi / 180) ##62.2 for picam V2, 53.5 for V1
vertical_fov = 48.8 * (math.pi / 180) ##48.8 for V2, 41.41 for V1

found_count=0
notfound_count=0

#########CAMERA INTRINSICS#######

dist_coeff = [0.0, 0.0, 0.0, 0.0, 0.0]
camera_matrix = [[1061.6538553425996, 0.0, 640.5],[0.0, 1061.6538553425996, 360.5],[0.0, 0.0, 1.0]]
np_camera_matrix = np.array(camera_matrix)
np_dist_coeff = np.array(dist_coeff)

time_last=0
time_to_wait = .1 ##100 ms
#########FUNCTIONS########

def msg_receiver(message):
global notfound_count, found_count, time_last, time_to_wait, id_to_find

if time.time() - time_last > time_to_wait:
    np_data = rnp.numpify(message) ##Deserialize image data into array
    gray_img = cv2.cvtColor(np_data, cv2.COLOR_BGR2GRAY)

    ids = ''
    (corners, ids, rejected) = aruco.detectMarkers(image=gray_img,dictionary=aruco_dict,parameters=parameters)

    try:
        if ids is not None:
            if ids[0]==id_to_find:
                ret = aruco.estimatePoseSingleMarkers(corners,marker_size,cameraMatrix=np_camera_matrix,distCoeffs=np_dist_coeff)
                (rvec, tvec) = (ret[0][0,0,:],ret[1][0,0,:])
                x = '{:.2f}'.format(tvec[0]) ###Xerror/distance between camera and aruco in CM
                y = '{:.2f}'.format(tvec[1]) ###Yerror/distance between camera and aruco is CM
                z = '{:.2f}'.format(tvec[2]) ###Zerror/distance between camera and arco in CM


                marker_position = 'MARKER POSITION: x='+x+' y='+y+' z='+z

                aruco.drawDetectedMarkers(np_data,corners)
                aruco.drawAxis(np_data,np_camera_matrix,np_dist_coeff,rvec,tvec,10)
                ##putText(image, text_to_draw, position,font_face,frontScale,color,thickness)
                cv2.putText(np_data,marker_position,(10,50),0,.5,(255,0,0),thickness=2)

                found_count = found_count + 1
            else:
                notfound_count  =notfound_count+1
        else:
            notfound_count=notfound_count+1
    except Exception as e:
        print 'Target likely not found'
        print e
        notfound_count=notfound_count+1
    new_msg = rnp.msgify(Image, np_data,encoding='rgb8')
    newimg_pub.publish(new_msg)
    time_last = time.time()
else:
    return None

def subscriber():
rospy.init_node(‘drone_node’, anonymous=False)
sub = rospy.Subscriber(’/camera/color/image_raw’, Image, msg_receiver)
rospy.spin()

if name==‘main’:
try:
subscriber()
except rospy.ROSInterruptException:
pass

#3

Hi there @Jspatafore49

I think if you remove the period ‘.’ in your topic name when defining the newimg_pub variable the problem will go away.

It should be:
‘/camera/color/image_new’
and not
‘./camera/color/image_new’

#4

Hi @caleberg ,

I went ahead and removed the ‘.’ in my topic name. This did get me a little further, but now image_new is just showing a gray screen. I did see another user had this same issue on the forum, but I wasn’t sure of the solution.

Thank you.

#5

Good to hear the progress!

When you run rostopic list do you see the /camera/color/image_new topic listed?

When you run rostopic hz /camera/color/image_new does it show that this topic is receiving data? (should be around 10 Hz)

I’ve also done this before… the track_aruco.py script must be running in order for there to be data to be published to image_new. The image_new topic could appear as a grayed out blob if the track_aruco.py script isn’t running

If not, would you mind emailing me your file so I could directly help with debugging?

Thanks!

#6

Hi Caleb,

After running the trak_aruco.py script, /camera/color/image_new does show up on rostopic list.

When I run “rostopic hz /camera/color/image_new” , it doesn’t really show any HZ data. Rather, it says,
"subscribed to [/camera/color/image_new]
WARNING: may be using simulated time
no new messages
no new messages
(keeps repeating no new messages ^^)

I’d be happy to email you my file, thanks a million!

#7

Hi there @Jspatafore49

After taking a peak at your track_aruco.py script, it appears your variable ‘newimg_pub’ where you declare the topic, was actually declared as ‘newing_pub’ with an ‘n’.

Then when you try and publish to that object later, you use the correct newimg_pub variable name, but since it wasn’t initialized the image can’t get written to the topic.

Look at line 17 for the misspelled variable and the script should work then!

#8

Man oh man, I am such a n00b. Thank you again!