Write OpenCV Script to Track Aruco pt 2

[Huang]

Hi, I’m Taiwanese English is not very good, watching the course，Follow the steps in the course of。
When I did not execute the image does not know what happened

[caleberg]

Hi there @Huang

Couple questions for you!

1. Does the topic /camera/color/image_raw show for you in RQT?
2. When you run the command “rostopic list” does the /camera/color/img_new appear for you in the terminal?
3. If it does and it is still not showing in RQT, try hitting that blue circular arrow to refresh the topics in RQT

[Huang]

1. Does the topic /camera/color/image_raw show for you in RQT?
   Ｑ：yes

2. When you run the command “rostopic list” does the /camera/color/img_new appear for you in the terminal?
   Ｑ：yes

3. If it does and it is still not showing in RQT, try hitting that blue circular arrow to refresh the topics in RQT
   Ｑ：No picture
   

   

[caleberg]

Hmm, that is odd Huang!

Let’s see if the problem is related to the image topic or rqt.

Try viewing the image a different way. With the gazebo_drone track_aruco.py node running, open a terminal and try this:

rosrun image_view image_view image:=/camera/color/img_new
(Note, you may need to install image_view with sudo apt-get install ros-melodic-image-view)

If this command does not open a new terminal with the img_new topic shown, please try running this command:

rostopic echo /camera/color/img_new

If you don’t see a giant array of numbers as the output, there isn’t any image data being published to the topic and there is likely something wrong with your script.

[Huang]

command operation result：

This is my script, I do not know where the problem：

＃!/usr/bin/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/img_new’, Image, queue_size=10)

id_to_find = 72 #aruco
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)
vertical_fov = 48.8*(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 = [[530.8269276712998, 0.0, 320.5],[0.0, 530.8269276712998, 240.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

#####################FUNCTION########################

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.detectMarker(imsge=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_poseition=‘MARKER POSITION: x=’+x+’ y=’+y+’ z=’+z

aruco.drawDetectedMarkers(np_date,corners)
aruco.drawAxis(np_data,np_camera_matrix,np_dist_coeff,rvec,tvec,10)
cv2.putText(np_data,marker_poseition,(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/img_raw’, Image, msg_receiver)
rospy.spin()

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

[caleberg]

Can you please check at:

ardupilot_gazebo/models/gimbal_small_2d_edit/model.sdf

and see what the name of the topic is for the camera?

I’m thinking maybe the camera is publishing data to /camera/color/image_raw but you are subscribing to /camera/color/img_raw so your subscriber() function never gets called.

Also make sure to remove gimbal_small_2d_edit from /usr/share/gazebo-9/models/gimbal_small_2d_edit if it is also there.

To test this you could also run rostopic echo on both image_raw and img_raw topics to see which topic is actually sending image data.

[Huang]

The problem how to solve, long Kunrao

[caleberg]

Hi there Huang,

Looks like ‘np_date’ should really be ‘np_data’

[Huang]

How can I do, thank you

##gazebo_landing.py

#!/usr/bin/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
from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal
from pymavlink import mavutil
from array import array

vehicle = connect(‘udp:127.0.0.1:14551’,wait_ready=True)
vehicle.parameters[‘PLND_ENABLED’]=1
vehicle.parameters[‘PLND_TYPE’]=1
vehicle.parameters[‘PLND_EST_TYPE’]=0
vehicle.parameters[‘LAND_SPEED’]=30

takeoff_height=5
velocity=4

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

id_to_find=72 #aruco # ID
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)
vertical_fov=48.8 * (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=[[530.8269276712998, 0.0, 320.5],[0.0, 530.8269276712998, 240.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 arm_and_takeoff(aTargetAltitude):
while not vehicle.is_armable:
print ‘waiting for vehicle to become armable’
time.sleep(1)
print ‘vehicle is now armable’

vehicle.mode = VehicleMode('GUIDED')
vehicle.armed = True
while not vehicle.armed:
    print 'waiting for drone to enter GUIDED flight mode'
    time.sleep(1)
print 'waiting now in GUIDED mode ,have fun!'

vehicle.armed = True
while vehicle.armed == False:
    print 'WARNING for vehicle to become armed '
    time.sleep(1)
print 'Look out '

print("Taking off!")
vehicle.simple_takeoff(aTargetAltitude)

while True:
    print(" Altitude: ",vehicle.location.global_relative_frame.alt)

    if vehicle.location.global_relative_frame.alt>= aTargetAltitude * 0.95:
        print("Reached targetaltitude")
        break

    time.sleep(1)
return None

def send_local_ned_velocity(vx,vy,vz):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,
        0,
        0,
        mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED,
        0b0000111111000111,
        0,
        0,
        0,
        vx,
        vy,
        vz,
        0,0,0,0,0
        )
    vehicle.send_mavlink(msg)
    vehicle.flush()

def send_land_message(x,y):
    msg = vehicle.message_factory.landing_target_encode(
        0,
        0,
        mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED,
        x,
        y,
        0,0,0
        )
    vehicle.send_mavlink(msg)
    vehicle.flush()

#####################FUNCTION########################

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])

                x_sum=0
                y_sum=0

                x_sum = corners[0][0][0][0]+corners[0][0][1][0]+corners[0][0][2][0]+corners[0][0][3][0]
                y_sum = corners[0][0][0][1]+corners[0][0][1][1]+corners[0][0][2][1]+corners[0][0][3][1]

                x_avg = x_sum / 4
                y_avg = y_sum / 4

                x_ang = (x_avg-horizontal_res*.5)*horizontal_fov/horizontal_res
                y_ang = (y_avg-vertical_res*.5)*vertical_fov/vertical_res

                if Vehicle.mode !='LAND':
                    Vehicle.mode = VehicleMode('LAND')
                    while Vehicle.mode !='LAND':
                        time.sleep(1)
                    print 'Vehicle in LAND mode'
                    send_land_message(x_ang,y_ang)
                else:
                    send_land_message(x_ang,y_ang)


                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)
                print(marker_position)
                print ('FOUND COUNT'+str(found_count)+'NOTFOUND COUNT'+str(notfound_count))

                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:
arm_and_takeoff(takeoff_height)
time.sleep(1)
send_local_ned_velocity(velocity,0,0)
time.sleep(1)
subscriber()
except rospy.ROSInterruptException:
pass

[caleberg]

Hmmm,

Try fixing your main if block.

Change
if name=='main':
to
if __name__=='__main__':

And see if that works

Also, in your msg_receiver() function, change your capital Vehicle variable to lowercase vehicle (line 146 147 148)

Other than that, it looks like the send_local_ned_velocity function should be working. I copied your code here into my environment and the script ran for me. I did change the shebang from
#!/usr/bin/python
to
#!/usr/bin/env python

[Huang]

After correcting the problem is not solved：

Traceback (most recent call last):
File “/home/huangyaoqing/catkin_ws/src/gazebo_drone/scripts/gazebo_landing.py”, line 192, in
send_local_ned_velocity(velocity,0,0)
NameError: name ‘send_local_ned_velocity’ is not defined

[caleberg]

@Huang

Did you try switching the shebang from #!/usr/bin/python to #!/usr/bin/env python
?

If that does not work either, can you please email me your exact ‘gazebo_landig.py’ file to caleberg@dojofordrones.com?

The reason this would be helpful is because I could see if you had any indentation errors or anything like that going on.

I’m not sure what else could be going wrong because I ran your the script you provided here successfully in my environment- the only thing I could think would be indentation errors.

I have emailed you two files:

1. your gazebo_landing.py script that I got to work on my environment
2. the gazebo_precision_landing.py script that was written in the course

Please let me know if one of these work for you

[Huang]

Thank you, please check your e-mail sent to you

[caleberg]

Thank you! That helped clear things up

You had an indentation problem with your script. Your ‘send_local_ned_velocity’ and ‘send_land_message’ were both indented into your ‘arm_and_takeoff’ function. If you remove a tab from every line for ‘send_local_ned_velocity’ and ‘send_land_message’ it will work for you!

It should look like this.

[Huang]

Why am I unable to land at the top of UAVs ArucoMark, As if not precise positioning?
I was teaching reference gazebo_precision_landing_array

[Huang]

During operation, the frame can not be located will be out of range

[Huang]

I still do not understand，Corners[0] point is defined by how？

x_sum = corners[0][0][0][0]+corners[0][0][1][0]+corners[0][0][2][0]+corners[0][0][3][0]

y_sum = corners[0][0][0][1]+corners[0][0][1][1]+corners[0][0][2][1]+corners[0][0][3][1]

(x,y)|---------------------|(x,y)=(0,1)?
*****|---------------------|
*****|------Aruco-------|
*****|------Marker.-----|
*****|---------------------|
(x,y)|---------------------|(x,y)

[caleberg]

Hi there @Huang

It appears you have an error with one of your array variables inside of your “try” block of the msg_receiver() function. This likely short circuits the code, skipping the precision landing on aruco 72, and triggers the except block which prints ‘Target likely not found’. If you can’t see anything wrong, feel free to email me your script again and I’d be happy to take a look!

[caleberg]

This is what the corners variable looks like:

It has four dimensions. Notice there are 4 open square [ brackers. Inside the variable is the x and y coordinates of pixel location of every aruco marker found in the frame. You’ll notice in the above picture, there are two arucos in the gazebo world, so there are 2 sets of 4 x,y pixel coordinates.

Look at the first embedded array with [304., 276.],

To access the x coordinate 304, you’d type x=corners[0][0][0][0]

If you wanted to access the y coordinate, 276, you’d type y=corners[0][0][0][1] cause it is the index 1 in the array [304,276].

Now let’s say you wanted to access the embedded array with [386. , 236.]
To access the x coordinate 386, you’d type x=corners[1][0][0][0]

Hope that clears things up

[Huang]

hello E-mail sent in the past