Hello. I’m very new to Mavros and Ros. Ive been using PX4 with the IRIS quadcopter. Ive been trying to enter offboard mode by publishing setpoints but for some reason, the drone fails to enter offboard mode and won’t takeoff. Any help is appreciated.
I am using ROS Melodic
Here is my code:
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Point, PoseStamped
from mavros_msgs.msg import *
from mavros_msgs.srv import *
class fcuModes:
def __init__(self):
pass
def setTakeoff(self):
rospy.wait_for_service('mavros/cmd/takeoff')
try:
takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', CommandTOL)
takeoffService(altitude = 3)
print ("Taking off")
except rospy.ServiceException as e:
print ("Service takeoff call failed: %s"%e)
def setArm(Self):
rospy.wait_for_service('mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
armService(True)
print ("Arming")
except rospy.ServiceException as e:
print ("Service arming call failed: %s"%e)
def setDisarm(self):
rospy.wait_for_service('mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
armService(False)
print ("Disarming")
except rospy.ServiceException as e:
print ("Service disarming call failed: %s"%e)
def setOffboardMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', SetMode)
flightModeService(custom_mode='OFFBOARD')
print ("Setting OFFBOARD mode")
except rospy.ServiceException as e:
print ("service set_mode call failed: %s. Offboard Mode could not be set."%e)
class Controller:
def __init__(self):
self.state = State()
def stateCb(self, msg):
self.state = msg
def main():
rospy.init_node('offb_test', anonymous=True)
modes = fcuModes()
cnt = Controller()
rate = rospy.Rate(20.0)
local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.Subscriber('mavros/state', State, cnt.stateCb)
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 3
# Sending a few setpoint messages
for i in range(100):
local_pos_pub.publish(pose)
rate.sleep()
print ("Setpoints published")
# activate OFFBOARD mode
modes.setOffboardMode()
# Make sure the drone is armed
while not cnt.state.armed:
modes.setArm()
rate.sleep()
print ("Armed fully")
# Takeoff to 3m
modes.setTakeoff()
rate.sleep()
print ("Taking off")
# ROS main loop
while not rospy.is_shutdown():
local_pos_pub.publish(pose)
rate.sleep()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
Here is my rostopic echo
It arms, but it wont enter offboard mode and it wont take-off. I dont know what im doing wrong.