无人机多点巡航

公爵爱学习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');
相关推荐
魔士于安5 天前
unity 丑陋哥布林 哥布林有humannoid 动画没 带动画
unity·游戏引擎·材质·贴图·模型
workflower5 天前
SSH（Struts+Spring+Hibernate）
人工智能·机器学习·机器人·云计算·无人机
YOLO数据集集合5 天前
天空红外小目标检测数据集 | 红外小目标 天空目标检测 无人机检测 鸟类识别 低空安防9099期
人工智能·目标检测·计算机视觉·目标跟踪·无人机·无人机视角·直升机检测
淡海水6 天前
13-04-面试-源码级深度追问链
数据结构·unity·面试·c#·游戏引擎·源码·il2cpp
Liaiyang666 天前
空圈容错视角下的无人机全链路审计：从理论框架到耦合式检验
人工智能·pytorch·python·深度学习·系统架构·自动驾驶·无人机
深蓝学院6 天前
15 m/s动态避障+15g无相机定位！浙大&微分智飞两篇新作：补齐微型敏捷无人机自主飞行能力
无人机
YOLO_DATA6 天前
遥感滑坡检测的数据集 2299 张 1类 yolo格式 遥感滑坡检测数据集
人工智能·深度学习·yolo·计算机视觉·无人机·yolo数据集·ai数据集
小小数媒成员6 天前
UGUI 性能优化(详细，全，深入)
开发语言·unity·游戏引擎
亥时科技6 天前
无人机和监控视频，真能一次性打通吗？
开源·无人机·ai巡检
XBWRJ7 天前
职业教育专业建设：《无人机装调维修师》的课程参考价值
无人机
热门推荐
01Telegram APK 下载安装完整指南 — 2026年最新02国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）03GitHub 镜像站点042026 爱知・名古屋亚运会怎么看？直播平台+赛程安排分享05历年考研数学一、数学二、数学三真题试卷及答案PDF06给AI一张“业务概念地图“：本体如何从哲学走向企业智能07蜘蛛磁力 搜索引擎大全，如何使用蜘蛛磁力查找磁力链接08jev-ultrafast 深度解析：7 秒订机票的浏览器 Agent 是如何炼成的09开源OV-Watch：怎么做一个智能手表10算法竞赛C++常用的STL