0
$\begingroup$

I'm simulating Iris quad sing px4+mavros+gazebo. I'm trying to get my drone to takeoff using python code. When I publish height using mavros/setpoint_position/pose, it takes off. But I have one problem. Whenever my other code finishes its job, this takeoff code takes over and sends it back. To eliminate the use of setpoints for takeoff, I'm trying to use the takeoff service. But it's not working as expected. Ubuntu 18.04 Python 2.7 Px4

#!/usr/bin/env python

import rospy
import mavros
import time

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import *
from mavros_msgs.srv import *


class Services():
    
    def __init__(self):
        self.local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
        self.current_state = State()

    def state_cb(state):
        global current_state
        current_state = state

    def arm(self):
        rospy.wait_for_service('/mavros/cmd/arming')
        try:
            arm = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
            arm(True)
        except rospy.ServiceException, e:
            print "Service arm call failed: %s"%e
    
    def offboard(self):
        rospy.wait_for_service('/mavros/set_mode')
        try:
            flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode) 
            flightModeService(custom_mode='OFFBOARD')
        except rospy.ServiceException, e:
            print "Service set_mode call failed: %s. Offboard Mode could not be set."%e

    def takeoff(self, height=3):
        rospy.wait_for_service('mavros/cmd/takeoff')
        try:
            takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', CommandTOL)
            takeoffService(altitude = height)
        except rospy.ServiceException, e:
            print "takeoff call failed: %s"%e

    def land(self):
        rospy.wait_for_service('/mavros/cmd/land')
        try:
            l = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
            l(altitude = 0)
        except rospy.ServiceException, e:
            print "Service land call failed: %s"%e



#!/usr/bin/env python

import rospy
import mavros
import time

from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import *
from mavros_msgs.srv import *
from services import Services

current_state = State() 

def state_cb(state):
    global current_state
    current_state = state

def command():
    service = Services()
    state_sub = rospy.Subscriber('/mavros/state', State, state_cb)
    rospy.init_node('mavros_takeoff_python')
    rate = rospy.Rate(10)

    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

    for i in range(100):
        service.local_pos_pub.publish(pose)
        rate.sleep()

    prev_state = current_state
    while not current_state.connected:
        rate.sleep()
    
    last_request = rospy.get_rostime()
    while not rospy.is_shutdown():
        now = rospy.get_rostime()
        if current_state.mode != "OFFBOARD" and (now - last_request > rospy.Duration(5.)):
            service.offboard()
            last_request = now 
        else:
            if not current_state.armed and (now - last_request > rospy.Duration(5.)):
               service.arm()
               last_request = now 

        # older versions of PX4 always return success==True, so better to check Status instead
        if prev_state.armed != current_state.armed:
            rospy.loginfo("Vehicle armed: %r" % current_state.armed)
        if prev_state.mode != current_state.mode: 
            rospy.loginfo("Current mode: %s" % current_state.mode)
        prev_state = current_state
        service.takeoff(3)
        rate.sleep()

        


if __name__ == '__main__':
    try:
        command()
    except rospy.ROSInterruptException:
        pass

This is the output I get when using takeoff service [INFO] [1629208144.895857, 700.624000]: Current mode: AUTO.TAKEOFF. This output is for position pub [INFO] [1629206964.285276, 93.652000]: Current mode: OFFBOARD [INFO] [1629206969.289668, 98.652000]: Vehicle armed: True Advance thanks for the help.

Edit 1 - I have another motion planning codes which should takeover after this code gets my drone to takeoff. So when I publish velocity setpoints or other setpoints, this code should not interfere with the other code. So I think if I use the takeoff service instead of setpoint_position, there won't be any problem. But the drone is not taking off when I use the takeoff service.

Presently I'm using the following code which interferes when i publish other setpoints.

def takeoff(self, height=3):
    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 3
    try:
        self.local_pos_pub.publish(pose)
    except rospy.ServiceException, e:
        print "takeoff call failed: %s"%e
$\endgroup$
2
  • $\begingroup$ I don't understand what you're doing or what exactly the issue is. You say But I have one problem. Whenever my other code finishes its job this takeoff code takes over and sends it back. What does 'send it back' mean here? Are you using the takeoff code to do something other than takeoffs? it's not working as expected. Could you please edit your question to include more information about what you're doing, what you expected to see, and what you're actually seeing? $\endgroup$
    – Chuck
    Aug 18 '21 at 13:12
  • $\begingroup$ I have edited the question. $\endgroup$
    – Sankeerth
    Aug 19 '21 at 14:17

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.