Commit 57083641 authored by CactusSoyeux's avatar CactusSoyeux
Browse files

change_node debug and add of joy

parent 03bd0ef9
{
"configurations": [
{
"browse": {
"databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/opt/ros/noetic/include/**",
"/usr/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}
\ No newline at end of file
{
"python.autoComplete.extraPaths": [
"/opt/ros/noetic/lib/python3/dist-packages"
],
"python.analysis.extraPaths": [
"/opt/ros/noetic/lib/python3/dist-packages"
]
}
\ No newline at end of file
### ROS Olympe interface for Parrot Anafi drone
# ROS Olympe interface for Parrot Anafi drone
## anafi_base
Package that contains a node that read the drone video and metadata stream and publish relevant infoamtions:
Inferface package between ROS and Olympe
### anafi_node
Node that takes velocity messages and services as input and allows to fly the drone.
It also publishes some informations for the user or other nodes:
* Image stream
* GPS
* Battery level
* State
* Orientation
## anafi_navigation
Package that takes waypoints or joystick info to convert them into cmd_vel messages.
## anafi_controll
Interface that convert ROS services and cmd_vel in Olympe compatible messages. It also allows the user to change the camera orientation.
\ No newline at end of file
## anafi_control
### control_node
Takes /joy messages as inputs and convert them into velocity messages and ROS services.
### change_node
Allows the user to switch between joystick and waypoint control.
\ No newline at end of file
<launch>
<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_change" pkg="anafi_control" type="change_node.py" output="screen"/>
<node name="anafi_control" pkg="anafi_control" type="control_node.py" output="screen"/>
<node name="anafi_base" pkg="anafi_base" type="anafi_node.py" output="screen"/>
</launch>
\ No newline at end of file
......@@ -10,10 +10,10 @@ import olympe
#ROS imports
import rospy
from cv_bridge import CvBridge
#Services types
from anafi_base.srv import TakeOffLand, TakeOffLandResponse
from cv_bridge import CvBridge
from geometry_msgs.msg import Quaternion, Twist, Vector3
#Olympe imports
from olympe.messages.ardrone3.Piloting import Landing, TakeOff
......@@ -27,20 +27,17 @@ from olympe.messages.gimbal import max_speed
from olympe.video.pdraw import Pdraw, PdrawState
#Pub types
from sensor_msgs.msg import Image, NavSatFix
from std_msgs.msg import Bool, Float32, Int8, String
#Sub types
from geometry_msgs.msg import Quaternion, Twist, 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 Anafi:
......@@ -64,7 +61,7 @@ class Anafi:
self.timeout = 5
#Initialise Publishers
self.anafi_img = None #cv2.imread(IMG_PATH)
self.anafi_img = None
self.bat_perc = 100
self.gnd_dist = None
self.air_speed = None
......@@ -233,7 +230,7 @@ class Anafi:
self.drone.start_piloting()
#ROS
rospy.loginfo("ROS Anafi Start")
rospy.loginfo("ROS Anafi base: Start")
while not rospy.is_shutdown():
#ROS Publishers
if self.anafi_img is not None:
......@@ -284,6 +281,7 @@ class Anafi:
self.pdraw.close()
self.pdraw.dispose()
self.drone.disconnect()
rospy.loginfo("ROS Anafi base: End")
def anafi_com():
anafi = Anafi()
......
......@@ -13,17 +13,19 @@ class Change:
#ROS Node init
rospy.init_node('anafi_change_control', anonymous=True)
self.br = CvBridge()
self.loop_rate = rospy.Rate(10)
self.loop_rate = rospy.Rate(1)
#ROS Publishers init
self.vel_pub = rospy.Publisher('ctrl_mode', String, queue_size=2)
self.change_pub = rospy.Publisher('ctrl_mode', String, queue_size=2, latch=True)
def start(self):
#ROS
rospy.loginfo("ROS Change control: Start")
self.change_pub.publish('joy')
while not rospy.is_shutdown():
pass
self.loop_rate.sleep()
def stop(self):
......
......@@ -2,23 +2,22 @@
from __future__ import print_function
#Olympe and basics imports
import olympe
#ROS imports
import rospy
from cv_bridge import CvBridge
#Services types
from anafi_control.srv import *
from cv_bridge import CvBridge
from geometry_msgs.msg import Twist
#Pub types
from std_msgs.msg import Float32, String
from geometry_msgs.msg import Twist
#Sub types
from sensor_msgs.msg import Joy
from std_msgs.msg import Float32
#Drone OS param
olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
#Control mode
class JoyCommand:
......@@ -31,6 +30,7 @@ class JoyCommand:
#Initialise Publishers
self.vel = Twist()
self.cam = 0
self.ctrl = 'joy'
#Other variables
self.TO = False
......@@ -41,7 +41,8 @@ class JoyCommand:
self.cam_pub = rospy.Publisher('cmd_cam', Float32, queue_size=2)
#ROS Subscriber init
rospy.Subscriber("/joy", Joy, self.joyCallback)
rospy.Subscriber("/joy", Joy, self.joy_cb)
rospy.Subscriber("/ctrl_mode", String, self.ctrl_cb)
def takeOffLandService(self, str):
rospy.wait_for_service("take_off_land")
......@@ -52,9 +53,12 @@ class JoyCommand:
except rospy.ServiceException as e:
print("Service call failed: %s" %e)
def joyCallback(self, joy):
def ctrl_cb(self, mode):
self.ctrl = mode
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]:
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
......@@ -91,26 +95,30 @@ class JoyCommand:
print(self.takeOffLandService("land"))
except :
print("Service call failed")
else:
elif (self.ctrl == 'joy'):
self.vel = Twist()
else:
pass
def start(self):
#ROS
rospy.loginfo("ROS Control Start")
rospy.loginfo("ROS Control node: Start")
while not rospy.is_shutdown():
if self.ctrl == 'joy':
#Publishers
if self.vel is not None:
self.vel_pub.publish(self.vel)
#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)
if self.cam is not None:
self.cam_pub.publish(self.cam)
self.loop_rate.sleep()
def stop(self):
pass
rospy.loginfo("ROS Control node: End")
if __name__ == "__main__":
joy_cmd = JoyCommand()
......
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