Commit ed7380d9 authored by CactusSoyeux's avatar CactusSoyeux
Browse files

Ajout modification vitesse rotation drone et camera et signal batterie faible (à finir)

parent 869217dc
<launch>
<node name="anafi_base" pkg="anafi_base" type="stream_node.py" output="screen"/>
<node name="anafi_control" pkg="anafi_control" type="control_node.py" output="screen"/>
<node name="joy" pkg="joy" type="joy_node" output="screen"/>
<node name="anafi_base" pkg="anafi_base" type="anafi_node.py" output="screen"/>
</launch>
\ No newline at end of file
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#Olympe imports
#Generic imports
import os
import queue
import threading
......@@ -15,11 +15,18 @@ from anafi_base.srv import TakeOffLand, TakeOffLandResponse
from cv_bridge import CvBridge
from geometry_msgs.msg import Quaternion, Twist, Vector3
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged
#Olympe imports
from olympe.messages.ardrone3.Piloting import Landing, TakeOff
from olympe.messages.ardrone3.PilotingState import FlyingStateChanged
from olympe.messages.ardrone3.SpeedSettings import MaxRotationSpeed
from olympe.messages.ardrone3.SpeedSettingsState import MaxRotationSpeedChanged
from olympe.messages.gimbal import set_max_speed
from olympe.messages.gimbal import max_speed
from olympe.video.pdraw import Pdraw, PdrawState
#from olympe.drone import start_piloting, piloting_pcmd, stop_piloting
#Pub types
from sensor_msgs.msg import Image, NavSatFix
......@@ -51,12 +58,14 @@ class Anafi:
except Exception as e:
print("Drone connection failed: %s" %e)
#Pdraw init
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
self.timeout = 5
#Initialise Publishers
self.anafi_img = None #cv2.imread(IMG_PATH)
self.bat_perc = None
self.bat_perc = 100
self.gnd_dist = None
self.air_speed = None
self.quat = Quaternion(0 , 1, 0, 0)
......@@ -70,6 +79,35 @@ class Anafi:
self.last_vel_time = rospy.get_time()
self.time_thrs = 2
#Olympe settings modifications
max_range_rot = 200
max_range_gimbal_pitch = 180
#Rotation Speed
try:
max_range_rot = self.drone.get_state(MaxRotationSpeedChanged)['max']
self.drone(MaxRotationSpeed(max_range_rot)).wait().success()
print('Setting max rotation speed to max')
except:
print('Failed to change max rotation speed')
#Gimbal pitch speed
try:
self.drone(olympe.messages.gimbal.set_max_speed(gimbal_id=0, roll = 10, pitch=max_range_gimbal_pitch, yaw = 0)).wait().success()
print('Setting max gimbal pitch speed to max')
except :
print('Failed to change max gimbal pitch speed')
'''
for i in self.drone.get_state(olympe.messages.gimbal.max_speed).values():
for k,v in i.items():
if (k == 'max_bound_pitch'):
max_range_gimbal = v
self.drone(olympe.messages.gimbal.set_max_speed(gimbal_id=0, roll = 10, pitch=max_range_gimbal, yaw = 0)).wait().success()
print('Setting max gimbal pitch speed to max')
'''
#Pdraw
self.pdraw = Pdraw()
self.pdraw.set_callbacks(
......@@ -95,6 +133,15 @@ class Anafi:
#ROS Services init
s = rospy.Service("take_off_land", TakeOffLand, self.handle_take_off_land)
def takeOffLandService(self, str):
rospy.wait_for_service("take_off_land")
try:
take_off_land = rospy.ServiceProxy("take_off_land", TakeOffLand)
resp = take_off_land(str)
return resp.str
except rospy.ServiceException as e:
print("Service call failed: %s" %e)
def handle_take_off_land(self, req):
if req.str == "take_off":
#print("Drone Take Off")
......@@ -180,7 +227,6 @@ class Anafi:
#Pdraw
self.pdraw.play(url=DRONE_STREAM)
self.timeout = 2
assert self.pdraw.wait(PdrawState.Playing, timeout=self.timeout)
#Start piloting
......@@ -204,7 +250,7 @@ class Anafi:
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')):
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)
......@@ -215,13 +261,25 @@ class Anafi:
else :
pass
if self.bat_perc < 10:
print("Battery low: Please Land")
'''
try:
print("Battery low: Call Land")
print(self.takeOffLandService("land"))
except :
print("Service call failed")
'''
self.loop_rate.sleep()
#Stop piloting
self.drone.stop_piloting()
def stop(self):
assert self.drone(Landing()).wait().success()
#assert self.drone(Landing()).wait().success()
assert self.pdraw.wait(PdrawState.Closed, timeout=self.timeout)
self.pdraw.close()
self.pdraw.dispose()
......
......@@ -40,6 +40,7 @@ class JoyCommand:
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=2)
self.cam_pub = rospy.Publisher('cmd_cam', Float32, queue_size=2)
#ROS Subscriber init
rospy.Subscriber("/joy", Joy, self.joyCallback)
def takeOffLandService(self, str):
......@@ -58,7 +59,7 @@ class JoyCommand:
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
self.vel.angular.z = - joy.axes[6] * self.sensivity
self.vel.angular.z = - joy.axes[0] * self.sensivity
#Modify command sensivity
if joy.axes[0] == -1 and self.sensivity < 2:
......@@ -68,10 +69,10 @@ class JoyCommand:
self.sensivity -= 0.1
#Modify camera angle
if joy.axes[1] == 1 and self.cam < 0:
if joy.axes[7] == 1 and self.cam < 0:
self.cam += 45
if joy.axes[1] == -1 and self.cam > -90:
if joy.axes[7] == -1 and self.cam > -90:
self.cam -= 45
#Take off and Land services
......
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