Hey guys im stuck on a problem im running the sitl precision landing script and evrything goes good until i run rosrun gazebo_drone gazebo_precision_landing.py vehicle arms takesoff then i get this error. Look over the script many times and cant find the solutuion. And under is my code. Thank you
rosrun gazebo_drone gazebo_precision_landing.py
vehicle is now armable
Waiting for drone to ender GUIDED mode
Vehicle now in GUIDED mode
Waiting for vehicle to become armed
Now Armed
Current Altitude: 0
Current Altitude: 0
Current Altitude: 0
Current Altitude: 0
Current Altitude: 2
Current Altitude: 3
Current Altitude: 3
Target altitude reached
Traceback (most recent call last):
File “/home/lorenzo/courseRoot/catkin_ws/src/gazebo_drone/scripts/gazebo_precision_landing.py”, line 201, in
send_local_ned_velocity(velocity,0,0)
NameError: name ‘send_local_ned_velocity’ is not defined
#!/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(‘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 ##CM/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 ##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 INTRISICS##
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##
time_last=0
time_to_wait = .1 ##100ms
###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 ender GUIDED mode'
time.sleep(1)
print 'Vehicle now in GUIDED mode'
vehicle.armed = True
while vehicle.armed ==False:
print 'Waiting for vehicle to become armed'
time.sleep(1)
print('Now Armed')
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]) ###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)
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(1)
subscriber()
except rospy.ROSInterruptException:
pass
