PX4飞控通过python代码控制能解锁成功但是不能起飞和飞行

qiqian1999

想通过
    local_pos_pub.publish(pose)的方式向无人机发送位置指令，通过更改PX4官方的python代码示例，发现解锁的功能和改变模式的功能能够成功，但是无人机不会飞到我们代码里指定的位置，比如(3,0,3)之类的，甚至也不能起飞，我们尝试直接执行takeoff的指令，发现确实传送了22的起飞指令，但是无人机并没有反应。下面是我们的代码：import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest

current_state = State()
flag=1

def state_cb(msg):
    global current_state
    current_state = msg
   
if __name__ == "__main__":
    rospy.init_node("offb_node_py")

    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)

    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)

    rospy.wait_for_service("/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)

    rospy.wait_for_service("/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)


    # Setpoint publishing MUST be faster than 2Hz
    rate = rospy.Rate(20)

    # Wait for Flight Controller connection
    while(not rospy.is_shutdown() and not current_state.connected):
        rate.sleep()

    pose = PoseStamped()

    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 1

    # Send a few setpoints before starting
    for i in range(100):
        if(rospy.is_shutdown()):
            break

        local_pos_pub.publish(pose)
        rate.sleep()

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = 'OFFBOARD'

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_req = rospy.Time.now()

    while(not rospy.is_shutdown()):
        if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            if(set_mode_client.call(offb_set_mode).mode_sent == True):
                rospy.loginfo("OFFBOARD enabled")

            last_req = rospy.Time.now()
        else:
            if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
                if(arming_client.call(arm_cmd).success == True):
                    rospy.loginfo("Vehicle armed")

                last_req = rospy.Time.now()
   
        if flag==1 and ((rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            rospy.loginfo("position1(0,0,3)")
            pose.pose.position.x = 0
            pose.pose.position.y = 0
            pose.pose.position.z = 3
            last_req = rospy.Time.now()      
            #local_pos_pub.publish(pose)
            rospy.sleep(3)
            flag=2
           
           
        if flag==2 and ((rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            rospy.loginfo("position1(3,0,3)")
            pose.pose.position.x = 3
            pose.pose.position.y = 0
            pose.pose.position.z = 3
            last_req = rospy.Time.now()
            #local_pos_pub.publish(pose)
            rospy.sleep(3)
            flag=3
      
      
       #local_pos_pub.publish(pose)  
        if flag==3 and ((rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            pose.pose.position.x = 0
            pose.pose.position.y = 0
            pose.pose.position.z = 0
            last_req = rospy.Time.now()
            #local_pos_pub.publish(pose)
            break
        local_pos_pub.publish(pose)
        rate.sleep()
    rospy.spin()   

我知道答案 回答被采纳将会获得4 阿木币 已有1人回答

amov-辰星

感觉很6