Калибровка движения Ardrone на основе расстояния в футах
Я давно работаю над ARDrone. Есть ли способ автоматизировать беспилотный летательный аппарат на основе измерений стоп?
Допустим, мне нужно летать на дроне всего в 6 футах от земли. Как я могу это сделать? У меня есть некоторые ростопики. Как использовать их, чтобы летать на дроне только на 6 футов?
#flyup
rostopic : /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 1.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}'
Я использую цикл для реализации одного и того же действия пару раз. Как я могу откалибровать его до 6 футов с ростопами?
Пожалуйста, проверьте код.
#!/usr/bin/env python
#import library ros
import rospy
import time
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from std_msgs.msg import Empty
from ardrone_autonomy.msg import Navdata
#import class status untuk menentukan status ddari quadcopter
#from drone_status import DroneStatus ` 1`
#COMMAND_PERIOD = 1000
class AutonomousFlight():
def __init__(self):
self.status = ""
rospy.init_node('forward', anonymous=False)
self.rate = rospy.Rate(1)
self.pubTakeoff = rospy.Publisher("ardrone/takeoff",Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land",Empty, queue_size=10)
self.pubCommand = rospy.Publisher('cmd_vel',Twist, queue_size=10)
self.command = Twist()
#self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SendCommand)
self.state_change_time = rospy.Time.now()
rospy.on_shutdown(self.SendLand)
def TakeOff(self):
self.pubTakeoff.publish(Empty())
self.rate.sleep()
def SendLand(self):
self.pubLand.publish(Empty())
def SetCommand(self, linear_x, linear_y, linear_z, angular_x, angular_y, angular_z):
self.command.linear.x = linear_x
self.command.linear.y = linear_y
self.command.linear.z = linear_z
self.command.angular.x = angular_x
self.command.angular.y = angular_y
self.command.angular.z = angular_z
self.pubCommand.publish(self.command)
self.rate.sleep()
if __name__ == '__main__':
try:
i = 0
distance = 0
uav = AutonomousFlight()
while not rospy.is_shutdown():
uav.TakeOff()
if i <=3 :
uav.SetCommand(0,0,1,0,0,0)
i+=1
elif i<=4:
uav.SetCommand(0,0,0,0,0,0)
i+=1
else:
break
uav.SendLand()
except rospy.ROSInterruptException:
pass
Из вышеприведенного кода, Drone просто взлетает и взлетает на 6 футов... как я могу это сделать?
С уважением, Катир