Run Script to Track Aruco

Jspatafore49 · 2020-10-29 21:32:17 UTC

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.

Jspatafore49 · 2020-10-29 21:45:05 UTC

#!/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

caleberg · 2020-10-31 17:31:41 UTC

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’

Jspatafore49 · 2020-11-02 01:05:04 UTC

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.

caleberg · 2020-11-05 01:58:38 UTC

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!

Jspatafore49 · 2020-11-05 02:55:57 UTC

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!

caleberg · 2020-11-06 03:12:08 UTC

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!

Jspatafore49 · 2020-11-06 20:55:50 UTC

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

BJ4 · 2025-01-30 06:42:38 UTC

I am experiencing a somewhat similar issue to the above …

Would someone please check where my following (and/or attached) script could be preventing ‘aruco_landing.py’ from lanching (attached)?

I think there may be an issue with ‘except Exception as e:’, etc. In the tutorial, I’m a little confused when we do & do not include ‘as’ …

The attached screenshot includes suggestions from VS Code. I’ve followed the suggestions but the problem is still not being resolved … file:///home/dronedojo/Downloads/Script%20(1).jpg

Caleb, I have emailed you a copy of this query …

track_aruco.py (2.7 KB)

#!/usr/bin/env python

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

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

id_to_find = 72
marker_size = 20

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

horizontal_res = 640
vertical_res = 480

horizontal_fov = 53.5 * (math.pi / 180)
vertical_fov = 41.41 * (math.pi / 180)

found_count=0
notfound_count=0

#CAMERA INTRINSICS#

dist_coeff = [0.0, 0.0, 0.0, 0.0, 0.0]
camera_matrix = [[1061.654, 0.0, 640.5],[0.0, 1061.654, 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

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)
    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])
                y = '{:.2f}'.format(tvec[1])
                z = '{:.2f}'.format(tvec[2])


                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)

                cv2.putText(np_data,marker_position,(10,50),0,.5(255,0,0),thickness=1)

                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)

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

jax200 · 2025-01-31 14:00:11 UTC

There are some typos, missing statements and syntax errors in your script (and his video).
I suggest downloading the .ova file and comparing his final script with yours.