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)
@alireza2033

This comment has been minimized.

Copy link

commented Jan 31, 2019

why drone doesn't take off?

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.