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架构
相关推荐
weixin_452600691 天前
D2632大电流低差压可调线性稳压器介绍,D2632可替换MIC29302
fpga开发·机器人·无人机·电动工具·车载摄像头
威视锐科技2 天前
无人机被动探测与定位
无人机·威视锐
这张生成的图像能检测吗2 天前
(论文速读)元结构增强的单传感器无人机螺旋桨故障定位方法
无人机·故障诊断·结构设计·缺陷识别
亥时科技2 天前
4K航拍视频,为什么能在浏览器里直接看?
开源·无人机·ai巡检
别催小唐敲代码2 天前
ROS2从入门到实战:去中心化机器人操作系统详解
wpf·ros2
这张生成的图像能检测吗2 天前
(论文速读)基于MFC传感和能量采集的无人机螺旋桨故障自能实时监测
无人机·故障诊断·传感器·旋翼故障·mfc系统
GTfiber3 天前
深圳光特通信光纤无人机专用光模块:构建无损高清图传通道,重塑工业无人机影像传输能力
科技·5g·无人机·信息与通信
大毛无人机3 天前
无人机服务服务商筛选 SOP
无人机·流程·验证·sop·2026无人机接单平台哪
大毛无人机3 天前
无人机服务核验流程 SOP
项目管理·无人机·无人机接单·资质核验
酸梅果茶4 天前
ROS2 foxy环境下源码安装Navigation2
ros2·navigation·agx