Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#!/usr/bin/env python
# ROS
import rospy
from mavros_msgs.msg import OverrideRCIn
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import CommandTOL
# System
import time
rospy.init_node('rosimple_node', anonymous = True)
# Set Mode
print("SET MODE")
rospy.wait_for_service('/mavros/set_mode')
try:
modeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
modeResponse = modeService(0, 'STABILIZE')
rospy.loginfo(modeResponse)
except rospy.ServiceException as e:
print("Service call failed: %s" %e)
# Arm
print("ARM")
rospy.wait_for_service('/mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
armResponse = armService(True)
rospy.loginfo(armResponse)
except rospy.ServiceException as e:
print("Service call failed: %s" %e)
time.sleep(3)
# TAKEOFF
print("TAKEOFF")
pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size = 10)
r = rospy.Rate(10) #10hz
msg = OverrideRCIn()
start = time.time()
exec_time = 4
flag = True
steer_channel = 0 # roll/steer movement
pitch_channel = 1 # pitch movement
throttle_channel = 2 # throttle up/down movement
yaw_channel = 3 # yaw movement
speed = 'SLOW'
if speed == 'SLOW':
msg.channels[throttle_channel] = 1558
elif speed == 'NORMAL':
msg.channels[throttle_channel] = 1565
elif speed == 'FAST':
msg.channels[throttle_channel] = 1570
direction = 'STRAIGHT'
if direction == 'STRAIGHT':
msg.channels[steer_channel] = 1385
elif direction == 'RIGHT':
msg.channels[steer_channel] = 1450
elif direction == 'LEFT':
msg.channels[steer_channel] = 1300
while not rospy.is_shutdown() and flag:
sample_time = time.time()
if ((sample_time - start) > exec_time):
flag = False
rospy.loginfo(msg)
pub.publish(msg)
r.sleep()
time.sleep(2)
# LAND
print("LAND")
msg.channels[steer_channel] = 1100
msg.channels[throttle_channel] = 1100
msg.channels[yaw_channel] = 0
rospy.loginfo(msg)
pub.publish(msg)
# Disarm
print("DISARM")
rospy.wait_for_service('/mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
armService(False)
except rospy.ServiceException as e:
print("Service call failed: %s" %e)
@noob211
Copy link

noob211 commented Jan 31, 2019

why drone doesn't take off?

@DiegoHerrera1890
Copy link

DiegoHerrera1890 commented Dec 2, 2019

How can I install MAVROS and how can I do a test?

why drone doesn't take off?

Did you fix it?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment