So, I have managed to make the drone recognize the aruco marker, slowly descend on the ground, but it doesn’t align with the target. I assume that it has something to do with the parameters and maybe something is not properly installed. The changes I have made are a different size of marker, and a different dictionary, but it is recognized by the drone.
Here is the code:
#!/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
from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal
from pymavlink import mavutil
from array import array
VARIABLES
vehicle = connect(‘127.0.0.1:14550’,wait_ready=True)
vehicle.parameters[‘PLND_ENABLED’]=1
vehicle.parameters[‘PLND_TYPE’]=1
vehicle.parameters[‘PLND_EST_TYPE’]=0
vehicle.parameters[‘LAND_SPEED’]=30
velocity=-.5
takeoff_height=10
newimg_pub = rospy.Publisher(’/webcam/image_new’, Image, queue_size=10)
id_to_find = 17
marker_size = 50
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_5X5_250)
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 = [[554.254691191187, 0.0, 320.5],[0.0, 554.254691191187, 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()
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)
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])
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
print('x_ang = ' + str(x_ang))
print('y_ang = ' + str(y_ang))
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(’/webcam/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
and a picture of the enviroment:
What do you suggest changing to make it work?