Commit 397dfd2f authored by CactusSoyeux's avatar CactusSoyeux
Browse files

fusion anafi_stream et anafi_fly en un seul package | suppression anciens...

fusion anafi_stream et anafi_fly en un seul package | suppression anciens fichier .py de anafi_base après renommage
parent f1e20c56
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#Olympe imports
import argparse
import queue
import threading
import cv2
import olympe
import os
import numpy as np
import sys
import time
from olympe.video.pdraw import Pdraw, PdrawState
#from olympe.video.renderer import PdrawRenderer
from olympe.messages.ardrone3.PilotingState import PositionChanged, GpsLocationChanged
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged,HomeChanged
#from olympe.messages.ardrone3.GPSState import NumberOfSatelliteChanged
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged
from olympe.messages.ardrone3.Piloting import TakeOff, Landing
#ROS imports
import rospy
......@@ -26,6 +21,9 @@ from sensor_msgs.msg import CompressedImage
from std_msgs.msg import String, Int8, Float32, Bool
from geometry_msgs.msg import Quaternion, Vector3
#Services types
from anafi_base.srv import TakeOffLand, TakeOffLandResponse
#Drone OS param
DRONE_IP = os.environ.get("DRONE_IP", "192.168.42.1")
DRONE_RTSP_PORT = os.environ.get("DRONE_RTSP_PORT", "554")
......@@ -37,8 +35,7 @@ IMG_PATH = os.path.join(MEDIA_PATH , 'output.jpg')
olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
class StreamingMeta:
class Anafi:
def __init__(self):
# Create the olympe.Drone object from its IP address
......@@ -81,6 +78,30 @@ class StreamingMeta:
self.sat_pub = rospy.Publisher('meta/sat', Bool, queue_size=2)
self.gps_pub = rospy.Publisher('meta/gps', NavSatFix, queue_size=2)
#ROS Subscribers init
rospy.Subscriber("/cmd_cam", Float32, self.camCallback)
#ROS Services init
s = rospy.Service("take_off_land", TakeOffLand, self.handle_take_off_land)
def handle_take_off_land(self, req):
if req.str == "take_off":
#print("Drone Take Off")
assert self.drone(TakeOff()).wait().success()
return TakeOffLandResponse("ok_take_off")
elif req.str == "land":
#print("Drone Land")
assert self.drone(Landing()).wait().success()
return TakeOffLandResponse("ok_land")
else :
#print("Error Take Off / Land")
return TakeOffLandResponse("error")
def camCallback(self, cam):
val = np.float32(cam.data)
pyval = val.item()
self.drone(olympe.messages.gimbal.set_target(gimbal_id=0, control_mode="position", yaw_frame_of_reference="relative", yaw=0.0, pitch_frame_of_reference="relative", pitch= pyval, roll_frame_of_reference="relative", roll=0.0)).wait()
def yuv_frame_cb(self, yuv_frame):
#Stream MetaData
......@@ -172,10 +193,10 @@ class StreamingMeta:
self.pdraw.dispose()
#self.drone.disconnect()
def streaming():
streaming_anafi = StreamingMeta()
streaming_anafi.start()
streaming_anafi.stop()
def anafi_com():
anafi = Anafi()
anafi.start()
anafi.stop()
if __name__ == "__main__":
streaming()
\ No newline at end of file
anafi_com()
\ No newline at end of file
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#Olympe imports
import argparse
import queue
import threading
import cv2
import olympe
import os
import numpy as np
import sys
import time
from olympe.video.pdraw import Pdraw, PdrawState
from olympe.video.renderer import PdrawRenderer
from olympe.messages.ardrone3.PilotingState import PositionChanged, GpsLocationChanged
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged,HomeChanged
from olympe.messages.ardrone3.GPSState import NumberOfSatelliteChanged
#ROS imports
import rospy
from cv_bridge import CvBridge
#Pub types
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
from std_msgs.msg import String, Int8, Float32
from geometry_msgs.msg import Quaternion, Vector3
#Drone OS param
DRONE_IP = os.environ.get("DRONE_IP", "192.168.42.1")
DRONE_RTSP_PORT = os.environ.get("DRONE_RTSP_PORT", "554")
DRONE_STREAM = f"rtsp://{DRONE_IP}:{DRONE_RTSP_PORT}/live"
#Can be deleted
MEDIA_PATH = '/home/anafi/catkin_ws/src/anafi_base/script/media'
IMG_PATH = os.path.join(MEDIA_PATH , 'output.jpg')
olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
class StreamingExample:
def __init__(self):
# Create the olympe.Drone object from its IP address
self.drone = olympe.Drone(DRONE_IP)
self.drone.connect()
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
#Initialise Publishers
self.anafi_img = None #cv2.imread(IMG_PATH)
self.bat_perc = None
self.gnd_dist = None
self.air_speed = None
self.quat = Quaternion(0 , 1, 0, 0)
self.speed = Vector3(0,0,0)
self.state = None
#Pdraw
self.pdraw = Pdraw()
self.pdraw.set_callbacks(
raw_cb = self.yuv_frame_cb,
flush_raw_cb = self.flush_cb
)
#ROS Node init
rospy.init_node('anafi_vid', anonymous=True)
self.br = CvBridge()
self.loop_rate = rospy.Rate(1)
#ROS Publishers init
self.img_pub = rospy.Publisher('camera/image', Image, queue_size=2)
self.bat_pub = rospy.Publisher('meta/battery', Int8, queue_size=2)
self.gnd_dist_pub = rospy.Publisher('meta/gnd_dist', Float32, queue_size=2)
self.air_speed_pub = rospy.Publisher('meta/air_speed', Float32, queue_size=2)
self.quat_pub = rospy.Publisher('meta/quat', Quaternion, queue_size=2)
self.speed_pub = rospy.Publisher('meta/speed', Vector3, queue_size=2)
self.state_pub = rospy.Publisher('meta/state', String, queue_size=2)
def yuv_frame_cb(self, yuv_frame):
#Stream MetaData
meta = yuv_frame.vmeta()
if meta[1] is not None:
self.bat_perc = meta[1]["battery_percentage"]
self.gnd_dist = meta[1]["ground_distance"]
self.air_speed = meta[1]["air_speed"]
self.quat = Quaternion(meta[1]["drone_quat"]["x"], meta[1]["drone_quat"]["y"], meta[1]["drone_quat"]["z"], meta[1]["drone_quat"]["w"])
self.speed = Vector3(meta[1]["speed"]["north"], meta[1]["speed"]["east"], meta[1]["speed"]["down"])
self.state = meta[1]["state"]
#print(meta[1]["drone_quat"]["x"])
#Convert image from YUV to CV2
cv2_cvt_color_flag = {
olympe.VDEF_I420: cv2.COLOR_YUV2BGR_I420,
olympe.VDEF_NV12: cv2.COLOR_YUV2BGR_NV12,
}[yuv_frame.format()]
self.anafi_img = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
#Add frame to queue
self.frame_queue.put_nowait(yuv_frame)
def flush_cb(self, stream):
if stream["vdef_format"] != olympe.VDEF_I420:
return True
with self.flush_queue_lock:
while not self.frame_queue.empty():
self.frame_queue.get_nowait().unref()
return True
def start(self):
#Olympe
self.drone = olympe.Drone(DRONE_IP)
print("Drone created")
self.drone.connect()
print("Drone connected")
self.drone(GPSFixStateChanged(_policy = 'wait'))
print("Drone GPSFixStateChanged")
#Pdraw
self.pdraw.play(url=DRONE_STREAM)
self.timeout = 2
assert self.pdraw.wait(PdrawState.Playing, timeout=self.timeout)
#Camera Angle
print("Drone Camera basic")
self.drone(olympe.messages.gimbal.set_target(gimbal_id=0, control_mode="position", yaw_frame_of_reference="relative", yaw=0.0, pitch_frame_of_reference="relative", pitch=-30, roll_frame_of_reference="relative", roll=0.0)).wait()
print("Drone Camera changed")
#ROS
rospy.loginfo("ROS 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.loop_rate.sleep()
#Olympe
#print("GPS Before Take Off: ", self.drone.get_state(HomeChanged))
#print("Number of GPS Satellite: ", self.drone.get_state(NumberOfSatelliteChanged)["numberOfSatellite"])
def stop(self):
assert self.pdraw.wait(PdrawState.Closed, timeout=self.timeout)
#self.renderer.stop()
self.pdraw.close()
self.pdraw.dispose()
self.drone.connect()
def streaming():
streaming_anafi = StreamingExample()
streaming_anafi.start()
streaming_anafi.stop()
if __name__ == "__main__":
streaming()
\ No newline at end of file
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