1.创建空间
$ mkdir catkin_ws
$ cd catkin_ws
$ mkdir src
$ cd src
$ catkin_init_workspace //当前文件夹初始化,变成ros工作空间
$ cd ..
$ catkin_make //编译ros,生成对应文件夹
$ cd src
$ catkin_create_pkg offboard_pkg roscpp std_msgs geometry_msgs mavros_msgs //创建offboard_pkg 功能包
2.编写官方代码
$ cd offboard_pkg /src
$ touch offboard_node.cpp // 创建cpp文件,并将下方3部分的程序复制到其中
$ cd ../../.. //回到catkin_ws工作目录
$ catkin_make //编译ROS程序
添加环境变量
$ source devel/setup.bash
$ source ~/.bash
3.offboard官方代码
offboard_node.cpp文件内容为:
#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;
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::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
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;
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();
while(ros::ok())
{
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();
}
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}
4.运行程序
终端1运行:
$ cd Firmware/ && make px4_sitl gazebo //打开仿真环境
终端2运行:
$ roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557" //MOVROS通讯
终端3运行:
$ rosrun offboard_pkg offboard_node //开启控制节点
终端4运行:
$ rostopic echo /mavros/state //查看飞机MAVROS状态
实验现象:
gazebo仿真界面的飞机飞到了指定坐标(0,0,2),然后悬停。
打开GQC地面站可以发现地面站自动连接到了仿真飞机,并显示其状态信息。
5.注:无法起飞
注:若您git下载PX4源码时,直接运行
$ git clone https://github.com/PX4/Firmware.git --recursive
会导致飞机无法起飞,运行rosrun offboard_pkg offboard_node时,一直输出Offboard enabled,别的终端输出
Failsafe mode deactivated
Failsafe mode activated
等,原因是建议部署旧版本代码,可解决问题:
$ git clone -b v1.12.3 https://github.com/PX4/Firmware.git --recursive
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)