|
| | __init__ (self, CopterID=1, ip='127.0.0.1', Com='udp', port=0) |
| | RflyRosCtrlApi的构造函数
|
| | convert_to_payload64 (self, payload_bytes) |
| | 将payload字节转换为Mavlink的payload64格式
|
| | convert_to_rosmsg (self, mavmsg, header) |
| | 将pymavlink消息转换为ROS的Mavlink消息
|
| | arm_px4 (self, isArm) |
| | 解锁或锁定PX4无人机
|
| | fillList (self, data, inLen, fill=0) |
| | 填充或截断列表，使其长度达到指定值
|
| | InitMavLoop (self) |
| | 初始化Mavlink连接，并启动Mavros
|
| | SendMavArm (self, isArm) |
| | 发送解锁或锁定命令到无人机
|
| | initOffboard (self) |
| | 初始化Offboard模式并发送初始命令
|
| | OffboardLoop (self) |
| | 持续发送Offboard命令以维持Offboard模式
|
| | endOffboard (self) |
| | 结束Offboard模式，停止相关线程，并关闭ROS2节点
|
| | stopRun (self) |
| | 停止运行，杀死子进程并提醒用户关闭所有终端窗口
|
| | calcTypeMask (self, EnList) |
| | 根据输入的启用列表计算Type Mask
|
| | SendVelNED (self, vx=math.nan, vy=math.nan, vz=math.nan, yawrate=math.nan) |
| | 发送NED（北、东、下）坐标系下的速度命令
|
| | SendVelFRD (self, vx=math.nan, vy=math.nan, vz=math.nan, yawrate=math.nan) |
| | 发送FRD（前、右、下）坐标系下的速度命令
|
| | SendPosNED (self, x=math.nan, y=math.nan, z=math.nan, yaw=math.nan) |
| | 发送NED坐标系下的位置命令
|
| | SendPosVelNED (self, PosE=[math.nan] *3, VelE=[math.nan] *3, yaw=math.nan, yawrate=math.nan) |
| | 发送NED坐标系下的位置和速度命令
|
| | local_pose_callback (self, msg) |
| | 处理本地位姿消息
|
| | local_vel_callback (self, msg) |
| | 处理本地速度消息
|
| | mavros_state_callback (self, msg) |
| | 处理Mavros状态消息
|
| | imu_callback (self, msg) |
| | 处理IMU消息
|
| | gps_callback (self, msg) |
| | 处理GPS消息
|
| | q2yaw (self, q) |
| | 从四元数计算偏航角
|
| | q2Euler (self, q) |
| | 从四元数计算欧拉角
|
| | SendMavArm (self, isArm=0) |
| | 发送解锁或锁定命令到无人机
|
| | arm (self) |
| | 解锁无人机
|
| | disarm (self) |
| | 锁定无人机
|
| | offboard (self) |
| | 切换飞行模式到Offboard模式
|
| | yawSat (self, yaw) |
| | 对偏航角进行限制，使其保持在[-π/2, π/2]范围内
|
| | sendMavSetParam (self, param_id_str, param_value, param_type) |
| | 设置MAVLink参数
|
| | SendMavCmdLong (self, command, param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, param7=0) |
| | 发送长命令到飞行控制器
|
| | SendHILCtrlMsg (self, ctrls=[0] *16, idx=0) |
| | 发送HIL（硬件在环）控制消息到Pixhawk，这些消息会转化为uORB消息rfly_ctrl
|
|
| bool | isCom = False |
| | 用于标识当前通信模式是否为串口模式
|
| | Com = Com |
| | 存储传入的通信模式参数
|
| int | baud = 115200 |
| | 波特率，默认设置为115200
|
| | ip = ip |
| | 存储数据发送的IP地址
|
| int | CopterID = CopterID |
| | 无人机的ID号，用于区分不同的无人机
|
| int | port = 20100+self.CopterID*2-2 |
| | 端口号，默认根据CopterID计算而得
|
|
str | rosName = "mavros" |
|
str | ComName = 'COM3' |
| | imu = None |
| | 存储无人机的IMU数据
|
| | gps = None |
| | 存储无人机的GPS数据
|
| | local_pose = None |
| | 存储无人机的本地位姿数据
|
| | mavros_state = None |
| | 存储无人机的当前状态信息
|
| | 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() |
| | 存储位置控制命令
|
| bool | isInOffboard = False |
| | 表示是否处于Offboard模式中
|
| list | uavAngEular = [0, 0, 0] |
| | 存储无人机的欧拉角信息
|
| list | uavAngRate = [0, 0, 0] |
| | 存储无人机的角速度信息
|
| list | uavPosNED = [0, 0, 0] |
| | 存储无人机的本地NED坐标系位置信息
|
| list | uavVelNED = [0, 0, 0] |
| | 存储无人机的本地NED坐标系速度信息
|
| list | uavAngQuatern = [0,0,0,0] |
| | 存储无人机的四元数姿态信息
|
| int | count = 0 |
| | 计数器，初始化为0
|
| int | countHil = 0 |
| | HIL（硬件在环）模式的计数器，初始化为0
|
|
| local_pose_sub = rospy.Subscriber("/"+self.rosName+"/local_position/pose", PoseStamped, self.local_pose_callback) |
|
| local_vel_sub = rospy.Subscriber("/"+self.rosName+"/local_position/velocity", TwistStamped, self.local_vel_callback) |
|
| mavros_sub = rospy.Subscriber("/"+self.rosName+"/state", State, self.mavros_state_callback) |
|
| gps_sub = rospy.Subscriber("/"+self.rosName+"/global_position/global", NavSatFix, self.gps_callback) |
|
| imu_sub = rospy.Subscriber("/"+self.rosName+"/imu/data", Imu, self.imu_callback) |
|
| vel_pub = rospy.Publisher("/"+self.rosName+"/setpoint_velocity/cmd_vel", TwistStamped,queue_size=10) |
|
| vel_raw_pub = rospy.Publisher("/"+self.rosName+"/setpoint_raw/local", PositionTarget, queue_size=10) |
|
| mav_raw_pub = rospy.Publisher("/mavlink/to",Mavlink, queue_size=10) |
|
| armService = rospy.ServiceProxy("/"+self.rosName+"/cmd/arming", CommandBool) |
|
| flightModeService = rospy.ServiceProxy("/"+self.rosName+"/set_mode", SetMode) |
|
| setparamService = rospy.ServiceProxy("/"+self.rosName+"/param/set", ParamSet) |
|
| sendCmdLongService = rospy.ServiceProxy("/"+self.rosName+"/cmd/command", CommandLong) |
|
| ros_node = Node("RflyRos"+str(self.CopterID)) |
|
| executor = rclpy.executors.MultiThreadedExecutor() |
|
| t1 = threading.Thread(target=self.executor.spin, args=()) |
|
| f = fifo |
|
| mav0 = mavlink2.MAVLink(self.f,255,1) |
|
| tgtSys = the_connection.target_system |
| | child |
|
| t2 = threading.Thread(target=self.OffboardLoop, args=()) |
RflyRosCtrlApi结构体类。提供了与无人机进行通信和控制的完整接口，支持多种通信模式，并集成了ROS系统
创建一个通信实例
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) # 指定串口，并设置波特率
1.14.0