超维小课堂 | 7、ROS使用offboard模式控制无人机定点悬停源码分析

引言:ROS使用offboard模式控制无人机进入定点悬停是学习ROS无人机控制的最经典的基本功能之一。基于此,本篇主要对此处的控制流程著一个简要的代码分析。(室内外通用代码)

顾名思义:offboard模式下的定点悬停是指通过机载电脑ROS发布指定的目标点,无人机按照程序发布的目标位置进行PID控制,到达后目标点后保持悬停不动。源码分析可以查看代码注释即可。

此处为视频详解:

源码视频详解:

特别注意:由于气压计以及GPS等定位存在漂移,会导致无人机在飞行过程中偏离程序设置的目标点。基于此,我们的代码已经对初始化漂移做了抵消,相对稳定,此处也是本此代码讲解的着重点。请确保使用我们的程序进行测试,否则可能造成无人机的失控

//包含ROS和MAVROS相关头文件 
#include <string> 
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>
#include <geometry_msgs/Twist.h>//室外宏定义飞行高度2米,室内可适当降低高度
#define ALTITUDE  2mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{current_state = *msg;
}//定义变量,用于接收无人机的里程计和姿态信息
tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false;
nav_msgs::Odometry local_pos;
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);
//回调函数接收无人机的里程计和姿态信息
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{local_pos = *msg;//程序启动后获取一次最新的GPS和气压计漂移数据作为初始值if (flag_init_position==false && (local_pos.pose.pose.position.z!=0)){init_position_x_take_off = local_pos.pose.pose.position.x;init_position_y_take_off = local_pos.pose.pose.position.y;init_position_z_take_off = local_pos.pose.pose.position.z;flag_init_position = true;          }//四元数转为欧拉角,后续程序使用tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);   tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}int main(int argc, char **argv)
{//初始化节点,ROSAPI接口ros::init(argc, argv, "offboard_single_position");//创建nh句柄,ROSAPI接口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::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);//创建无人机解锁客户端,用于想飞控请求解锁命令ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//创建模式切换客户端,用于ROS程序想底层飞控请求进入offbaord或者其他模式ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");//offboard模式下,需要保持2Hz以上频率的心跳包,此处设置为20Hz,可适当调整ros::Rate rate(20.0);//等待连接飞控,连接后再执行后续部分while(ros::ok() && !current_state.connected){ros::spinOnce();rate.sleep();}//设置预发布位置,有了预发布位置,才能切入到offboard模式geometry_msgs::PoseStamped pose;pose.pose.position.x =init_position_x_take_off + 0;pose.pose.position.y =init_position_y_take_off + 0;pose.pose.position.z =init_position_z_take_off + ALTITUDE;//发布者发布期望位置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;//获取系统当前时间给变量last_requestros::Time last_request = ros::Time::now();//此处满足一次请求进入offboard模式即可,官方例成循环切入offboard会导致无人机无法使用遥控器控制while(ros::ok()){//请求进入OFFBOARD模式,每隔5秒请求一次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();flag_init_position = false;         }else {//请求解锁,每隔5秒请求一次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();flag_init_position = false;         }}//1、添加高度判断,使得无人机跳出模式切换循环,误差小于0.5米即认为已经达到if(fabs(local_pos.pose.pose.position.z- init_position_z_take_off -ALTITUDE)<0.5){   //延时三秒后推出循环if(ros::Time::now() - last_request > ros::Duration(3.0)){break;}}//2、添加时间判断,使得无人机跳出模式切换循环if(ros::Time::now() - last_request > ros::Duration(8.0)){break;}//此处添加是为增加无人机的安全性能,在实际测试过程中,采用某款国产的GPS和飞控,气压计和GPS定位误差极大,//导致了无人机起飞后直接飘走,高度和位置都不正常,无法跳出模式循环,导致遥控且无法接管//因此增加了时间判断,确保无人机在切入offboard模式和解锁后,确保任何情况下,8秒后遥控器都能切入其他模式接管无人机  //注意:一定要确定GPS和飞控传感器都是正常的//注意:一定要确定GPS和飞控传感器都是正常的//注意:一定要确定GPS和飞控传感器都是正常的//注意:一定要确定GPS和飞控传感器都是正常的//注意:一定要确定GPS和飞控传感器都是正常的//发布期望位置信息pose.pose.position.x =init_position_x_take_off + 0;pose.pose.position.y =init_position_y_take_off + 0;pose.pose.position.z =init_position_z_take_off + ALTITUDE;local_pos_pub.publish(pose);ros::spinOnce();rate.sleep();}       while(ros::ok()){//发布期望位置local_pos_pub.publish(pose);ros::spinOnce();//配合rate(20.0)使用,使得while按照20Hz的频率执行;rate.sleep();}return 0;
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/bicheng/27274.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

SpringSecurity6从入门到实战之SpringSecurity6自定义认证规则

SpringSecurity6从入门到实战之SpringSecurity6自定义认证规则 Spring Security 中默认所有的 http 请求都需要先认证通过后&#xff0c;才能访问。那么&#xff0c; 如何指定不需要认证就可以直接访问的资源呢&#xff1f;比如 用户的登录页面和注册页面&#xff0c;都是不需要…

ramda函数式编程库--可以帮助处理不可变数据

ramda 介绍深拷贝与浅拷贝示例代码**使用 clone 函数****clone 函数也可以用来深拷贝数组&#xff1a;** 介绍 ramda 是一个功能强大的函数式编程库&#xff0c;可以帮助你处理不可变数据。 clone 是 ramda 中的一个函数&#xff0c;用于深拷贝对象或数组。以下是关于 clone 函…

内网管理软件IP-Guard实施方案

1. 引言 本方案旨在指导企业如何实施内网管理软件IP-Guard&#xff0c;通过服务器的安装、员工客户端的部署以及基本策略的定制&#xff0c;实现对企业内部网络的有效管理和监控。IP-Guard是一款集成了设备管控、数据防泄漏、日志审计等多种功能于一体的内网安全管理软件&…

【面试干货】深入理解Java中的final关键字

【面试干货】深入理解Java中的final关键字 一、被 final 修饰的类二、被 final 修饰的方法三、被 final 修饰的变量四、被 final 修饰的常量 &#x1f496;The Begin&#x1f496;点点关注&#xff0c;收藏不迷路&#x1f496; 在Java中&#xff0c;final关键字有多种用途&…

C++ Primer 学习 -- Day 1

第 1、2 章知识点总结 1、Hello&#xff0c;World&#xff01;1.1、输入输出1.2、while(std::cin >> value) 2、变量和基本类型2.1.1、unsigned建议 2.1.2 类型转换建议建议 2.2.1 变量定义提醒列表初始化提醒 2.2.2 变量声明和定义提醒 2.3.1 引用引用特点 2.3.2 指针指…

快人一步!利用LLM实现数据处理自动化

使用大型语言模型&#xff08;LLM&#xff09;来帮助处理数据具有以下几个主要优势&#xff1a; 1. 自动化处理 LLM能够自动处理大量的重复性任务&#xff0c;减少人工干预&#xff0c;从而提高数据处理效率。例如&#xff0c;它可以自动完成数据加载、预处理、筛选、去重和合并…

RV32F\RV32D指令集

RV32F\RV32D指令集 F扩展1、浮点控制状态寄存器2、指令类型F扩展 F扩展增加了32个浮点寄存器f0-f31,每个32位宽,以及一个浮点控制和状态寄存器fcsr,其中包含浮点单元的工作模式和异常状态。FLEN=32表示F单精度浮点扩展,大多数浮点指令对浮点寄存器中的值进行操作。浮点加载…

基于JSP技术的电子商城系统

开头语&#xff1a; 你好&#xff0c;我是计算机学长码农猫哥。如果你对电子商城系统感兴趣或有相关开发需求&#xff0c;欢迎联系我。 开发语言&#xff1a;Java 数据库&#xff1a;MySQL 技术&#xff1a;JSP技术 工具&#xff1a;Eclipse、Tomcat 系统展示 首页 管理…

08_基于GAN实现人脸图像超分辨率重建实战_超分辨基础理论

1. 超分辨的概念与应用 我们常说的图像分辨率指的是图像长边像素数与图像短边像素数的乘积,比如iPhoneX手机拍摄照片的分辨率为 4032px3024px,为1200万像素。 显然,越高的分辨率能获得更清晰的成像。与之同时,分辨率越高也意味着更大的存储空间,对于空间非常有限的移动设…

Spring IOC 容器的构建流程?

Spring loc (Inversion of Control) 是一种设计模式&#xff0c;其中对象的创建和依赖关系由框架管理&#xff0c;而不是由应用程序直接管理。Spring loc容器是Spring框架的核心&#xff0c;它使用loC模式来管理应用程序中的对象 Spring loC容器的构建过程如下: 1.配置元数据…

【Pytorch】一文向您详细介绍 torch.nn.DataParallel() 的作用和用法

【Pytorch】一文向您详细介绍 torch.nn.DataParallel() 的作用和用法 下滑查看解决方法 &#x1f308; 欢迎莅临我的个人主页 &#x1f448;这里是我静心耕耘深度学习领域、真诚分享知识与智慧的小天地&#xff01;&#x1f387; &#x1f393; 博主简介&#xff1a;985高…

[xmake]xmake常用命令

xmake&#xff1a;编译程序代码 xmake run&#xff1a;运行编译好的程序 xmake create hello&#xff1a;用 xmake 生成自带的 hello world 模板项目 xmake f -p windows&#xff1a;指定构建项目的目标平台为windows xmake f --toolchainclang&#xff1a;工具链切换为cla…

UnityAPI学习之延时调用(Invoke)

延时调用&#xff08;Invoke&#xff09; 当我们进行简单函数的延时调用不想使用协程时&#xff0c;我们可以使用Invoke()函数 using System.Collections; using System.Collections.Generic; using UnityEngine;public class NO15_Invoke : MonoBehaviour {//显示在每次生成…

计算机专业:选择、挑战与未来

随着2024年高考的落幕&#xff0c;众多学子面临着选择大学专业的重大抉择。在众多专业中&#xff0c;计算机相关专业一直是备受瞩目的热门选择。计算机科学与技术、人工智能、网络安全、软件工程等专业在过去几年中&#xff0c;凭借广阔的就业前景和应用领域&#xff0c;吸引了…

黑色格调qss

/*菜单*/ /*QMenu Start*/ QMenu { text-align: left; width: 246px; color: rgb(200, 200, 200); background-color: rgb(35, 35, 35); margin: 0px; padding: 0px; border: 1px solid rgb(100, 100, 100); font: 15px "Microsoft Yahei…

WARNING: pip is configured with locations that require TLS/SSL

在pycharm中运行pip下载软件包遇到该问题&#xff1a;WARNING: pip is configured with locations that require TLS/SSL, however the ssl module in Python is not available 原因&#xff1a;没有安装openssl&#xff1b; 到https://slproweb.com/products/Win32OpenSSL.ht…

Python实现逻辑回归与判别分析--西瓜数据集

数据 数据data内容如下&#xff1a; 读取数据&#xff1a; import numpy as np import pandas as pd data pd.read_excel(D:/files/data.xlsx) 将汉字转化为01变量&#xff1a; label [] for i in data[好瓜]:l np.where(i 是,1,0)label.append(int(l)) data[label] lab…

【unity笔记】一、常见技术名词解析(HDRP/URP)

一、简介 在Unity中&#xff0c;Shader是用于控制图形渲染过程中顶点和像素处理的程序。Shader通常用于定义物体在屏幕上呈现的外观&#xff0c;包括光照、纹理、颜色和其他视觉效果。Shader编写在特定的着色语言中&#xff0c;如HLSL&#xff08;High-Level Shading Language…

计算机网络重要知识点

OSI 七层模型 是国际标准化组织提出的一个网络分层模型。 TCP/IP 四层模型 是目前被广泛采用的一种模型,我们可以将 TCP / IP 模型看作是 OSI 七层模型的精简版本&#xff0c;由以下 4 层组成&#xff1a; 应用层传输层网络层网络接口层 复杂的系统需要分层&#xff0c;因为每…

Vue中实现锚点滚动至指定区域

简介 本文将指导你如何使用Vue.js构建一个通过实现一个智能化的侧边栏导航功能&#xff0c;让用户能够一键跳转到不同类型的报告区域&#xff0c;大大提升操作便捷性。 项目背景 想象一下&#xff0c;你是xxxx科技有限公司的一名前端开发工程师&#xff0c;负责优化公司管理…