深入了解ROS之编写无人机控制程序包

最新推荐文章于 2024-04-20 16:51:08 发布
最新推荐文章于 2024-04-20 16:51:08 发布
阅读量804 收藏 26
点赞数 3
文章标签： 无人机
原文链接：https://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247639224&idx=3&sn=ed4fff3dfae06ec7c4c95416a04979c3&chksm=fbf28b8ccc85029a8c3b47deca7a7e7231360f965ac5dc5545f170468f2f8be8f9f21623138b&scene=126&sessionid=0
版权

来源丨古月居

点击进入—>3D视觉工坊学习交流群

这篇教程来详细介绍一下如何编写出一个控制无人机的ROS程序包

编写ROS程序包推荐roboware-studio，这款软件是在开源的vscode基础上二次开发，用于ROS程序包的开发，非常好用

roboware-studio的安装，roboware-studio软件包的下载地址：

https://github.com/TonyRobotics/RoboWare/tree/master/Studio

官网已经进不去了，直接到github上下载deb包就好，下载时注意软件包有32位和64位之分，根据自己系统的类型进行下载，可以双击安装或者使用命令：

sudo dpkg -i xxxxxxx.deb

这款集成开发环境很好用，减少了很多手工操作，但是还是建议先弄明白每个文件都是什么作用，先手工编写几次软件包后再考虑使用这种自动化的IDE

开始编写ROS程序包

前面的建立程序包的操作这里不赘述了，参考这篇：

https://blog.csdn.net/weixin_44479297/article/details/105053555

直接介绍如何编写src文件夹下的.cpp文件，这里以一个控制无人机起飞和降落的程序为例进行讲解


示例程序如下：

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */


#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>


mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
geometry_msgs::PoseStamped aim_pos;
bool flag = true;
void arrive_pos(const geometry_msgs::PoseStamped::ConstPtr& msg){
    if((*msg).pose.position.z > aim_pos.pose.position.z * 0.95){
        flag = false;
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "takeoff_land_node");
    ros::NodeHandle nh;


    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose",10,arrive_pos);


    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);


    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }


    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 10;


    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }


    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";


    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;


    ros::Time last_request = ros::Time::now();
    flag = true;
    aim_pos = pose;
    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.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(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }


        local_pos_pub.publish(pose);


        ros::spinOnce();
        rate.sleep();
        if(flag == false){
            break;
        }
    }




    mavros_msgs::SetMode land_set_mode;
    land_set_mode.request.custom_mode = "AUTO.LAND";
    if(set_mode_client.call(land_set_mode) && land_set_mode.response.mode_sent){
        ROS_INFO("land enabled");
        }
    return 0;
}

首先解释一下文件头

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

这里包含了五个头文件，mavros_msgs软件包包含操作MAVROS软件包提供的服务和主题所需的所有自定义消息每个头文件都有作用，具体的作用我们到初始化订阅器和发布器的时候解释

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

这里首先定义了一个全局变量current_state用来记录无人机的状态，然后创建了一个回调函数，这个回调函数会实时改变全局变量current_state的值

geometry_msgs::PoseStamped aim_pos;
bool flag = true;
void arrive_pos(const geometry_msgs::PoseStamped::ConstPtr& msg){
    if((*msg).pose.position.z > aim_pos.pose.position.z * 0.95){
        flag = false;
    }
}

这里定义了一个全局变量aim_pos用来记录无人机运动的目标位置，回调函数会判断是否到达目标位置，当到达目标位置后，标志变量flag置为false，表示没有运动命令正在执行，可以执行下一个命令

ros::init(argc, argv, "takeoff_land_node");
ros::NodeHandle nh;

初始化ROS，参数是节点的名称，名称不能包含/等符号，然后为这个进程的节点创建一个句柄，第一个创建的NodeHandle会为节点进行初始化，最后一个销毁的NodeHandle则会释放该节点的所占用的所有资源。

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose",10,arrive_pos);

这里初始化了四个对象，有订阅器、发布器、两个客户端，如果对订阅器和发布器之类的不熟悉，建议重新看一下，了解这些对象都有什么作用。

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);

这句代码实例化一个订阅器，这里是对类模板的实例化，参数是消息类型（使用这个消息类型需要添加头文件），类模板是C++的相关知识点，不在这里做说明。

("mavros/state", 10, state_cb)第一个参数是一个订阅器的topic，state_sub可以从这个topic上获得无人机的状态信息，第二个参数是发布序列的大小，如果我们发布的消息的频率太高，缓冲区中的消息在大于10个时就会开始丢弃先前发布的消息。

这句代码告诉master要订阅"mavros/state"话题上的消息，当有消息发布到这个话题时，ROS就会调用state_cb

ros::Rate rate(20.0);

ros::Rate 对象可以允许你指定自循环的频率。它会追踪记录自上一次调用 Rate::sleep() 后时间的流逝，并休眠直到一个频率周期的时间，在这个程序中我们让它以10Hz的频率运行，而且无人机进入offboard模式后必须以大于2HZ的频率发布消息，否则无人机会退出offboard模式

while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

在发布任何内容之前，我们等待在MAVROS和自动驾驶仪之间建立连接，收到心跳消息后，此循环应立即退出

geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 10;

即使PX4在航空航天NED坐标系中运行，MAVROS也会将这些坐标转换为标准ENU框架，这里就直接使用坐标就行，

for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

在进入offboard模式之前，必须已经开始流式传输设定值，否则，模式切换将被拒绝，在这里，100就是随便设置的，稍微大一些就行

mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";


    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

这两段放一起解释，第一段设置一个offboard模式的消息，供设置模式的客户端调用时做参数，第二段是设置一个命令的消息，供设置arm的客户端调用时做参数。后续代码中会对这两个变量的使用进行说明。

ros::Time last_request = ros::Time::now();
    flag = true;
    aim_pos = pose;

这段就是记录当前时间，设置flag值为true,表示接下来要执行无人机飞行命令，记录一下目标位置，这样当目标位置到达后，会结束当前执行的飞行命令

while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.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(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }


        local_pos_pub.publish(pose);


        ros::spinOnce();
        rate.sleep();
        if(flag == false){
            break;
        }
    }

这段代码比较好理解了，首先检测当前模式是否是offboard，不是则置为offboard，如果当前无人机没有armed，则设置为armed，然后向无人机的位置设置节点发布位置消息，最后检测一下是否到达目标位置，到达目标位置后退出循环

mavros_msgs::SetMode land_set_mode;
    land_set_mode.request.custom_mode = "AUTO.LAND";
    if(set_mode_client.call(land_set_mode) && land_set_mode.response.mode_sent){
        ROS_INFO("land enabled");
        }

当无人机飞到指定位置后，想无人机发布位置信息的循环会结束，这里设置无人机的模式为land模式，无人机会自动降落，降落到当前位置的下方。


到这里代码部分就结束了

ROS程序包的编译和运行

编写完程序包后，先进入catkin工作目录使用catkin_make对程序包进行编译

cd /catkin_ws
catkin_make

这时候编写的ROS程序就可以执行了，不过执行之前先启动一下无人机仿真环境，选择一个启动脚本执行

roslaunch px4 XXXXXXXX.launch

重新打开一个终端执行：

source /catkin_ws/devel/setup.bash
rosrun 程序包名  节点名

这时候正常就可以看见无人机缓慢移动到10米的高空后又缓缓下落，至此对ROS程序包编写编译执行过程的介绍结束。欢迎批评指正。

版权声明：本文为CSDN博主「不断积淀」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：

https://blog.csdn.net/weixin_44479297/article/details/105086582

本文仅做学术分享，如有侵权，请联系删文。

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

8.从零搭建一套结构光3D重建系统[理论+源码+实践]

9.单目深度估计方法：算法梳理与代码实现

10.自动驾驶中的深度学习模型部署实战

11.相机模型与标定(单目+双目+鱼眼）

12.重磅！四旋翼飞行器：算法与实战

13.ROS2从入门到精通：理论与实战

14.国内首个3D缺陷检测教程：理论、源码与实战

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

重磅！粉丝学习交流群已成立

交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

299341c2a807c04346715bd61105b70d.jpeg

▲长按加微信群或投稿，微信号：dddvisiona

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看，3天内无条件退款

54e896d39117095aaf20369cac8b7167.jpeg

高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~  

3Ｄ视觉工坊
关注
  • 3
    点赞
  • 踩
  • 26
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
APM固件下使用ROS控制巡航以及二维码识别降落程序
03-07
在本主题中，我们将深入探讨如何在使用APM（ArduPilot Mission Planner）固件的无人机系统上，结合ROS（Robot Operating System）进行巡航控制和二维码识别降落程序的实现。ROS是一个开源操作系统，用于机器人系统的...
ROS-基于PX4的无人机SLAM建图(Cartographer)仿真
weixin_42600623的博客
06-21 5519
ROS-基于PX4的无人机SLAM建图(Cartographer)仿真
ROS学习笔记——基于Prometheus无人机开源项目仿真环境配置
qq_52199356的博客
09-23 2717
本笔记基于ubuntu18.04版本，且已经在电脑上安装了ROS，Mavros功能包，其余可参考Amov教程， https://github.com/amov-lab/Prometheus/wiki/%E5%AE%89%E8%A3%85%E5%8F%8A%E7%BC%96%E8%AF%91https://github.com/amov-lab/Prometheus/wiki/%E5%AE%89%E8%A3%85%E5%8F%8A%E7%BC%96%E8%AF%91...
C语言与无人机飞行控制系统开发：姿态解算、PID控制与航路规划（二）
weixin_56154577的博客
04-20 1273
欧拉角是一种常用的空间旋转参数表示法，通过三个互不干涉的旋转角（一般为偏航角、俯仰角、翻滚角）来描述一个刚体相对于固定参考系的三维旋转。欧拉角直观易懂，适用于许多工程应用，但在连续旋转时可能出现万向锁现象（Gimbal Lock），即某些角度组合导致旋转自由度丢失。
PX4中offboard模式
kjb655的博客
07-01 512
定高1m拓展到飞四边形
关于ROS开源程序包——gps_common
诗筱涵的博客
05-25 2238
转载自：https://zhuanlan.zhihu.com/p/156716344 关于ROS开源程序包——gps_common 好严谨的名字 离牛逼就差一步 项目需要，看了下gps_common的源码文件，非常简单的一个小项目，包含的程序却非常实用。 经纬度坐标与UTM坐标的相互转换 包中最重要，也是被其他开源项目引用最多的一段程序，莫过于位于gps_common/include/gps_common文件下的conversion.h头文件，文件中的函数都是以C++内联函数的形式写成，所以
ROS实验笔记之——无人机在VICON下定点飞行
gwpscut的博客
01-26 1238
For cooperaive inquiries, please visit the website：guanweipeng.com
无人机编程实战第1讲——无人机简介
ouyanghuangxun的博客
04-12 2255
本系列文章就将带领大家从零开始，写出一套属于自己的飞控代码，拥有一架属于自己的多旋翼无人机。
linux ros环境搭建无人机飞行平台！
07-26
在本文中，我们将深入探讨如何在Linux环境下使用Robot Operating System (ROS) 搭建一个无人机飞行平台。ROS是一个开源操作系统，专为机器人系统设计，提供了丰富的软件库、工具和服务，便于开发、测试和部署机器人...
RTK ROS驱动程序，Python
11-01
ROS（Robot Operating System）是机器人领域广泛使用的开源操作系统，它为机器人硬件抽象、低级设备控制、常见任务的中间件提供了框架。RTK（Real-Time Kinematic）是一种高精度的全球定位系统（GPS），通过使用差分...
ROS下玩转四旋翼无人机(仿真).zip
最新发布
05-05
在ROS中，我们可以通过编写节点和消息来控制无人机的运动，包括起飞、悬停、飞行路径规划等。 首先，了解ROS的基本概念是至关重要的。ROS主要由节点、话题、服务、参数和包等组成。节点是执行特定任务的程序，话题...
走进ros2_胡春旭.pdf
05-04
ROS是用于编写机器人软件程序的一种具有高度灵活性的软件架构。它包含了大量工具软件、库代码和约定协议，旨在简化跨机器人平台创建复杂、鲁棒的机器人行为这一过程的难度与复杂度。ROS2是对ROS的升级版本，随着ROS2...
ros学习笔记ros::Rate
qq_33973712的博客
11-21 1万+
在学习过程中碰到下面程序： ros::Rate rate_20hz(20); // 20Hz while (true) { messenger.PublishStateToROS(); ros::spinOnce(); rate_20hz.sleep(); } ros::Rate起到的是一个定时器的作用，我们县来看如下例程： ros::Rate r(10); // 10 hz while (ros::ok()) { //...
ros中关于ros::Rate 和ros::spin()等的理解
weixin_62705892的博客
11-03 6582
ros中关于ros::Rate 和ros::spin()等的理解
【PX4-AutoPilot教程-Offboard】MAVROS功能包控制无人机进入offboard模式飞行官方例程（C++实现）
在后厂村路努力科研搬砖的无人机控制方向练习生一名～
12-21 5478
主要介绍如何通过MAVROS功能包使用PX4飞控中的offboard模式控制gazebo中的飞机起飞到高度两米，代码用C++实现。在运行过程中，可能会出现一个BUG，就是无人机无法起飞，文章中简述了解决方法。
ROS功能包|mav_control_rw（基于MPC的无人机轨迹跟踪控制）---gazebo仿真测试
月照银海似蛟龙的博客
04-01 1119
打开Plugins菜单中visualization里的Message Publisher，发布的话题名称为/firefly/command/pose即可实现定点控制拉。更改position 的 x y z 的值，就是给无人机发的位置控制指令。在topic的下拉菜单中选择/firefly/command/pose。然后勾选那个方框把topic发送出去。启动 linear mpc 控制器。启动gazebo并加载无人机模型。无人机运动到了目标位置处。整体的运行效果很理想。
ros中无人机基于话题＜geographic_msgs/GeoPoseStamped.h＞的指点飞行控制
weixin_60364671的博客
12-16 912
ros中的无人机gps控制指点飞行。
ROS系列——mavros功能包中常用话题和服务介绍，包括消息名称、类型、头文件、成员变量、示例代码
曾经也曾立志
12-21 1万+
ROS系列——mavros功能包中常用话题和服务介绍，包括消息名称、类型、头文件、成员变量、示例代码
ROS--geometry_msgs/PoseStanped消息解读
热门推荐
z1872385的博客
08-25 1万+
http://wiki.ros.org/geometry_msgs 可以看到不同类型的消息，点击PoseStamped进入PoseStamped message 页面 1、通过包含头文件可以调用该类型的消息 #include "geometry_msgs/PoseStamped.h" 2、通过定义msg对象调用数据类型为std_msgs/Header,geometry_msgs/Pose的两个成员header, pose geometry_msgs::PoseStamped msg msg.heade
能用ros编写无人机通信程序吗
02-13
是的，可以使用ROS (Robot Operating System) 来编写无人机通信程序。ROS是一个开源机器人操作系统，为机器人开发提供了很多强大的功能，包括通信、导航、规划等。ROS也可以用于编写无人机控制程序，并且已经有很多现成的无人机相关的ROS包可以使用。因此，使用ROS编写无人机通信程序是可行的。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值