手机网站和网站一体,友情链接推广平台,wordpress改cms,重庆装修公司网站建设引言#xff1a; 由于GPS在室外漂移的误差比较大#xff0c;在长时间静止后启动#xff0c;程序发布的位置可能已经和预期的位置相差较大#xff0c;导致无法完成任务#xff0c;尤其是气压计的数据不准#xff0c;可能会导致无人机不能起飞或者一飞冲天。本文主要是在进…引言 由于GPS在室外漂移的误差比较大在长时间静止后启动程序发布的位置可能已经和预期的位置相差较大导致无法完成任务尤其是气压计的数据不准可能会导致无人机不能起飞或者一飞冲天。本文主要是在进行程序控制的时候首先拿到最新的漂移误差在此基础上进行程序控制可以保证程序的稳定运行。
步骤一创建订阅者订阅无人机的里程计信息
//创建一个Subscriber订阅者订阅名为/mavros/local_position/odom的topic注册回调函数local_pos_cb
ros::Subscriber local_pos_sub nh.subscribenav_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_positionfalse (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_positionfalse (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.advertisegeometry_msgs::PoseStamped(mavros/setpoint_position/local, 10);//创建一个Subscriber订阅者订阅名为/mavros/state的topic注册回调函数state_cbros::Subscriber state_sub nh.subscribemavros_msgs::State(mavros/state, 10, state_cb);//创建一个Subscriber订阅者订阅名为/mavros/local_position/odom的topic注册回调函数local_pos_cbros::Subscriber local_pos_sub nh.subscribenav_msgs::Odometry(/mavros/local_position/odom, 10, local_pos_cb);//创建一个服务客户端连接名为/mavros/cmd/arming的服务用于请求无人机解锁ros::ServiceClient arming_client nh.serviceClientmavros_msgs::CommandBool(mavros/cmd/arming);//创建一个服务客户端连接名为/mavros/set_mode的服务用于请求无人机进入offboard模式ros::ServiceClient set_mode_client nh.serviceClientmavros_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;
}
