Rospy初次使用記錄-定點飛行

由於接觸到pytorch，所以用python完成與ROS的通信，下面例子爲從程序中摘出來的一部分，用到了ROS消息的訂閱與發佈，服務的通信，可以作爲參考使用：

import rospy
from mavros_msgs.msg import AttitudeTarget
from geometry_msgs.msg import PoseStamped 
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import SetMode
setpoint_pos = PoseStamped()
setpoint_pos.pose.position.x = 0
setpoint_pos.pose.position.y = 0
setpoint_pos.pose.position.z = 2
local_position = PoseStamped()
locat_attitude_target = AttitudeTarget()
locat_attitude_target.thrust = 0.7
locat_attitude_target.type_mask = 0b10000111
arm_state = CommandBool()
def ros_test():
    rospy.init_node('rospy_node',anonymous=True)
    thrust_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=1)
    state_arm_srv = rospy.ServiceProxy('/mavros/cmd/arming',CommandBool)
    state_mode_srv = rospy.ServiceProxy('/mavros/set_mode',SetMode)
    rospy.Subscriber('/mavros/state',State,VehicleState_callback)
    setpoint_pos_pub = rospy.Publisher('/mavros/setpoint_position/local',PoseStamped,queue_size=1)
    rospy.Subscriber('/mavros/local_position/pose',PoseStamped,local_pos_cb)
    rate = rospy.Rate(100)
    
    while not rospy.is_shutdown():
        # print(vehicle_state)
        # rospy.loginfo(vehicle_state.mode.)
        if vehicle_state.mode != 'OFFBOARD':
            state_mode_srv.call(custom_mode='OFFBOARD')
        else:
            # print(vehicle_state.armed)
            if vehicle_state.armed == False:
                state_arm_srv.call(True)
            else:
                # thrust_pub.publish(locat_attitude_target)
                # data = rospy.wait_for_message("/mavros/local_position/pose",PoseStamped,timeout=None)
                print(local_position.pose.position.z)
        setpoint_pos_pub.publish(setpoint_pos)
        rate.sleep()

使用方法：啓用gazebo，與mavros，然後運行腳本（本腳本需要修改後使用），可以完成無人機的定點懸停，以及定油門飛行功能