先附上代码,代码是改自阿木实验室:Mavros教程—offboard模式下自主飞行课程姿态控制那一章的代码,课程的代码给的有问题,所以需要改动。软件流程图如下:
有些代码写了但是没起作用,因为只是用来调试的,没有注释掉,然后代码附上:
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <stdio.h>
#include <math.h>
#include <tf/tf.h>
#define PI 3.14159
//
geometry_msgs::PoseStamped pos_point, local_pos,a;
//float THRUST_FULL=60;
geometry_msgs::Point ned2enu_angle(geometry_msgs::Point ned_angle);
//float thrust_scale(float thrust);
geometry_msgs::Quaternion euler_to_quat(geometry_msgs::Point euler_angle_enu);
geometry_msgs::Point ned2enu_angle(geometry_msgs::Point ned_angle)
{
geometry_msgs::Point enu_angle;
enu_angle.x = ned_angle.y;
enu_angle.y = ned_angle.x;
enu_angle.z = -ned_angle.z;
return enu_angle;
}
mavros_msgs::State current_state;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
local_pos = *msg;
}
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
current_state = *msg;
}
//eluer to quad
geometry_msgs::Quaternion euler_to_quat(geometry_msgs::Point euler_angle_enu)
{
tfScalar yaw,pitch,roll;
tf::Quaternion q;
geometry_msgs::Quaternion quat;
roll = euler_angle_enu.y;
pitch = euler_angle_enu.x;
yaw = euler_angle_enu.z;
q.setEulerZYX(yaw,roll,pitch);
quat.x = q.x();
quat.y = q.y();
quat.z = q.z();
quat.w = q.w();
return quat;
}
//calc thrust_scale 归一化处理
/*float thrust_scale(float thrust)
{
float thrust_scale = 0.0;
if (thrust>=THRUST_FULL)
{
thrust= THRUST_FULL;
}
thrust_scale = thrust/ THRUST_FULL;
}
*/
int main (int argc,char** argv)
{
ros::init(argc,argv,"pub_setpoints");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state",10,state_cb);
ros::Publisher pub_pose = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local",100);
ros::Publisher raw_pub_att = nh.advertise<mavros_msgs::AttitudeTarget>("mavros/setpoint_raw/attitude",100);
ros::Subscriber local_posnodes = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose",100,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");
mavros_msgs::AttitudeTarget raw_att;
ros::Rate rate(20.0);
//ros::Rate loop_rate(100);
//set offboard mode and then arm the vechicl
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value=true;
geometry_msgs::Point enu_euler;
geometry_msgs::Point ned_euler;
ned_euler.x = 0;
ned_euler.y = 0;
ned_euler.z = PI/6;
enu_euler = ned2enu_angle(ned_euler);
pos_point.pose.position.x= 0;
pos_point.pose.position.y= 0;
pos_point.pose.position.z= 5;
geometry_msgs::Quaternion q;
q=euler_to_quat(enu_euler);
// float thr;
ros::Time last_request = ros::Time::now();
raw_att.thrust = 0.8;
raw_att.orientation.x=q.x;
raw_att.orientation.y=q.y;
raw_att.orientation.z=-PI/6;
raw_att.orientation.w=q.w;
while (ros::ok() && !current_state.connected)
{
/* wait until px4 connected */
ROS_INFO("angle=%f",q.z);
ros::spinOnce();
rate.sleep();
}
int orention_flag = 0;
int control=0,i=0;
while (ros::ok())
{
/* set offboard and then arm the vechile */
if (current_state.mode != "OFFBOARD" &&ros::Time::now() - last_request > ros::Duration(3.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(3.0)))
{
ROS_INFO("arm enabled or not ");
if(arming_client.call(arm_cmd)&&arm_cmd.response.success)
{
ROS_INFO("Vehicle armed");
last_request = ros::Time::now();
} // raw_pub_att.publish(raw_att);
}
else
{
switch(control)
{
case 0:
pub_pose.publish(pos_point);
raw_att.header.stamp = ros::Time::now();
if(local_pos.pose.position.z >4.9&&(ros::Time::now() - last_request > ros::Duration(5.0)))
{
if(i>100)
{
ROS_INFO("ARIVED");
control=1;
i=0;
}
else
{
i++;
}
}
else
{
i=0;
}
break;
case 1:
pub_pose.publish(pos_point);
raw_pub_att.publish(raw_att);
ROS_INFO("yaw=%f",local_pos.pose.orientation.z);
if(local_pos.pose.orientation.z < PI/5&&local_pos.pose.orientation.z > PI/7)
{
if(i>10)
{
control=2;
}
else
{i++;}
}
else
{i=0;}
break;
case 2:
ROS_INFO("preparation for LAND");
offb_set_mode.request.custom_mode = "AUTO.LAND";
if(current_state.mode !="AUTO.LAND"&&ros::Time::now()-last_request >ros::Duration(3.0))
{
if(set_mode_client.call(offb_set_mode)&&offb_set_mode.response.mode_sent)
{
ROS_INFO("AUTO.LAND enabled");
}
}
break;
default:
break;
}
}
}
ros::spinOnce();
rate.sleep();
}
return 0;
}
mavros_msgs::AttitudeTarget类型的对象一般用两个参数:拉力thrust和orientation姿态,设置的拉力决定无人机是上升还是降落,不知道调多少才会悬停。姿态就是四元数,把x,y,z,w设置好就可以。
这里代码有几个要点:
1. 最大的while循环里,进入offboard模式和armed模式不能在switch case这种结构里面,一般是放在if语句里,没有什么问题这种配置模式的代码不要乱动,容易出问题。
ros::Time::now()-last_request >ros::Duration(3.0)这条命令一般是为了在两个模式配置之间设置一定间隔时间,在进行飞行操作时尽量不要用到,用其他延迟时间的方式,因为进行飞行操作时比如说发布位置命令,不是发一次就行,需要不断循环发送,如果在循环的过程中写入了last_request=ros::Time::now(),下个飞行操作又设置了判断条件ros::Time::now()-last_request >ros::Duration(3.0),会导致上次请求的时间一直被更新,现在的时间和上次请求的时间相距很近,无法满足判断条件,下个飞行操作无法进入。
2. setEulerZYX这个函数有点问题,可以使用,但要看看输出的数据对不对。坐标系的问题,我发布命令时,偏航角给的是-PI/6,无人机返回给我的角度是PI/6,PX4无人机使用的是北东地坐标系,而mavros消息使用的坐标系是东北天坐标系,无人机收到偏航角后,使用北东地坐标系进行偏航,无人机返回给mavros的消息是东北天的消息,所以会取反,在设置判断条件时需注意。
3. if else命令的问题。确保一个if命令完成后,不需要再进入该if命令时,程序不会再进入,避免发送死循环。如果有时候实在避免不了,可以使用switch case命令,保证一段结束后会跳入下一段,而不会发送一些意外,比如说已经完成的功能一直在重复。
4. QGC地面站的使用。可以知道无人机的高度速度,以及发生了什么问题,这个有必要了解以下QGC的使用。