基于ROS的PX4+Gazebo仿真——PX4一键起飞及飞行控制
一键起飞参考CSDN作者战争果子https://blog.csdn.net/EnthusiasmZing/article/details/79165152?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165225670016781483769357%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=165225670016781483769357&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allsobaiduend~default-2-79165152-null-null.142v9pc_search_result_control_group,157v4control&utm_term=%E4%B8%80%E9%94%AE%E8%B5%B7%E9%A3%9E&spm=1018.2226.3001.4187
飞行控制参考网站：https://gaas.gitbook.io/guide/software-realization-build-your-own-autonomous-drone/wu-ren-ji-zi-dong-jia-shi-xi-lie-offboard-kong-zhi-yi-ji-gazebo-fang-zhen
一键起飞
一、一键起飞功能包创建及代码编译
- 创建新的功能包，名为
offboard_run
终端输入
cd catkin_ws/src //在catkin_ws/src/这个文件夹里面再进行创建
然后在src文件夹内
catkin_create_pkg offboard_run rospy roscpp
- 在新创建的功能包
offboard_run的src文件内（路径：catkin_ws/src/offboard_run/src
），放入一键起飞代码
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/
#include 
3. 对一键起飞代码进行编译
①到我们创建的offboard_run功能包里面的CMakeLists.txt文件，在Build的最后添加下面两行代码，然后保存关闭
add_executable(offboard_run1 src/offboard_run1.cpp)
target_link_libraries(offboard_run1 ${catkin_LIBRARIES})
②到catkin_ws目录下对工作空间进行编译（也就是对刚刚我们添加的C++代码进行编译）
终端输入
catkin_make
出现上图所示结果说明编译已完成
二、一键起飞代码运行
- 打开PX4仿真
终端先输入
cd PX4_Firmware/
再输入
roslaunch px4 mavros_posix_sitl.launch
仿真界面出现后，我们打开一个新的终端（ctrl+alt+T）
终端输入
rosrun offboard_run offboard_run1
运行一键起飞代码，从仿真界面可以看到无人机已经起飞
注意：
运行代码可以使用rosrun的命令
rosrun [代码所在功能包的名] [代码文件名（不用加.cpp）]
最后，结束无人机的悬停直接在终端ctrl+C停止，这时无人机出发安全着陆模式，自行着陆
三、任意修改一键起飞后无人机的位置
只需修改代码文件里面的x、y、z数值即可，记得重新保存和编译，再运行
或者另创建offboard_run2.cpp、offboard_run3.cpp等文件，每个代码里设定的坐标位置各不相同
四、一键起飞python版
1.在offboard_run功能包先创建一个文件夹，名为scripts，专门放置python文件
然后把一键起飞的python代码放进scripts文件内，这里给它的命名为px4_mavros_run.py
import rospy
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, Float64, String
import time
from pyquaternion import Quaternion
import math
import threading
class Px4Controller:
def __init__(self):
self.imu = None
self.gps = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.takeoff_height = 3.2
self.local_enu_position = None
self.cur_target_pose = None
self.global_target = None
self.received_new_task = False
self.arm_state = False
self.offboard_state = False
self.received_imu = False
self.frame = "BODY"
self.state = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_callback)
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback)
self.set_target_position_sub = rospy.Subscriber("gi/set_pose/position", PoseStamped, self.set_target_position_callback)
self.set_target_yaw_sub = rospy.Subscriber("gi/set_pose/orientation", Float32, self.set_target_yaw_callback)
self.custom_activity_sub = rospy.Subscriber("gi/set_activity/type", String, self.custom_activity_callback)
'''
ros publishers
'''
self.local_target_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
print("Px4 Controller Initialized!")
def start(self):
rospy.init_node("offboard_node")
for i in range(10):
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
self.cur_target_pose = self.construct_target(0, 0, self.takeoff_height, self.current_heading)
#print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose))
for i in range(10):
self.local_target_pub.publish(self.cur_target_pose)
self.arm_state = self.arm()
self.offboard_state = self.offboard()
time.sleep(0.2)
if self.takeoff_detection():
print("Vehicle Took Off!")
else:
print("Vehicle Took Off Failed!")
return
'''
main ROS thread
'''
while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False):
self.local_target_pub.publish(self.cur_target_pose)
if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.state = "DISARMED"
time.sleep(0.1)
def construct_target(self, x, y, z, yaw, yaw_rate = 1):
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
'''
cur_p : poseStamped
target_p: positionTarget
'''
def position_distance(self, cur_p, target_p, threshold=0.1):
delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x)
delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y)
delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z)
if (delta_x + delta_y + delta_z < threshold):
return True
else:
return False
def local_pose_callback(self, msg):
self.local_pose = msg
self.local_enu_position = msg
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
def imu_callback(self, msg):
global global_imu, current_heading
self.imu = msg
self.current_heading = self.q2yaw(self.imu.orientation)
self.received_imu = True
def gps_callback(self, msg):
self.gps = msg
def FLU2ENU(self, msg):
FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading)
FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading)
FLU_z = msg.pose.position.z
return FLU_x, FLU_y, FLU_z
def set_target_position_callback(self, msg):
print("Received New Position Task!")
if msg.header.frame_id == 'base_link':
'''
BODY_FLU
'''
# For Body frame, we will use FLU (Forward, Left and Up)
# +Z +X
# ^ ^
# | /
# |/
# +Y <------body
self.frame = "BODY"
print("body FLU frame")
ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg)
ENU_X = ENU_X + self.local_pose.pose.position.x
ENU_Y = ENU_Y + self.local_pose.pose.position.y
ENU_Z = ENU_Z + self.local_pose.pose.position.z
self.cur_target_pose = self.construct_target(ENU_X,
ENU_Y,
ENU_Z,
self.current_heading)
else:
'''
LOCAL_ENU
'''
# For world frame, we will use ENU (EAST, NORTH and UP)
# +Z +Y
# ^ ^
# | /
# |/
# world------> +X
self.frame = "LOCAL_ENU"
print("local ENU frame")
self.cur_target_pose = self.construct_target(msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z,
self.current_heading)
'''
Receive A Custom Activity
'''
def custom_activity_callback(self, msg):
print("Received Custom Activity:", msg.data)
if msg.data == "LAND":
print("LANDING!")
self.state = "LAND"
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
0.1,
self.current_heading)
if msg.data == "HOVER":
print("HOVERING!")
self.state = "HOVER"
self.hover()
else:
print("Received Custom Activity:", msg.data, "not supported yet!")
def set_target_yaw_callback(self, msg):
print("Received New Yaw Task!")
yaw_deg = msg.data * math.pi / 180.0
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
yaw_deg)
'''
return yaw from current IMU
'''
def q2yaw(self, q):
if isinstance(q, Quaternion):
rotate_z_rad = q.yaw_pitch_roll[0]
else:
q_ = Quaternion(q.w, q.x, q.y, q.z)
rotate_z_rad = q_.yaw_pitch_roll[0]
return rotate_z_rad
def arm(self):
if self.armService(True):
return True
else:
print("Vehicle arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print("Vehicle disarming failed!")
return False
def offboard(self):
if self.flightModeService(custom_mode='OFFBOARD'):
return True
else:
print("Vechile Offboard failed")
return False
def hover(self):
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
self.current_heading)
def takeoff_detection(self):
if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state:
return True
else:
return False
if __name__ == '__main__':
con = Px4Controller()
con.start()
然后记得对python文件进行权限修改，在执行处打勾
2. 同样，先启动PX4仿真（参考上面的步骤）
然后到刚才存放python代码的目录内，打开终端
在终端直接输入
python px4_mavros_run.py
此时能看到无人机起飞成功
飞行控制
一、代码
- 把控制无人机的代码放进
catkin_ws/src/offboard_run/scripts，这里有两个python代码文件，同时放进该路径
commander.py代码如下：
import rospy
from mavros_msgs.msg import GlobalPositionTarget, State
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, String
from pyquaternion import Quaternion
import time
import math
class Commander:
def __init__(self):
rospy.init_node("commander_node")
rate = rospy.Rate(20)
self.position_target_pub = rospy.Publisher('gi/set_pose/position', PoseStamped, queue_size=10)
self.yaw_target_pub = rospy.Publisher('gi/set_pose/orientation', Float32, queue_size=10)
self.custom_activity_pub = rospy.Publisher('gi/set_activity/type', String, queue_size=10)
def move(self, x, y, z, BODY_OFFSET_ENU=True):
self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU))
def turn(self, yaw_degree):
self.yaw_target_pub.publish(yaw_degree)
# land at current position
def land(self):
self.custom_activity_pub.publish(String("LAND"))
# hover at current position
def hover(self):
self.custom_activity_pub.publish(String("HOVER"))
# return to home position with defined height
def return_home(self, height):
self.position_target_pub.publish(self.set_pose(0, 0, height, False))
def set_pose(self, x=0, y=0, z=2, BODY_FLU = True):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
# ROS uses ENU internally, so we will stick to this convention
if BODY_FLU:
pose.header.frame_id = 'base_link'
else:
pose.header.frame_id = 'map'
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
return pose
if __name__ == "__main__":
con = Commander()
time.sleep(2)
con.move(1, 0, 0)
time.sleep(2)
con.turn(90)
time.sleep(2)
con.land()
conclude.py代码如下：
import rospy
from mavros_msgs.msg import GlobalPositionTarget, State,PositionTarget
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32,Float64,String
from pyquaternion import Quaternion
import time
import math
import threading
class Px4Controller:
def __init__(self):
self.imu = None
self.gps = None
self.local_pose = None
self.current_state = None
self.current_heading = None
self.takeoff_height = 3.2
self.local_enu_position = None
self.cur_target_pose = None
self.global_target = None
self.received_new_task = False
self.arm_state = False
self.offboard_state = False
self.received_imu = False
self.frame = "BODY"
self.state = None
'''
ros subscribers
'''
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_callback)
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback)
self.set_target_position_sub = rospy.Subscriber("gi/set_pose/position", PoseStamped, self.set_target_position_callback)
self.set_target_yaw_sub = rospy.Subscriber("gi/set_pose/orientation", Float32, self.set_target_yaw_callback)
self.custom_activity_sub = rospy.Subscriber("gi/set_activity/type", String, self.custom_activity_callback)
'''
ros publishers
'''
self.local_target_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=10)
'''
ros services
'''
self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode)
print("Px4 Controller Initialized!")
def start(self):
rospy.init_node("offboard_node")
for i in range(10):
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
self.cur_target_pose = self.construct_target(0, 0, self.takeoff_height, self.current_heading)
#print ("self.cur_target_pose:", self.cur_target_pose, type(self.cur_target_pose))
for i in range(10):
self.local_target_pub.publish(self.cur_target_pose)
self.arm_state = self.arm()
self.offboard_state = self.offboard()
time.sleep(0.2)
if self.takeoff_detection():
print("Vehicle Took Off!")
else:
print("Vehicle Took Off Failed!")
return
'''
main ROS thread
'''
while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False):
self.local_target_pub.publish(self.cur_target_pose)
if (self.state is "LAND") and (self.local_pose.pose.position.z < 0.15):
if(self.disarm()):
self.state = "DISARMED"
time.sleep(0.1)
def construct_target(self, x, y, z, yaw, yaw_rate = 1):
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
'''
cur_p : poseStamped
target_p: positionTarget
'''
def position_distance(self, cur_p, target_p, threshold=0.1):
delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x)
delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y)
delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z)
if (delta_x + delta_y + delta_z < threshold):
return True
else:
return False
def local_pose_callback(self, msg):
self.local_pose = msg
self.local_enu_position = msg
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
def imu_callback(self, msg):
global global_imu, current_heading
self.imu = msg
self.current_heading = self.q2yaw(self.imu.orientation)
self.received_imu = True
def gps_callback(self, msg):
self.gps = msg
def FLU2ENU(self, msg):
FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(self.current_heading)
FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(self.current_heading)
FLU_z = msg.pose.position.z
return FLU_x, FLU_y, FLU_z
def set_target_position_callback(self, msg):
print("Received New Position Task!")
if msg.header.frame_id == 'base_link':
'''
BODY_FLU
'''
# For Body frame, we will use FLU (Forward, Left and Up)
# +Z +X
# ^ ^
# | /
# |/
# +Y <------body
self.frame = "BODY"
print("body FLU frame")
ENU_X, ENU_Y, ENU_Z = self.FLU2ENU(msg)
ENU_X = ENU_X + self.local_pose.pose.position.x
ENU_Y = ENU_Y + self.local_pose.pose.position.y
ENU_Z = ENU_Z + self.local_pose.pose.position.z
self.cur_target_pose = self.construct_target(ENU_X,
ENU_Y,
ENU_Z,
self.current_heading)
else:
'''
LOCAL_ENU
'''
# For world frame, we will use ENU (EAST, NORTH and UP)
# +Z +Y
# ^ ^
# | /
# |/
# world------> +X
self.frame = "LOCAL_ENU"
print("local ENU frame")
self.cur_target_pose = self.construct_target(msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z,
self.current_heading)
'''
Receive A Custom Activity
'''
def custom_activity_callback(self, msg):
print("Received Custom Activity:", msg.data)
if msg.data == "LAND":
print("LANDING!")
self.state = "LAND"
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
0.1,
self.current_heading)
if msg.data == "HOVER":
print("HOVERING!")
self.state = "HOVER"
self.hover()
else:
print("Received Custom Activity:", msg.data, "not supported yet!")
def set_target_yaw_callback(self, msg):
print("Received New Yaw Task!")
yaw_deg = msg.data * math.pi / 180.0
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
yaw_deg)
'''
return yaw from current IMU
'''
def q2yaw(self, q):
if isinstance(q, Quaternion):
rotate_z_rad = q.yaw_pitch_roll[0]
else:
q_ = Quaternion(q.w, q.x, q.y, q.z)
rotate_z_rad = q_.yaw_pitch_roll[0]
return rotate_z_rad
def arm(self):
if self.armService(True):
return True
else:
print("Vehicle arming failed!")
return False
def disarm(self):
if self.armService(False):
return True
else:
print("Vehicle disarming failed!")
return False
def offboard(self):
if self.flightModeService(custom_mode='OFFBOARD'):
return True
else:
print("Vechile Offboard failed")
return False
def hover(self):
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
self.current_heading)
def takeoff_detection(self):
if self.local_pose.pose.position.z > 0.1 and self.offboard_state and self.arm_state:
return True
else:
return False
class Commander:
def __init__(self):
rospy.init_node("commander_node")
rate = rospy.Rate(20)
self.position_target_pub = rospy.Publisher('gi/set_pose/position', PoseStamped, queue_size=10)
self.yaw_target_pub = rospy.Publisher('gi/set_pose/orientation', Float32, queue_size=10)
self.custom_activity_pub = rospy.Publisher('gi/set_activity/type', String, queue_size=10)
def move(self, x, y, z, BODY_OFFSET_ENU=True):
self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU))
def turn(self, yaw_degree):
self.yaw_target_pub.publish(yaw_degree)
# land at current position
def land(self):
self.custom_activity_pub.publish(String("LAND"))
# hover at current position
def hover(self):
self.custom_activity_pub.publish(String("HOVER"))
# return to home position with defined height
def return_home(self, height):
self.position_target_pub.publish(self.set_pose(0, 0, height, False))
def set_pose(self, x=0, y=0, z=2, BODY_FLU = True):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
# ROS uses ENU internally, so we will stick to this convention
if BODY_FLU:
pose.header.frame_id = 'base_link'
else:
pose.header.frame_id = 'map'
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
return pose
if __name__ == "__main__":
con = Px4Controller()
con.start()
con = Commander()
time.sleep(2)
con.move(1, 0, 0)
time.sleep(2)
con.turn(90)
time.sleep(2)
con.land()
注意：记得修改python代码可执行权限
二、运行
- 打开PX4仿真
终端先输入
cd PX4_Firmware/
再输入
roslaunch px4 mavros_posix_sitl.launch
- 到
catkin_ws/src/offboard_run/scripts下打开终端，输入
python px4_mavros_run.py
使飞机起飞
- 到
catkin_ws/src/offboard_ru/scripts下，在终端输入
ipython
- 随后依次输入
from commander import Commander //输入后回车
con=Commander ( ) //输入后回车
- 然后依据
catkin_ws/src/offboard_run/scripts里面的commander.py
在上一步(第4步)接着输入指令，使其飞行，如：
输入con.move(1,0,0), 则向北飞行1m
输入con.turn(90),则转动90°方向
