目录
一、创建工作空间
参考ROS学习–第3篇：ROS基础—创建工作空间
1.创建工作空间 catkin_ws
打开终端，创建src文件夹：
mkdir -p ~/catkin_ws/src进入src文件夹：
cd ~/catkin_ws/src初始化文件夹：
catkin_init_workspace2.编译工作空间 catkin_make
进入catkin_ws文件夹：
cd ~/catkin_ws/编译：
catkin_make二、offboard位置控制定点起飞
参考ROS+PX4学习与开发 2.0 ROS与PX4的通信、[PX4]mavros安装+offboard控制过程记录
1.准备工作
进入catking_ws/src文件夹：
cd ~/catkin_ws/src创建功能包文件夹：
catkin_create_pkg offboard_pkg roscpp std_msgs geometry_msgs mavros_msgs
进入创建好的功能包文件夹的src文件夹内：
cd ~/catkin_ws/src/offboard_pkg/src/新建一个offboard_node.cpp文件：
touch offboard_node.cpp然后打开.cpp文件进行编辑，这里用gedit：
gedit offboard_node.cpp复制进官方示例，然后保存关闭：
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/
#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");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
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();
}
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;
}
打开目录~/catkin_ws/src/offboard_pkg/下的CMakeLists.txt，添加下面的两行：
add_executable(offboard_node src/offboard_node.cpp)
target_link_libraries(offboard_node ${catkin_LIBRARIES})
进入catkin_ws文件夹并编译
cd ~/catkin_ws/
catkin_make打开终端设置环境变量：
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc到此前期准备已经完毕，可以启动。
2.启动
打开1号终端，进入PX4文件夹，打开gazebo：
cd ~/PX4-Autopilot/
make px4_sitl gazebo_iris
启动QGC，等待QGC连接成功。
打开2号终端，启动PX4与Mavros之间的连接：
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
打开3号终端，运行官方示例：
rosrun offboard_pkg offboard_node
稍微等待一会，3号终端如下图所示，正常情况下能看到gazebo中的无人机升高到2m位置。
如果启动之后，3号终端一直跳offboard enabled但是始终不见Vehicle armed，而且gazebo中无人机也没有反应，可能是PX4版本太新了(13.0)，解决办法是安装1.12.3。
在主文件夹中删除PX4-Autopilot整个文件夹，然后打开终端克隆旧版本：
git clone -b v1.12.3 https://github.com/PX4/PX4-Autopilot.git --recursive
如果下载失败提前结束，报错：
error: RPC failed; curl 56 GnuTLS recv error (-9): A TLS packet with unexpected length was received.
fatal: The remote end hung up unexpectedly
fatal: early EOF
fatal: index-pack failed
Failed during: git fetch origin –force
首先检查网络情况(梯子)，网络检查过没问题还是不行，则在终端输入，然后重新下载：
git config --global http.postBuffer 524288000
git config --global http.maxRequestBuffer 100M
git config --global core.compression 0
export GIT_TRACE_PACKET=1
export GIT_TRACE=1
export GIT_CURL_VERBOSE=1
如果还是解决不了，参考以下帖子：
【Git】error: RPC failed； curl 56 GnuTLS recv error (-9): A TLS packet with unexpected length was rece
下载PX4固件时网络太慢，经常出现克隆失败
解决git clone 完成后提示’error: RPC failed; curl 56 GnuTLS recv error (-9)’
RPC failed; curl 56 GnuTLS recv error (-54): Error in the pull function.
下载成功之后，编译。
cd PX4-Autopilot
sudo bash ./Tools/setup/ubuntu.sh
make px4_sitl_default gazebo
如果途中出现问题，参考PX4从放弃到精通（二）：ubuntu18.04配置px4编译环境及mavros环境
重新在.bashrc中添加一下px4源码的路径：
gedit .bashrc
打开后应该可以找到以前添加过得这一段：
source ~/PX4-Autopilot/Tools/setup_gazebo.bash ~/PX4-Autopilot/ ~/PX4-Autopilot/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot/Tools/sitl_gazebo
将其剪切到.bashrc的最下方，保存关闭，然后终端进行刷新
source .bashrc
之后问题解决，重新上面的启动步骤。
如果出现了屏幕突然旋转的情况，解决办法：
xrandr --output eDP-1 --rotate normal其中eDP-1是显示屏名字，替换为自己的显示屏名字。
三、offboard位置控制y=x²轨迹飞行
参考ROS+PX4无人机室内多点飞行代码
对其.cpp文件修改。
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>//里程计信息格式
int flag = 1;
mavros_msgs::State current_state;
nav_msgs::Odometry local_pos;
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg);
int main(int argc, char **argv)
{
ros::init(argc, argv, "offboard_multi_position");
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::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, 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");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
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 = 3;
//send a few setpoints before starting
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();
//此处满足一次请求进入offboard模式即可，官方例成循环切入offboard会导致无人机无法使用遥控器控制
while(ros::ok())
{
//请求进入OFFBOARD模式
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();
}
}
if(ros::Time::now() - last_request > ros::Duration(5.0))
{
break;
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
while(ros::ok())
{
if((flag == 1) && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
ROS_INFO("Position_1");
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=2;
}
if((flag ==2) && (ros::Time::now() - last_request > ros::Duration(5.5)))
{
ROS_INFO("Position_2 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=3;
}
if((flag ==3) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_3 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=4;
}
if((flag ==4) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_4 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=5;
}
if((flag ==5) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_5 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=6;
}
if((flag ==6) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_6 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=7;
}
if((flag ==7) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_7 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=8;
}
if((flag ==8) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_8 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=9;
}
if((flag ==9) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_9 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=10;
}
if((flag ==10) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_10 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=11;
}
if((flag ==11) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_11 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=12;
}
if((flag ==12) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_12 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=13;
}
if((flag ==13) && (ros::Time::now() - last_request > ros::Duration(0.5)))
{
ROS_INFO("Position_13 ");
pose.pose.position.x = pose.pose.position.x + 0.5;
pose.pose.position.y = (pose.pose.position.x)*(pose.pose.position.x);
pose.pose.position.z = 3;
last_request = ros::Time::now();
flag=14;
}
if((flag ==14) && (ros::Time::now() - last_request > ros::Duration(8.0)))
{
break;
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
ROS_INFO("AUTO.LAND");
offb_set_mode.request.custom_mode = "AUTO.LAND";
set_mode_client.call(offb_set_mode);
return 0;
}
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
current_state = *msg;
}
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
local_pos = *msg;
}
版权声明：本文为博主作者：小奥比waste原创文章，版权归属原作者，如果侵权，请联系我们删除！
原文链接：https://blog.csdn.net/qq_36144028/article/details/131413266
