博客地址：http://www.rzrgm.cn/zylyehuo/
最終效果圖
（一）PX4 仿真開發
搭建仿真環境
運行案例（C++）
（二）添加鍵盤控制
/home/yehuo/project_ws/change_mode.sh
#!/usr/bin/env python
# coding=utf-8
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import *
from mavros_msgs.msg import State
import math
import sys, select, termios, tty
# 空格：降落
# 5 ：開啟offboard模式
# 6 ：解鎖，準備起飛
# 7 ：起飛
msg = """
Control Your Turtlebot!
---------------------------
Moving around:
u i o
j k l
m , .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
b : switch to OmniMode/CommonMode
CTRL-C to quit
"""
Omni = 0 #全向移動模式
#鍵值對應移動/轉向方向
moveBindings = {
'i':( 1, 0),
'o':( 1,-1),
'j':( 0, 1),
'l':( 0,-1),
'u':( 1, 1),
',':(-1, 0),
'.':(-1, 1),
'm':(-1,-1),
}
#鍵值對應速度增量
speedBindings={
'q':(1.1,1.1),
'z':(0.9,0.9),
'w':(1.1,1),
'x':(0.9,1),
'e':(1, 1.1),
'c':(1, 0.9),
}
#獲取鍵值函數
def getKey():
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
speed = 0.2 #默認移動速度 m/s
turn = 1 #默認轉向速度 rad/s
#以字符串格式返回當前速度
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
sita = 0.0 # 朝向
z = 0
w = 0
zf = 1
# 回調函數:訂閱無人機位姿
def pose_cb(m):
global sita
global z
global w
global zf
z = m.pose.orientation.z
w = m.pose.orientation.w
# 計算朝向在x軸的上方還是下方
if z*w > 0:
zf = 1
else:
zf = -1
sita = 2*math.acos(w)*180/math.pi
# rospy.loginfo('%.2f\r',sita)
current_state = State()
# 回調函數：訂閱mavros狀態
def state_cb(state):
global current_state
current_state = state
#主函數
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin) #獲取鍵值初始化，讀取終端相關屬性
rospy.init_node('turtlebot_teleop') #創建ROS節點
pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=5) #創建速度話題發布者
# 訂閱無人機位姿
rospy.Subscriber('mavros/local_position/pose',PoseStamped, pose_cb)
# 訂閱mavros狀態
rospy.Subscriber('mavros/state',State,state_cb)
# 定義起飛降落服務客戶端（起飛，降落）
setModeServer = rospy.ServiceProxy('mavros/set_mode',SetMode)
armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
x = 0 #前進后退方向
y = 0 #左右移動方向
z = 0 #上下移動方向
th = 0 #轉向/橫向移動方向
count = 0 #鍵值不再范圍計數
target_speed = 0 #前進后退目標速度
target_z_speed = 0 #上下運動目標速度
target_turn = 0 #轉向目標速度
control_speed = 0 #前進后退實際控制速度
control_z_speed = 0 #上下運動實際控制速度
control_turn = 0 #轉向實際控制速度
try:
print(msg) #打印控制說明
print(vels(speed,turn)) #打印當前速度
while(1):
key = getKey() #獲取鍵值
# if key:
# print('key = ',key)
#判斷鍵值是否在移動/轉向方向鍵值內
# if key in moveBindings.keys():
# x = moveBindings[key][0]
# th = moveBindings[key][1]
# count = 0
if key == 'i': #前進
count = 0
x = 1
z = 0
elif key == ',': #后退
count = 0
x = -1
z = 0
elif key == 'j': #往左轉
count = 0
th = 1
z = 0
elif key == 'l': #往右轉
count = 0
th = -1
z = 0
elif key == 'r': #上升
count = 0
z = 1
elif key == 'f': #下降
count = 0
z = -1
#判斷鍵值是否在速度增量鍵值內
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
count = 0
print(vels(speed,turn)) #速度發生變化，打印出來
#空鍵值/'k',相關變量置0
elif key == 'k' :
x = 0
y = 0
z = 0
th = 0
control_speed = 0
control_z_speed = 0
control_turn = 0
# 降落
elif key == ' ':
print("Vehicle Land")
setModeServer(custom_mode='AUTO.LAND')
# 開啟offboard模式
elif key == '5':
if current_state.mode != "OFFBOARD" :
setModeServer(custom_mode='OFFBOARD')
print("Offboard enabled")
# 解鎖，準備起飛
elif key == '6':
armServer(True)
print("Vehicle armed")
# 起飛
elif key == '7':
print("Vehicle Takeoff")
setModeServer(custom_mode='AUTO.TAKEOFF')
#長期識別到不明鍵值，相關變量置0
else:
count = count + 1
if count > 4:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
#根據速度與方向計算目標速度
target_speed = speed * x
target_z_speed = speed * z
target_turn = turn * th
#x方向平滑控制，計算前進后退實際控制速度
if target_speed > control_speed:
control_speed = min( target_speed, control_speed + 0.1 )
elif target_speed < control_speed:
control_speed = max( target_speed, control_speed - 0.1 )
else:
control_speed = target_speed
#z方向平滑控制，實際控制速度
if target_z_speed > control_z_speed:
control_z_speed = min( target_z_speed, control_z_speed + 0.1 )
elif target_z_speed < control_z_speed:
control_z_speed = max( target_z_speed, control_z_speed - 0.1 )
else:
control_z_speed = target_z_speed
#平滑控制，計算轉向實際控制速度
if target_turn > control_turn:
control_turn = min( target_turn, control_turn + 0.5 )
elif target_turn < control_turn:
control_turn = max( target_turn, control_turn - 0.5 )
else:
control_turn = target_turn
# 計算出y方向的sin值
y_sita = math.sin(sita/180*math.pi)
# 如果小于0，則改為正數
if y_sita < 0:
y_sita = -y_sita
# 乘以y分量的正負（通過四元數z*w獲得，z*w>0,y分量在x軸上方）
y_sita = y_sita * zf
twist = Twist() #創建ROS速度話題變量
twist.linear.x = control_speed * math.cos(sita/180*math.pi)
twist.linear.y = control_speed * y_sita # 朝向速度乘以y軸sin值
twist.linear.z = control_z_speed
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = control_turn
pub.publish(twist) #ROS發布速度話題
#運行出現問題則程序終止并打印相關錯誤信息
except Exception as e:
print(e)
#程序結束前發布速度為0的速度話題
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
pub.publish(twist)
#程序結束前設置終端相關屬性
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
（三）添加相機
/home/yehuo/下載/PX4_Firmware/launch/mavros_posix_sitl.launch
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
<!-- vehicle pose -->
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="iris"/>
<arg name="my_model" default="iris_depth_camera"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="respawn_gazebo" default="false"/>
<!-- MAVROS configs -->
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
<arg name="respawn_mavros" default="false"/>
<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- PX4 SITL and Gazebo -->
<include file="$(find px4)/launch/posix_sitl.launch">
<arg name="x" value="$(arg x)"/>
<arg name="y" value="$(arg y)"/>
<arg name="z" value="$(arg z)"/>
<arg name="R" value="$(arg R)"/>
<arg name="P" value="$(arg P)"/>
<arg name="Y" value="$(arg Y)"/>
<arg name="world" value="$(arg world)"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<!-- <arg name="sdf" value="$(arg sdf)"/> -->
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="interactive" value="$(arg interactive)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<!-- GCS link is provided by SITL -->
<arg name="gcs_url" value=""/>
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
</include>
</launch>
（四）基于ar_track_alvar的二維碼識別
/home/yehuo/project_ws/src/install/ar_track_alvar/share/ar_track_alvar/launch/ar_track_camera.launch
<launch>
<node pkg="tf" type="static_transform_publisher" name="world_to_cam" args="0 0 0.5 0 1.57 0 world camera_link 10" />
<arg name="marker_size" default="5" />
<arg name="max_new_marker_error" default="0.08" />
<arg name="max_track_error" default="0.2" />
<arg name="cam_image_topic" default="/camera/rgb/image_raw" />
<arg name="cam_info_topic" default="/camera/rgb/camera_info" />
<arg name="output_frame" default="/camera_link" />
<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
<param name="marker_size" type="double" value="$(arg marker_size)" />
<param name="max_new_marker_error" type="double" value="$(arg max_new_marker_error)" />
<param name="max_track_error" type="double" value="$(arg max_track_error)" />
<param name="output_frame" type="string" value="$(arg output_frame)" />
<remap from="camera_image" to="$(arg cam_image_topic)" />
<remap from="camera_info" to="$(arg cam_info_topic)" />
</node>
<!--rviz view /-->
<node pkg="rviz" type="rviz" name="rviz" args="-d /home/yehuo/project_ws/src/ar_track_alvar/ar_track_alvar/config/ar_track_camera.rviz"/>
</launch>
/home/yehuo/project_ws/src/ar_track_alvar/ar_track_alvar/config/ar_track_camera.rviz
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5
Tree Height: 298
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Camera
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
ar_marker_0:
Value: true
ar_marker_1:
Value: true
ar_marker_192:
Value: true
ar_marker_2:
Value: true
ar_marker_4:
Value: true
ar_marker_5:
Value: true
ar_marker_7:
Value: true
ar_marker_8:
Value: true
usb_cam:
Value: true
world:
Value: true
Marker Scale: 0.300000012
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
world:
usb_cam:
ar_marker_0:
{}
ar_marker_1:
{}
ar_marker_192:
{}
ar_marker_2:
{}
ar_marker_4:
{}
ar_marker_5:
{}
ar_marker_7:
{}
ar_marker_8:
{}
Update Interval: 0
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
basic_shapes: true
Queue Size: 100
Value: true
- Class: rviz/Camera
Enabled: true
Image Rendering: background and overlay
Image Topic: /camera/rgb/image_raw
Name: Camera
Overlay Alpha: 0.5
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Visibility:
Grid: true
Marker: true
TF: true
Value: true
Zoom Factor: 1
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.3831408
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.158190101
Y: -0.0226284917
Z: 0.342923284
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.484797359
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.52039075
Saved: ~
Window Geometry:
Camera:
collapsed: false
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000231000002c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000016b000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000199000001530000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005b60000003efc0100000002fb0000000800540069006d00650100000000000005b60000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000037f000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1462
X: 268
Y: 154
Rviz 界面截圖
（五）在gazebo中貼圖二維碼
my_ground_plane.material
material MyGroundPlane/Image
{
technique
{
pass
{
ambient 1 1 1 1.000000
diffuse 1 1 1 1.000000
specular 0.03 0.03 0.03 1.000000
texture_unit
{
texture MarkerData_0.png
}
}
}
}
model.sdf
<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.4">
<model name="my_ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>1 1</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>0.5</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>1 1</size>
</plane>
</geometry>
<material>
<script>
<uri>model://my_ground_plane/materials/scripts</uri>
<uri>model://my_ground_plane/materials/textures/</uri>
<name>MyGroundPlane/Image</name>
</script>
</material>
</visual>
</link>
</model>
</sdf>
model.config
<?xml version="1.0" encoding="UTF-8"?>
<model>
<name>My Ground Plane</name>
<version>1.0</version>
<sdf version="1.4">model.sdf</sdf>
<description>My textured ground plane.</description>
</model>
效果圖
（六）終端運行指令羅列
~$ roscore
~$ rosrun usb_cam usb_cam_node
~$ roslaunch px4 mavros_posix_sitl.launch
~/project_ws$ ./change_mode.sh
~$ roslaunch ar_track_alvar ar_track_camera.launch
（七）問題匯總
浙公網安備 33010602011771號