解决Ardupilot+gazebo+mavros在仿真状态下无人机能解锁，但是不能起飞的问题

1. 问题

在进行无人机仿真时，我采用的是Ardupilot+gazebo+mavros和Ubuntu 18.04环境。在通过编写ros程序控制无人机在gazebo中起飞时，无人机能解锁，它的桨叶可以旋转，但是不能执行起飞动作，无人机不停地解锁然后又上锁，始终不能起飞。

查阅了网络上的各种资料和博客大家给出来的代码都是如下代码，该代码是px4固件官网提供的代码，由于我使用的是ardupilot环境，所以并不能成功起飞。


#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_INFO("HELLO");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 = "GUIDED";mavros_msgs::CommandBool arm_cmd;arm_cmd.request.value = true;ros::Time last_request = ros::Time::now();while(ros::ok()){if( current_state.mode != "GUIDED" &&(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;
}

2. 解决办法

查阅了网上的资料，最终确认在使用如下代码发布无人机位置控制时，必须先执行无人机起飞动作时候，这条命令才会生效。

ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);

所以将代码进行修改，并且增加无人机更复杂的动作，最终修改的代码如下：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>mavros_msgs::State current_state;// 无人机状态回调函数
void state_callback(const mavros_msgs::State::ConstPtr &msg) {current_state = *msg;
}int main(int argc, char **argv) {ros::init(argc, argv, "offboard");ros::NodeHandle n;ros::Subscriber state_sub = n.subscribe("/mavros/state", 10, state_callback);ros::Rate rate(20);ROS_INFO("Initializing...");while (ros::ok() && !current_state.connected) {ros::spinOnce();rate.sleep();}ROS_INFO("Connected.");// 设置无人机模式为GUIDEDros::ServiceClient cl = n.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");mavros_msgs::SetMode srv_setMode;srv_setMode.request.base_mode = 0;srv_setMode.request.custom_mode = "GUIDED";if (cl.call(srv_setMode)) {ROS_INFO("setmode send ok %d value:", srv_setMode.response.mode_sent);} else {ROS_ERROR("Failed SetMode");return -1;}// 解锁无人机ros::ServiceClient arming_cl = n.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");mavros_msgs::CommandBool srv;srv.request.value = true;if (arming_cl.call(srv)) {ROS_INFO("ARM send ok %d", srv.response.success);} else {ROS_ERROR("Failed arming or disarming");}// 发布起飞的指令，向上飞行10米ros::ServiceClient takeoff_cl = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");mavros_msgs::CommandTOL srv_takeoff;srv_takeoff.request.altitude = 10;srv_takeoff.request.latitude = 0;srv_takeoff.request.longitude = 0;srv_takeoff.request.min_pitch = 0;srv_takeoff.request.yaw = 0;if (takeoff_cl.call(srv_takeoff)) {ROS_INFO("srv_takeoff send ok %d", srv_takeoff.response.success);} else {ROS_ERROR("Failed Takeoff");}// 等待10秒.  这一行代码非常重要，如果没有则不能飞行sleep(10);geometry_msgs::PoseStamped pose;pose.header.frame_id = "map";pose.pose.position.x = 0;pose.pose.position.y = 0;pose.pose.position.z = 2;geometry_msgs::Twist vel;vel.linear.x = 0.0;vel.linear.y = 0.0;vel.linear.z = 0.0;vel.angular.x = 0.0;vel.angular.y = 0.0;vel.angular.z = 0.0;ros::Publisher local_vel_pub = n.advertise<geometry_msgs::Twist>("mavros/setpoint_velocity/cmd_vel_unstamped", 10);ros::Publisher local_pos_pub = n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);ros::Time time_start = ros::Time::now();//转圈圈飞行while (ros::ok()) {pose.pose.position.x = sin(2.0 * M_PI * 2.0 * (ros::Time::now() - time_start).toSec());pose.pose.position.y = cos(2.0 * M_PI * 2.0 * (ros::Time::now() - time_start).toSec());vel.linear.x = 2.0 * M_PI * 2.0 * cos(2.0 * M_PI * 0.1 * (ros::Time::now() - time_start).toSec());vel.linear.y = -2.0 * M_PI * 2.0 * sin(2.0 * M_PI * 0.1 * (ros::Time::now() - time_start).toSec());local_pos_pub.publish(pose);local_vel_pub.publish(vel);ros::spinOnce();rate.sleep();}return 0;
}

解决Ardupilot+gazebo+mavros在仿真状态下无人机能解锁，但是不能起飞的问题相关推荐

Android仿QQ锁屏状态下消息提醒（震动+提示音）
导读: 最近在开发一个定时提醒业务,类似于闹钟,然后遇到了一个问题,当APP应用在后台运行时,用户关闭了手机屏幕(手机进入灭屏休眠状态),这个时候使用系统震动和闹钟没有起到作用.why? 同样是灭屏休 ...
游戏陪玩平台源码开发，锁屏状态下消息提醒的实现
当游戏陪玩平台源码在后台运行时,如果用户关闭了手机屏幕(手机进入灭屏休眠状态),这个时候有消息进来怎么办?这就需要在游戏陪玩平台源码开发时实现手机锁屏状态下消息提醒功能. 解决思路: 1.游戏陪玩平台 ...
安卓开发--关于锁屏状态下如何实现消息推送
安卓开发–关于锁屏状态下如何实现消息推送 1. 新建一个AlarmHandlerActivity并设置在锁屏状态下弹窗! onCreate方法中添加标志: /* * 分别是锁屏状态下显示,解锁,保持屏 ...
ubuntu16.04在英文状态下安装中文语言包的过程(法一：图形界面的方式) 以及安装中文语言包后无法选择汉语问题的解决
ubuntu16.04在英文状态下安装中文语言包的过程(法一:图形界面的方式) 以及安装中文语言包后无法选择汉语问题的解决参考文章: (1)ubuntu16.04在英文状态下安装中文语言包的过程( ...
视觉学习笔记Week9 Gazebo仿真环境下相机焦距计算
第九周仿真环境下相机焦距计算这周更新了曲线,按照嘉元和建荣的新曲线,落点应在TR后方一个车身位左右,速度和效率以及容错率可以达到比较好的状态. 焦距计算: ...
英文状态下输入字母，字母之间的间隔较大怎么解决？
1.问题描述:在英文状态输入字母时,不是这种情况(hello)而是(hello). 2.问题解决:原因是你在英文状态下开了全角,所以会出现这种状况,鼠标点到'英'字,然后右键,如下图所示,将全角改为半 ...
在联网状态下,有很多网页或者应用无法联网问题,如360安全卫士, Smartscreen筛选器无法访问, 部分网页无法访问等问题的解决方法
在联网状态下,有很多应用无法联网问题,如360安全卫士, Smartscreen筛选器无法访问, 部分网页无法访问等问题的解决方法解决方法: 打开IE浏览器打开设置, 选择Internet选项找 ...
ROS开发笔记（5）——基于 python 开发 Turtlebot3 Gazebo仿真环境下键盘操控移动机器人（Teleop-bot ）
前文中记录了随机移动机器人的开发过程,本文内容为Turtlebot3 Gazebo仿真环境下Teleop-bot 键盘操控移动机器人,主要包含以下几个部分: 1.键盘驱动(按键驱动发布keys话题) ...
虚拟机未关机状态下关闭windows主机导致网络连接不上，虚拟机中无ens33文件的解决办法
目录前言解决步骤总结前言最近在学习mysql高级的过程中,发现一个问题.那就是当vm虚拟机没有关机,就关闭电脑主机,第二天用xshell连接发现连接不上.去虚拟机中使用ifconfig查看发 ...

解决Ardupilot+gazebo+mavros在仿真状态下无人机能解锁，但是不能起飞的问题

1. 问题

2. 解决办法

解决Ardupilot+gazebo+mavros在仿真状态下无人机能解锁，但是不能起飞的问题相关推荐

最新文章

热门文章