原文地址：https://404warehouse.net/2015/12/20/autopilot-offboard-control-using-mavros-package-on-ros/
This post is written by Jaeyoung Lim
Overview
Recently opensource autopilots have become reliable by various failsafe functions integrated by the opensource community. Developing different softwares for each application can be time consuming and hard to verify the reliablility of the software. An alternative to modifying the autopilot software, is to use an onboard computer to command the autopilot using high level commands. This way, the developer can use a reliable autopilot software which is still reliable and capable of staying in the air and use a software developed for the developer’s own purpose.
This article is to introduce how easy and practical it is to connect a PC, using ROS to control the autopilot running onbaord an onboard PC. A nice tutorial is up on the PX4 website but is a little unfriendly and I wanted to share some of the troubleshooting experiences while I was implementing the system.
what is ROS?
ROS is a framework for incorporating various modules of robotics by configuring modules as a TCP network. The ROS community is currently growing in a very fast pace both in the community and industry. The ROS wiki has great tutorials in understanding the framework. Please get used to using ROS following tutorials 1.~9. in the tutorials page.
Introduction to MAVROS
MAVROS is a ROS package which enables ROS to interact with autopilot software using the MAVLink protocol. MAVlink consists of 17 bytes and includes the message ID, target ID and data. The message ID shows what the data is. Message IDs can be seen in the messageID command set.
This enables MAVLink to be able to get information from multiple UAVs if messages are transferred in the same channel. Messages can either be transmitted through wireless signals.
Installing MAVROS
MAVROS can be installed using the source in the mavros repository. The default dialect of MAVROS is apm. As we are installing px4 on the pixhawk flight controller, MAVROS should be built from source, by configuring mavros by the command below.
catkin config --cmake-args -DMAVLINK_DIALECT=commonConfiguring PX4 for ROS
For the companion computer to communicate with the flight controller, a USB2TTL converter is needed to convert the voltage of the communication levels. A brief overview of configuring the companion computer with the pixhawk is shown in this link.
It is recommended that the offboard control is done through the TELEM2 port. Telem2 port can be activated usign the SYS_COMPANION parameter. This can be done by modifying the parameter on qgroundcontrol.
SYS_COMPANION 921600Configuring MAVROS for PX4
To launch MAVROS, the easiest way is to use the launch file
roslaunch mavros px4.launchLaunching MAVROS, it may not work on the first time. Do not panic!
Some parameters should be modified to be able to talk with the flight controller. The device name and baudrate should be configured to be able to talk to the autopilot.
|
1
2
3
4
5
6
7
8
9
10
11
12
|
<
node
name
=
"mavros"
pkg
=
"mavros"
type
=
"mavros_node"
output
=
"screen"
>
<
param
name
=
"fcu_url"
value
=
"$(arg fcu_url)"</code> <code class="xml plain">/></code></div><div class="line number3 index2 alt2"><code class="xml spaces"> </code><code class="xml plain"><</code><code class="xml keyword">param</code> <code class="xml color1">name</code><code class="xml plain">=</code><code class="xml string">"gcs_url"</code> <code class="xml color1">value</code><code class="xml plain">=</code><code class="xml string">"$(arg gcs_url)"
/>
<
param
name
=
"target_system_id"
value
=
"$(arg tgt_system)"</code> <code class="xml plain">/></code></div><div class="line number5 index4 alt2"><code class="xml spaces"> </code><code class="xml plain"><</code><code class="xml keyword">param</code> <code class="xml color1">name</code><code class="xml plain">=</code><code class="xml string">"target_component_id"</code> <code class="xml color1">value</code><code class="xml plain">=</code><code class="xml string">"$(arg tgt_component)"
/>
<!-- enable heartbeat send and reduce timeout -->
<
param
name
=
"conn_heartbeat"
value
=
"5.0"
/>
<
param
name
=
"conn_timeout"
value
=
"5.0"
/>
<!-- automatically start mavlink on USB -->
<
param
name
=
"startup_px4_usb_quirk"
value
=
"true"
/>
</
node
>
|
Setting Setpoints
Setpoints should be sent to the flight controller with a 0.5s time out. This is a safety factor so that the control should converge to a specific objective. Otherwise the flight controller will not go into offboard mode. Setpoints that can be used are as below.
- Position Setpoints
- Velocity Setpoints
- Attitude Setpoints
- Acceleration Setpoints
- Actuator Control
An example node written in cpp is shown below. The node publishes setpoint_poistion_local message.
Integrating Companion Computer to quadrotor
The quadrotor is based on a F330 frame and the current integration follows as below. 330 frame feeled a little small for an onboard computer. For future integration, 450 frame should be more appropriate.
Position Control Example
For position control, external position estimation using the Vicon motion capture system was used for more precise position measurement. The block diagram visualized using rtq_graph is shown as below.
This is an example to show how to control the quadrotor using the setpoint_position topic. External position estimation was used using the vicon motion capture system.
ctrl_position.launch
|
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
|
<
launch
>
<!--arg name="fcu_url" default="serial:///dev/ttyUSB0:921600" -->
<
arg
name
=
"tgt_system"
default
=
"1"
/>
<
arg
name
=
"tgt_component"
default
=
"1"
/>
<
node
name
=
"mavros"
pkg
=
"mavros"
type
=
"mavros_node"
output
=
"screen"
>
<
param
name
=
"fcu_url"
value
=
"$(arg fcu_url)"</code> <code class="xml plain">/></code></div><div class="line number12 index11 alt1"><code class="xml spaces"> </code><code class="xml plain"><</code><code class="xml keyword">param</code> <code class="xml color1">name</code><code class="xml plain">=</code><code class="xml string">"gcs_url"</code> <code class="xml color1">value</code><code class="xml plain">=</code><code class="xml string">"$(arg gcs_url)"
/>
<
param
name
=
"target_system_id"
value
=
"$(arg tgt_system)"</code> <code class="xml plain">/></code></div><div class="line number14 index13 alt1"><code class="xml spaces"> </code><code class="xml plain"><</code><code class="xml keyword">param</code> <code class="xml color1">name</code><code class="xml plain">=</code><code class="xml string">"target_component_id"</code> <code class="xml color1">value</code><code class="xml plain">=</code><code class="xml string">"$(arg tgt_component)"
/>
<!--rosparam command="load" file="$(find mavros)/launch/px4_blacklist.yaml"-->
<!-- enable heartbeat send and reduce timeout -->
<
param
name
=
"conn_heartbeat"
value
=
"5.0"
/>
<
param
name
=
"conn_timeout"
value
=
"5.0"
/>
<!-- automatically start mavlink on USB -->
<
param
name
=
"startup_px4_usb_quirk"
value
=
"true"
/>
<
param
name
=
"mocap/use_tf"
value
=
"true"
/>
<
param
name
=
"mocap/use_pose"
value
=
"false"
/>
</
node
>
<
node
name
=
"setpoint_pub"
pkg
=
"inrol_ros"
type
=
"pub_setpoints_pos"
output
=
"screen"
>
</
node
>
<
node
pkg
=
"vicon_bridge"
type
=
"vicon_bridge"
name
=
"vicon"
output
=
"screen"
>
<
param
name
=
"stream_mode"
value
=
"ServerPush"
type
=
"str"
/>
<!--param name="datastream_hostport" value="147.47.175.54:801" type="str" /-->
<
param
name
=
"datastream_hostport"
value
=
"147.46.175.54:801"
type
=
"str"
/>
<
param
name
=
"tf_ref_frame_id"
value
=
"/world"
type
=
"str"
/>
<
remap
from
=
"/vicon/quad/quad"
to
=
"/mavros/mocap/tf"
/>
</
node
>
</
launch
>
|
pub_setpoints.cpp
|
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
|
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
int
main(
int
argc,
char
**argv)
{
ros::init(argc, argv,
"pub_setpoints"
);
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>(
"/mavros/setpoint_position/local"
,100);
ros::Rate loop_rate(100);
ros::spinOnce();
geometry_msgs::PoseStamped msg;
int
count = 1;
//PositionReciever qp;:
//Body some_object;
//qp.connect_to_server();
while
(ros::ok()){
//some_object = qp.getStatus();
// some_object.print();
//printf("%f\n",some_object.position_x);
msg.header.stamp = ros::Time::now();
msg.header.seq=count;
msg.header.frame_id = 1;
msg.pose.position.x = 0.0;
//0.001*some_object.position_x;
msg.pose.position.y = 0.0;
//0.001*some_object.position_y;
msg.pose.position.z = 1.0;
//0.001*some_object.position_z;
msg.pose.orientation.x = 0;
msg.pose.orientation.y = 0;
msg.pose.orientation.z = 0;
msg.pose.orientation.w = 1;
chatter_pub.publish(msg);
ros::spinOnce();
count++;
loop_rate.sleep();
}
return
0;
}
|
Attitude Control Example
pub_att.cpp
|
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
|
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include <math.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include "std_msgs/Float64.h"
//#include </home/mahesh/catkin_ws/src/beginner_tutorials/src/Qualisys.h>
int
main(
int
argc,
char
**argv)
{
ros::init(argc, argv,
"pub_setpoints"
);
ros::NodeHandle n;
ros::Publisher pub_att = n.advertise<geometry_msgs::PoseStamped>(
"/mavros/setpoint_attitude/attitude"
,100);
ros::Publisher pub_thr = n.advertise<std_msgs::Float64>(
"/mavros/setpoint_attitude/att_throttle"
, 100);
ros::Rate loop_rate(100);
ros::spinOnce();
geometry_msgs::PoseStamped cmd_att;
std_msgs::Float64 cmd_thr;
int
count = 1;
double
v[3]={1.0, 0.0, 0.0};
double
lambda = 0.3;
double
v_norm=
sqrt
(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
double
theta=0.0;
//PositionReciever qp;
//Body some_object;
//qp.connect_to_server();
while
(ros::ok()){
//some_object = qp.getStatus();
// some_object.print();
//printf("%f\n",some_object.position_x);
//Create attitude command message
cmd_att.header.stamp = ros::Time::now();
cmd_att.header.seq=count;
cmd_att.header.frame_id = 1;
cmd_att.pose.position.x = 0.0;
//0.001*some_object.position_x;
cmd_att.pose.position.y = 0.0;
//0.001*some_object.position_y;
cmd_att.pose.position.z = 0.0;
//0.001*some_object.position_z;
cmd_att.pose.orientation.x =
sin
(theta/2.0)*v[0]/v_norm;
cmd_att.pose.orientation.y =
sin
(theta/2.0)*v[1]/v_norm;
cmd_att.pose.orientation.z =
sin
(theta/2.0)*v[2]/v_norm;
cmd_att.pose.orientation.w =
cos
(theta/2.0);
/*
double q_norm = sqrt(sin(theta/2.0)*sin(theta/2.0)+cos(theta/2.0)*cos(theta/2.0));
printf("%f\t",v_norm);
printf("%f\n", q_norm);
*/
//Create throttle command message
cmd_thr.data = lambda;
pub_att.publish(cmd_att);
pub_thr.publish(cmd_thr);
ros::spinOnce();
count++;
theta=0.3*
sin
(count/300.0);
loop_rate.sleep();
/*
if(count>1000){
count =0;
}
*/
}
return
0;
}
|
The video below shows some bias in yaw when controlling the attitude of the quadrotor. I believe this is due to the induced magnetic field from the motor currents which will offset the magnetic field in high attitudes.