Stl drone did not land on aruco marker


#1

Hi!

I do not know why the stl drone do not land on the aruco marker.

#!/usr/bin/env python

########import#########

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

#######variable########

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 ##cms/s

velocity=-.5 #m/s
takeoff_height=4 #m
########################
newimg_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 = [[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 ##100 ms
################FUNCTIONS###############
def arm_and_takeoff(targetHeight):
while vehicle.is_armable !=True:
print(‘Waiting for vehicle to become armable’)
time.sleep(1)
print(‘Vehicle is now armable’)

vehicle.mode = VehicleMode('GUIDED')

while vehicle.mode !='GUIDED':
    print('Waiting for drone to enter GUIDED flight mode')
    time.sleep(1)
print('Vehicle now in GUIDED mode. Have Fun!')

vehicle.armed = True
while vehicle.armed ==False:
    print('Waiting for vehicle to become armed.')
    time.sleep(1)
print('Look out! Virtual props are spinning!')

vehicle.simple_takeoff(targetHeight)

while True:
    print('Current Altitude: %d'%vehicle.location.global_relative_frame.alt)
    if vehicle.location.global_relative_frame.alt >=.95*targetHeight:
        break
    time.sleep(1)
print('Target altitude reached!')

return None

##Send velocity command to drone
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()

####responsible for precision landing
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()

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 in CM
                z = '{:.2f}'.format(tvec[2]) ### Zerror/distance between camera and aruco in CM

                y_sum=0
                x_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)
                ##putText(image, text_to_draw,position,font_face,fontScale,color,thickness)
                cv2.putText(np_data,marker_position,(10,50),0,.7,(255,0,0),thickness=2)
                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(5)
subscriber()
except rospy.ROSInterruptException:
pass


#2

@caleberg ?? :slight_smile:


#3

Your code looks good, so not sure. You can send me the code archived to dronedojoforum@gmail.com and I can try running it.

What are some of the marker positions reported at the apex and soon after the land command?
image

What if you try the velocity command using y rather than x, or different velocities?

send_local_ned_velocity(0,.5,0)

I’ve experimented using different gazebo parameters to stress the system, such as changing wind speed, which can push the drone off enough that it cannot find the aruco. You might want to verify it’s not high.


#4

Thanks @jax200 ! i will try :slight_smile:


#5

I am having the exact same issue so any solution would be very helpful! I tried reducing the wind which helped a bit but the drone still seems to just come straight down now without going toward the marker.


#6

Send the zipped code to the above address if you would like me to try it.

Jack


#7

@pattent Your code is identical to mine and performed as it should. Perhaps you changed another file it uses?

@Adel I looked at your code again and there are problems with it. For example, it is missing

velocityx = np.random.uniform((-0.5, 0.5)


#8

Thanks I will take a look. Not sure what other files I would’ve changed to affect it.


#9

if youre still having issues, try adding in parameters for a rangefinder, as seen in this link. Not sure what the default values are but after specifying them in the precision landing script it seemed to work https://ardupilot.org/dev/docs/using-sitl-for-ardupilot-testing.html


#10

@pattent nailed it.

Precision landing is configured to not perform if a rangefinder is not detected. By default it probably won’t be activated on the SITL vehicle.

So you have to enable it for the PL mavlink messages to work.

You can do this with MAVProxy
Check what the current rangefinder is with this:

param show RNGFND1_TYPE

Then set it with this

param set RNGFND1_TYPE 1

10 points @pattent


closed #11