Python_mavros_manual_contoller

2021-11-02 13:03:01 阅读：304 来源： 互联网

标签：contoller Python mavros vehicleState manual rospy import man

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

标签：contoller,Python,mavros,vehicleState,manual,rospy,import,man
来源： https://blog.csdn.net/qq_15390133/article/details/121096622

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

Python_mavros_manual_contoller