-
/*
头文件,包括常见的geometry_msgs和mavros通信需要的mavros_msgs,添加上就行
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
/*
current_state表示的是无人机的状态,在主函数中订阅了对应的话题,这个状态就会不断更新,表示无人机当前的状态。state_cb就是对应的回调函数,会不断的执行,更新状态。现在先不用管为什么需要这个状态,后面的代码会解释。
*/
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);
/*
发布一个geometry_msgs::PoseStamped的消息,需要知道的是,这个消息是控制无人机的一种方式,将指定坐标包裹进这个消息,然后发布出去,无人机就能自动飞行到指定的坐标地点
*/
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
/*
无人机有一个锁,如果不解锁,无人机虽然接受了命令但是不会动被锁住了,只有解锁了才能对无人机进行控制,下面这个服务调用就是用来请求解锁无人机。上面的current_state就包含了无人机是否解锁的信息,若没解锁就需要解锁,否则就不用,其用途在这就体现出来
*/
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
/*
无人机飞行有很多种模式,如果需要用代码操控无人机,我们就需要切换到OFFBOARD模式。上面的current_state也包含了无人机当前的飞行模式,若不是OFFBOARD就需要切换到该模式。下面的这个服务调用就是用来请求切换无人机飞行模式。
*/
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
/*
在OFFBOARD模式下,需要以>=2HZ的频率向无人机发送消息,否则无人机会回退到OFFBOARD模式之前所在的模式,因此这里的rate需要设置的比2大就行
*/
ros::Rate rate(20.0);
// wait for FCU connection
/*
等待无人机与控制站连接(代码的方式就是代理),只有连接了才能发送消息
*/
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
/*
pose就是坐标,本实例是让无人机在2m处悬空,因此z设置为2,z表示的就是高度
*/
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
// 下面这个感觉有没有都无所谓
//send a few setpoints before starting
for(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";
// 请求解锁的消息,arm表示解锁,设置为true,disarm是上锁
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
// 记录上次请求的时间
ros::Time last_request = ros::Time::now();
while(ros::ok()){
// 如果无人机模式不是OFFBOARD并且离上次操作时间大于5秒就发送请求切换,这里的5s是为了演示清楚设置的延时
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 {
// 如果当前未解锁且与请求时间大于5s,就发送请求解锁
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();
}
}
// 不断发送位置消息,但是只有解锁后才能真正开始运动,如果不发送就会退出OFFBOARD模式,因为请求发送速度要>=2HZ
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}
/**
* @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>
#include <nav_msgs/Odometry.h>//里程计信息格式
int flag = 1;
mavros_msgs::State current_state;
nav_msgs::Odometry local_pos;
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg);
int main(int argc, char **argv)
{
ros::init(argc, argv, "offboard_multi_position");
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::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);
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 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(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 = 0.3;
//send a few setpoints before starting
for(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();
//此处满足一次请求进入offboard模式即可,官方例成循环切入offboard会导致无人机无法使用遥控器控制
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(ros::Time::now() - last_request > ros::Duration(5.0))
{
break;
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
while(ros::ok())
{
if((flag == 1) && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
ROS_INFO("Position_1");
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0.3;
last_request = ros::Time::now();
flag=2;
}
if((flag ==2) && (ros::Time::now() - last_request > ros::Duration(8.0)))
{
ROS_INFO("Position_2 ");
pose.pose.position.x = 0.5;
pose.pose.position.y = 0;
pose.pose.position.z = 0.3;
last_request = ros::Time::now();
flag=3;
}
if((flag ==3) && (ros::Time::now() - last_request > ros::Duration(8.0)))
{
ROS_INFO("Position_3 ");
pose.pose.position.x = 0.5;
pose.pose.position.y = 0.5;
pose.pose.position.z = 0.3;
last_request = ros::Time::now();
flag=4;
}
if((flag ==4) && (ros::Time::now() - last_request > ros::Duration(8.0)))
{
ROS_INFO("Position_4 ");
pose.pose.position.x = 0;
pose.pose.position.y = 0.5;
pose.pose.position.z = 0.3;
last_request = ros::Time::now();
flag=5;
}
if((flag ==5) && (ros::Time::now() - last_request > ros::Duration(8.0)))
{
ROS_INFO("Position_1 ");
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0.3;
last_request = ros::Time::now();
flag=6;
}
if((flag ==6) && (ros::Time::now() - last_request > ros::Duration(8.0)))
{
break;
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
ROS_INFO("AUTO.LAND");
offb_set_mode.request.custom_mode = "AUTO.LAND";
set_mode_client.call(offb_set_mode);
return 0;
}
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;
}