ROS学习记录

最新推荐文章于 2025-04-24 18:23:11 发布
最新推荐文章于 2025-04-24 18:23:11 发布
阅读量1k 收藏 12
点赞数 13
文章标签： ubuntu c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/2303_79734344/article/details/143250270
版权

第二次任务完成结果记录

2.1 安装PX4仿真环境

2.1.1 ubuntu安装MAVROS

  • 该安装是参考了优快云上的一篇帖子完成的

ubuntu搭建PX4无人机仿真环境(2) —— MAVROS安装

a. 安装

终端运行以下代码

sudo apt install ros-$ROS_DISTRO-mavros ros-$ROS_DISTRO-mavros-extras
wget https://gitee.com/tyx6/mytools/raw/main/mavros/install_geographiclib_datasets.sh
sudo chmod a+x ./install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh 这一行运行所需的时间很长
b. 检测

终端运行以下代码

roscd mavros

2.1.2 ubuntu安装QGC地面站

  • 该安装是参考了优快云上的一篇帖子完成的

ubuntu搭建PX4无人机仿真环境(3) —— ubuntu安装QGC地面站

a. 准备
  • 在终端输入以下代码
sudo usermod -a -G dialout $USER
sudo apt-get remove modemmanager -y
sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y
sudo apt install libfuse2 -y
sudo apt install libxcb-xinerama0 libxkbcommon-x11-0 libxcb-cursor0 -y
b. 安装

  • 在QGC官网上找到这一部分

  • 点击超链接下载文件，下载完成在终端输入以下代码

cd ~/Downloads/
chmod +x QGroundControl.AppImage
./QGroundControl.AppImage

2.1.3 仿真环境搭建

  • 该安装是参考了优快云上的一篇帖子完成的

ubuntu搭建PX4无人机仿真环境(4) —— 仿真环境搭建

a. 准备
  • 由于需要从git上下载源码，所以需要先安装git
sudo apt install git
  • 下载源码
git clone https://github.com/PX4/PX4-Autopilot.git
mv PX4-Autopilot PX4_Firmware
cd PX4_Firmware
git checkout -b dev v1.13.2
git submodule update --init --recursive
  • 在git clone过程中，可能会遇到一些网络问题，例如Git拒绝链接，这是可以上网查询github的域名地址，这是我使用的网站https://tool.chinaz.com/。这是，我们可以修改hosts文件。如下，图中文件第三行就是我查到的IP位置，在该文件中进行修改即可。

修改hosts文件

sudo gedit /etc/hosts
  • 对于这个问题，可以尝试终端输入以下代码
git config --global http.proxy http://127.0.0.1:1080
git config --global https.proxy http://127.0.0.1:1080
git config --global --unset http.proxy
git config --global --unset https.proxy
  • 当然，我在网上还看到一种检验方式，由于我是在问题解决后尝试的，不知道实际效果，在终端输入代码如下。
ssh -T git@git.oschina.net
  • 当出现以下文字时，即代表github链接成功

Permission denied (publickey).

b. 安装
  • 网上推荐在安装之前先查看自己的系统中有没有Gazebo。如果有，推荐删除。
gazebo --version #查看是否安装

sudo apt-get remove gazebo* 
sudo apt-get remove libgazebo*
sudo apt-get remove ros-$ROS_DISTRO-gazebo*s
  • 卸载完成后输入以下指令安装
cd ~/PX4_Firmware/Tools/setup
./ubuntu.sh
  • 以上代码会自动安装Gazebo，运行结束后可以验证一下Gazebo来看有没有安装成功。gazebo --version或者直接运行一下gazebo。
c. 下载Gazebo模型
  • 下载
git clone https://github.com/osrf/gazebo_models.git
if [ ! -d "~/.gazebo/models" ]; then mkdir -p ~/.gazebo/models ;fi
mv ./gazebo_models/* ~/.gazebo/models/
  • 编译
cd ~/PX4_Firmware
make px4_sitl_default gazebo
  • 将下面语句添加到 ~/.bashrc(此文件在主目录下，是个隐藏文件) 文件中gedit ~/.bashrc。
source ~/catkin_ws/devel/setup.bash
source ~/PX4_Firmware/Tools/setup_gazebo.bash ~/PX4_Firmware/ ~/PX4_Firmware/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware/Tools/sitl_gazebo
  • 更新环境：
source ~/.bashrc
d. 测试
  • 在终端输入下面命令：
roslaunch px4 mavros_posix_sitl.launch
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
  • 打开另一个终端，运行下面命令，查看MAVROS与仿真无人机通信状况。若connected: True，则通信成功，如果是false，一般是因为 .bashrc 里的路径写的不对，请仔细检查。
rostopic echo /mavros/state | grep connected

2.2 地面站使用

2.2.1 航点设置

  • 使用QGroundControl地面站发布航点任务，使无人机依次访问航点并留下飞行轨迹，要求生成姓名首字母大写的飞行轨迹。

2.2.2 问题回答

a. QGroundControl地面站在无人机系统中发挥的作用？
  • QGroundControl地面站是无人机操作员与无人机之间的关键界面，它提供了以下功能：
  1. 飞行控制：允许操作员通过图形界面控制无人机的飞行，包括起飞、降落、导航等。
  2. 状态监控：实时显示无人机的状态信息，如位置、高度、速度、电池电量等。
  3. 任务规划：允许操作员规划无人机的飞行路径，包括航点任务、自动航线等。
  4. 日志记录：记录飞行过程中的数据，用于故障分析和飞行数据的回顾。
  5. 安全控制：提供紧急停止、返航等安全功能，确保飞行安全。
b. 自稳、定高和定点模式的区别？
  • 自稳（Stabilized）模式

自稳模式是无人机飞行控制的基础模式，主要关注于保持无人机的姿态稳定。在自稳模式下，无人机能够自动调整其姿态，以保持水平飞行或按照预定的航向飞行。这种模式通过控制无人机的滚转、俯仰和偏航角来实现，确保无人机在飞行过程中保持稳定，不受外部干扰的影响。自稳模式通常作为其他高级控制模式的基础。

  • 定高（Altitude）模式

定高模式是一种飞行控制模式，其主要功能是保持无人机在预定的高度上飞行。在定高模式下，无人机的飞行控制系统会持续监测无人机的高度，并在必要时调整无人机的垂直速度，以确保无人机保持在设定的高度。这种模式对于执行需要在特定高度上进行的作业（如航拍、测绘）非常有用。定高模式通常需要与自稳模式配合使用，以确保无人机在保持高度的同时也能保持稳定姿态。

  • 定点（Position）模式

定点模式是一种更高级的控制模式，它不仅要求无人机保持在预定的高度，还要求无人机能够精确地定位在三维空间中的特定点。在定点模式下，无人机的飞行控制系统不仅要控制无人机的高度，还要管理无人机的水平位置，确保无人机能够准确地到达并保持在预定的三维坐标位置。定点模式通常用于执行需要精确位置控制的任务，如精确农业、搜索与救援、测绘等。为了实现精确的定位，定点模式可能需要结合GPS、视觉定位、惯性导航等多种定位技术。

c. Offboard模式和其他三种模式的区别？
  • 无人机的 Offboard 模式是一种更高级的控制模式，它允许外部设备（如地面站或其他控制系统）直接发送控制指令给无人机，而不是依赖于无人机自身的传感器和控制系统。

2.3 mavros控制

  • 创建功能包，编写ROS程序，使用程序控制仿真环境中的无人机运动，在QGroundControl 地面站中留下运动轨迹：长方形或圆形等基本图像皆可

2.3.1 运行代码

#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 <cmath>

struct point {
    double x;
    double y;
    double z;
};

// 假设的矩形顶点（在NED坐标系中）
struct point rect_points[4] = {
    {0, 0, 10},
    {10, 0, 10},
    {10, 10, 10},
    {0, 10, 10}
};

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;   // 无人机当前状态
}

geometry_msgs::PoseStamped current_pose;
void get_pose(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    current_pose = *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::Subscriber pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, get_pose);

    //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 = 10;

    //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();

    int point_index = 0;

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            // 切换 Offboard 模式
            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();
            }
        }
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = "local_ned";
        pose.pose.position.x = rect_points[point_index].x;
        pose.pose.position.y = rect_points[point_index].y;
        pose.pose.position.z = rect_points[point_index].z;
        local_pos_pub.publish(pose); // 发送目标点位置

        if (fabs(current_pose.pose.position.x - pose.pose.position.x) < 0.2 &&
               fabs(current_pose.pose.position.y - pose.pose.position.y) < 0.2 &&
               fabs(current_pose.pose.position.z - pose.pose.position.z) < 0.2) {
            point_index = (point_index + 1) % 4;
        }
        
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
  • 代码的模板是官方提供的代码，我在其基础上进行了修改，改了pose的发布。
  • 对于任务位置控制实现姓名首字母大写，与本例的区别就是多了几个点，其余没有任何差别。

2.3.2 出现的问题

a. Failsafe enabled: No manual control stick input
  • 解决：https://blog.youkuaiyun.com/weixin_44537885/article/details/125946076
  • 在QGC中修改飞行器的参数：COM_RC_IN_MODE = 1, COM_RCL_EXCEPT = 3, NAV_RCL_ACT = 0
b. Offboard enabled Unexpected command 176, result 0
  • 解决：https://blog.youkuaiyun.com/weixin_43049176/article/details/124301786
c. positionTargetGlobal failed because no origin
  • 解决：写一个新节点，发布原点。
d. CMD: Unexpected command 176, result 1
  • 解决：最新版不稳定，退回之前版本。重新在GitHub上下载PX4.
e. Resource not found: gazebo_ros
  • 输入代码sudo apt install ros-melodic-gazebo-ros-pkgs
f. Failsafe enabled: no RC and no offboard
  • 这个是我主要遇到的问题，在相当一段时间内，我一直没有找到这个问题的解决方案。源代码在temp.cpp文件中，可以看一下，修改后的在try.cpp中，主要差别在我把发送OFFBOARD信号放到的循环中，不断的发送以维持OFFBOARD模式。

2.4 加分任务

  • 速度控制实现姓名首字母大写，使用了PID控制算法中的比例控制来保证轨迹的可预测性。我做了一个简单的矩形来作为实验。

  • 我最初使用的小海龟的经验来写这个代码，发现在QGC上无人机一边转圈一边沿着直线行驶，可见无人机的angular和linaer是互不影响的，我们可以使用linear中x，y，z来控制速度。

2.4.1 试验代码

#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 <geometry_msgs/Twist.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

// 订阅的无人机当前位置数据
geometry_msgs::PoseStamped local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    local_pos = *msg;
}

float err_x = 0;     float err_x0 = 0;      float err_x_err = 0;
float err_y = 0;     float err_y0 = 0;      float err_y_err = 0;
float err_z = 0;     float err_z0 = 0;      float err_z_err = 0;
float vel_x = 0;
float vel_y = 0;
float vel_z = 0;
float adj_kp = 1.0;
float adj_kd = 1.88;    // -0. 2 超调量1 左右   + 0.2 超调量减小

void local_pos_control(float pos_x, float pos_y, float pos_z)
{
    // 误差
    err_x = pos_x - local_pos.pose.position.x;
    err_y = pos_y - local_pos.pose.position.y;
    err_z = pos_z - local_pos.pose.position.z;
    // 误差的误差
    err_x_err = err_x - err_x0;
    err_y_err = err_y - err_y0;
    err_z_err = err_z - err_z0;
    // 保存本次误差
    err_x0 = err_x;
    err_y0 = err_y;
    err_z0 = err_z;

    // PD控制
    vel_x = adj_kp * err_x + adj_kd * err_x_err;
    vel_y = adj_kp * err_y + adj_kd * err_y_err;
    vel_z = adj_kp * err_z + adj_kd * err_y_err;

    // 比例控制
    // vel_x = adj_kp * err_x;
    // vel_y = adj_kp * err_y;
    // vel_z = adj_kp * err_z;

    ROS_INFO("Pose-x: %f",local_pos.pose.position.x);
    ROS_INFO("Pose-y: %f",local_pos.pose.position.y);
    ROS_INFO("Pose-z: %f",local_pos.pose.position.z);
}

int main(int argc, char **argv)
{   

    ros::init(argc, argv, "offb_cfx");
    ros::NodeHandle nh;
    
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb); 
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, local_pos_cb);
    ros::Publisher vec_pub = nh.advertise<geometry_msgs::Twist>
            ("mavros/setpoint_velocity/cmd_vel_unstamped", 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::Twist vector;
    vector.linear.x = 0.0;
    vector.linear.y = 0.0;
    vector.linear.z = 0.0;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        vec_pub.publish(vector);
        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();

    int step = 0;
    int sametimes = 0;

    while(ros::ok()){
        // 无人机状态设定与判断      
        // 进入while循环后，先循环5s，然后再向客户端发送无人机状态设置的消息
        // set_mode_client.call   arming_client.call 
        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();
            }
            else    //  无人机 Offboard enabled && Vehicle armed 后
            {       //  无人机走矩形 每到达一个点停一会
                    //  z: 0-->10 10-->10 10-->10 10-->10  10-->10 10-->0 
                    //  x: 0-->0   0-->40   40-->40 40-->0    0-->0     0-->0
                    //  y: 0-->0   0-->0     0-->20   20-->20  20-->0   0-->0
                    //  local_pos_pub.publish(pose);

                switch (step)
                {
                case 0:
                    local_pos_control(0,0,2);  // 速度控制
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (local_pos.pose.position.z >1.95 && local_pos.pose.position.z <2.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 1;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 1:
                    local_pos_control(20,0,2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (local_pos.pose.position.x >19.95 && local_pos.pose.position.x <20.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 2;
                        }
                        else
                            sametimes ++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 2:
                    local_pos_control(20,20,2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (local_pos.pose.position.y >19.95 && local_pos.pose.position.y <20.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 3;
                        }
                        else
                            sametimes ++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 3:
                    local_pos_control(0,20,2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (local_pos.pose.position.x >-0.05 && local_pos.pose.position.x <0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 4;
                        }
                        else
                            sametimes ++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 4:
                    local_pos_control(0,0,2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (local_pos.pose.position.y >-0.05 && local_pos.pose.position.y <0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 5;
                        }
                        else
                            sametimes ++;
                    }
                    else
                        sametimes = 0;
                    break; 
                case 5:    // 准备降落
                    offb_set_mode.request.custom_mode = "AUTO.LAND";
                    if (current_state.mode != "AUTO.LAND" && (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("AUTO.LAND enabled");
                            step = 6;
                        }
                        last_request = ros::Time::now();
                    }
                    break;

                default:
                    break;
                }
            }

        }

        // 发布位置控制信息
        vec_pub.publish(vector);
        ros::spinOnce();
        rate.sleep();   // 影响消息发布与更新的周期
    }

    return 0;
}
  • 该代码的主体是switch分支，依靠switch分支中的每一步到达下一个点。

2.4.2 正式代码

#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 <geometry_msgs/Twist.h>
#include <cmath>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

// 订阅的无人机当前位置数据
geometry_msgs::PoseStamped local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    local_pos = *msg;
}

float err_x = 0;
float err_x0 = 0;
float err_x_err = 0;
float err_y = 0;
float err_y0 = 0;
float err_y_err = 0;
float err_z = 0;
float err_z0 = 0;
float err_z_err = 0;
float vel_x = 0;
float vel_y = 0;
float vel_z = 0;
float adj_kp = 1.0;
float adj_kd = 1.88; // -0. 2 超调量1 左右   + 0.2 超调量减小

void local_pos_control(float pos_x, float pos_y, float pos_z)
{
    // 误差
    err_x = pos_x - local_pos.pose.position.x;
    err_y = pos_y - local_pos.pose.position.y;
    err_z = pos_z - local_pos.pose.position.z;
    // 误差的误差
    err_x_err = err_x - err_x0;
    err_y_err = err_y - err_y0;
    err_z_err = err_z - err_z0;
    // 保存本次误差
    err_x0 = err_x;
    err_y0 = err_y;
    err_z0 = err_z;

    vel_x = adj_kp * err_x + adj_kd * err_x_err;
    vel_y = adj_kp * err_y + adj_kd * err_y_err;
    vel_z = adj_kp * err_z + adj_kd * err_y_err;

    // 比例控制
    // vel_x = adj_kp * err_x;
    // vel_y = adj_kp * err_y;
    // vel_z = adj_kp * err_z;

    ROS_INFO("Pose-x: %f", local_pos.pose.position.x);
    ROS_INFO("Pose-y: %f", local_pos.pose.position.y);
    ROS_INFO("Pose-z: %f", local_pos.pose.position.z);
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "offb_cfx");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, local_pos_cb);
    ros::Publisher vec_pub = nh.advertise<geometry_msgs::Twist>("mavros/setpoint_velocity/cmd_vel_unstamped", 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::Twist vector;
    vector.linear.x = 0.0;
    vector.linear.y = 0.0;
    vector.linear.z = 0.0;

    // send a few setpoints before starting
    for (int i = 100; ros::ok() && i > 0; --i)
    {
        vec_pub.publish(vector);
        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();

    int step = 0;
    int sametimes = 0;

    while (ros::ok())
    {
        // 无人机状态设定与判断
        // 进入while循环后，先循环5s，然后再向客户端发送无人机状态设置的消息
        // set_mode_client.call   arming_client.call
        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();
            }
            else //  无人机 Offboard enabled && Vehicle armed 后
            {    //  无人机走矩形 每到达一个点停一会
              //  z: 0-->10 10-->10 10-->10 10-->10  10-->10 10-->0
              //  x: 0-->0   0-->40   40-->40 40-->0    0-->0     0-->0
              //  y: 0-->0   0-->0     0-->20   20-->20  20-->0   0-->0
              //  local_pos_pub.publish(pose);

                switch (step)
                {
                case 0:
                    local_pos_control(0, 0, 2); // 速度控制
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (local_pos.pose.position.z > 1.95 && local_pos.pose.position.z < 2.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 1;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 1:
                    local_pos_control(10, 0, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 2;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 2:
                    local_pos_control(15, 5, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 15) < 0.05 && fabs(local_pos.pose.position.y - 5) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 3;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 3:
                    local_pos_control(10, 10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 10) < 0.05 && 
                            fabs(local_pos.pose.position.y - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 4;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 4:
                    local_pos_control(0, 10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 0) < 0.05 && 
                            fabs(local_pos.pose.position.y - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 5;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 5:
                    local_pos_control(0, -10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 0) < 0.05 && 
                            fabs(local_pos.pose.position.y + 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 6;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 6:
                    local_pos_control(0, 0, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 0) < 0.05 && 
                            fabs(local_pos.pose.position.y - 0) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 7;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 7:
                    local_pos_control(10, -10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 10) < 0.05 && 
                            fabs(local_pos.pose.position.y + 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 8;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 8:
                    local_pos_control(20, -10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 20) < 0.05 && 
                            fabs(local_pos.pose.position.y + 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 9;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 9:
                    local_pos_control(20, 10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 20) < 0.05 && 
                            fabs(local_pos.pose.position.y - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 10;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 10:
                    local_pos_control(20, 0, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 20) < 0.05 && 
                            fabs(local_pos.pose.position.y - 0) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 11;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 11:
                    local_pos_control(30, 0, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 30) < 0.05 && 
                            fabs(local_pos.pose.position.y - 0) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 12;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 12:
                    local_pos_control(30, 10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 30) < 0.05 && 
                            fabs(local_pos.pose.position.y - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 13;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 13:
                    local_pos_control(30, -10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 30) < 0.05 && 
                            fabs(local_pos.pose.position.y + 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 14;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 14:
                    local_pos_control(40, -10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 40) < 0.05 && 
                            fabs(local_pos.pose.position.y + 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 15;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 15:
                    local_pos_control(40, 0, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 40) < 0.05 && 
                            fabs(local_pos.pose.position.y - 0) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 16;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 17;
                    break;

                case 16:
                    local_pos_control(30, 10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 30) < 0.05 && 
                            fabs(local_pos.pose.position.y - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 17;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 17:
                    local_pos_control(40, 0, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 40) < 0.05 && 
                            fabs(local_pos.pose.position.y - 0) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 18;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                case 18:
                    local_pos_control(50, 10, 2);
                    vector.linear.x = vel_x;
                    vector.linear.y = vel_y;
                    vector.linear.z = vel_z;
                    if (fabs(local_pos.pose.position.x - 50) < 0.05 && 
                            fabs(local_pos.pose.position.y - 10) < 0.05)
                    {
                        if (sametimes == 40)
                        {
                            step = 19;
                        }
                        else
                            sametimes++;
                    }
                    else
                        sametimes = 0;
                    break;

                
                case 19: // 准备降落
                    offb_set_mode.request.custom_mode = "AUTO.LAND";
                    if (current_state.mode != "AUTO.LAND" && (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("AUTO.LAND enabled");
                            step = 6;
                        }
                        last_request = ros::Time::now();
                    }
                    break;

                default:
                    break;
                }
            }
        }

        // 发布位置控制信息
        vec_pub.publish(vector);
        ros::spinOnce();
        rate.sleep(); // 影响消息发布与更新的周期
    }

    return 0;
}
GMm--r2
关注
【linux】CentOS查看系统信息
数字人生
03-18 3415
如果需要查看更详细的内存使用情况，可以使用`ps`命令结合`grep`来查找特定进程的内存占用情况。如果在图形用户界面(GUI)模式下，可以点击左上角菜单按钮，选择“系统工具”，在下拉菜单中找到并点击“系统监视器”选项，然后在新打开的窗口中，可以看到CentOS版本号及内核信息。注意：因为CentOS版本的更新，某些命令可能在最新的CentOS版本中更改或被替换，所以如果发现某个命令不起作用，可能需要查找该版本系统中的替代命令。：输入`top`命令后，会看到系统的整体情况，包括内存使用情况。
Centos8查看系统版本&内核版本的命令
夫君子之行,静以修身,俭以养德,非淡泊无以明志,非宁静无以致远.
05-25 819
1. 查看系统版本2. 查看内核版本登录后复制或者uname -a。
CentOS查看系统信息-CentOS查看命令
eagle的专栏
12-21 1820
一：查看cpu 1. more /proc/cpuinfo | grep "model name" 2.grep "model name" /proc/cpuinfo 如果觉得需要看的更加舒服 grep "model name" /proc/cpuinfo | cut -f2 -d: 二：查看内 [root@CentOS ~]# grep MemTotal
一文吃透CentOS：Linux世界的中流砥柱
最新发布
大雨淅淅的博客
04-24 1223
CentOS，全称 Community Enterprise Operating System，即社区企业操作系统 ，是一款基于 Red Hat Enterprise Linux（RHEL）源代码重新编译而成的开源 Linux 发行版。它诞生于 2004 年，凭借着与 RHEL 的高度兼容性、强大的稳定性和安全性，以及免费开源的特性，在 Linux 操作系统家族中占据了重要地位。CentOS 与 RHEL 之间有着千丝万缕的联系。
查看centos版本号
IDO老徐软件测试窝
03-09 983
  －－写在开始－－ 玩Linux，不同的版本会有一些细微区别； so，经常需要查看服务器版本号；   －－正文－－ 有以下命令可以查看linux服务器版本号：# lsb_release -a LSB Version: :base-4.0-amd64:base-4.0-noarch:core-4.0-amd64:core-4.0-noarch Distributor ID...
centos 查看版本
ljh574649119的专栏
12-08 3704
在 CentOS 中，查看系统版本有多种方法。
如何查看CentOS操作系统版本
qq_38229543的博客
10-06 5275
如何查看CentOS操作系统版本，linux的也一样。 工具/原料 CentOS 方法/...
详解linux下查看系统版本号信息的方法（总结）
01-20
有时候需要查看自己系统的CentOs的版本,有一些命令可以查看,分别介绍如下：查看Linux内核版本命令（两种方法）和Linux系统版本的命令（3种方法） 一、查看Linux内核版本命令（两种方法）： 1、cat /proc/version ...
linux服务器用centos还是ubuntu系统
01-10
linux服务器系统多使用CentOS、uBuntu、Gentoo、FreeBSD、Debian。服务器操作系统应该选择 Debian/Ubuntu 还是 CentOS ，CENTOS现在市场占有率第一了 【小议】centos与ubuntu的区别 CentOS（Community ENTerprise ...
Linux CentOS 系统 安装和简单使用 - 小白入门
p.deng²⁰⁵ᐟ₅₁₂
05-25 3135
通常可以通过光驱、U盘、服务器管理界面等方式进行操作系统的安装。具体步骤通常包括：获取系统镜像->选择并准备安装方式->安装操作系统。
linux centos 如何查看操作系统版本信息？
weixin_34029680的博客
12-23 414
本文介绍常用的四种查看linux下查看系统版本信息的方法：一、uname -a[app@VM_11_211_centos ~]$ uname -aLinux VM_11_211_centos 2.6.32-504.30.3.el6.x86_64 #1 SMP Wed Jul 15 10:13:09 UTC 2015 x86_64 x86_64 x86_64 GNU/Linux...
查看CentOS版本方法
youyiweiqiuxuezhe的专栏
05-19 837
有以下命令可以查看： # lsb_release -a LSB Version:    :core-3.1-ia32:core-3.1-noarch:graphics-3.1-ia32:graphics-3.1-noarch Distributor ID: CentOS Description:    CentOS release 5.4 (Final) Release:    
查看Linux centOS服务器系统的版本
weixin_47681230的博客
07-21 3925
查看LinuxcentOS服务器系统的版本的五个命令......
查看CentOS的系统版本
GinChou的萌新博客
06-24 4万+
记录一下 以免忘记： CentOS查看版本有如下： cat /etc/redhat-release uname -a cat /etc/issue #这个好像7以上看不了 cat /proc/version #这个是和内核有关的数据 getconf LONG_BIT or getconf WORD_BIT #查看版本位数是64位还是32位 ...
Linux-Centos版本查看
u010404909的博客
06-22 330
查看内核版本 查看发布版本
centos查看系统版本
Kim_Weir的专栏
06-08 714
（1）cat  /etc/redhat-release （2）cat /proc/version（3）uname -a（4）查看位数  getconf LONG_BIT（5）file /bin/ls
CentOS怎样查看系统信息
06-17 127
一、查看系统版本和核心版本 1 登陆CentOS，启动终端。 2 登陆root帐户，输入cat /etc/redhat-release，即可显示系统版本。 3 输入uname -r ，可以查询内核版本。 END 二、查看各分区使用情况 1 输入...
查看CentOS版本信息
热门推荐
Alan Zhuang的博客
09-19 53万+
今天小编将给大家详细讲解查看CentOS版本信息的命令。     (一) 查看已经安装的CentOS版本信息     1.cat /etc/issue 查看版本     cat  缩写concatenate  cat命令可以用来显示、合并文件。     CentOS release 6.6 (Final)     CentOS 发行版6.6      etc  初期et
centos查看系统
非-浪的博客
08-02 229
系统版本 - cat /etc/redhat-release 内核版本 - uname -a
centos查看系统版本linux命令
07-06
在CentOS系统中，你可以通过运行以下Linux命令来查看系统的内核版本和发行版信息： 1. 查看内核版本： ``` cat /etc/os-release ``` 或者 ``` uname -r ``` 2. 查看完整系统版本信息，包括发行版名称、版本号等： ``` sudo lsb_release -a ``` 如果你想查看更详细的软件包信息，可以尝试： ``` rpm -q --queryformat '%{NAME} %{VERSION}-%{RELEASE}\n' * | sort -u ``` 这将显示已安装软件包的基本信息。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值