无人机多点巡航

公爵爱学习2026-09-07 10:37

1.建立一个world里面添加 无人机常用的测试门文件

核心代码：

bash 复制代码 
     <!-- ==================== 4 个穿越门（开口2.4m） ==================== -->

    <!-- 门1：红色，位置(4,0,2.5)，朝向90° -->
    <model name='gate_red'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>4 0 2.5 0 0 1.5708</pose>
    </model>

    <!-- 门2：绿色，位置(0,4,2.8)，朝向180° -->
    <model name='gate_green'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>0 4 2.8 0 0 3.1416</pose>
    </model>

    <!-- 门3：蓝色，位置(-4,0,3.0)，朝向-90° -->
    <model name='gate_blue'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>-4 0 3.0 0 0 -1.5708</pose>
    </model>

    <!-- 门4：橙色，位置(0,-4,2.6)，朝向0° -->
    <model name='gate_orange'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0.5 0 1</ambient><diffuse>1 0.5 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0.5 0 1</ambient><diffuse>1 0.5 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0.5 0 1</ambient><diffuse>1 0.5 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>0 -4 2.6 0 0 0</pose>
    </model>

    <!-- ==================== 门结束 ==================== -->

四个门东南西北

第二步：多点巡航的程序,核心代码如下，完整代码联系我

python 复制代码 
   mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    mavros_msgs::SetMode land_set_mode;
    land_set_mode.request.custom_mode = "AUTO.LAND";
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    int wp_index = 0;
    ros::Time last_wp = ros::Time::now();
    bool cruise_started = false;      // 是否已开始巡航
    bool land_sent = false;           // 是否已发降落指令

    while(ros::ok()){
        // ① 切 offboard 模式
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(2.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        // ② 解锁
        else if( !current_state.armed &&
            (ros::Time::now() - last_request > ros::Duration(2.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }

第三步：订阅RVIZ话题的程序

python 复制代码 
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    if(pts.empty() || dist(msg->pose.position, pts.back().pose.position) > 0.1)
    {
        pts.push_back(*msg);
        cols.push_back(current_color);   // 记录该点对应的门颜色

        visualization_msgs::Marker m;
        m.header.frame_id = "map";
        m.header.stamp = ros::Time::now();
        m.ns = "gate_traj";
        m.id = 0;
        m.type = visualization_msgs::Marker::LINE_STRIP;
        m.action = visualization_msgs::Marker::ADD;
        m.scale.x = 0.12;                // 粗线

        for(size_t i = 0; i < pts.size(); i++){
            m.points.push_back(pts[i].pose.position);
            m.colors.push_back(cols[i]);
        }
        marker_pub.publish(m);

最后一步：在Cmake.list里面添加 编译规则

终端1无人机 终端2订阅RVIZ 终端3.打开RVIZ，这里要选择Marker话题选对 终端4.发布巡航程序

当然也可以进行rosbag记录速度与位置信息

然后如上一篇操作，进行转化成csv格式，把下面代码放入matlab中

python 复制代码 
%% ============ 图1：3D轨迹 + 门 ============
figure('Color','w','Position',[80 80 650 560]);
plot3(x, y, z, '-', 'Color', [0 0.45 0.74], 'LineWidth', lw); hold on;
for i = 1:4
    plot3(gx(i), gy(i), gz(i), 's', 'MarkerSize', 15, ...
        'MarkerFaceColor', gc(i,:), 'MarkerEdgeColor','k','LineWidth',1.2);
end
hold off;
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
legend([{'Trajectory'},{'Gate1','Gate2','Gate3','Gate4'}], ...
    'Location','best','Box','off');
grid on; box on; axis equal; view(40,25);
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig1_3d_traj.pdf','ContentType','vector');

%% ============ 图2：位置-时间 ============
figure('Color','w','Position',[80 680 650 400]);
plot(tp,x,'-','Color',[0.85 0.33 0.10],'LineWidth',lw); hold on;
plot(tp,y,'-','Color',[0.47 0.67 0.19],'LineWidth',lw);
plot(tp,z,'-','Color',[0.00 0.45 0.74],'LineWidth',lw); hold off;
xlabel('Time (s)'); ylabel('Position (m)');
legend({'x','y','z'},'Location','best','Box','off','NumColumns',3);
grid on; box on;
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig2_position.pdf','ContentType','vector');

%% ============ 图3：速度-时间（平滑）============
figure('Color','w','Position',[780 80 650 400]);
plot(tv,vx_s,'-','Color',[0.85 0.33 0.10],'LineWidth',lw); hold on;
plot(tv,vy_s,'-','Color',[0.47 0.67 0.19],'LineWidth',lw);
plot(tv,vz_s,'-','Color',[0.00 0.45 0.74],'LineWidth',lw); hold off;
xlabel('Time (s)'); ylabel('Velocity (m/s)');
legend({'v_x','v_y','v_z'},'Location','best','Box','off','NumColumns',3);
grid on; box on;
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig3_velocity.pdf','ContentType','vector');

%% ============ 图4：总速度 ============
figure('Color','w','Position',[780 560 650 400]);
plot(tv, speed_s, '-', 'Color', [0.20 0.20 0.20], 'LineWidth', lw);
xlabel('Time (s)'); ylabel('Speed |v| (m/s)');
grid on; box on;
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig4_speed.pdf','ContentType','vector');
相关推荐
寂寞的船42 分钟前
【Unity Bug挑战】第10期 SerializeReference 内存泄漏之谜
unity·c#·游戏引擎·bug
亥时无人机管理系统17 小时前
一网统飞低空管控平台源码 全域无人机集中管理系统功能与落地优势
私有化部署·无人机·低空经济·无人机管理系统·一网统飞·行业巡检
拓恒技术3 天前
从“可见光漏检”到“热异常锁定”：无人机河道排污口识别与溯源的技术链路
人工智能·视觉检测·无人机
tealcwu3 天前
【Unity高级】Unity URP：构建同时支持 2D 与 3D 场景的项目完全指南
3d·unity·游戏引擎
QXWZ_IA3 天前
无人机自主巡检从半自动到全闭环的阶段划分与定位要求
人工智能·安全·无人机
淡海水3 天前
10-01-互操作-C#与C++边界的内存管理
unity·c#·游戏引擎·.netcore·gc
AiNightVision3 天前
无人机夜视模块如何选择？——从参数指标到系统集成的完整选型指南
人工智能·算法·计算机视觉·车载系统·无人机·视频·智能硬件
小小数媒成员3 天前
UGUI原理（一）
开发语言·游戏·unity·游戏引擎
暴走约伯3 天前
【VFX工具链源码拆解 04】拯救丢失的绑定：Timeline 一键克隆与层级相对路径重映射算法
unity·c#·游戏引擎·硬件工程
小小数媒成员3 天前
Unity UGUI布局组件（LayoutGroup、ContentSizeFitter、LayoutElement）详解
unity·游戏引擎
热门推荐
01如何新建文件夹？ 电脑新建文件夹的4种方法02国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）03历年考研数学一、数学二、数学三真题试卷及答案PDF04几个好用的ip纯净度检测网站05GitHub 镜像站点06苹果iPA游戏软件资源下载网站07昇腾910B系列芯片完全指南：四款型号对比、服务器选型与大模型部署实战08蜘蛛磁力 搜索引擎大全，如何使用蜘蛛磁力查找磁力链接09微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新10Dify：一站式AI应用开发平台全解析与Windows部署指南