Offboard例程的python版本

因为python的库很丰富，有现成的解方程库，比如sympy。
自己写了一份python版本的自动起飞2m的程序。
假设自定义的包名为offb，在offb下新建scripts文件夹，将py文件放在这个文件夹下，文件名为take_off.py。
python代码为：

#!/usr/bin/env python

import rospy
from mavros_msgs.msg import State, PositionTarget
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import SetMode

def Intarget_local():
    set_target_local = PositionTarget()
    set_target_local.type_mask=0b100111111000
    set_target_local.coordinate_frame=1
    set_target_local.position.x=0
    set_target_local.position.y=0
    set_target_local.position.z=2
    return set_target_local

current_state = State()
def state_cb(msg):
    global current_state
    current_state = msg

current_pos= PoseStamped()
def  pos_cb(msg):
    global current_pos 
    current_pos = msg

if __name__ == "__main__":
    rospy.init_node("multi_uav")
    rospy.Subscriber("/mavros/state", State, state_cb, queue_size=10)
    rospy.Subscriber("/mavros/local_position/pose", PoseStamped, pos_cb,queue_size=10)
    armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
    setModeServer = rospy.ServiceProxy('/mavros/set_mode', SetMode)
    local_target_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)

    rate = rospy.Rate(20.0)

    Position_Target = PositionTarget()
    Position_Target = Intarget_local()
    
    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" :
            setModeServer(custom_mode='OFFBOARD')
            local_target_pub.publish(Position_Target)
            print("Offboard enabled")
        else:
            if not current_state.armed:
                armServer(True) 
                print("Vehicle armed")
            else:
                local_target_pub.publish(Position_Target)
                print("x:%f,y:%f,z:%f" %(current_pos.pose.position.x,current_pos.pose.position.y,current_pos.pose.position.z))
        
        rate.sleep()        

同时修改Cmakelists文件

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
  scripts/take_off.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

修改py文件的可执行性

chmod +x *.py

运行节点

rosrun offb take_off.py