Commit e7071b1e authored by CactusSoyeux's avatar CactusSoyeux
Browse files

Last commit remote anafi

parent 57083641
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#!/usr/bin/env python
#Generic imports
import os
......@@ -24,6 +24,7 @@ from olympe.messages.ardrone3.SpeedSettingsState import MaxRotationSpeedChanged
from olympe.messages.gimbal import set_max_speed
from olympe.messages.gimbal import max_speed
from olympe.messages.gimbal import max_speed
from olympe.video.pdraw import Pdraw, PdrawState
......@@ -51,10 +52,16 @@ class Anafi:
# Create the olympe.Drone object from its IP address
try:
self.drone = olympe.Drone(DRONE_IP)
self.drone.connect()
except Exception as e:
print("Drone connection failed: %s" %e)
while not self.drone.connection_state():
try:
print("Trying to connect to the drone....")
self.drone.connect(timeout = 10)
except Exception as e:
print("Drone connection failed: %s" % e)
#Pdraw init
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
......@@ -230,47 +237,65 @@ class Anafi:
self.drone.start_piloting()
#ROS
rospy.loginfo("ROS Anafi base: Start")
while not rospy.is_shutdown():
#ROS Publishers
if self.anafi_img is not None:
self.img_pub.publish(self.br.cv2_to_imgmsg(self.anafi_img, "rgb8"))
#Not None verified in the frame call back
self.bat_pub.publish(self.bat_perc)
self.gnd_dist_pub.publish(self.gnd_dist)
self.air_speed_pub.publish(self.air_speed)
self.quat_pub.publish(self.quat)
self.speed_pub.publish(self.speed)
self.state_pub.publish(self.state)
self.sat_pub.publish(self.sat)
self.gps_pub.publish(self.gps)
#Convert cmd_vel in drone mouvement
if (self.vel != Twist()) & ((self.last_vel_time - rospy.get_time()) <= self.time_thrs) & ((self.state == 'FLYING')|(self.state == 'HOVERING') & (self.bat_perc > 10)):
self.drone.piloting(np.int32(self.vel.linear.y).item(),np.int32(self.vel.linear.x).item(),np.int32(self.vel.angular.z).item(),np.int32(self.vel.linear.z).item(),0)
elif ((self.state == 'HOVERING')|(self.state == 'FLYING')) & (self.vel == Twist()):
rospy.loginfo("ROS Anafi base: Start")
while self.drone.connection_state():
#ROS Publishers
if self.anafi_img is not None:
self.img_pub.publish(self.br.cv2_to_imgmsg(self.anafi_img, "rgb8"))
#Not None verified in the frame call back
self.bat_pub.publish(self.bat_perc)
self.gnd_dist_pub.publish(self.gnd_dist)
self.air_speed_pub.publish(self.air_speed)
self.quat_pub.publish(self.quat)
self.speed_pub.publish(self.speed)
self.state_pub.publish(self.state)
self.sat_pub.publish(self.sat)
self.gps_pub.publish(self.gps)
#Convert cmd_vel in drone mouvement
if (self.vel != Twist()) & ((self.last_vel_time - rospy.get_time()) <= self.time_thrs) & ((self.state == 'FLYING')|(self.state == 'HOVERING') & (self.bat_perc > 10)):
self.drone.piloting(np.int32(self.vel.linear.y).item(),np.int32(self.vel.linear.x).item(),np.int32(self.vel.angular.z).item(),np.int32(self.vel.linear.z).item(),0)
self.drone.piloting(0,0,0,0,0)
else :
elif ((self.state == 'HOVERING')|(self.state == 'FLYING')) & (self.vel == Twist()):
self.drone.piloting(0,0,0,0,0)
else :
pass
if self.bat_perc < 10:
pass
print("Battery low: Please Land")
if self.bat_perc < 10:
'''
try:
print("Battery low: Call Land")
print(self.takeOffLandService("land"))
except :
print("Service call failed")
'''
print(self.drone.get_state(olympe.messages.gimbal.attitude)[0]['pitch_absolute'])
self.loop_rate.sleep()
print("Battery low: Please Land")
rospy.loginfo("ROS Anafi base: End")
'''
while not self.drone.connection_state():
try:
print("Battery low: Call Land")
print(self.takeOffLandService("land"))
except :
print("Service call failed")
'''
self.loop_rate.sleep()
print("Trying to connect to the drone....")
self.drone.connect(timeout = 10)
except Exception as e:
print("Drone connection failed: %s" % e)
self.pdraw.close()
self.pdraw.dispose()
#Stop piloting
self.drone.stop_piloting()
......
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#!/usr/bin/env python
#ROS imports
import rospy
......
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#!/usr/bin/env python
from __future__ import print_function
......@@ -54,11 +54,11 @@ class JoyCommand:
print("Service call failed: %s" %e)
def ctrl_cb(self, mode):
self.ctrl = mode
self.ctrl = mode.data
def joy_cb(self, joy):
#Update Twist values if one of the DMS (Dead Man Switch) is triggered
if joy.buttons[4] or joy.buttons[5] and (self.ctrl == 'joy'):
if (joy.buttons[4] or joy.buttons[5]) and (self.ctrl == 'joy'):
self.vel.linear.x = joy.axes[4] * self.sensivity
self.vel.linear.y = - joy.axes[3] * self.sensivity
self.vel.linear.z = ((joy.axes[2]+1)/2 - (joy.axes[5]+1)/2) * self.sensivity
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment