Mavros controls UAV to conduct binocular slam in gazebo environment

Mavros Control the UAV in gazebo Binocular... In the environment SLAM

Let's put the reference here first , Keyboard control is mainly from the initial contact ROS At the time of the turtlesim Function pack under the keyboard control node to get the method .

Reference resources

• ROS tutorial-turtle_teleop_key, Keyboard control program
• Mavros wiki, Find out the meaning of the topic and the data type

topic of conversation

Two different topics are used ：

• setpoint_velocity/cmd_vel_unstamped
• setpoint_raw/local

Both are Mavros Subscription topics , among

• The former explanation is ,Send velocity setpoint to FCU, The data type is geometry_msgs::Twist, This is also the topic of turtle speed control , branch 6 Directional linear velocity and angular velocity .
• The latter explanation is ,Send RAW setpoint messages to FCU and provide loopback topics (PX4), The data type is mavros_msgs::PositionTarget

The following are the definitions of two information contents ：

# This expresses velocity in free space broken into its linear and angular parts.
Vector3  linear
Vector3  angular

# Message for SET_POSITION_TARGET_LOCAL_NED
#
# Some complex system requires all feautures that mavlink
# message provide. See issue #402.

std_msgs/Header header

uint8 coordinate_frame
uint8 FRAME_LOCAL_NED = 1
uint8 FRAME_LOCAL_OFFSET_NED = 7
uint8 FRAME_BODY_NED = 8
uint8 FRAME_BODY_OFFSET_NED = 9

uint16 type_mask
uint16 IGNORE_PX = 1 # Position ignore flags
uint16 IGNORE_PY = 2
uint16 IGNORE_PZ = 4
uint16 IGNORE_VX = 8 # Velocity vector ignore flags
uint16 IGNORE_VY = 16
uint16 IGNORE_VZ = 32
uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags
uint16 IGNORE_AFY = 128
uint16 IGNORE_AFZ = 256
uint16 FORCE = 512 # Force in af vector flag
uint16 IGNORE_YAW = 1024
uint16 IGNORE_YAW_RATE = 2048

geometry_msgs/Point position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration_or_force
float32 yaw
float32 yaw_rate

Both can control linear velocity and angular velocity according to the parameters , See the code for details .

Another topic that caught my attention , yes mavros released , by setpoint_raw/target_local (mavros_msgs/PositionTarget), It is described as Local target loopback, This loopback Is not very good .

Code

By topic , There are two ：

• Twist,TwistControl
• PositionTarget,PositionTargetControl

experiment

• Experiment 1 , Fly best , The worst way to build a map
• Experiment two , Flying in general , The construction of drawings is general
• Experiment three , The worst flight , The worst way to build a map

There is a common problem , There is no loopback .

Program usage

I used it all the time Twist Control procedures .

Instruction list ：

Key	effect
T	cut Offboard, Unlock , Take off to the designated altitude , Hover briefly , The control is returned to the keyboard
H	The speed in both directions is set to 0, Designed to stay in place
L	cut Land Pattern
W	Add X Linear velocity
S	reduce X Linear velocity
A	Add Y Linear velocity
D	reduce Y Linear velocity
page up	Fuel valve
page down	Reduce the throttle
home（left）	Plus yaw rate
end（right）	Reduce yaw rate

Both press T take off , There are some problems in program design ,Offboard Press and hold after hovering at the predetermined height H keep ;

PositionTarget It often falls out of the program Offboard, however Twist As long as it falls out , Just long press a key and you can go back to Offboard, So I used it all the time Twist, In fact, the program is not well written . I often use it in my experiments H Give the UAV time to acquire feature points .

Bug And suggestions for improvement

Bug It's obviously in Offboard The mode of , After the UAV enters this mode , It is not only necessary to publish messages no less frequently than $2Hz$, And the news can't be broken , Because a break is equivalent to no RC,failsafe Take effect and drop out directly Offboard;

• stay for Add judgment to the loop , Without keyboard input, the UAV remains in its original position ; But there are also problems , The keyboard doesn't press that fast , This judgment always triggers , The action of the UAV is too laggy , jumping
• Other ways to keep sending messages , I didn't think of it

There is also the code , There are many things that can be improved , I tried to write a Mavros+PX4 class , It turned out current_pose and current_state The callback function of must be static , And I can't get the class data , Write a this If the pointer passes , The callback function becomes two parameters again ,mavros Mismatch , Then quit .