PX4 全驱无人机 MAVROS 发布 actuator_control 消息

如果想要通过发布力和力矩控制全驱无人机，那么就需要发布三个方向的力矩和三个方向的力，总共六个控制量，而对于默认原版的actuator_control消息，其中只有四个控制量，因此需要进行修改。具体修改流程如下：

一、修改UORB消息

在msg文件夹下对actuator_controls.msg文件进行修改，增添三方向力参数，分别命名为INDEX_X_THRUST、INDEX_Y_THRUST、INDEX_Z_THRUST，控制通道数由8改为11

修改完成后编译，会生成对应.h头文件

make px4_sitl gazebo

二、修改MAVROS包

因为我们需要对MAVROS和MAVLINK包进行二次开发，因此这里不建议使用二进制安装，更推荐使用源码安装，安装步骤可参考我之前的博客：源码方式安装MAVROS包；如果是已经进行过二进制安装的，也建议将之前的包删除，避免不必要的麻烦。

在mavros_msg/msg文件夹下找到ActuatorControl.msg文件，将控制通道数由8改为11

这里我们先不编译，等到修改MavLink包后一同编译

三、修改MAVLINK包

在mavlink/message_definition/v1.0/文件夹下找到common.xml文件，分别将“SET_ACTUATOR_CONTROL_TARGET”和“ACTUATOR_CONTROL_TARGET”这两块对应的控制通道数改为11

保存后进行编译

catkin build

若修改成功，可以在build文件夹下找到mavlink_msg_actuator_control_target.h文件，检查通道数与一些其他参数是否发生改变；若发生改变，则说明修改成功

四、修改PX4/Firmware中的mavlink包

步骤同上，在mavlink/include/mavlink/v2.0/message_definition/文件夹下找到common.xml文件，分别将“SET_ACTUATOR_CONTROL_TARGET”和“ACTUATOR_CONTROL_TARGET”这两块对应的控制通道数改为11

保存后，首先需要使用Mavlink Generator工具进行编译，可以在源码安装的mavlink包中找到mavgenerate.py文件，在命令行中输入

python3 mavgenerate.py

即可运行Mavlink Generator工具

XML一栏选择～/Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml
输出一栏选择～/Firmware/mavlink/include/mavlink/v2.0
语言一栏选择 C
选择 2.0
勾选 Validate
点击 Generate 按钮

若修改成功，可以在mavlink/include/mavlink/v2.0/common文件夹下找到mavlink_msg_actuator_control_target.h文件，检查通道数与一些其他参数是否发生改变；若发生改变，则说明修改成功，注意此时与步骤三中最后生成的文件完成一致，若有部分参数不一致，说明修改失败

五、修改PX4/Firmware中的mavlink_receiver模块

在src/modules/mavlink文件夹下找到mavlink_receiver.cpp，对handle_message_set_actuator_control_target函数进行修改

六、修改参数ENABLE_LOCKSTEP_SCHEDULER

在boards/px4/sitl文件夹下找到default.cmake文件，修改参数ENABLE_LOCKSTEP_SCHEDULER为no

在对应模型的sdf文件中修改mavlink插件下的参数enable_lockstep为0

结束后再对整个固件进行编译

make px4_sitl gazebo

七、测试

启动mavros仿真

roslaunch mavros_posix_sitl.launch

运行自定义mavros程序，发布actuator_control数据，示例代码如下：

#! /usr/bin/env python

import rospy
import math
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State, ActuatorControl
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest

current_state = State()

def state_cb(msg):
    global current_state
    current_state = msg

local_pos = PoseStamped()

def local_pos_cb(local_pos_msg):
    global local_pos
    local_pos = local_pos_msg

if __name__ == "__main__":
    rospy.init_node("offb_force_py")

    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
    local_pos_sb = rospy.Subscriber("mavros/local_position/pose", PoseStamped, callback = local_pos_cb)

    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
    ActuatorControl_pub = rospy.Publisher("mavros/actuator_control", ActuatorControl, 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(250)

    # 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 = 2

    actuator_control = ActuatorControl()

    actuator_control.group_mix = 0
    actuator_control.controls[0] = 0.000          # ROLL
    actuator_control.controls[1] = 0.000          # PITCH
    actuator_control.controls[2] = 0.000          # YAW
    actuator_control.controls[3] = 0.824        # THROTTLE
    actuator_control.controls[4] = 0.0          # FLAPS
    actuator_control.controls[5] = 0.0          # SPOILERS
    actuator_control.controls[6] = 0.0          # AIRBRAKES
    actuator_control.controls[7] = -1.0          # LANDING_GEAR
    actuator_control.controls[8] = 0.000         # X_THRUST
    actuator_control.controls[9] = -0.00          # Y_THRUST
    actuator_control.controls[10] = -0.824        # Z_THRUST
    # actuator_control.header.seq = 1
    # actuator_control.header.stamp = rospy.Time.now()
    # actuator_control.header.frame_id = "test"

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

        local_pos_pub.publish(pose)
        ActuatorControl_pub.publish(actuator_control)
        rate.sleep()

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

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_req = rospy.Time.now()
    
    count = 0
    take_off = False
    reach_height = False

    while(not rospy.is_shutdown()):
        if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(2.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(2.0)):
                if(arming_client.call(arm_cmd).success == True):
                    rospy.loginfo("Vehicle armed")
                    take_off = True

                last_req = rospy.Time.now()

        if(count == 0 and not reach_height):
            local_pos_pub.publish(pose)
            if(math.fabs(local_pos.pose.position.z - 2) <= 0.3):
                reach_height = True
                rospy.loginfo("Reach Height!")
                count = 1
        elif(take_off and count == 1):
            actuator_control.header.stamp = rospy.Time.now()        
            ActuatorControl_pub.publish(actuator_control)
        

        rate.sleep()

在mavros仿真窗口中输入