0
点赞
收藏
分享

微信扫一扫

2.1、ROS+PX4仿真---定点飞行控制


**

引言:本小节是在已经安装好仿真环境的情况下进行。仿真环境的安装可以参考视频讲解:链接如下

**

​​环境安装和定点飞行代码详解​​

视频从小海龟例程开始,详细阐述了话题的订阅和发布,后续分析了本小节代码

​​视频:代码详解​​

1、进入到Firmware文件夹下,启动gazebo仿真环境:

make px4_sitl_default gazebo

2.1、ROS+PX4仿真---定点飞行控制_自动驾驶

2、启动MAVROS仿真

`roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"`

2.1、ROS+PX4仿真---定点飞行控制_c++_02

3、启动飞行节点

rosrun offboard_node offboard_node

2.1、ROS+PX4仿真---定点飞行控制_#include_03

4、代码注释:

//相关头文件包含
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

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_msgs::State>
("mavros/state", 10, state_cb);
//创建一个发布者,发布"mavros/setpoint_position/local"话题
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
//创建一个客户端,该客户端用来请求PX4无人机的解锁
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
//创建一个客户端,该客户端用来请求进入offboard模式
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("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;
}



举报

相关推荐

PX4 ROS2 真机

0 条评论