Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
executable file 216 lines (177 sloc) 6.4 KB
#!/usr/bin/env python
# vim:set ts=4 sw=4 et:
#
# Copyright 2014 Vladimir Ermakov.
#
# This file is part of the mavros package and subject to the license terms
# in the top-level LICENSE file of the mavros repository.
# https://github.com/mavlink/mavros/tree/master/LICENSE.md
from __future__ import print_function
import argparse
import rospy
import sys, select, termios, tty
from tf.transformations import quaternion_from_euler
from sensor_msgs.msg import Joy
from std_msgs.msg import Header, Float64, Empty
from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3, Quaternion, Point
from mavros.msg import OverrideRCIn
from mavros.srv import CommandBool
from mavros.srv import CommandTOL
from mavros.srv import SetMode
def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def arm(args, state):
try:
arming_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/arming", CommandBool)
ret = arming_cl(value=state)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
rospy.loginfo("ARM Request failed.")
else:
rospy.loginfo("ARM Request success.")
def takeoff(args):
try:
takeoff_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/takeoff", CommandTOL)
ret = takeoff_cl(altitude=2, latitude=0, longitude=0, min_pitch=0, yaw=0)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
rospy.loginfo("TAKEOFF Request failed.")
else:
rospy.loginfo("TAKEOFF Request success.")
def land(args):
try:
land_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/land", CommandTOL)
ret = land_cl(altitude=0, latitude=0, longitude=0, min_pitch=0, yaw=0)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
rospy.loginfo("LAND Request failed.")
else:
rospy.loginfo("LAND Request success.")
def guided(args, state):
try:
guided_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/guided_enable", CommandBool)
ret = guided_cl(value=state)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
rospy.loginfo("GUIDED Request failed.")
else:
rospy.loginfo("GUIDED Request success.")
def set_mode(args, mode):
try:
setmode_cl = rospy.ServiceProxy(args.mavros_ns + "/set_mode", SetMode)
ret = setmode_cl(base_mode=0, custom_mode=mode)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
rospy.loginfo("SET MODE Request failed.")
else:
rospy.loginfo("SET MODE Request success.")
def rc_override_control(args):
rospy.init_node("mavteleop")
rospy.loginfo("MAV-Teleop: RC Override control type.")
override_pub = rospy.Publisher(args.mavros_ns + "/rc/override_joy", OverrideRCIn, queue_size=10)
pos_pub = rospy.Publisher(args.mavros_ns + "/setpoint_position/local", PoseStamped, queue_size=10)
test_video_lag_pub = rospy.Publisher("/test_video_lag", Empty, queue_size=1)
throttle_ch = 1000
while(1):
roll = 1500
pitch = 1500
key = getKey()
#rospy.loginfo("Key: %s", key)
if key == 'a':
arm(args, True)
elif key == 'd':
arm(args, False)
elif key == 't':
takeoff(args)
elif key == 'l':
land(args)
elif key == 'z':
pose = PoseStamped(header=Header(stamp=rospy.get_rostime()))
pose.pose.position = Point(x=0.0, y=0.0, z=2.3)
q = quaternion_from_euler(0, 0, 0.0)
pose.pose.orientation = Quaternion(*q)
pos_pub.publish(pose)
elif key == 'x':
pose = PoseStamped(header=Header(stamp=rospy.get_rostime()))
pose.pose.position = Point(x=0.0, y=0.0, z=0.3)
q = quaternion_from_euler(0, 0, 0.0)
pose.pose.orientation = Quaternion(*q)
pos_pub.publish(pose)
elif key == 'q':
msg = Empty()
test_video_lag_pub.publish(msg)
rospy.loginfo("Test video lag")
elif key == 'G':
set_mode(args, "GUIDED")
elif key == 'H':
set_mode(args, "ALT_HOLD")
elif key == 'L':
set_mode(args, "LOITER")
elif key == 'r': #UP
throttle_ch+=10
elif key == 'f': #FIX
throttle_ch=1500
elif key == 'v': #DOWN
throttle_ch-=10
#elif key == 'j': #LEFT
# roll=1600
#elif key == 'l': #RIGHT
# roll=1400
#elif key == 'i': #FORWARD
# pitch=1600
#elif key == 'k': #BACKWARD
# pitch=1400
elif key == '0': #FIX
throttle_ch=1000
elif key == '1':
throttle_ch=1100
elif key == '2':
throttle_ch=1200
elif key == '3':
throttle_ch=1300
elif key == '4':
throttle_ch=1400
elif key == '5':
throttle_ch=1500
elif key == '6':
throttle_ch=1600
if (key == '\x03'):
break
rc = OverrideRCIn()
rc.channels[0] = roll
rc.channels[1] = pitch
rc.channels[2] = throttle_ch
rc.channels[3] = 1500 #yaw
rc.channels[4] = 1000
rc.channels[5] = 1000
rc.channels[6] = 1000
rc.channels[7] = 1000
#rospy.loginfo("Channels: %d %d %d %d", rc.channels[0], rc.channels[1],rc.channels[2] , rc.channels[3])
override_pub.publish(rc)
def main():
parser = argparse.ArgumentParser(description="Teleoperation script for Copter-UAV")
parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default="/mavros")
parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
mode_group = parser.add_mutually_exclusive_group(required=True)
mode_group.add_argument('-rc', '--rc-override', action='store_true', help="use rc override control type")
mode_group.add_argument('-att', '--sp-attitude', action='store_true', help="use attitude setpoint control type")
mode_group.add_argument('-vel', '--sp-velocity', action='store_true', help="use velocity setpoint control type")
mode_group.add_argument('-pos', '--sp-position', action='store_true', help="use position setpoint control type")
args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])
if args.rc_override:
rc_override_control(args)
if __name__ == '__main__':
settings = termios.tcgetattr(sys.stdin)
main()
You can’t perform that action at this time.