Commit 869217dc authored by CactusSoyeux's avatar CactusSoyeux
Browse files

fix conflits dans l'envoie des services TO / land

parent c036eb91
......@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
rospy
sensor_msgs
std_msgs
message_generation
actionlib_msgs
)
## System dependencies are found with CMake's conventions
......@@ -56,11 +58,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
add_service_files(
FILES
TakeOffLand.srv
)
## Generate actions in the 'action' folder
# add_action_files(
......@@ -70,10 +71,13 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# sensor_msgs# std_msgs
# )
generate_messages(
DEPENDENCIES
geometry_msgs
sensor_msgs
std_msgs
actionlib_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
......@@ -105,10 +109,10 @@ find_package(catkin REQUIRED COMPONENTS
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES anafi_base
# CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs
# DEPENDS system_lib
INCLUDE_DIRS include
LIBRARIES anafi_base
CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs
DEPENDS system_lib
)
###########
......
<launch>
<node name="anafi_base" pkg="anafi_base" type="anafi_stream_ros.py" output="screen"/>
<node name="anafi_base" pkg="anafi_base" type="stream_node.py" output="screen"/>
</launch>
\ No newline at end of file
......@@ -54,16 +54,22 @@
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
......
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
#Olympe imports
import os
import queue
import threading
import cv2
import olympe
import os
import numpy as np
from olympe.video.pdraw import Pdraw, PdrawState
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged
from olympe.messages.ardrone3.Piloting import TakeOff, Landing
import olympe
#ROS imports
import rospy
#Services types
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
from olympe.messages.ardrone3.Piloting import Landing, TakeOff
from olympe.messages.ardrone3.PilotingState import FlyingStateChanged
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
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
from std_msgs.msg import Bool, Float32, Int8, String
#Drone OS param
DRONE_IP = os.environ.get("DRONE_IP", "192.168.42.1")
......@@ -38,9 +39,18 @@ olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
class Anafi:
def __init__(self):
#ROS Node init
rospy.init_node('anafi_stream', anonymous=True)
self.br = CvBridge()
self.loop_rate = rospy.Rate(10)
# Create the olympe.Drone object from its IP address
self.drone = olympe.Drone(DRONE_IP)
self.drone.connect()
try:
self.drone = olympe.Drone(DRONE_IP)
self.drone.connect()
except Exception as e:
print("Drone connection failed: %s" %e)
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
......@@ -54,6 +64,11 @@ class Anafi:
self.state = None
self.sat = None
self.gps = NavSatFix()
#Drone variables
self.vel = Twist()
self.last_vel_time = rospy.get_time()
self.time_thrs = 2
#Pdraw
self.pdraw = Pdraw()
......@@ -62,11 +77,6 @@ class Anafi:
flush_raw_cb = self.flush_cb
)
#ROS Node init
rospy.init_node('anafi_stream', anonymous=True)
self.br = CvBridge()
self.loop_rate = rospy.Rate(10)
#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)
......@@ -80,6 +90,7 @@ class Anafi:
#ROS Subscribers init
rospy.Subscriber("/cmd_cam", Float32, self.camCallback)
rospy.Subscriber("/cmd_vel", Twist, self.velCallback)
#ROS Services init
s = rospy.Service("take_off_land", TakeOffLand, self.handle_take_off_land)
......@@ -87,27 +98,44 @@ class Anafi:
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")
try:
assert self.drone(TakeOff()>> FlyingStateChanged(state="hovering", _timeout=5)).wait().success()
return TakeOffLandResponse("ok_take_off")
except:
return TakeOffLandResponse("fail_take_off")
elif req.str == "land":
#print("Drone Land")
assert self.drone(Landing()).wait().success()
return TakeOffLandResponse("ok_land")
try:
assert self.drone(Landing()>> FlyingStateChanged(state="landed", _timeout=10)).wait().success()
return TakeOffLandResponse("ok_land")
except :
return TakeOffLandResponse("fail_land")
else :
#print("Error Take Off / Land")
#print("Error")
return TakeOffLandResponse("error")
def camCallback(self, cam):
val = np.float32(cam.data)
def camCallback(self, cam_msg):
val = np.float32(cam_msg.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 velCallback(self, vel_msg):
self.vel = vel_msg
#Update last time the drone received a cmd_vel msg
self.last_vel_time = rospy.get_time()
def yuv_frame_cb(self, yuv_frame):
#Stream MetaData
meta = yuv_frame.vmeta()
#Get stream MetaData is there is any received
try:
meta = yuv_frame.vmeta()
except:
meta[1] = None
if meta[1] is not None:
self.bat_perc = meta[1]["battery_percentage"]
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"])
......@@ -149,22 +177,17 @@ class Anafi:
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)
#Start piloting
self.drone.start_piloting()
#ROS
rospy.loginfo("ROS Start")
rospy.loginfo("ROS Anafi Start")
while not rospy.is_shutdown():
#ROS Publishers
if self.anafi_img is not None:
......@@ -180,18 +203,29 @@ class Anafi:
self.sat_pub.publish(self.sat)
self.gps_pub.publish(self.gps)
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"])
#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.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()):
self.drone.piloting(0,0,0,0,0)
else :
pass
self.loop_rate.sleep()
#Stop piloting
self.drone.stop_piloting()
def stop(self):
assert self.drone(Landing()).wait().success()
assert self.pdraw.wait(PdrawState.Closed, timeout=self.timeout)
#self.renderer.stop()
self.pdraw.close()
self.pdraw.dispose()
#self.drone.disconnect()
self.drone.disconnect()
def anafi_com():
anafi = Anafi()
......@@ -199,4 +233,4 @@ def anafi_com():
anafi.stop()
if __name__ == "__main__":
anafi_com()
\ No newline at end of file
anafi_com()
......@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
rospy
sensor_msgs
std_msgs
message_generation
actionlib_msgs
)
## System dependencies are found with CMake's conventions
......@@ -56,11 +58,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
add_service_files(
FILES
TakeOffLand.srv
)
## Generate actions in the 'action' folder
# add_action_files(
......@@ -70,10 +71,13 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# sensor_msgs# std_msgs
# )
generate_messages(
DEPENDENCIES
geometry_msgs
sensor_msgs
std_msgs
actionlib_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
......@@ -105,10 +109,10 @@ find_package(catkin REQUIRED COMPONENTS
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES anafi_control
# CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs
# DEPENDS system_lib
INCLUDE_DIRS include
LIBRARIES anafi_base
CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs
DEPENDS system_lib
)
###########
......
<launch>
<node name="anafi_control" pkg="anafi_control" type="anafi_ctrl.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
......@@ -54,17 +54,22 @@
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
......
#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
import olympe
import os
from olympe.messages.ardrone3.Piloting import TakeOff, moveBy, Landing
from olympe.messages.ardrone3.PilotingState import FlyingStateChanged
DRONE_IP = os.environ.get("DRONE_IP", "192.168.42.1")
def test_moveby():
drone = olympe.Drone(DRONE_IP)
drone.connect()
assert drone(
TakeOff()
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait().success()
assert drone(
moveBy(1, 0, 0, 0)
>> FlyingStateChanged(state="hovering", _timeout=5)
).wait().success()
assert drone(Landing()).wait().success()
drone.disconnect()
if __name__ == "__main__":
test_moveby()
\ No newline at end of file
......@@ -4,53 +4,37 @@ from __future__ import print_function
#Olympe and basics imports
import olympe
import os
import argparse
import queue
import threading
import cv2
import numpy as np
import sys
import time
#Olympe Flight imports
from olympe.messages.ardrone3.Piloting import TakeOff, moveBy, Landing
from olympe.messages.ardrone3.PilotingState import FlyingStateChanged
#ROS imports
import rospy
#Services types
from anafi_control.srv import *
from cv_bridge import CvBridge
from geometry_msgs.msg import Twist
#Pub types
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32
#Services types
from anafi_control.srv import *
#Drone OS param
#DRONE_IP = os.environ.get("DRONE_IP", "192.168.42.1")
olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
class JoyCommand:
def __init__(self):
# Create the olympe.Drone object from its IP address
#self.drone = olympe.Drone(DRONE_IP)
#self.drone.connect()
#ROS Node init
rospy.init_node('anafi_joy_control', anonymous=True)
self.br = CvBridge()
self.loop_rate = rospy.Rate(10)
#Initialise Publishers
self.vel = Twist()
self.cam = 0
self.sensivity = 1
#ROS Node init
rospy.init_node('anafi_joy_control', anonymous=True)
self.br = CvBridge()
self.loop_rate = rospy.Rate(10)
#Other variables
self.TO = False
self.sensivity = 100
#ROS Publishers init
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=2)
......@@ -65,16 +49,16 @@ class JoyCommand:
resp = take_off_land(str)
return resp.str
except rospy.ServiceException as e:
print("Serice call failed: %s" %e)
print("Service call failed: %s" %e)
def joyCallback(self, joy):
#Update Twist values if one of the DMS (Dead Man Switch) is triggered
if joy.buttons[4] or joy.buttons[5]:
self.vel.linear.x = joy.axes[4] * self.sensivity
self.vel.linear.y = joy.axes[6] * self.sensivity
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
self.vel.angular.z = joy.axes[3] * self.sensivity
self.vel.angular.z = - joy.axes[6] * self.sensivity
#Modify command sensivity
if joy.axes[0] == -1 and self.sensivity < 2:
......@@ -85,46 +69,49 @@ class JoyCommand:
#Modify camera angle
if joy.axes[1] == 1 and self.cam < 0:
self.cam += 15
self.cam += 45
if joy.axes[1] == -1 and self.cam > -90:
self.cam -= 15
self.cam -= 45
#Take off and Land services
if joy.buttons[7] and not joy.buttons[6]:
#Send Take off service
print(self.takeOffLandService("take_off"))
elif joy.buttons[6] and not joy.buttons[7]:
#Send Land service
print(self.takeOffLandService("land"))
if joy.buttons[7] and not joy.buttons[6] and not self.TO:
self.TO = True
try:
print("Call Take Off")
print(self.takeOffLandService("take_off"))
except :
print("Service call failed")
elif joy.buttons[6] and not joy.buttons[7] and self.TO:
self.TO = False
try:
print("Call Land")
print(self.takeOffLandService("land"))
except :
print("Service call failed")
else:
self.vel = Twist()
#Publishers
if self.vel is not None:
self.vel_pub.publish(self.vel)
if self.cam is not None:
self.cam_pub.publish(self.cam)
def start(self):
#Olympe
#self.drone = olympe.Drone(DRONE_IP)
#print("Drone created")
#self.drone.connect()
#print("Drone connected")
#ROS
rospy.loginfo("ROS Start")
rospy.loginfo("ROS Control Start")
while not rospy.is_shutdown():
#Publishers
if self.vel is not None:
self.vel_pub.publish(self.vel)
if self.cam is not None:
self.cam_pub.publish(self.cam)
self.loop_rate.sleep()
def stop(self):
#self.drone.disconnect()
pass
if __name__ == "__main__":
joy_cmd = JoyCommand()
joy_cmd.start()
joy_cmd.stop()
\ No newline at end of file
joy_cmd.stop()
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