Python_mavros_manual_contoller

利用python完成mavros与PX4的通信工程，同时也完成了对应的PX4中对应消息代码的调试查看。

from __future__ import print_function
import rospy 
from mavros_msgs.msg import AttitudeTarget, State, ManualControl, OverrideRCIn
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import SetMode
import threading
import time

vehicleState = State()
def vehicleStateCallBack(msg):
    global vehicleState
    vehicleState = msg
    print("armed:" + str(vehicleState.armed) + "\tconnect:" + str(vehicleState.connected) + "\tmode:" + str(vehicleState.mode))

manualControl_ = ManualControl()
def ManualControlCallBack(msg):
    global manualControl_
    manualControl_ = msg
    print("x=" + str(manualControl_.x) + "\ty=" + str(manualControl_.y) + "\tz=" + str(manualControl_.z))


rospy.init_node('rospy_node', anonymous=True)
rospy.Subscriber('/mavros/state', State, vehicleStateCallBack)
manual_contoller_pub = rospy.Publisher('/mavros/manual_control/send', ManualControl, queue_size=10)
rospy.Subscriber('/mavros/manual_control/control', ManualControl, ManualControlCallBack)
arm_ser = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
set_mode_ser = rospy.ServiceProxy('/mavros/set_mode', SetMode)
rc_in_pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
manual_controller = ManualControl()
manual_controller.x = -300
manual_controller.y = 0
manual_controller.z = 700
manual_controller.r = 500
rate = rospy.Rate(20)

rc_in = OverrideRCIn()
rc_in.channels = [1500, 1500, 1800, 1500, 0, 0, 0, 0]

def main_threading():
    while not rospy.is_shutdown():
        global vehicleState

        if not vehicleState.armed:
            arm_ser(True) 
    # set_mode_ser(custom_mode = 'OFFBOARD')
        manual_contoller_pub.publish(manual_controller)
    # rc_in_pub.publish(rc_in)
        rate.sleep()
    

if __name__ == "__main__":
    main_threading()

对应px4中的代码段存在与src/module/mavlink/mavlink_receiver.cpp中， 通过PX4_INFO完成消息的查看。
} else {
manual_control_setpoint_s manual{};
PX4_INFO(“manual.x = %d, manual.y = %d, manual.r = %d, manual.z = %d”, man.x, man.y, man.r, man.z);
manual.timestamp = hrt_absolute_time();
manual.x = man.x / 1000.0f;
manual.y = man.y / 1000.0f;
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();

_manual_control_setpoint_pub.publish(manual);

可以看到飞机自动进入的poshold模式，也就是position模式，而非offboard模式。对应视频为：
python_mavros_manual_controller