RflySimSDK v4.01
RflySimSDK说明文档
载入中...
搜索中...
未找到
PX4MavCtrler类 参考

无人机通信实例。 更多...

Public 成员函数

 __init__ (self, CopterID=1, ip="127.0.0.1", Com="udp", port=0)
 构造函数。
 spin (self)
 ROS守护进程spin函数,保持节点运行
 convert_to_payload64 (self, payload_bytes)
 将传入的字节序列(payload_bytes)转换成MAVLink协议中的payload64格式。
 convert_to_rosmsg (self, mavmsg, header)
 将 pymavlink 库中的一个MAVLink消息对象转换为ROS消息格式。
 setGPSOriLLA (self, LonLatAlt=[40.1540302, 116.2593683, 50])
 设置GPS原点
 arm_px4 (self, isArm)
 给PX4发送解锁指令。
 fillList (self, data, inLen, fill=0)
 将输入数据填充或截断到指定的长度。
 InitMavLoop (self)
 仿真初始化,启动无人机仿真循环
 sendStartMsg (self, copterID=-1)
 发送开始仿真信息
 waitForStartMsg (self)
 等待个特定的启动消息,每个无人机或节点等待接收特定的启动信号以开始仿真
 SendMavArm (self, isArm)
 发送解锁指令
 initOffboard (self)
 初始化Offboard模式
 OffboardLoop (self)
 启动Offboard模式循环
 endOffboard (self)
 结束Offboard模式
 stopRun (self)
 停止仿真
 calcTypeMask (self, EnList)
 根据提供的布尔列表 EnList 计算一个类型掩码(type mask)。
 SendVelNED (self, vx=math.nan, vy=math.nan, vz=math.nan, yawrate=math.nan)
 发送北东地坐标系下的速度指令。
 SendVelFRD (self, vx=math.nan, vy=math.nan, vz=math.nan, yawrate=math.nan)
 发送机体坐标系下的速度指令。
 SendPosNED (self, x=math.nan, y=math.nan, z=math.nan, yaw=math.nan)
 发送北东地坐标系下的位置指令。
 SendPosVelNED (self, PosE=[math.nan] *3, VelE=[math.nan] *3, yaw=math.nan, yawrate=math.nan)
 发送北东地坐标系下的位置、速度指令,位置和速度共同控制接口,如果某个通道不想控制,设置为nan即可。
 local_pose_callback (self, msg)
 处理ROS中订阅的本地位置话题的消息,更新无人机的位置和姿态信息
 local_vel_callback (self, msg)
 处理ROS中订阅的本地速度话题的消息,更新无人机的速度和角速率信息
 mavros_state_callback (self, msg)
 处理ROS中订阅的MAVROS状态主题的消息,更新无人机的飞行模式信息。
 imu_callback (self, msg)
 处理ROS中订阅的IMU话题的消息,更新无人机的IMU数据
 gps_callback (self, msg)
 处理ROS中订阅的GPS话题的消息,更新无人机的GPS数据
 q2yaw (self, q)
 从输入的四元数(q)来计算偏航角
 q2Euler (self, q)
 从输入的四元数(q)来计算欧拉角
 SendMavArm (self, isArm=1)
 通过MAVLink发送解锁指令给PX4
 arm (self)
 检查PX4的解锁状态
 disarm (self)
 检查PX4的上锁状态
 offboard (self)
 设置PX4无人机进入Offboard控制模式,尝试将无人机切换到Offboard模式,这是一种允许外部控制命令覆盖无人机内部控制的模式。
 land (self)
 yawSat (self, yaw)
 对偏航角进行饱和处理,此函数接收一个偏航角值,并确保其在 -π/2 到 π/2 弧度(或等效的 -90 到 90 度)的范围内。如果输入的偏航角超出这个范围,函数会相应地调整它,使其不超过阈值。
 sendMavSetParam (self, param_id_str, param_value, param_type)
 发送MAVLink设置参数请求,用于通过MAVLink协议设置无人机的参数。根据参数类型,它可以设置整型或实型参数。
 SendMavCmdLong (self, command, param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, param7=0)
 发送MAVLink长格式命令,该函数用于通过MAVLink协议发送长格式的命令给无人机。命令可以包括一系列的参数,用于指定命令的具体行为。
 SendHILCtrlMsg (self, ctrls=[0] *16, idx=0)
 发送硬件在环仿真的hil_actuator_controls消息(消息定义可见:https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS) 代码的核心是创建一个Mavlink消息,并转换为ros_mavlink消息,在Python中比较复杂 在C语言中很简单,利用\opt\ros[noetic或foxy]\mavlink_convert.h中的 bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg)函数可以轻松转换,然后发布即可 RflySim平台需要借用hil_actuator_controls来将数据传到rfly_ctrl、rfly_ctrl1和rfly_ctrl2中 通过mavros的接口,将一条mavlink的buf包消息,传给飞控

Public 属性

bool isCom = False
 UDP模式解析
 Com = Com
int baud = 115200
 COM波特率。
 ip = ip
int CopterID = CopterID
int port = 20100 + self.CopterID * 2 - 2
str rosName = "mavros"
 ROS名称
str ComName = "COM3"
 串口名称,串口连接模式解析。
 imu = None
 IMU数据。
 gps = None
 GPS数据。
 local_pose = None
 本地位置量
 mavros_state = None
 MAVROS状态
 current_heading = None
 当前机头状态
 local_vel = None
 本地速度量
bool arm_state = False
 解锁状态
bool offboard_state = False
 Offboard模式状态
bool received_imu = False
 接收到的IMU数据
str frame = "BODY"
 坐标系名称
 state = None
 无人机状态量
 command = TwistStamped()
 指令
 offCmd = PositionTarget()
 mavros_msgs包中的目标位置信息。
bool isInOffboard = False
 是否进入Offboard模式的标志位。
list uavAngEular = [0, 0, 0]
 无人机欧拉角状态量
list uavAngRate = [0, 0, 0]
 无人机欧拉角速率状态量
list uavPosNED = [0, 0, 0]
 无人机北东地位置状态量
list uavVelNED = [0, 0, 0]
 无人机北东地速度状态量
list uavAngQuatern = [0, 0, 0, 0]
 无人机姿态角四元数状态量
list uavGlobalPos
 从PX4转移到UE4地图的估计全球位置
list trueGpsUeCenter = [40.1540302, 116.2593683, 50]
 真实全球GPS位置
 geo = EarthModel.EarthModel()
 地理坐标转换的模块
int count = 0
 软件在环仿真计数
int countHil = 0
 硬件在环仿真计数
bool hasInit = False
 初始化是否完成标志位
 local_pose_sub
 订阅ROS中的本地位置
 local_vel_sub
 订阅ROS中的本地速度
 mavros_sub
 订阅mavros包中消息
 gps_sub
 订阅ROS中的GPS信息
 imu_sub
 订阅ROS中的IMU信息
 vel_pub
 发布期望速度数据到ROS中
 vel_raw_pub
 发布本地原始期望速度数据到ROS中
 mav_raw_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=10)
 发布原始MAVLink消息到ROS中
 armService
 创建解锁服务代理
 flightModeService
 创建飞行模式服务代理
 setparamService
 创建参数设置服务代理
 sendCmdLongService
 创建发送长格式的命令给无人机服务代理
 ros_node = Node("RflyRos" + str(self.CopterID))
 创建ROS节点
 executor = rclpy.executors.MultiThreadedExecutor()
 创建一个多线程执行器
 t1 = threading.Thread(target=self.executor.spin, args=())
 创建一个线程并开始执行
 f = fifo
 MAVLink初始化
 mav0 = mavlink2.MAVLink(self.f, 255, 1)
 发送任意mavlink消息
int tgtSys = the_connection.target_system
 child = subprocess.Popen(cmdStr, shell=True, stdout=subprocess.PIPE)
 t2 = threading.Thread(target=self.OffboardLoop, args=())

详细描述

无人机通信实例。

此类通过UDP、COM方式与无人机模拟器进行通信,控制模拟器的操作。

ID: 表示飞机的CopterID号。按平台规则,port=20100+CopterID*2-2。
ip: 数据向外发送的IP地址。默认是发往本机的127.0.0.1的IP;在分布式仿真时,也可以指定192.168打头的局域网电脑IP;也可以使用255.255.255.255的广播地址(会干扰网络其他电脑)
Com: 与Pixhawk的连接模式。
    Com='udp',表示使用默认的udp模式接收数据,这种模式下,是接收CopterSim转发的PX4的MAVLink消息(或UDP_full,simple)消息包
             使用port+1端口收和port端口发(例如,1号飞机是20101端口收,20100端口发,与CopterSim对应)。
    Com='COM3'(Widnows下)或 Com='/dev/ttyUSB0'(Linux系统,也可能是ttyS0、ttyAMA0等),表示通过USB线(或者数传)连接飞控,使用默认57600的波特率。注意:波特率使用port口设置,默认port=0,会重映射为57600

port: UDP模式下默认情况下设为0,会自动根据IP填充,按平台规则,port=20100+CopterID*2-2。如果这里赋值大于0,则会强制使用port定义的端口。
      COM模式下,Port默认表示波特率self.baud=port。如果port=0,则会设置self.baud=57600


接口示例:
UDP模式
PX4MavCtrler(1) # 默认IP
PX4MavCtrler(1,'192.168.31.24') # 指定IP,用于远程控制

串口模式
PX4MavCtrler(1,'127.0.0.1','com1',57600) # 指定串口,并设置波特率

构造及析构函数说明

◆ __init__()

__init__ ( self,
CopterID = 1,
ip = "127.0.0.1",
Com = "udp",
port = 0 )

构造函数。

参数
CopterID初始化时设置的无人机ID,默认为:1。
ipip地址,默认为:127.0.0.1。
portIP端口,默认为:0。

成员函数说明

◆ arm()

arm ( self)

检查PX4的解锁状态

返回
返回解锁标准的真假值
函数调用图:
这是这个函数的调用关系图:

◆ arm_px4()

arm_px4 ( self,
isArm )

给PX4发送解锁指令。

参数
isArm解锁标志位。
返回
返回解锁信息
这是这个函数的调用关系图:

◆ calcTypeMask()

calcTypeMask ( self,
EnList )

根据提供的布尔列表 EnList 计算一个类型掩码(type mask)。

参数
EnList布尔列表
返回
返回计算后的类型掩码
这是这个函数的调用关系图:

◆ convert_to_payload64()

convert_to_payload64 ( self,
payload_bytes )

将传入的字节序列(payload_bytes)转换成MAVLink协议中的payload64格式。

参数
payload_bytes字节序列
返回
MAVLink协议中的payload64格式。
这是这个函数的调用关系图:

◆ convert_to_rosmsg()

convert_to_rosmsg ( self,
mavmsg,
header )

将 pymavlink 库中的一个MAVLink消息对象转换为ROS消息格式。

参数
mavmsgMAVLink消息对象
header消息头对象
返回
创建的Mavlink对象。
Convert pymavlink message to Mavlink.msg

Currently supports MAVLink v1.0 only.
函数调用图:
这是这个函数的调用关系图:

◆ disarm()

disarm ( self)

检查PX4的上锁状态

返回
返回上锁标准的真假值
函数调用图:
这是这个函数的调用关系图:

◆ endOffboard()

endOffboard ( self)

结束Offboard模式

◆ fillList()

fillList ( self,
data,
inLen,
fill = 0 )

将输入数据填充或截断到指定的长度。

参数
data需要填充或截断的数据,可以是列表或NumPy数组。
inLen目标长度,即填充或截断后的长度。
fill填充值,当数据长度小于目标长度时,会用这个值来填充数据。默认值为0。
返回
返回填充或截断后的数据
这是这个函数的调用关系图:

◆ gps_callback()

gps_callback ( self,
msg )

处理ROS中订阅的GPS话题的消息,更新无人机的GPS数据

参数
msg从ROS中接收到的消息对象

◆ imu_callback()

imu_callback ( self,
msg )

处理ROS中订阅的IMU话题的消息,更新无人机的IMU数据

参数
msg从ROS中接收到的消息对象
函数调用图:

◆ InitMavLoop()

InitMavLoop ( self)

仿真初始化,启动无人机仿真循环

◆ initOffboard()

initOffboard ( self)

初始化Offboard模式

函数调用图:

◆ local_pose_callback()

local_pose_callback ( self,
msg )

处理ROS中订阅的本地位置话题的消息,更新无人机的位置和姿态信息

参数
msg从ROS中接收到的消息对象
函数调用图:

◆ local_vel_callback()

local_vel_callback ( self,
msg )

处理ROS中订阅的本地速度话题的消息,更新无人机的速度和角速率信息

参数
msg从ROS中接收到的消息对象

◆ mavros_state_callback()

mavros_state_callback ( self,
msg )

处理ROS中订阅的MAVROS状态主题的消息,更新无人机的飞行模式信息。

参数
msg从ROS中接收到的消息对象

◆ offboard()

offboard ( self)

设置PX4无人机进入Offboard控制模式,尝试将无人机切换到Offboard模式,这是一种允许外部控制命令覆盖无人机内部控制的模式。

返回
返回是否成功进入Offboard控制模式的布尔值
这是这个函数的调用关系图:

◆ OffboardLoop()

OffboardLoop ( self)

启动Offboard模式循环

这是这个函数的调用关系图:

◆ q2Euler()

q2Euler ( self,
q )

从输入的四元数(q)来计算欧拉角

参数
q四元数
返回
返回计算后的姿态角。
这是这个函数的调用关系图:

◆ q2yaw()

q2yaw ( self,
q )

从输入的四元数(q)来计算偏航角

参数
q四元数
这是这个函数的调用关系图:

◆ SendHILCtrlMsg()

SendHILCtrlMsg ( self,
ctrls = [0] * 16,
idx = 0 )

发送硬件在环仿真的hil_actuator_controls消息(消息定义可见:https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS) 代码的核心是创建一个Mavlink消息,并转换为ros_mavlink消息,在Python中比较复杂 在C语言中很简单,利用\opt\ros[noetic或foxy]\mavlink_convert.h中的 bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg)函数可以轻松转换,然后发布即可 RflySim平台需要借用hil_actuator_controls来将数据传到rfly_ctrl、rfly_ctrl1和rfly_ctrl2中 通过mavros的接口,将一条mavlink的buf包消息,传给飞控

参数
ctrls16维控制量
idx索引值,用于选择不同的uORB消息ID,默认为0
Send hil_actuator_controls command to PX4, which will be transferred to uORB message rfly_ctrl
https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS
函数调用图:

◆ SendMavArm() [1/2]

SendMavArm ( self,
isArm )

发送解锁指令

函数调用图:
这是这个函数的调用关系图:

◆ SendMavArm() [2/2]

SendMavArm ( self,
isArm = 1 )

通过MAVLink发送解锁指令给PX4

参数
isArm解锁标志位
  • 0表示上锁
  • 1表示解锁
函数调用图:
这是这个函数的调用关系图:

◆ SendMavCmdLong()

SendMavCmdLong ( self,
command,
param1 = 0,
param2 = 0,
param3 = 0,
param4 = 0,
param5 = 0,
param6 = 0,
param7 = 0 )

发送MAVLink长格式命令,该函数用于通过MAVLink协议发送长格式的命令给无人机。命令可以包括一系列的参数,用于指定命令的具体行为。

参数
command要发送的MAVLink命令的ID
param1命令的第一个参数,默认为0
param2命令的第二个参数,默认为0
param3命令的第三个参数,默认为0
param4命令的第四个参数,默认为0
param5命令的第五个参数,默认为0
param6命令的第六个参数,默认为0
param7命令的第七个参数,默认为0

◆ sendMavSetParam()

sendMavSetParam ( self,
param_id_str,
param_value,
param_type )

发送MAVLink设置参数请求,用于通过MAVLink协议设置无人机的参数。根据参数类型,它可以设置整型或实型参数。

参数
param_id_str参数的名称字符串标识符
param_value参数的值,可以是整型或实型
param_type参数的类型,'INT' 表示整型,其他值表示实型
这是这个函数的调用关系图:

◆ SendPosNED()

SendPosNED ( self,
x = math.nan,
y = math.nan,
z = math.nan,
yaw = math.nan )

发送北东地坐标系下的位置指令。

参数
xX轴位置
yY轴位置
zZ轴位置
yaw偏航角角度
函数调用图:
这是这个函数的调用关系图:

◆ SendPosVelNED()

SendPosVelNED ( self,
PosE = [math.nan] * 3,
VelE = [math.nan] * 3,
yaw = math.nan,
yawrate = math.nan )

发送北东地坐标系下的位置、速度指令,位置和速度共同控制接口,如果某个通道不想控制,设置为nan即可。

参数
PosEX、Y、Z轴位置指令
VelEX、Y、Z轴速度指令
yaw偏航角角度
yawrate偏航角速率
函数调用图:

◆ sendStartMsg()

sendStartMsg ( self,
copterID = -1 )

发送开始仿真信息

参数
copterID无人机ID,默认为:-1。

◆ SendVelFRD()

SendVelFRD ( self,
vx = math.nan,
vy = math.nan,
vz = math.nan,
yawrate = math.nan )

发送机体坐标系下的速度指令。

参数
vxX轴速度
vyY轴速度
vzZ轴速度
yawrate偏航角速率
函数调用图:

◆ SendVelNED()

SendVelNED ( self,
vx = math.nan,
vy = math.nan,
vz = math.nan,
yawrate = math.nan )

发送北东地坐标系下的速度指令。

参数
vxX轴速度
vyY轴速度
vzZ轴速度
yawrate偏航角速率
函数调用图:
这是这个函数的调用关系图:

◆ setGPSOriLLA()

setGPSOriLLA ( self,
LonLatAlt = [40.1540302, 116.2593683, 50] )

设置GPS原点

参数
LonLatAltGPS的经度、纬度和高度

◆ spin()

spin ( self)

ROS守护进程spin函数,保持节点运行

参数
返回

◆ stopRun()

stopRun ( self)

停止仿真

◆ waitForStartMsg()

waitForStartMsg ( self)

等待个特定的启动消息,每个无人机或节点等待接收特定的启动信号以开始仿真

◆ yawSat()

yawSat ( self,
yaw )

对偏航角进行饱和处理,此函数接收一个偏航角值,并确保其在 -π/2 到 π/2 弧度(或等效的 -90 到 90 度)的范围内。如果输入的偏航角超出这个范围,函数会相应地调整它,使其不超过阈值。

参数
yaw输入的偏航角,以弧度为单位
返回
返回饱和后的偏航角值
这是这个函数的调用关系图:

类成员变量说明

◆ arm_state

bool arm_state = False

解锁状态

◆ armService

armService
初始值:
= rospy.ServiceProxy(
"/" + self.rosName + "/cmd/arming", CommandBool
)

创建解锁服务代理

◆ baud

int baud = 115200

COM波特率。

◆ command

command = TwistStamped()

指令

◆ ComName

str ComName = "COM3"

串口名称,串口连接模式解析。

◆ count

int count = 0

软件在环仿真计数

◆ countHil

int countHil = 0

硬件在环仿真计数

◆ current_heading

current_heading = None

当前机头状态

◆ executor

executor = rclpy.executors.MultiThreadedExecutor()

创建一个多线程执行器

◆ f

f = fifo

MAVLink初始化

◆ flightModeService

flightModeService
初始值:
= rospy.ServiceProxy(
"/" + self.rosName + "/set_mode", SetMode
)

创建飞行模式服务代理

◆ frame

str frame = "BODY"

坐标系名称

◆ geo

地理坐标转换的模块

◆ gps

gps = None

GPS数据。

◆ gps_sub

gps_sub
初始值:
= rospy.Subscriber(
"/" + self.rosName + "/global_position/global",
NavSatFix,
self.gps_callback,
)

订阅ROS中的GPS信息

◆ hasInit

bool hasInit = False

初始化是否完成标志位

◆ imu

imu = None

IMU数据。

◆ imu_sub

imu_sub
初始值:
= rospy.Subscriber(
"/" + self.rosName + "/imu/data", Imu, self.imu_callback
)

订阅ROS中的IMU信息

◆ isCom

bool isCom = False

UDP模式解析

◆ isInOffboard

bool isInOffboard = False

是否进入Offboard模式的标志位。

◆ local_pose

local_pose = None

本地位置量

◆ local_pose_sub

local_pose_sub
初始值:
= rospy.Subscriber(
"/" + self.rosName + "/local_position/pose",
PoseStamped,
self.local_pose_callback,
)

订阅ROS中的本地位置

◆ local_vel

local_vel = None

本地速度量

◆ local_vel_sub

local_vel_sub
初始值:
= rospy.Subscriber(
"/" + self.rosName + "/local_position/velocity_local",
TwistStamped,
self.local_vel_callback,
)

订阅ROS中的本地速度

◆ mav0

mav0 = mavlink2.MAVLink(self.f, 255, 1)

发送任意mavlink消息

◆ mav_raw_pub

mav_raw_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=10)

发布原始MAVLink消息到ROS中

◆ mavros_state

mavros_state = None

MAVROS状态

◆ mavros_sub

mavros_sub
初始值:
= rospy.Subscriber(
"/" + self.rosName + "/state", State, self.mavros_state_callback
)

订阅mavros包中消息

◆ offboard_state

bool offboard_state = False

Offboard模式状态

◆ offCmd

offCmd = PositionTarget()

mavros_msgs包中的目标位置信息。

◆ received_imu

bool received_imu = False

接收到的IMU数据

◆ ros_node

ros_node = Node("RflyRos" + str(self.CopterID))

创建ROS节点

◆ rosName

str rosName = "mavros"

ROS名称

◆ sendCmdLongService

sendCmdLongService
初始值:
= rospy.ServiceProxy(
"/" + self.rosName + "/cmd/command", CommandLong
)

创建发送长格式的命令给无人机服务代理

◆ setparamService

setparamService
初始值:
= rospy.ServiceProxy(
"/" + self.rosName + "/param/set", ParamSet
)

创建参数设置服务代理

◆ state

state = None

无人机状态量

◆ t1

t1 = threading.Thread(target=self.executor.spin, args=())

创建一个线程并开始执行

◆ trueGpsUeCenter

list trueGpsUeCenter = [40.1540302, 116.2593683, 50]

真实全球GPS位置

◆ uavAngEular

list uavAngEular = [0, 0, 0]

无人机欧拉角状态量

◆ uavAngQuatern

list uavAngQuatern = [0, 0, 0, 0]

无人机姿态角四元数状态量

◆ uavAngRate

list uavAngRate = [0, 0, 0]

无人机欧拉角速率状态量

◆ uavGlobalPos

list uavGlobalPos
初始值:
= [
0,
0,
0,
]

从PX4转移到UE4地图的估计全球位置

◆ uavPosNED

list uavPosNED = [0, 0, 0]

无人机北东地位置状态量

◆ uavVelNED

list uavVelNED = [0, 0, 0]

无人机北东地速度状态量

◆ vel_pub

vel_pub
初始值:
= rospy.Publisher(
"/" + self.rosName + "/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=10,
)

发布期望速度数据到ROS中

◆ vel_raw_pub

vel_raw_pub
初始值:
= rospy.Publisher(
"/" + self.rosName + "/setpoint_raw/local",
PositionTarget,
queue_size=10,
)

发布本地原始期望速度数据到ROS中


该类的文档由以下文件生成: