|
👉👉👉**无人机硬件，提供全程指导**👈👈👈
**
引言：本小节是在已经安装好仿真环境的情况下进行。仿真环境的安装可以参考视频讲解：链接如下
**
环境安装和定点飞行代码详解
视频从小海龟例程开始，详细阐述了话题的订阅和发布，后续分析了本小节代码 视频：代码详解
1、进入到Firmware文件夹下，启动gazebo仿真环境：
make px4_sitl_default gazebo
 2、启动MAVROS仿真
`roslaunch mavros px4.launch fcu_url:="udp://:[email protected]:14557"`
 3、启动飞行节点
rosrun offboard_node offboard_node
 4、代码注释：
//相关头文件包含
#include
#include
#include
#include
#include
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;
//创建订阅者，订阅话题mavros/state
ros::Subscriber state_sub = nh.subscribe
("mavros/state", 10, state_cb);
//创建一个发布者，发布"mavros/setpoint_position/local"话题
ros::Publisher local_pos_pub = nh.advertise
("mavros/setpoint_position/local", 10);
//创建一个客户端，该客户端用来请求PX4无人机的解锁
ros::ServiceClient arming_client = nh.serviceClient
("mavros/cmd/arming");
//创建一个客户端，该客户端用来请求进入offboard模式
ros::ServiceClient set_mode_client = nh.serviceClient
("mavros/set_mode");
//搭配rate.sleep()函数，实现20Hz频率的循环;
ros::Rate rate(20.0);
// 等待飞控连接
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 = 2;
//预发布期望位置信息，给无人机目标点
//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";
//设置客户请无人机解锁
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
//获取时间戳
ros::Time last_request = ros::Time::now();
while(ros::ok()){
//if语句循环请求进入OFFBOARD模式，进入以后则会进入else语句请求对无人机解锁。
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();
}
}
//循环发布期望位置信息，20Hz频率
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}
|