Gazebo仿真无人机接入物理遥控控制

1. 技术方案概述

在Gazebo仿真环境中,将PX4飞控无人机接入物理遥控器有三种主要技术路径:

1.1 MAVROS方案

MAVROS是ROS与MAVLink协议之间的桥梁,通过ROS节点将物理遥控器的输入转换为MAVLink消息发送给PX4。

优点

  • 与ROS生态深度集成
  • 提供丰富的ROS服务和话题接口
  • 支持多种遥控器类型(Joystick、Gamepad等)

缺点

  • 需要ROS中间件,系统复杂度较高
  • 实时性相对较差

1.2 MAVLink直连方案

通过MAVLink库直接与PX4通信,绕过ROS中间层。

优点

  • 延迟更低,实时性更好
  • 系统架构更简洁
  • 资源占用更少

缺点

  • 需要手动处理MAVLink协议细节
  • 缺少ROS生态的便利工具

1.3 PX4 DDS方案

利用PX4的DDS(Data Distribution Service)中间件进行通信。

优点

  • 符合PX4现代架构
  • 支持多种通信模式(发布/订阅、请求/响应)
  • 良好的可扩展性

缺点

  • 学习曲线较陡
  • 文档和示例相对较少

2. MAVROS方案实现详解

2.1 系统架构

复制代码
物理遥控器 → ROS Joystick驱动 → MAVROS节点 → MAVLink → PX4飞控 → Gazebo仿真

2.2 C++代码示例

cpp 复制代码
/**
 * @file remote_control_mavros.cpp
 * @brief 通过MAVROS将物理遥控器输入发送给PX4飞控
 */

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <mavros_msgs/ManualControl.h>

class RemoteControlMavros {
public:
    RemoteControlMavros() {
        // 初始化ROS节点
        ros::NodeHandle nh;
        
        // 订阅Joy话题(物理遥控器输入)
        joy_sub_ = nh.subscribe<sensor_msgs::Joy>(
            "joy", 10, &RemoteControlMavros::joyCallback, this);
        
        // 发布ManualControl消息给MAVROS
        manual_control_pub_ = nh.advertise<mavros_msgs::ManualControl>(
            "/mavros/manual_control/send", 10);
        
        ROS_INFO("RemoteControlMavros节点已启动");
    }

private:
    /**
     * @brief Joy话题回调函数
     * @param msg Joy消息,包含遥控器各轴和按钮状态
     */
    void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) {
        // 创建ManualControl消息
        mavros_msgs::ManualControl manual_msg;
        
        // 设置消息头
        manual_msg.header.stamp = ros::Time::now();
        manual_msg.header.frame_id = "base_link";
        
        // 映射遥控器摇杆到控制通道
        // 假设:axes[0]=滚转,axes[1]=俯仰,axes[2]=油门,axes[3]=偏航
        manual_msg.x = msg->axes[0] * 1000;  // 滚转 [-1000, 1000]
        manual_msg.y = msg->axes[1] * 1000;  // 俯仰 [-1000, 1000]
        manual_msg.z = msg->axes[2] * 1000;  // 油门 [-1000, 1000]
        manual_msg.r = msg->axes[3] * 1000;  // 偏航 [-1000, 1000]
        
        // 设置按钮状态(0=未按下,1=按下)
        manual_msg.buttons = 0;
        if (msg->buttons.size() > 0 && msg->buttons[0] > 0) {
            manual_msg.buttons |= 0x01;  // 按钮A:解锁
        }
        if (msg->buttons.size() > 1 && msg->buttons[1] > 0) {
            manual_msg.buttons |= 0x02;  // 按钮B:上锁
        }
        
        // 发布控制消息
        manual_control_pub_.publish(manual_msg);
        
        // 调试输出
        ROS_DEBUG_THROTTLE(1.0, 
            "控制量: 滚转=%.1f, 俯仰=%.1f, 油门=%.1f, 偏航=%.1f",
            manual_msg.x, manual_msg.y, manual_msg.z, manual_msg.r);
    }
    
    ros::Subscriber joy_sub_;
    ros::Publisher manual_control_pub_;
};

int main(int argc, char** argv) {
    // 初始化ROS
    ros::init(argc, argv, "remote_control_mavros");
    
    // 创建控制器实例
    RemoteControlMavros controller;
    
    // 进入ROS主循环
    ros::spin();
    
    return 0;
}

2.3 启动文件配置

xml 复制代码
<!-- remote_control.launch -->
<launch>
    <!-- 启动Joy节点(物理遥控器驱动) -->
    <node pkg="joy" type="joy_node" name="joy_node">
        <param name="dev" value="/dev/input/js0" />
        <param name="deadzone" value="0.1" />
        <param name="autorepeat_rate" value="20" />
    </node>
    
    <!-- 启动MAVROS节点 -->
    <include file="$(find mavros)/launch/px4.launch">
        <arg name="fcu_url" value="udp://:14540@127.0.0.1:14557" />
        <arg name="gcs_url" value="" />
        <arg name="tgt_system" value="1" />
        <arg name="tgt_component" value="1" />
    </include>
    
    <!-- 启动自定义遥控器节点 -->
    <node pkg="remote_control" type="remote_control_mavros" 
          name="remote_control_mavros" output="screen" />
</launch>

3. MAVLink直连方案实现

3.1 系统架构

复制代码
物理遥控器 → 自定义驱动 → MAVLink库 → UDP/TCP → PX4飞控 → Gazebo仿真

3.2 C++代码示例

cpp 复制代码
/**
 * @file remote_control_mavlink.cpp
 * @brief 通过MAVLink直连将物理遥控器输入发送给PX4飞控
 */

#include <iostream>
#include <thread>
#include <chrono>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <linux/joystick.h>
#include <fcntl.h>

// MAVLink头文件
#include <mavlink/v2.0/common/mavlink.h>

class RemoteControlMavlink {
public:
    RemoteControlMavlink(const std::string& ip = "127.0.0.1", 
                         int port = 14540)
        : server_ip_(ip), server_port_(port), sockfd_(-1), 
          joystick_fd_(-1), running_(false) {
        
        // 初始化MAVLink系统
        system_id_ = 255;      // GCS系统ID
        component_id_ = 190;   // MAV_COMP_ID_MISSIONPLANNER
        target_system_ = 1;    // PX4飞控系统ID
        target_component_ = 1; // PX4飞控组件ID
    }
    
    ~RemoteControlMavlink() {
        stop();
    }
    
    /**
     * @brief 初始化网络连接和遥控器
     */
    bool initialize() {
        // 1. 创建UDP socket
        sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
        if (sockfd_ < 0) {
            std::cerr << "创建socket失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        // 2. 设置目标地址(PX4的MAVLink端口)
        memset(&dest_addr_, 0, sizeof(dest_addr_));
        dest_addr_.sin_family = AF_INET;
        dest_addr_.sin_port = htons(server_port_);
        inet_pton(AF_INET, server_ip_.c_str(), &dest_addr_.sin_addr);
        
        // 3. 打开遥控器设备
        joystick_fd_ = open("/dev/input/js0", O_RDONLY | O_NONBLOCK);
        if (joystick_fd_ < 0) {
            std::cerr << "打开遥控器设备失败: " << strerror(errno) << std::endl;
            close(sockfd_);
            return false;
        }
        
        // 4. 获取遥控器信息
        char name[128];
        if (ioctl(joystick_fd_, JSIOCGNAME(sizeof(name)), name) >= 0) {
            std::cout << "遥控器名称: " << name << std::endl;
        }
        
        int axes = 0, buttons = 0;
        ioctl(joystick_fd_, JSIOCGAXES, &axes);
        ioctl(joystick_fd_, JSIOCGBUTTONS, &buttons);
        std::cout << "摇杆轴数: " << axes << ", 按钮数: " << buttons << std::endl;
        
        return true;
    }
    
    /**
     * @brief 启动控制线程
     */
    void start() {
        running_ = true;
        control_thread_ = std::thread(&RemoteControlMavlink::controlLoop, this);
        std::cout << "MAVLink遥控器控制已启动" << std::endl;
    }
    
    /**
     * @brief 停止控制线程
     */
    void stop() {
        running_ = false;
        if (control_thread_.joinable()) {
            control_thread_.join();
        }
        if (joystick_fd_ >= 0) {
            close(joystick_fd_);
        }
        if (sockfd_ >= 0) {
            close(sockfd_);
        }
    }
    
private:
    /**
     * @brief 控制主循环
     */
    void controlLoop() {
        struct js_event event;
        int16_t axes[8] = {0};
        uint8_t buttons[16] = {0};
        
        while (running_) {
            // 读取遥控器事件(非阻塞)
            while (read(joystick_fd_, &event, sizeof(event)) > 0) {
                switch (event.type & ~JS_EVENT_INIT) {
                    case JS_EVENT_AXIS:
                        if (event.number < 8) {
                            axes[event.number] = event.value;
                        }
                        break;
                    case JS_EVENT_BUTTON:
                        if (event.number < 16) {
                            buttons[event.number] = event.value;
                        }
                        break;
                }
            }
            
            // 发送MAVLink MANUAL_CONTROL消息
            sendManualControl(axes, buttons);
            
            // 控制频率:50Hz
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
        }
    }
    
    /**
     * @brief 发送MAVLink手动控制消息
     */
    void sendManualControl(int16_t axes[8], uint8_t buttons[16]) {
        mavlink_message_t msg;
        uint8_t buf[MAVLINK_MAX_PACKET_LEN];
        
        // 创建MANUAL_CONTROL消息
        mavlink_msg_manual_control_pack(
            system_id_, component_id_, &msg,
            target_system_,           // 目标系统
            axes[0],                  // X轴(滚转)[-1000, 1000]
            axes[1],                  // Y轴(俯仰)[-1000, 1000]
            axes[2],                  // Z轴(油门)[0, 1000]
            axes[3],                  // R轴(偏航)[-1000, 1000]
            buttons[0] ? 1 : 0        // 按钮状态
        );
        
        // 编码消息
        uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
        
        // 发送UDP数据包
        sendto(sockfd_, buf, len, 0, 
               (struct sockaddr*)&dest_addr_, sizeof(dest_addr_));
        
        // 调试输出
        static int count = 0;
        if (++count % 50 == 0) {
            std::cout << "发送控制量: X=" << axes[0] 
                      << " Y=" << axes[1] 
                      << " Z=" << axes[2] 
                      << " R=" << axes[3] << std::endl;
        }
    }
    
    // 成员变量
    std::string server_ip_;
    int server_port_;
    int sockfd_;
    int joystick_fd_;
    struct sockaddr_in dest_addr_;
    
    std::thread control_thread_;
    bool running_;
    
    // MAVLink参数
    uint8_t system_id_;
    uint8_t component_id_;
    uint8_t target_system_;
    uint8_t target_component_;
};

int main() {
    RemoteControlMavlink controller;
    
    if (!controller.initialize()) {
        std::cerr << "初始化失败" << std::endl;
        return 1;
    }
    
    controller.start();
    
    // 等待用户输入退出
    std::cout << "按Enter键退出..." << std::endl;
    std::cin.get();
    
    controller.stop();
    return 0;
}

3.3 编译配置(CMakeLists.txt)

cmake 复制代码
cmake_minimum_required(VERSION 3.10)
project(remote_control_mavlink)

set(CMAKE_CXX_STANDARD 17)

# 查找MAVLink
find_package(MAVLink REQUIRED)

# 包含MAVLink头文件
include_directories(${MAVLINK_INCLUDE_DIRS})

add_executable(remote_control_mavlink
    src/remote_control_mavlink.cpp
)

target_link_libraries(remote_control_mavlink
    ${CMAKE_THREAD_LIBS_INIT}
)

4. PX4 DDS方案实现

4.1 系统架构

复制代码
物理遥控器 → MicroXRCEAgent → DDS中间件 → uORB → PX4飞控 → Gazebo仿真

4.2 C++代码示例

cpp 复制代码
/**
 * @file remote_control_dds.cpp
 * @brief 通过PX4 DDS将物理遥控器输入发送给PX4飞控
 */

#include <iostream>
#include <thread>
#include <chrono>
#include <cstring>
#include <linux/joystick.h>
#include <fcntl.h>
#include <unistd.h>

// Fast DDS头文件
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/topic/Topic.hpp>

// PX4 DDS消息定义
#include "px4_msgs/msg/ManualControlSetpoint.hpp"

using namespace eprosima::fastdds::dds;

class RemoteControlDDS {
public:
    RemoteControlDDS() : participant_(nullptr), 
                         publisher_(nullptr),
                         topic_(nullptr),
                         writer_(nullptr),
                         joystick_fd_(-1),
                         running_(false) {
        
        // 初始化DDS类型支持
        manual_control_type_.register_type(participant_);
    }
    
    ~RemoteControlDDS() {
        stop();
    }
    
    /**
     * @brief 初始化DDS和遥控器
     */
    bool initialize() {
        // 1. 创建DDS域参与者
        DomainParticipantQos participant_qos;
        participant_qos.name("RemoteControlParticipant");
        participant_ = DomainParticipantFactory::get_instance()
            ->create_participant(0, participant_qos);
        
        if (!participant_) {
            std::cerr << "创建DDS参与者失败" << std::endl;
            return false;
        }
        
        // 2. 创建发布者
        publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT);
        if (!publisher_) {
            std::cerr << "创建发布者失败" << std::endl;
            return false;
        }
        
        // 3. 创建主题
        topic_ = participant_->create_topic(
            "ManualControlSetpoint_PubSubTopic",
            manual_control_type_.get_type_name(),
            TOPIC_QOS_DEFAULT);
        
        if (!topic_) {
            std::cerr << "创建主题失败" << std::endl;
            return false;
        }
        
        // 4. 创建数据写入器
        writer_ = publisher_->create_datawriter(topic_, DATAWRITER_QOS_DEFAULT);
        if (!writer_) {
            std::cerr << "创建数据写入器失败" << std::endl;
            return false;
        }
        
        // 5. 打开遥控器设备
        joystick_fd_ = open("/dev/input/js0", O_RDONLY | O_NONBLOCK);
        if (joystick_fd_ < 0) {
            std::cerr << "打开遥控器设备失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        // 6. 获取遥控器信息
        char name[128];
        if (ioctl(joystick_fd_, JSIOCGNAME(sizeof(name)), name) >= 0) {
            std::cout << "遥控器名称: " << name << std::endl;
        }
        
        int axes = 0, buttons = 0;
        ioctl(joystick_fd_, JSIOCGAXES, &axes);
        ioctl(joystick_fd_, JSIOCGBUTTONS, &buttons);
        std::cout << "摇杆轴数: " << axes << ", 按钮数: " << buttons << std::endl;
        
        std::cout << "DDS遥控器控制初始化成功" << std::endl;
        return true;
    }
    
    /**
     * @brief 启动控制线程
     */
    void start() {
        running_ = true;
        control_thread_ = std::thread(&RemoteControlDDS::controlLoop, this);
        std::cout << "DDS遥控器控制已启动" << std::endl;
    }
    
    /**
     * @brief 停止控制线程
     */
    void stop() {
        running_ = false;
        if (control_thread_.joinable()) {
            control_thread_.join();
        }
        if (joystick_fd_ >= 0) {
            close(joystick_fd_);
        }
        
        // 清理DDS资源
        if (writer_) {
            publisher_->delete_datawriter(writer_);
        }
        if (topic_) {
            participant_->delete_topic(topic_);
        }
        if (publisher_) {
            participant_->delete_publisher(publisher_);
        }
        if (participant_) {
            DomainParticipantFactory::get_instance()->delete_participant(participant_);
        }
    }
    
private:
    /**
     * @brief 控制主循环
     */
    void controlLoop() {
        struct js_event event;
        int16_t axes[8] = {0};
        uint8_t buttons[16] = {0};
        
        while (running_) {
            // 读取遥控器事件(非阻塞)
            while (read(joystick_fd_, &event, sizeof(event)) > 0) {
                switch (event.type & ~JS_EVENT_INIT) {
                    case JS_EVENT_AXIS:
                        if (event.number < 8) {
                            // 将原始值[-32767, 32767]映射到[-1000, 1000]
                            axes[event.number] = static_cast<int16_t>(
                                (event.value * 1000.0f) / 32767.0f);
                        }
                        break;
                    case JS_EVENT_BUTTON:
                        if (event.number < 16) {
                            buttons[event.number] = event.value;
                        }
                        break;
                }
            }
            
            // 发送DDS手动控制消息
            sendManualControl(axes, buttons);
            
            // 控制频率:50Hz
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
        }
    }
    
    /**
     * @brief 发送DDS手动控制消息
     */
    void sendManualControl(int16_t axes[8], uint8_t buttons[16]) {
        px4_msgs::msg::ManualControlSetpoint manual_msg;
        
        // 设置时间戳(纳秒)
        auto now = std::chrono::system_clock::now();
        auto duration = now.time_since_epoch();
        manual_msg.timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count();
        
        // 设置控制量
        // 注意:PX4的ManualControlSetpoint使用[-1.0, 1.0]的浮点数范围
        manual_msg.x = axes[0] / 1000.0f;  // 滚转 [-1.0, 1.0]
        manual_msg.y = axes[1] / 1000.0f;  // 俯仰 [-1.0, 1.0]
        manual_msg.z = (axes[2] + 1000) / 2000.0f;  // 油门 [0.0, 1.0],映射从[-1000, 1000]到[0, 1]
        manual_msg.r = axes[3] / 1000.0f;  // 偏航 [-1.0, 1.0]
        
        // 设置按钮状态
        manual_msg.data_source = 1;  // 1 = MAVLINK
        manual_msg.valid = true;
        
        // 设置辅助开关
        manual_msg.flaps = buttons[0] ? 1.0f : 0.0f;      // 按钮A:襟翼
        manual_msg.aux1 = buttons[1] ? 1.0f : 0.0f;       // 按钮B:辅助1
        manual_msg.aux2 = buttons[2] ? 1.0f : 0.0f;       // 按钮X:辅助2
        manual_msg.aux3 = buttons[3] ? 1.0f : 0.0f;       // 按钮Y:辅助3
        manual_msg.aux4 = buttons[4] ? 1.0f : 0.0f;       // 按钮LB:辅助4
        manual_msg.aux5 = buttons[5] ? 1.0f : 0.0f;       // 按钮RB:辅助5
        manual_msg.aux6 = buttons[6] ? 1.0f : 0.0f;       // 按钮Back:辅助6
        
        // 发布消息
        if (writer_) {
            writer_->write(&manual_msg);
        }
        
        // 调试输出
        static int count = 0;
        if (++count % 50 == 0) {
            std::cout << "发送控制量: X=" << manual_msg.x 
                      << " Y=" << manual_msg.y 
                      << " Z=" << manual_msg.z 
                      << " R=" << manual_msg.r 
                      << " 按钮: " << (buttons[0] ? "A" : "-")
                      << (buttons[1] ? "B" : "-")
                      << (buttons[2] ? "X" : "-")
                      << (buttons[3] ? "Y" : "-") << std::endl;
        }
    }
    
    // 成员变量
    DomainParticipant* participant_;
    Publisher* publisher_;
    Topic* topic_;
    DataWriter* writer_;
    TypeSupport manual_control_type_;
    
    int joystick_fd_;
    std::thread control_thread_;
    bool running_;
};

int main() {
    RemoteControlDDS controller;
    
    if (!controller.initialize()) {
        std::cerr << "DDS控制器初始化失败" << std::endl;
        return 1;
    }
    
    controller.start();
    
    // 等待用户输入退出
    std::cout << "DDS遥控器控制运行中,按Enter键退出..." << std::endl;
    std::cin.get();
    
    controller.stop();
    std::cout << "DDS遥控器控制已停止" << std::endl;
    
    return 0;
}

4.3 编译配置(CMakeLists.txt)

cmake 复制代码
cmake_minimum_required(VERSION 3.16)
project(remote_control_dds)

set(CMAKE_CXX_STANDARD 17)

# 查找Fast DDS
find_package(fastrtps REQUIRED)
find_package(fastcdr REQUIRED)

# 查找PX4 DDS消息包(假设已安装px4_msgs)
find_package(px4_msgs REQUIRED)

# 包含头文件
include_directories(
    ${FASTRTPS_INCLUDE_DIRS}
    ${FASTCDR_INCLUDE_DIRS}
    ${px4_msgs_INCLUDE_DIRS}
)

# 添加可执行文件
add_executable(remote_control_dds
    src/remote_control_dds.cpp
)

# 链接库
target_link_libraries(remote_control_dds
    ${FASTRTPS_LIBRARIES}
    ${FASTCDR_LIBRARIES}
    ${px4_msgs_LIBRARIES}
    pthread
)

# 安装目标(可选)
install(TARGETS remote_control_dds
    RUNTIME DESTINATION bin
)

4.4 运行配置

  1. 环境准备

    bash 复制代码
    # 安装Fast DDS
    sudo apt-get install ros-${ROS_DISTRO}-fastrtps
    
    # 安装PX4 DDS消息包
    git clone https://github.com/PX4/px4_msgs.git
    cd px4_msgs && mkdir build && cd build
    cmake .. && make && sudo make install
    
    # 启动MicroXRCEAgent(DDS代理)
    MicroXRCEAgent udp4 -p 8888
  2. 编译运行

    bash 复制代码
    mkdir build && cd build
    cmake .. && make
    ./remote_control_dds
  3. PX4配置

    • 确保PX4已编译DDS支持
    • 在PX4启动脚本中启用DDS通信
    • 配置MicroXRCEAgent与PX4的连接

4.5 方案对比总结

特性 MAVROS方案 MAVLink直连 PX4 DDS方案
实时性 中等
系统复杂度 中等
开发难度 中等
生态支持 丰富 一般 较新
适用场景 ROS集成项目 高性能需求 现代PX4架构
相关推荐
HXSYS6 小时前
飞培机构炸机损耗大,《无人机装调维修师》降低运营成本
无人机
CedarQR8 小时前
云卓 G20 遥控器 Android 开发实录:从 USB 设备到读取摇杆/按键通道值
android·linux·无人机
qq_252941316811 小时前
低空飞行物目标检测数据集 | 低空飞行物 鸟类检测 反无人机 航空安全 目标检测 YOLO格式9017期
人工智能·yolo·目标检测·计算机视觉·无人机·低空飞行物
YOLO数据集集合12 小时前
钢材表面缺陷目标检测数据集 | 钢材缺陷检测 表面质检 工业视觉 目标检测9009期
人工智能·目标检测·计算机视觉·目标跟踪·无人机·钢材数据集·钢材表面缺陷
亥时科技12 小时前
大疆无人机上云,难的从来不是“飞起来”
开源·无人机·ai巡检
tianxuanjg14 小时前
工业机器人精密零件采购:如何选关节 CNC 加工供应商
经验分享·机器人·无人机·制造·材质
渡之14 小时前
ArduPilot LowPassFilter 深度解析
算法·无人机
daad7771 天前
无人机垂直通道卡尔曼滤波器:从状态空间模型到六传感器融合的数值实例
无人机·efk
应用市场1 天前
无人机姿态控制与悬停的关键技术原理:从四元数到串级 PID,再到 INDI
无人机
HXSYS1 天前
无人机租赁市场升温,《无人机装调维修师》筑牢设备周转底气
无人机