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

This comment has been minimized.

Copy link

noob211 commented Jan 31, 2019

why drone doesn't take off?

@DiegoHerrera1890

This comment has been minimized.

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
You can’t perform that action at this time.