先附上代码，代码是改自阿木实验室：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的使用。
753
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
