无人机通信实例。 更多...
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__ | ( | self, | |
| CopterID = 1, | |||
| ip = "127.0.0.1", | |||
| Com = "udp", | |||
| port = 0 ) |
构造函数。
| CopterID | 初始化时设置的无人机ID,默认为:1。 |
| ip | ip地址,默认为:127.0.0.1。 |
| port | IP端口,默认为:0。 |
| arm | ( | self | ) |
检查PX4的解锁状态
| arm_px4 | ( | self, | |
| isArm ) |
给PX4发送解锁指令。
| isArm | 解锁标志位。 |
| calcTypeMask | ( | self, | |
| EnList ) |
根据提供的布尔列表 EnList 计算一个类型掩码(type mask)。
| EnList | 布尔列表 |
| convert_to_payload64 | ( | self, | |
| payload_bytes ) |
将传入的字节序列(payload_bytes)转换成MAVLink协议中的payload64格式。
| payload_bytes | 字节序列 |
| convert_to_rosmsg | ( | self, | |
| mavmsg, | |||
| header ) |
将 pymavlink 库中的一个MAVLink消息对象转换为ROS消息格式。
| mavmsg | MAVLink消息对象 |
| header | 消息头对象 |
Convert pymavlink message to Mavlink.msg Currently supports MAVLink v1.0 only.
| disarm | ( | self | ) |
检查PX4的上锁状态
| endOffboard | ( | self | ) |
结束Offboard模式
| fillList | ( | self, | |
| data, | |||
| inLen, | |||
| fill = 0 ) |
将输入数据填充或截断到指定的长度。
| data | 需要填充或截断的数据,可以是列表或NumPy数组。 |
| inLen | 目标长度,即填充或截断后的长度。 |
| fill | 填充值,当数据长度小于目标长度时,会用这个值来填充数据。默认值为0。 |
| gps_callback | ( | self, | |
| msg ) |
处理ROS中订阅的GPS话题的消息,更新无人机的GPS数据
| msg | 从ROS中接收到的消息对象 |
| imu_callback | ( | self, | |
| msg ) |
处理ROS中订阅的IMU话题的消息,更新无人机的IMU数据
| msg | 从ROS中接收到的消息对象 |
| InitMavLoop | ( | self | ) |
仿真初始化,启动无人机仿真循环
| initOffboard | ( | self | ) |
初始化Offboard模式
| local_pose_callback | ( | self, | |
| msg ) |
处理ROS中订阅的本地位置话题的消息,更新无人机的位置和姿态信息
| msg | 从ROS中接收到的消息对象 |
| local_vel_callback | ( | self, | |
| msg ) |
处理ROS中订阅的本地速度话题的消息,更新无人机的速度和角速率信息
| msg | 从ROS中接收到的消息对象 |
| mavros_state_callback | ( | self, | |
| msg ) |
处理ROS中订阅的MAVROS状态主题的消息,更新无人机的飞行模式信息。
| msg | 从ROS中接收到的消息对象 |
| offboard | ( | self | ) |
设置PX4无人机进入Offboard控制模式,尝试将无人机切换到Offboard模式,这是一种允许外部控制命令覆盖无人机内部控制的模式。
| OffboardLoop | ( | self | ) |
启动Offboard模式循环
| q2Euler | ( | self, | |
| q ) |
从输入的四元数(q)来计算欧拉角
| q | 四元数 |
| q2yaw | ( | self, | |
| q ) |
从输入的四元数(q)来计算偏航角
| q | 四元数 |
| 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包消息,传给飞控
| ctrls | 16维控制量 |
| 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 | ( | self, | |
| isArm ) |
发送解锁指令
| SendMavArm | ( | self, | |
| isArm = 1 ) |
通过MAVLink发送解锁指令给PX4
| isArm | 解锁标志位
|
| 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 | ( | self, | |
| param_id_str, | |||
| param_value, | |||
| param_type ) |
发送MAVLink设置参数请求,用于通过MAVLink协议设置无人机的参数。根据参数类型,它可以设置整型或实型参数。
| param_id_str | 参数的名称字符串标识符 |
| param_value | 参数的值,可以是整型或实型 |
| param_type | 参数的类型,'INT' 表示整型,其他值表示实型 |
| SendPosNED | ( | self, | |
| x = math.nan, | |||
| y = math.nan, | |||
| z = math.nan, | |||
| yaw = math.nan ) |
发送北东地坐标系下的位置指令。
| x | X轴位置 |
| y | Y轴位置 |
| z | Z轴位置 |
| yaw | 偏航角角度 |
| SendPosVelNED | ( | self, | |
| PosE = [math.nan] * 3, | |||
| VelE = [math.nan] * 3, | |||
| yaw = math.nan, | |||
| yawrate = math.nan ) |
发送北东地坐标系下的位置、速度指令,位置和速度共同控制接口,如果某个通道不想控制,设置为nan即可。
| PosE | X、Y、Z轴位置指令 |
| VelE | X、Y、Z轴速度指令 |
| yaw | 偏航角角度 |
| yawrate | 偏航角速率 |
| sendStartMsg | ( | self, | |
| copterID = -1 ) |
发送开始仿真信息
| copterID | 无人机ID,默认为:-1。 |
| SendVelFRD | ( | self, | |
| vx = math.nan, | |||
| vy = math.nan, | |||
| vz = math.nan, | |||
| yawrate = math.nan ) |
发送机体坐标系下的速度指令。
| vx | X轴速度 |
| vy | Y轴速度 |
| vz | Z轴速度 |
| yawrate | 偏航角速率 |
| SendVelNED | ( | self, | |
| vx = math.nan, | |||
| vy = math.nan, | |||
| vz = math.nan, | |||
| yawrate = math.nan ) |
发送北东地坐标系下的速度指令。
| vx | X轴速度 |
| vy | Y轴速度 |
| vz | Z轴速度 |
| yawrate | 偏航角速率 |
| setGPSOriLLA | ( | self, | |
| LonLatAlt = [40.1540302, 116.2593683, 50] ) |
设置GPS原点
| LonLatAlt | GPS的经度、纬度和高度 |
| spin | ( | self | ) |
ROS守护进程spin函数,保持节点运行
| 无 |
| stopRun | ( | self | ) |
停止仿真
| waitForStartMsg | ( | self | ) |
等待个特定的启动消息,每个无人机或节点等待接收特定的启动信号以开始仿真
| yawSat | ( | self, | |
| yaw ) |
对偏航角进行饱和处理,此函数接收一个偏航角值,并确保其在 -π/2 到 π/2 弧度(或等效的 -90 到 90 度)的范围内。如果输入的偏航角超出这个范围,函数会相应地调整它,使其不超过阈值。
| yaw | 输入的偏航角,以弧度为单位 |
| bool arm_state = False |
解锁状态
| armService |
创建解锁服务代理
| int baud = 115200 |
COM波特率。
| command = TwistStamped() |
指令
| str ComName = "COM3" |
串口名称,串口连接模式解析。
| int count = 0 |
软件在环仿真计数
| int countHil = 0 |
硬件在环仿真计数
| current_heading = None |
当前机头状态
| executor = rclpy.executors.MultiThreadedExecutor() |
创建一个多线程执行器
| f = fifo |
MAVLink初始化
| flightModeService |
创建飞行模式服务代理
| str frame = "BODY" |
坐标系名称
| geo = EarthModel.EarthModel() |
地理坐标转换的模块
| gps = None |
GPS数据。
| gps_sub |
订阅ROS中的GPS信息
| bool hasInit = False |
初始化是否完成标志位
| imu = None |
IMU数据。
| imu_sub |
订阅ROS中的IMU信息
| bool isCom = False |
UDP模式解析
| bool isInOffboard = False |
是否进入Offboard模式的标志位。
| local_pose = None |
本地位置量
| local_pose_sub |
订阅ROS中的本地位置
| local_vel = None |
本地速度量
| local_vel_sub |
订阅ROS中的本地速度
| mav0 = mavlink2.MAVLink(self.f, 255, 1) |
发送任意mavlink消息
| mav_raw_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=10) |
发布原始MAVLink消息到ROS中
| mavros_state = None |
MAVROS状态
| mavros_sub |
订阅mavros包中消息
| bool offboard_state = False |
Offboard模式状态
| offCmd = PositionTarget() |
mavros_msgs包中的目标位置信息。
| bool received_imu = False |
接收到的IMU数据
| ros_node = Node("RflyRos" + str(self.CopterID)) |
创建ROS节点
| str rosName = "mavros" |
ROS名称
| sendCmdLongService |
创建发送长格式的命令给无人机服务代理
| setparamService |
创建参数设置服务代理
| state = None |
无人机状态量
| t1 = threading.Thread(target=self.executor.spin, args=()) |
创建一个线程并开始执行
| list trueGpsUeCenter = [40.1540302, 116.2593683, 50] |
真实全球GPS位置
| list uavAngEular = [0, 0, 0] |
无人机欧拉角状态量
| list uavAngQuatern = [0, 0, 0, 0] |
无人机姿态角四元数状态量
| list uavAngRate = [0, 0, 0] |
无人机欧拉角速率状态量
| list uavGlobalPos |
从PX4转移到UE4地图的估计全球位置
| list uavPosNED = [0, 0, 0] |
无人机北东地位置状态量
| list uavVelNED = [0, 0, 0] |
无人机北东地速度状态量
| vel_pub |
发布期望速度数据到ROS中
| vel_raw_pub |
发布本地原始期望速度数据到ROS中