ROS无人机初始化GPS定位漂移误差，确保无人机稳定飞行

引言： 由于GPS在室外漂移的误差比较大，在长时间静止后启动，程序发布的位置可能已经和预期的位置相差较大，导致无法完成任务，尤其是气压计的数据不准，可能会导致无人机不能起飞或者一飞冲天。本文主要是在进行程序控制的时候，首先拿到最新的漂移误差，在此基础上进行程序控制，可以保证程序的稳定运行。

步骤一：创建订阅者订阅无人机的里程计信息

//创建一个Subscriber订阅者，订阅名为/mavros/local_position/odom的topic，注册回调函数local_pos_cb
ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);

步骤二：创建相关变量，获取无人机的实时位置信息

这里定义了较多的变量，最重要的是位置和标志为，其余的可以自行设置

//定义变量，用于接收无人机的里程计信息
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)
{local_pos = *msg;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);
}

注：启动这个控制节点后，或订阅最新的初始位置，并且通过标志为控制只订阅一次，这个值就是最新的漂移值

步骤四：发布目标点，一定要把获取到的初始漂移值加上，如下：

//发布期望位置信息
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);

这里ALTITUDE是我宏定义的0.5，表明无人机飞0.5米高度即可。室外使用的话，最好高度设置大于1米

步骤五：整体代码如下：

//包含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>#define ALTITUDE  0.5//定义变量，用于接收无人机的状态信息
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& 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 state_cb(const mavros_msgs::State::ConstPtr& msg)
{current_state = *msg;
}//回调函数接收无人机的里程计信息
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{local_pos = *msg;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)
{//防止中文乱码setlocale(LC_ALL, "");//ROS节点初始化，节点名为offboard_single_positionros::init(argc, argv, "offboard_single_position");//创建节点句柄ros::NodeHandle nh;ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);//创建一个Subscriber订阅者，订阅名为/mavros/state的topic，注册回调函数state_cbros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);//创建一个Subscriber订阅者，订阅名为/mavros/local_position/odom的topic，注册回调函数local_pos_cbros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);//创建一个服务客户端，连接名为/mavros/cmd/arming的服务，用于请求无人机解锁ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//创建一个服务客户端，连接名为/mavros/set_mode的服务，用于请求无人机进入offboard模式ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");//设置话题发布频率，需要大于2Hz，飞控连接有500ms的心跳包ros::Rate rate(20.0);//等待连接到飞控while(ros::ok() && !current_state.connected){ros::spinOnce();rate.sleep();}//设置无人机的期望位置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;//send a few setpoints before startingfor(int i = 100; ros::ok() && i > 0; --i){local_pos_pub.publish(pose);ros::spinOnce();rate.sleep();}//定义客户端变量，设置为offboard模式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();while(ros::ok()){//请求进入OFFBOARD模式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();}}if(fabs(local_pos.pose.pose.position.z- init_position_z_take_off -ALTITUDE)<0.2){	if(ros::Time::now() - last_request > ros::Duration(3.0)){break;}}//发布期望位置信息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()){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();}return 0;
}

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

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

相关文章

53.网游逆向分析与插件开发-游戏反调试功能的实现-通过内核信息检测调试器

53.网游逆向分析与插件开发-游戏反调试功能的实现-通过内核信息检测调试器

码云地址&#xff08;master分支&#xff09;&#xff1a;https://gitee.com/dye_your_fingers/sro_-ex.git 码云版本号&#xff1a;b44fddef016fc1587eda40ca7f112f02a8289504 代码下载地址&#xff0c;在 SRO_EX 目录下&#xff0c;文件名为&#xff1a;SRO_Ex-通过内核信息…
阅读更多...
ceph集群搭建到应用从入门到熟练，包含块存储、对象存储、cephfs的应用、cephx认证等

ceph集群搭建到应用从入门到熟练，包含块存储、对象存储、cephfs的应用、cephx认证等

ceph-deploy比较适合生产环境&#xff0c;不是用cephadm搭建。相对麻烦一些&#xff0c;但是并不难&#xff0c;细节把握好就行&#xff0c;只是命令多一些而已。 ceph理论知识 略… ceph集群实验环境 服务器主机public网段IP&#xff08;对外服务&#xff09;cluster网段I…
阅读更多...
取出一个时间序列中每一个元素里的日期Series.dt.date()

取出一个时间序列中每一个元素里的日期Series.dt.date()

【小白从小学Python、C、Java】 【计算机等考500强证书考研】 【Python-数据分析】 取出一个时间序列中 每一个元素里的年月日 Series.dt.date [太阳]选择题 以下代码的输出结果中正确的是? import pandas as pd t pd.Series(pd.date_range("2023-12-28", periods4…
阅读更多...
OpenCV中使用Mask R-CNN实现图像分割的原理与技术实现方案

OpenCV中使用Mask R-CNN实现图像分割的原理与技术实现方案

本文详细介绍了在OpenCV中利用Mask R-CNN实现图像分割的原理和技术实现方案。Mask R-CNN是一种先进的深度学习模型&#xff0c;通过结合区域提议网络&#xff08;Region Proposal Network&#xff09;和全卷积网络&#xff08;Fully Convolutional Network&#xff09;&#xf…
阅读更多...
Java企业电子招投标系统源代码，支持二次开发，采用Spring cloud框架

Java企业电子招投标系统源代码，支持二次开发，采用Spring cloud框架

在数字化采购领域&#xff0c;企业需要一个高效、透明和规范的管理系统。通过采用Spring Cloud、Spring Boot2、Mybatis等先进技术&#xff0c;我们打造了全过程数字化采购管理平台。该平台具备内外协同的能力&#xff0c;通过待办消息、招标公告、中标公告和信息发布等功能模块…
阅读更多...
饥荒Mod 开发(二四)：制作一把万能工具

饥荒Mod 开发(二四)：制作一把万能工具

饥荒Mod 开发(二三)&#xff1a;显示物品栏详细信息 饥荒Mod 开发(二五)&#xff1a;常用组件 总结 源码 饥荒中的每种工具都有独特的功能&#xff0c;比如 斧头用来砍树&#xff0c; 铲子用来 挖东西&#xff0c;鹤嘴锄用来挖矿&#xff0c; 锤子可以敲碎东西&#xff0c;所以…
阅读更多...
2013年第二届数学建模国际赛小美赛A题数学与经济灾难解题全过程文档及程序

2013年第二届数学建模国际赛小美赛A题数学与经济灾难解题全过程文档及程序

2013年第二届数学建模国际赛小美赛 A题 数学与经济灾难 原题再现&#xff1a; 2008年的市场崩盘使世界陷入经济衰退&#xff0c;目前世界经济仍处于低迷状态&#xff0c;其原因是多方面的。其中之一是数学。   当然&#xff0c;并非只有金融界依赖于并非总是可靠的数学模型…
阅读更多...
编程艺术C代码学习注释

编程艺术C代码学习注释

一、左旋转字符串 详情见文章参考&#xff1a; github:程序员编程艺术csdn:程序员编程艺术第一章 1.暴力移位 void leftShift1(char * arr, int n) //n位移动的位数 {size_t tmpLen strlen(arr);char tmpChar;int i, j;if (n > 0){for (i 0; i < n; i){tmpChar …
阅读更多...
postman使用-03发送请求

postman使用-03发送请求

文章目录 请求1.新建请求2.选择请求方式3.填写请求URL4.填写请求参数get请求参数在params中填写&#xff08;填完后在url中会自动显示&#xff09;post请求参数在body中填写&#xff0c;根据接口文档请求头里面的content-type选择body中的数据类型post请求参数为json-选择raw-选…
阅读更多...
Flask 与微信小程序对接

Flask 与微信小程序对接

Flask 与微信小程序的对接 在 web/controllers/api中增建py文件&#xff0c;主要是给微信小程序使用的。 web/controllers/init.py # -*- coding: utf-8 -*- from flask import Blueprint route_api Blueprint( api_page,__name__ )route_api.route("/") def ind…
阅读更多...
软件测试/测试开发丨Pytest测试用例生命周期管理-Fixture

软件测试/测试开发丨Pytest测试用例生命周期管理-Fixture

1、Fixture 用法 Fixture 特点及优势 1&#xff64;命令灵活&#xff1a;对于 setup,teardown,可以不起这两个名字2&#xff64;数据共享&#xff1a;在 conftest.py 配置⾥写⽅法可以实现数据共享&#xff0c;不需要 import 导⼊。可以跨⽂件共享3&#xff64;scope 的层次及…
阅读更多...
Linux内核中断

Linux内核中断

Linux内核中断 ARM里当按下按键的时候&#xff0c;他首先会执行汇编文件start.s里面的异常向量表里面的irq,在irq里面进行一些操作。 再跳转到C的do_irq(); 进行操作&#xff1a;1&#xff09;判断中断的序号&#xff1b;2&#xff09;处理中断&#xff1b;3&#xff09;清除中…
阅读更多...
2024美赛数学建模思路A题B题C题D题E题F题思路汇总 选题分析

2024美赛数学建模思路A题B题C题D题E题F题思路汇总 选题分析

文章目录 1 赛题思路2 美赛比赛日期和时间3 赛题类型4 美赛常见数模问题5 建模资料 1 赛题思路 (赛题出来以后第一时间在CSDN分享) https://blog.csdn.net/dc_sinor?typeblog 2 美赛比赛日期和时间 比赛开始时间&#xff1a;北京时间2024年2月2日&#xff08;周五&#xff…
阅读更多...
模型部署之——ONNX模型转RKNN

模型部署之——ONNX模型转RKNN

提示&#xff1a;这里可以添加学习目标 提示&#xff1a;文章写完后&#xff0c;目录可以自动生成&#xff0c;如何生成可参考右边的帮助文档 文章目录 一、加载Docker镜像二、转换脚本 一、加载Docker镜像 加载rknn官方提供的基于x86架构下模型转换的镜像文件&#xff0c;生成…
阅读更多...
【JavaScript】new原理解析

【JavaScript】new原理解析

✨ 专栏介绍 在现代Web开发中&#xff0c;JavaScript已经成为了不可或缺的一部分。它不仅可以为网页增加交互性和动态性&#xff0c;还可以在后端开发中使用Node.js构建高效的服务器端应用程序。作为一种灵活且易学的脚本语言&#xff0c;JavaScript具有广泛的应用场景&#x…
阅读更多...
视频人脸识别马赛克处理

视频人脸识别马赛克处理

文章目录 前言一、实现思路&#xff1f;二、Coding三、实现效果 前言 前面几篇文章我们尝试了使用opencv完成图像人脸识别以及识别后贴图或者打马赛克的方法。 偶尔我们也会有需求在视频中将人脸马赛克化&#xff0c;opencv也提供了相应的方法来实现这个功能。 一、实现思路&a…
阅读更多...
贪心算法的运用

贪心算法的运用

贪心算法&#xff08;Greedy Algorithm&#xff09;是一种常用的算法思想&#xff0c;通常用来解决最优化问题。可以使用贪心算法来解决一些问题&#xff0c;例如找零钱、任务调度等。 以找零钱为例&#xff0c;简单介绍一下贪心算法的应用 假设有一堆不同面额的硬币&#xff…
阅读更多...
netty源码：（40）ReplayingDecoder

netty源码：（40）ReplayingDecoder

ReplayingDecoder是ByteToMessageDecoder的子类&#xff0c;我们继承这个类时&#xff0c;也要实现decode方法&#xff0c;示例如下&#xff1a; package cn.edu.tju;import io.netty.buffer.ByteBuf; import io.netty.channel.ChannelHandlerContext; import io.netty.handle…
阅读更多...
Ubuntu20.04服务器使用教程（安装教程、常用命令、故障排查）持续更新中.....

Ubuntu20.04服务器使用教程（安装教程、常用命令、故障排查）持续更新中.....

安装教程&#xff08;系统、驱动、CUDA、CUDNN、Pytorch、Timeshift、ToDesk&#xff09; 制作U盘启动盘&#xff0c;并安装系统 在MSDN i tell you下载Ubuntu20.04 Desktop 版本&#xff0c;并使用Rufus制作UEFI启动盘&#xff0c;参考UEFI安装Ubuntu使用GPTUEFI模式安装&am…
阅读更多...
【IoT网络层】STM32 + ESP8266 +MQTT + 阿里云物联网平台 |开源，附资料|

【IoT网络层】STM32 + ESP8266 +MQTT + 阿里云物联网平台 |开源，附资料|

目标&#xff1a;实现STM32连接阿里云物联网平台发送数据同时接收数据&#xff0c;IOT studio界面显示数据。具体来说&#xff1a;使用ESP8266 ESP-01来连接网络&#xff0c;获取设备数据发送到阿里云物联网平台并显示且oled显示屏当前的设备数据&#xff0c;通过IOT studio界面…
阅读更多...
最新文章

Copyright @ 2022~2023