PX4学习笔记——PX4仿真开环控制无人机转圈_px4仿真无人机圆周运动

发布话题

我们这里用话题mavros/setpoint_raw/local来发布控制命令。mavros/setpoint_raw/local的消息内容可见官方文档：
http://docs.ros.org/en/api/mavros_msgs/html/msg/PositionTarget.html

用话题mavros/cmd/arming和mavros/set_mode来设定无人机的状态（解锁）和模式（OFFBOARD）。
其中进入OFFBOARD模式需要以>=2HZ的频率向无人机发送消息,否则无人机会回退到OFFBOARD模式之前所在的模式。

圆圈参数方程

我们要发布无人机的目标位置，其中发布目标位置的消息将分为x坐标、y坐标和z坐标，因此我们需要写出圆圈的参数方程。

{ x = r c o s ( θ ) y = r s i n ( θ ) − r

$$\begin{cases} x = rcos(\theta) \\ y = rsin(\theta)-r \end{cases}$$ {x=rcos(θ)y=rsin(θ)−r​
$$st.-\frac{\pi }{2}\le \theta \le \frac{3\pi }{2}$$
无人机速度为
$$v_x=-rsin(\theta )$$
$$v_y=rcos(\theta )$$
无人机的偏航角为
$$\theta =arctan(\frac{v_y}{x_x})$$

具体代码

创建cpp文件offb_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>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/PositionTarget.h>

#define RATE 20 // 20hz
#define r 2.5     // radius
#define cycle_s 15 
#define STEP (cycle_s * RATE)
#define PI 3.14

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;
}

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::Publisher target_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/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(RATE);

    mavros_msgs::PositionTarget Target_P;
    Target_P.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    Target_P.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | 
                         mavros_msgs::PositionTarget::IGNORE_VY |
                         mavros_msgs::PositionTarget::IGNORE_VZ |
                         mavros_msgs::PositionTarget::IGNORE_AFX |
                         mavros_msgs::PositionTarget::IGNORE_AFY |
                         mavros_msgs::PositionTarget::IGNORE_AFZ |
                         mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
    Target_P.position.x = 0;
    Target_P.position.y = 0;
    Target_P.position.z = 0;
    Target_P.velocity.x = 0;
    Target_P.velocity.y = 0;
    Target_P.velocity.z = 0;
    Target_P.acceleration_or_force.x = 0;
    Target_P.acceleration_or_force.y = 0;
    Target_P.acceleration_or_force.z = 0;
    Target_P.yaw = 0;
    Target_P.yaw_rate = 0;

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

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

//  用于走圈的变量
    int step = 0;
    int sametimes = 0;
    int i = 0;
    double theta = -PI/2;

    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 后
            {    
                switch (step)
                {
                case 0:
                    Target_P.position.z = 2.0;
                    if(local_pos.pose.position.z >1.95 && local_pos.pose.position.z <2.05)
                        step = 1;
                    break;

                case 1:
                    theta += 2*PI/STEP;
                    Target_P.position.x = r*cos(theta);
                    Target_P.position.y = r*sin(theta) + r;
                    Target_P.position.z = 2.0;

                    Target_P.velocity.x = -r*sin(theta);
                    Target_P.velocity.y = r*cos(theta);
                    Target_P.velocity.z = 0;

                    Target_P.acceleration_or_force.x = -r*cos(theta);
                    Target_P.acceleration_or_force.y = -r*sin(theta);
                    Target_P.acceleration_or_force.x = 0;

                    Target_P.yaw = atan2(Target_P.velocity.y, Target_P.velocity.x = -r*sin(theta));

                    i ++;
                    if(i > STEP)
                    {
                        i = 1; 
                        step = 2;
                    }
                    break;
                
                case 2:
                    if(sametimes < 40)
                    {
                        sametimes ++;
                    }
                    else
                    {
                        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 = 3;
                            }
                            last_request = ros::Time::now();
                        }
                    }
                    break;
                default:
                    break;
                }
            }
        }
        // 发布位置控制信息
        target_pub.publish(Target_P);
        ros::spinOnce();
        rate.sleep();   // 影响消息发布与更新的周期
    }

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180

其中，代码中

    Target_P.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | 
                         mavros_msgs::PositionTarget::IGNORE_VY |
                         mavros_msgs::PositionTarget::IGNORE_VZ |
                         mavros_msgs::PositionTarget::IGNORE_AFX |
                         mavros_msgs::PositionTarget::IGNORE_AFY |
                         mavros_msgs::PositionTarget::IGNORE_AFZ |
                         mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
1
2
3
4
5
6
7

把速度和加速度以及偏航角速度忽略了，本质上来讲还是位置控制。

CMakeList以及运行脚本offb.sh

在CmakeList中添加

cmake_minimum_required(VERSION 3.0.2)
project(off_node)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  mavros_msgs
  roscpp
  std_msgs
)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(offb_node src/offb_node.cpp)
target_link_libraries(offb_node ${catkin_LIBRARIES})
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

创建脚本offb.sh

#!/bin/zsh
source ~/.zshrc
gnome-terminal --window -e 'zsh -c "roscore; exec zsh"' \
--tab -e 'zsh -c "sleep 5; roslaunch px4 mavros_posix_sitl.launch; exec zsh"' \
--tab -e 'zsh -c "sleep 5; source ~/catkin_ws/devel/setup.zsh; rosrun off_node offb_node; exec zsh"' \
1
2
3
4
5

这里我用的终端是zsh的，如果是bash可以自行将zsh换成bash即可。

结果

参考链接

https://zhuanlan.zhihu.com/p/476941058
https://zhuanlan.zhihu.com/p/440996013?utm_id=0
第二篇文章有速度闭环控制（但是是画矩形）