px4 运行mavros offboard仿真例程
2024-04-22 09:09:04
文章目录
- 前言
- 一、创建工作空间
- 二、创建功能包
- 三、单独进行位置、速度、加速度控制。
- 四、组合进行位置、速度、加速度控制。
前言
Ubuntu 18.04
ROS Melodic
PX4 1.13.0
一、创建工作空间
mkdir -p ~/mavros_ws/src
cd ~/mavros_ws/src
catkin_init_workspace
cd ~/mavros_ws/
catkin_make
然后
gedit .bashrc
将source ~/mavros_ws/devel/setup.bash加到.bashrc中并保存然后source ~/.bashrc
然后执行
echo $ROS_PACKAGE_PATH
如果会显示工作空间路径，说明上面的步骤执行正确
mavros_ws/src:以及ROS安装路径/opt/ros/melodic/share
二、创建功能包
cd ~/mavros_ws/src
catkin_create_pkg offboard roscpp std_msgs geometry_msgs mavros_msgs
cd offboard/src
gedit offboard_node.cpp
复制以下代码进去
#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;
}
cd ~/mavros_ws/src/offboard
gedit CMakeLists.txt
在最末尾添加如下内容后保存退出
add_executable(${PROJECT_NAME}_node src/offboard_node.cpp)
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})
然后编译
cd ~/mavros_ws
catkin_make
然后先
roslaunch px4 mavros_posix_sitl.launch
由于仿真中没有遥控器，会触发PX4的失控保护，导致无法进入offboard，因此需要在地面站上设置一下遥控器。
打开QGC地面站，勾选“虚拟游戏手柄”
再启动offboard节点
rosrun offboard offboard_node
正常的话无人机会起飞至2米
如果执行上面两步的时候提示找不到包或者launch文件，可能是因为第一步有问题，检查第一步。
注意export 开头的环境变量必须放在最下面，source开头的如果放在最下的话，会导致无法使用roslaunch px4 mavros_posix_sitl.launch，以及直接启动launch文件是提示找不到路径
代码功能包地址：https://gitee.com/Mbot/mavros.git
三、单独进行位置、速度、加速度控制。
第二步通过位置控制使无人机上升2米，如果想控制速度、加速度，可以用下面的代码替换offboard_node.cpp中的内容：
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/PositionTarget.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::Publisher local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);ros::Publisher local_acc_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/mavros/setpoint_accel/accel", 10);ros::Publisher AttitudeTarget_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);ros::Publisher PositionTarget_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/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;geometry_msgs::TwistStamped vel_linear;vel_linear.twist.linear.x=1;vel_linear.twist.linear.y=1;vel_linear.twist.linear.z=1;geometry_msgs::Vector3Stamped acc;acc.vector.x=1;acc.vector.y=1;acc.vector.z=1;//mavros_msgs::PositionTarget PositionTarget;mavros_msgs::AttitudeTarget AttitudeTarget;local_pos_pub.publish(pose);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();int flag=0;while(ros::ok()){if( current_state.mode != "OFFBOARD" &&(ros::Time::now() - last_request > ros::Duration(1.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(1.0))){if( arming_client.call(arm_cmd) &&arm_cmd.response.success){ROS_INFO("Vehicle armed");}last_request = ros::Time::now();}}if(ros::Time::now() - last_request > ros::Duration(10.0)){flag++;last_request = ros::Time::now();}if(flag==0){local_pos_pub.publish(pose);ROS_INFO("position only");}if(flag==1){local_vel_pub.publish(vel_linear);ROS_INFO("velocity only");}if(flag==2){local_acc_pub.publish(acc);ROS_INFO("acceleration_or_force only");}// if(flag==0)
// {
// PositionTarget.position.x=0;
// PositionTarget.position.y=0;
// PositionTarget.position.z=2;
// PositionTarget.coordinate_frame=PositionTarget.FRAME_LOCAL_NED;
// PositionTarget.type_mask=PositionTarget.IGNORE_VX|PositionTarget.IGNORE_VY|PositionTarget.IGNORE_VZ|PositionTarget.IGNORE_AFX|PositionTarget.IGNORE_AFY|PositionTarget.IGNORE_AFZ|PositionTarget.IGNORE_YAW|PositionTarget.IGNORE_YAW_RATE;
// PositionTarget_pub.publish(PositionTarget);
// ROS_INFO("position");
// }
// if(flag==1)
// {
// PositionTarget.velocity.x=1;
// PositionTarget.velocity.y=1;
// PositionTarget.position.z=3;
// PositionTarget.type_mask=PositionTarget.IGNORE_PX|PositionTarget.IGNORE_PY|PositionTarget.IGNORE_VZ|PositionTarget.IGNORE_AFX|PositionTarget.IGNORE_AFY|PositionTarget.IGNORE_AFZ|PositionTarget.IGNORE_YAW|PositionTarget.IGNORE_YAW_RATE;
// PositionTarget.coordinate_frame=PositionTarget.FRAME_LOCAL_NED;
// PositionTarget_pub.publish(PositionTarget);
// ROS_INFO("velocity and alt");
// }
// if(flag==2)
// {
// PositionTarget.acceleration_or_force.x=1;
// PositionTarget.acceleration_or_force.y=1;
// PositionTarget.position.z=2;
// PositionTarget.type_mask=PositionTarget.IGNORE_PX|PositionTarget.IGNORE_PY|PositionTarget.IGNORE_VZ|PositionTarget.IGNORE_VX|PositionTarget.IGNORE_VY|PositionTarget.IGNORE_AFZ|PositionTarget.IGNORE_YAW|PositionTarget.IGNORE_YAW_RATE;
// PositionTarget.coordinate_frame=PositionTarget.FRAME_LOCAL_NED;
// PositionTarget_pub.publish(PositionTarget);
// ROS_INFO("acceleration_or_force and alt");
// }ros::spinOnce();rate.sleep();}return 0;
}
无人机会先进行位置控制，再进行速度控制，再进行加速度控制
四、组合进行位置、速度、加速度控制。
第三节中只能单独的进行位置、速度、加速度控制，例如想控制速度的时候只能控制XYZ的速度，有的时候我们想组合进行位置、速度、加速度控制，例如控制高度不变，在XY方向进行速度控制。可以用下面的代码替换offboard_node.cpp中的内容：
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/PositionTarget.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::Publisher local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);ros::Publisher local_acc_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/mavros/setpoint_accel/accel", 10);ros::Publisher AttitudeTarget_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);ros::Publisher PositionTarget_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/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;geometry_msgs::TwistStamped vel_linear;vel_linear.twist.linear.x=1;vel_linear.twist.linear.y=1;vel_linear.twist.linear.z=1;geometry_msgs::Vector3Stamped acc;acc.vector.x=1;acc.vector.y=1;acc.vector.z=1;//mavros_msgs::PositionTarget PositionTarget;mavros_msgs::AttitudeTarget AttitudeTarget;local_pos_pub.publish(pose);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();int flag=0;while(ros::ok()){if( current_state.mode != "OFFBOARD" &&(ros::Time::now() - last_request > ros::Duration(1.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(1.0))){if( arming_client.call(arm_cmd) &&arm_cmd.response.success){ROS_INFO("Vehicle armed");}last_request = ros::Time::now();}}if(ros::Time::now() - last_request > ros::Duration(10.0)){flag++;last_request = ros::Time::now();}// if(flag==0)
// {
// local_pos_pub.publish(pose);
// ROS_INFO("position only");
// }
// if(flag==1)
// {
// local_vel_pub.publish(vel_linear);
// ROS_INFO("velocity only");
// }
// if(flag==2)
// {
// local_acc_pub.publish(acc);
// ROS_INFO("acceleration_or_force only");
// }if(flag==0){PositionTarget.position.x=0;PositionTarget.position.y=0;PositionTarget.position.z=2;PositionTarget.coordinate_frame=PositionTarget.FRAME_LOCAL_NED;PositionTarget.type_mask=PositionTarget.IGNORE_VX|PositionTarget.IGNORE_VY|PositionTarget.IGNORE_VZ|PositionTarget.IGNORE_AFX|PositionTarget.IGNORE_AFY|PositionTarget.IGNORE_AFZ|PositionTarget.IGNORE_YAW|PositionTarget.IGNORE_YAW_RATE;PositionTarget_pub.publish(PositionTarget);ROS_INFO("position");}if(flag==1){PositionTarget.velocity.x=1;PositionTarget.velocity.y=1;PositionTarget.position.z=3;PositionTarget.type_mask=PositionTarget.IGNORE_PX|PositionTarget.IGNORE_PY|PositionTarget.IGNORE_VZ|PositionTarget.IGNORE_AFX|PositionTarget.IGNORE_AFY|PositionTarget.IGNORE_AFZ|PositionTarget.IGNORE_YAW|PositionTarget.IGNORE_YAW_RATE;PositionTarget.coordinate_frame=PositionTarget.FRAME_LOCAL_NED;PositionTarget_pub.publish(PositionTarget);ROS_INFO("velocity and alt");}if(flag==2){PositionTarget.acceleration_or_force.x=1;PositionTarget.acceleration_or_force.y=1;PositionTarget.position.z=2;PositionTarget.type_mask=PositionTarget.IGNORE_PX|PositionTarget.IGNORE_PY|PositionTarget.IGNORE_VZ|PositionTarget.IGNORE_VX|PositionTarget.IGNORE_VY|PositionTarget.IGNORE_AFZ|PositionTarget.IGNORE_YAW|PositionTarget.IGNORE_YAW_RATE;PositionTarget.coordinate_frame=PositionTarget.FRAME_LOCAL_NED;PositionTarget_pub.publish(PositionTarget);ROS_INFO("acceleration_or_force and alt");}ros::spinOnce();rate.sleep();}return 0;
}
代码中在高度上一直使用位置控制，在XY方向上分别使用位置、速度、加速度控制。需要注意的是，在组合控制时需要设置type_mask，除了需要控制的几个量，其他量都要设置为IGNORE。
px4 运行mavros offboard仿真例程相关推荐
- ROS实践--运行小海龟仿真例程
ROS实践–运行小海龟仿真例程 由于在之前就已经安装了ROS,所以本文章只有小海龟的相关实验. 1.启动小海龟及其键盘控制 1.1 启动rosmaster 打开一个终端(快捷键ctrl+alt+t), ...
- mavros的常用服务介绍（怪不得普罗米修斯解锁和切offboard是通过调用服务实现的，PX4官方的offboard示例代码也是通过调用服务切offboard的，原来服务是在MAVROS里写的！）
怪不得普罗米修斯解锁和切offboard是通过调用服务实现的(输入999的方式),PX4官方的offboard示例代码也是通过调用服务切offboard的,我之前还好奇服务是写在哪里的,原来是MAVR ...
- 无人机仿真—PX4编译，gazebo仿真及简单off board控制模式下无人机起飞
无人机仿真-PX4编译,gazebo仿真及简单off board控制模式下无人机起飞 前言 在上篇记录中,已经对整体的PX4仿真环境有了一定的了解,现如今就要开始对无人机进行起飞等仿真环境工作,在整体 ...
- PX4无人机ROS下仿真开发
PX4无人机ROS下仿真开发 Overview Simulation Px4_control Slam Map Image_process Planning Volans 项目地址volans 注:有 ...
- Airsim环境下的px4硬件在环仿真
文章目录 前言 1.Airsim和硬件在环介绍 2.硬件在环测试的准备工作(硬件和软件) 2.1 usb-ttl转接线的制作: 2.2 px4的ttl端口波特率设置 3 硬件在环测试过程中的问题 4 ...
- PX4位置控制offboard模式说明
offboard模式的开发及应用 一.px4固件的模式 px4固件支持10几种飞行模式,从代码结构上分析,分为基本模式.自定义模式和自定义子模式. 1.基本模式 基本模式又分为,位置控制模式.自稳模式 ...
- matlab地球卫星模型,地球卫星三维运行轨道MATLAB仿真
地球卫星三维运行轨道MATLAB仿真 1.问题的描述 3 轨道上运行的地球卫星,根据牛顿第二定律F=ma以及万有引力定律F=-GmME*r/r, 3可得a=-GME*r/r,即 x''= -GME*x ...
- 基于单片机的TLC稳压电源系统设计-基于单片机大脑运算能力智力测试仪-基于单片机超声波测距系统仿真设计（报告 PCB 原理图）-基于单片机超高精度电参数测试设计-基于单片机变电站变压器运行参数监测仿真
1316基于单片机的TLC稳压电源系统设计-毕设课设仿真资料 三极管射极电压是稳压电源的输出电压,可以接用电器或负载,这个电压值通过TLC549(A/D,同TLC548)数据转换后,送往单片机处理并显 ...
- px4通过mavros+wifi+板载计算机连接地面站
px4通过mavros+wifi+板载计算机连接地面站 https://blog.csdn.net/zouxu634866/article/details/106835967
最新文章
- python处理excel-使用python将数据写入excel
- Python的try... excep异常捕捉机制
- 用VLC读取摄像头产生RTSP流，DSS主动取流转发（一）
- 训练数据集时为何要先加载预训练模型作为初始化，这样做有何好处？
- php zip怎么安装,php如何安装zip模块？（方法介绍）
- Linux环境编译时报错/lib64/libdl.so.2: could not read symbols: Invalid operation
- java rsa 128_如何用java实现128位密钥的RSA算法
- 观察者模式-对象行为型
- fiddler抓包第一课--手机数据抓包
- HDU3787 A+B【进制】
- 加密算法实现数据通讯
- Winform开发技术详解 - 应用环境 相关技术介绍
- 计算机随机数是如何生成的？（平分取中法、线性同余法）
- 思科OSPF配置实例(转)
- ACM Plan UVa - 10102 The path in the colored field
- 互联网的一些事 - 科学与灵修：创业者精神修炼问题
- 一个关于微信微信免费自动投票软件与专门投票的微信群的教程介绍
- Dilated Residual Networks
- canvas+gif.js打造自己的数字雨头像
- Qt编写安防视频监控系统49-多数据库支持