PX4官方手册给的mavros-offborad代码页面复制,做备份用,方便自己日后随时翻看。

本文主要是介绍PX4官方手册给的mavros-offborad代码页面复制,做备份用,方便自己日后随时翻看。,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

PX4官方手册给的mavros-offborad代码页面复制,做备份用,方便自己日后随时翻看。

 也就是这个页面

摘自:https://dev.px4.cc/master/en/ros/mavros_offboard.html

MAVROS Offboard control example

This tutorial shows the basics of Offboard control with MAVROS, using an Iris quadcopter simulated in Gazebo/SITL. At the end of the tutorial, you should see the same behaviour as in the video below, i.e. a slow takeoff to an altitude of 2 meters.

Offboard control is dangerous. If you are operating on a real vehicle be sure to have a way of gaining back manual control in case something goes wrong.

 

This example uses C++. Similar examples in Python can be found here: integrationtests/python_src/px4_it/mavros.

Code

Create the offb_node.cpp file in your ROS package (by also adding it to your CMakeList.txt so it is compiled), and paste the following inside it:

/*** @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;
}int main(int argc, char **argv)
{ros::init(argc, argv, "offb_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");//the setpoint publishing rate MUST be faster than 2Hzros::Rate rate(20.0);// wait for FCU connectionwhile(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 = 2;//send a few setpoints before startingfor(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();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();}return 0;
}

Code explanation

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

The mavros_msgs package contains all of the custom messages required to operate services and topics provided by the MAVROS package. All services and topics as well as their corresponding message types are documented in the mavros wiki.

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

We create a simple callback which will save the current state of the autopilot. This will allow us to check connection, arming and Offboard flags.

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");

We instantiate a publisher to publish the commanded local position and the appropriate clients to request arming and mode change. Note that for your own system, the "mavros" prefix might be different as it will depend on the name given to the node in it's launch file.

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

PX4 has a timeout of 500ms between two Offboard commands. If this timeout is exceeded, the commander will fall back to the last mode the vehicle was in before entering Offboard mode. This is why the publishing rate must be faster than 2 Hz to also account for possible latencies. This is also the same reason why it is recommended to enter Offboard mode from Position mode, this way if the vehicle drops out of Offboard mode it will stop in its tracks and hover.

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

Before publishing anything, we wait for the connection to be established between MAVROS and the autopilot. This loop should exit as soon as a heartbeat message is received.

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

Even though the PX4 Pro Flight Stack operates in the aerospace NED coordinate frame, MAVROS translates these coordinates to the standard ENU frame and vice-versa. This is why we set z to positive 2.

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

Before entering Offboard mode, you must have already started streaming setpoints. Otherwise the mode switch will be rejected. Here, 100 was chosen as an arbitrary amount.

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

We set the custom mode to OFFBOARD. A list of supported modes is available for reference.

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;ros::Time last_request = ros::Time::now();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();
}

The rest of the code is pretty self explanatory. We attempt to switch to Offboard mode, after which we arm the quad to allow it to fly. We space out the service calls by 5 seconds so to not flood the autopilot with the requests. In the same loop, we continue sending the requested pose at the appropriate rate.

This code has been simplified to the bare minimum for illustration purposes. In larger systems, it is often useful to create a new thread which will be in charge of periodically publishing the setpoints.

这篇关于PX4官方手册给的mavros-offborad代码页面复制,做备份用,方便自己日后随时翻看。的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/507196

相关文章

利用Python调试串口的示例代码

《利用Python调试串口的示例代码》在嵌入式开发、物联网设备调试过程中,串口通信是最基础的调试手段本文将带你用Python+ttkbootstrap打造一款高颜值、多功能的串口调试助手,需要的可以了... 目录概述:为什么需要专业的串口调试工具项目架构设计1.1 技术栈选型1.2 关键类说明1.3 线程模

Python Transformers库(NLP处理库)案例代码讲解

《PythonTransformers库(NLP处理库)案例代码讲解》本文介绍transformers库的全面讲解,包含基础知识、高级用法、案例代码及学习路径,内容经过组织,适合不同阶段的学习者,对... 目录一、基础知识1. Transformers 库简介2. 安装与环境配置3. 快速上手示例二、核心模

Java的栈与队列实现代码解析

《Java的栈与队列实现代码解析》栈是常见的线性数据结构,栈的特点是以先进后出的形式,后进先出,先进后出,分为栈底和栈顶,栈应用于内存的分配,表达式求值,存储临时的数据和方法的调用等,本文给大家介绍J... 目录栈的概念(Stack)栈的实现代码队列(Queue)模拟实现队列(双链表实现)循环队列(循环数组

使用Java将DOCX文档解析为Markdown文档的代码实现

《使用Java将DOCX文档解析为Markdown文档的代码实现》在现代文档处理中,Markdown(MD)因其简洁的语法和良好的可读性,逐渐成为开发者、技术写作者和内容创作者的首选格式,然而,许多文... 目录引言1. 工具和库介绍2. 安装依赖库3. 使用Apache POI解析DOCX文档4. 将解析

C++使用printf语句实现进制转换的示例代码

《C++使用printf语句实现进制转换的示例代码》在C语言中,printf函数可以直接实现部分进制转换功能,通过格式说明符(formatspecifier)快速输出不同进制的数值,下面给大家分享C+... 目录一、printf 原生支持的进制转换1. 十进制、八进制、十六进制转换2. 显示进制前缀3. 指

使用Python实现全能手机虚拟键盘的示例代码

《使用Python实现全能手机虚拟键盘的示例代码》在数字化办公时代,你是否遇到过这样的场景:会议室投影电脑突然键盘失灵、躺在沙发上想远程控制书房电脑、或者需要给长辈远程协助操作?今天我要分享的Pyth... 目录一、项目概述:不止于键盘的远程控制方案1.1 创新价值1.2 技术栈全景二、需求实现步骤一、需求

Java中Date、LocalDate、LocalDateTime、LocalTime、时间戳之间的相互转换代码

《Java中Date、LocalDate、LocalDateTime、LocalTime、时间戳之间的相互转换代码》:本文主要介绍Java中日期时间转换的多种方法,包括将Date转换为LocalD... 目录一、Date转LocalDateTime二、Date转LocalDate三、LocalDateTim

jupyter代码块没有运行图标的解决方案

《jupyter代码块没有运行图标的解决方案》:本文主要介绍jupyter代码块没有运行图标的解决方案,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录jupyter代码块没有运行图标的解决1.找到Jupyter notebook的系统配置文件2.这时候一般会搜索到

Python通过模块化开发优化代码的技巧分享

《Python通过模块化开发优化代码的技巧分享》模块化开发就是把代码拆成一个个“零件”,该封装封装,该拆分拆分,下面小编就来和大家简单聊聊python如何用模块化开发进行代码优化吧... 目录什么是模块化开发如何拆分代码改进版:拆分成模块让模块更强大:使用 __init__.py你一定会遇到的问题模www.

springboot循环依赖问题案例代码及解决办法

《springboot循环依赖问题案例代码及解决办法》在SpringBoot中,如果两个或多个Bean之间存在循环依赖(即BeanA依赖BeanB,而BeanB又依赖BeanA),会导致Spring的... 目录1. 什么是循环依赖?2. 循环依赖的场景案例3. 解决循环依赖的常见方法方法 1:使用 @La