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，然后运行脚本（本脚本需要修改后使用），可以完成无人机的定点悬停，以及定油门飞行功能

本文地址：https://blog.csdn.net/qq_15390133/article/details/107144356