利用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