RflySimSDK v4.11
RflySimSDK说明文档
载入中...
搜索中...
未找到
VisionComm

RflySim 平台视觉相关通信协议

1. 传感器

对于传感器有两个坐标系参数，一个内参，另一个是外参

1.1 通信报文帧头数据解析

报文帧头解析默认都是用小端格式

字段名 所占字节 类型 说明
check_sum [0,3] int32 用来区分通信数据分类
pkg_len [4,7] int32 一帧数据，可能会被拆分成多个包
pkg_id [8,11] int32 获取包的编号，后续要做拼接
pkg_num [12,15] int32 一帧被拆分成多少个包，可以用来判断是否丢包
frame_id [16,19] int32 数据帧的编号，同来判断当前包数据哪一帧的数据
preserve [20,23] int32 暂不使用
time_stamp [24,31] double64 在RflySim3D里面生成数据的时间戳

具体的数据解析在对应不同的 传感器里面提，前32个字节是给帧头的

1.2. 激光雷达

与激光雷达一同传递的还有里程计数据，这个里程计数据是和激光雷达数据一同生成的，做了时间对齐的。帧占用32字节，剩下的全是传感器数据，为了方便阅读，这里传感器数据从字节0开始。

字段 所占字节 类型 说明
copter id [0,3] int32 当前传感器所在飞机的编号
axis_type [4,7] int32 里程计数据参考的坐标系，0：local, 1: world
posAng [8,31] float32 position: x,y,z , Angel: roll, pitch, yaw
Point number [32,35] int32 点的数量
point1 [36,51] float32 [x,y,z,seg]，每个元素占四个字节，最后一个seg 用来做点云分割用（限特定的场景）
point2 按照16个字节往后推 float32 [x,y,z,seg]，每个元素占四个字节，最后一个seg 用来做点云分割用（限特定的场景）
point3 按照16个字节往后推 float32 [x,y,z,seg]，每个元素占四个字节，最后一个seg 用来做点云分割用（限特定的场景）
point4 按照16个字节往后推 float32 [x,y,z,seg]，每个元素占四个字节，最后一个seg 用来做点云分割用（限特定的场景）
...

1.1.2 激光雷达 ROS 通信

  • 消息类型： ROS1(sensor_msgs/PointCloud2), ROS2(sensor_msgs/msg/PointCloud2)
  • 具体说明，因为除了传感器类型不一样，点云的格式都是一样的
TypeID 内参参考系 外参参考系 挂载方式（mount type影响） 话题名 备注
20 FLU(front, left, up) END(east，north,down) /rflysim/sensor0/vehicle_lidar 机械式扫描激光雷达，局部坐标系，,sensor 后面的编号自动根据Config.json的seq_id来
21 END(east，north,down) /rflysim/sensor0/golobal_lidar 机械式扫描激光雷达，全局坐标系，sensor 后面的编号自动根据Config.json的seq_id来
22 FLU(front, left, up) END(east，north,down) /rflysim/sensor0/livox_lidar livox 雷达系列，sensor 后面的编号自动根据Config.json的seq_id来
23 FLU(front, left, up) END(east，north,down) /rfysim/sensor0/mid360_lidar mid360激光雷达，sensor 后面的编号自动根据Config.json的seq_id来

  • ROS消息结构：

    std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
    uint32 height
    uint32 width
    sensor_msgs/PointField[] fields #平台点云有4个维度，（x,y,z,seg）,除了位置信息，还包含分割信息
    uint8 INT8=1
    uint8 UINT8=2
    uint8 INT16=3
    uint8 UINT16=4
    uint8 INT32=5
    uint8 UINT32=6
    uint8 FLOAT32=7
    uint8 FLOAT64=8
    string name
    uint32 offset
    uint8 datatype
    uint32 count
    bool is_bigendian
    uint32 point_step #点的偏移量
    uint32 row_step
    uint8[] data #点云数据
    bool is_dense

    例子：

    msg = sensor.PointCloud2()
    msg.height = 1
    msg.width = self.Img[idx].shape[0]
    msg.fields = [
    sensor.PointField("x", 0, sensor.PointField.FLOAT32, 1),
    sensor.PointField("y", 4, sensor.PointField.FLOAT32, 1),
    sensor.PointField("z", 8, sensor.PointField.FLOAT32, 1),
    sensor.PointField("seg", 12, sensor.PointField.FLOAT32, 1),
    ]
    msg.is_bigendian = False
    msg.point_step = 16 # --Lidar
    msg.row_step = msg.point_step * self.Img[idx].shape[0]
    msg.is_dense = False
    msg.data = np.asarray(self.Img[idx], np.float32).tostring()

1.1.3 深度转点云

这是在UE内部进行的深度转点云操作，目前这个使用的是一个3D点云，

字段 所占字节 类型 说明
copter id [0,3] int32 当前传感器所在飞机的编号
axis_type [4,7] int32 里程计数据参考的坐标系，0：local, 1: world
posAng [8,31] float32 position: x,y,z , Angel: roll, pitch, yaw
Point number [32,35] int32 点的数量
point1 [36,47] float32 [x,y,z]，每个元素占3个字节
point2 按照16个字节往后推 float32 [x,y,z]，每个元素占3个字节
point3 按照16个字节往后推 float32 [x,y,z]，每个元素占3个字节
point4 按照16个字节往后推 float32 [x,y,z]，每个元素占3个字节
...

消息类型：sensor_msgs/PointCloud2

话题名：/rflysim/sensor0/Depth_Cloud

1.2 视觉传感器

对于视觉传感器的报文解析比较简单，直接从一个uint8的数组里面按照传感器类型进行编码就行。因此这里只提不同传感器对应编码格式

type id 传感器类型 编码格式 说明
1 可见光相机 BGR8
2 深度相机 mono16
3 灰度图 mono8
4 分割图 BRG8 需要做特出的设置，特定的场景才能使用
8 鱼眼相机 BRG8
9 吊舱 BGR8 吊舱有些特殊，这里是只是可视化，还涉及到很多控制接口，在后面篇章里面提
40 红外灰度 mono8 需要做特出的设置，特定的场景才能使用
41 红外彩色 BGR8 需要做特出的设置，特定的场景才能使用
  • 消息类型 ROS1(sensor_msgs/Image), ROS2(sensor_msgs/msg/Image)
  • 具体说明
传感器类型 内参系 外参系 挂在方式，mount type的影响 话题名 备注
1 END(east，north,down) /rfysim/sensor0/img_rgb sensor 后面的编号自动根据Config.json的seq_id来
2 END(east，north,down) /rfysim/sensor0/img_depth sensor 后面的编号自动根据Config.json的seq_id来
3 END(east，north,down) /rfysim/sensor0/img_gray sensor 后面的编号自动根据Config.json的seq_id来
4 END(east，north,down) /rfysim/sensor0/img_Segementation sensor 后面的编号自动根据Config.json的seq_id来
8 END(east，north,down) /rfysim/sensor0/fisheye sensor 后面的编号自动根据Config.json的seq_id来
9 END(east，north,down) /rfysim/sensor0/img_cine sensor 后面的编号自动根据Config.json的seq_id来
40 END(east，north,down) /rfysim/sensor0/img_Infrared_Gray sensor 后面的编号自动根据Config.json的seq_id来
41 END(east，north,down) /rfysim/sensor0/img_Infrared sensor 后面的编号自动根据Config.json的seq_id来

  • ROS消息结构：

    C++
    std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
    uint32 height #图像的高
    uint32 width # 图像的宽
    string encoding #图像的编码格式
    uint8 is_bigendian #数据大小端
    uint32 step
    uint8[] data # 图像数据

    例子：

    msg.height = self.Img[idx].shape[0]
    msg.width = self.Img[idx].shape[1]
    msg.encoding = encoding_
    msg.data = self.Img[idx].tostring()
    msg.step = msg.width * byte_num

1.3 激光测距仪

和其他传感器一样需要一个帧头，下面是数据内容

字段 所占字节 数据类型 说明
Distance [0,3] float32 激光hit点距离
copterid [4,7] int32 返回把载体飞机id
RayStart [8,19] float32 射线起点的坐标，
AngEular [20,31] float32 传感器安装的欧拉角[roll,pitch,yaw]
ImpactPoint [32,43] float32 hit点的坐标
Boxori [44,55] flaot32 hit到目标的box中心点

激光测距不单独使用，一般结合吊舱使用，像这种情况就是要绑定到别的传感器上去。

消息类型：std_msgs/Float32

话题名： /rflysim/sensor0/distance

1.4 里程计

里程计数据数据随激光雷达发布，为了数据的时间同步，虽然里程计数据可以通过mavros 那边获得，但是时间戳上与激光雷达点云没有对其操作，里程计数据有两个系，RflySim3D 给出的数据里面包含copter_id,

  • 消息类型： ROS1(nav_msgs/Odometry), ROS2(nav_msgs/msg/Odometry)
坐标系 话题名 备注
local /rflysim/uav1/local/odom 一般情况单机使用
global /rflysim/uav1/global/odom 多机情况下使用

  • ROS1消息数据：

    std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
    string child_frame_id
    geometry_msgs/PoseWithCovariance pose
    geometry_msgs/Pose pose
    geometry_msgs/Point position # 位置数据
    float64 x
    float64 y
    float64 z
    geometry_msgs/Quaternion orientation #姿态数据
    float64 x
    float64 y
    float64 z
    float64 w
    float64[36] covariance
    geometry_msgs/TwistWithCovariance twist
    geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear # 速度
    float64 x
    float64 y
    float64 z
    geometry_msgs/Vector3 angular #角速度
    float64 x
    float64 y
    float64 z
    float64[36] covariance

1.4 IMU

从CopterSim 获取到原始数据集数据参考坐标系FRD ，而通常在ROS这边使用的是FLU,因此在接口里面做了一个转换。代码如下：

self.ros_imu.linear_acceleration.x = self.acc[0]
self.ros_imu.linear_acceleration.y = -self.acc[1]
self.ros_imu.linear_acceleration.z = -self.acc[2]
self.ros_imu.angular_velocity.x = self.rate[0]
self.ros_imu.angular_velocity.y = -self.rate[1]
self.ros_imu.angular_velocity.z = -self.rate[2]
  • 消息类型: rosmsg show sensor_msgs/Imu

  • ROS1消息结构:

    yaml
    std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
    geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
    float64[9] orientation_covariance
    geometry_msgs/Vector3 angular_velocity
    float64 x
    float64 y
    float64 z
    float64[9] angular_velocity_covariance
    geometry_msgs/Vector3 linear_acceleration
    float64 x
    float64 y
    float64 z
    float64[9] linear_acceleration_covariance

    多个IMU的情况，需要通过命名空间来区,如：/uav1/rflysim/imu

2. 吊舱

吊舱有点特殊，需要单独用一个篇章来操作，首先吊舱ROS接口没有通用的消息结构，所有的需要我们自己定义，其次吊舱是多个传感器组合的结果，吊舱分为创建，请求（控制），状态返回.

2.1 吊舱控制

主要控制吊舱底座的pitch，yaw, pitch_rate, yaw_rate, 回中，平台通过端口（20010 + windID）发送请求，请求的结构体如下

# struct VisionSensorReq {
uint16 checksum; //数据校验位，12345
uint16 SeqID; //内存序号ID
uint32 bitmask; // 控制
uint16 TypeID; //传感器类型ID
uint16 TargetCopter; //绑定的目标飞机 //可改变
uint16 TargetMountType; //绑定的类型 //可改变
uint16 DataWidth; //数据或图像宽度
uint16 DataHeight; //数据或图像高度
uint16 DataCheckFreq; //检查数据更新频率
uint16 SendProtocol[8]; //传输类型（共享内存、UDP传输无压缩、UDP视频串流），IP地址，端口号，...
float CameraFOV; //相机视场角（仅限视觉类传感器） //可改变
float SensorPosXYZ[3]; // 传感器安装位置 //可改变
float EularOrQuat; //选择欧拉角或四元数方式，大于0.5就是四元数
float SensorAngEular[3]; //传感器安装角度 //可改变
float SensorAngQuat[4]; //传感器安装四元数 //可改变
float otherParams[16]; //预留的16位数据位
# }2H1I14H28f

与其他传感器请求的不同，吊舱多了 一个bitmask,这是一个控制吊舱某个状态的控制掩码。吊舱的状态接受（除AI功能外）也都是通过这个结构体接受的。

bit mask otherParams对应的值 值范围 说明
1 << 4 otherParams[0]. 设置焦距
1 << 5 otherParams[1]. 0,1 回中
1 << 6 otherParams[2]. 自定义约束条件 俯仰角控制角速度
1 << 6 otherParams[3]. 单位：弧度/秒 偏航角控制角速度
1 << 7 otherParams[4]. 0，1，2，3.. 光学变倍，基于变倍基数
1 << 13 otherParams[5]. (4,1000) 变倍基数
1 << 8 otherParams[5]. copter id 跟踪目标的ID
1 << 9 otherParams[6]. 0,1 开启测距
1 << 10 otherParams[7] 0：RGB ；1：热力图；2：红外灰度 控制吊舱输出的光源
1 << 11 otherParams[8] 1: 框选所有copterid 对象，2：框选鼠标点击像素点目标，像素点坐标分别放在otherParams[9]和 otherParams[10]里面 Ai功能框选目标

除了bit mask 与 otherParams 组合控制吊舱外，还可以直接设置吊舱的角度，这个只要改结构体内的SensorAngQuat或SensorAngEular 设置，这和其他传感器一样的

2.2 吊舱数据获取

吊舱的控制与状态获取，目前是通过VisionCaptrueAPI.py 和 UE4CtrlAPI.py 来完成，控制使用VisionCaptrueAPI, 状态接受使用UE4CtrlAPI.py

2.2.1 吊舱图像获取

吊舱的图像获取与其他的传感器一样，详细参考传感器篇章

2.2.2 吊舱状态获取

状态获取通过端口20006获取的，

  • 姿态获取，读取结构体的SensorAngQuat 或者SensorAngEular ；
  • 获取当前焦距，otherParams[0];
  • 获取视场角, CameraFOV；
  • 获取角速度，otherParams[1,2]
  • 获取fx,fy otherParams[3,4]
  • 获取激光测距距离值：otherParams[7]
  • 获取像素的宽，高：otherParams[5,6]

2.3 吊舱ROS话题通信协议

2.3.1 吊舱内参控制协议

消息类型： CameraCtrl.msg

std_msgs/Header header
#控制类型选择，如果要控制分辨率：0x1 ，控制视场角：0x2; 控制姿态： 0x4; 控制光学变倍； 0x8; 其他控制；多选控制选时，使用值或运算
uint8 ctrl_type
uint32 height
uint32 width
uint32 fov #单位为deg*1e2
int32 roll
int32 pitch
int32 yaw
#光学变倍，int32_max:表示增加一倍； int32_min 表示减小一倍； 中间一直表示控制量
int32 optics_multiply
uint32 base_optics #光学变倍的基数
#其他特有的控制,
#对于others[0]:表示回中：0不作用，1：使能； others[1] yaw角方向控制：-1：左，0：不作用，1：右； others[2]:表示yaw角控制速度（deg/s * 1e2）
#others[3]:表示pitch角度控制方，-1：下，0：不作用，1：上 ; others[4]：表示pitch角控制速度（deg/s * 1e2）
int32[] others

话题：/rflysim/sensor0/camera/ctrl

2.3.2 吊舱AI控制协议

消息类型：CameraAICtrl.msg

std_msgs/Header header
uint8 ctrl_id #旋转控制类型，0： 默认框选所有目标 1: 则需要获取目标所在像素的坐标，通过下面的参数传递
uint32[] pixel_pos #c传递像素坐标值

话题名：/rflsyim/sensor0/camera/ai_ctrl

2.3.3 吊舱状态接受协议

消息类型: CaramerStatus.msg

std_msgs/Header header
uint8 id #设备id
float32 frame_rate
float32 capture_max_sec
float32 capture_min_sec
float32 capture_cur_sec
uint32 height
uint32 width
uint32 fov
uint8 type #0:对地； 1：相对云台 2；相对于飞机
int32 roll
int32 pitch
int32 yaw
int32 optics_multiply
int32[] others
CameraParams camera_params

吊舱内参：

消息类型：CameraParams.msg

#intrinsics params
float32 fx
float32 fy
float32 cx
float32 cy
float32 k1
float32 k2
float32 p1
float32 p2
float32 k3
#extrinsics params
uint8 att_type_valid_mask
uint8 PLANE_TO_CAM_VALID = 1 # Plane frame rotate to camera frame
uint8 NED_TO_CAM_VALID = 2 # NED frame rotate to camera frame
uint8 LEVEL_PLANE_TO_CAM_VALID = 4
float32 p2c_roll_deg #Plane frame rotate to camera frame(321).
float32 p2c_pitch_deg #Plane frame rotate to camera frame(321).
float32 p2c_yaw_deg #Plane frame rotate to camera frame(321).
float32 ned2c_roll_deg #Ned frame rotate to camera frame(321).
float32 ned2c_pitch_deg #Ned frame rotate to camera frame(321).
float32 ned2c_yaw_deg #Ned frame rotate to camera frame(321).
float32 level2c_roll_deg #level plane frame rotate to camera frame(321).
float32 level2c_pitch_deg #level plane frame rotate to camera frame(321).
float32 level2c_yaw_deg #level plane frame rotate to camera frame(321).
float32 p2c_front_m #Plane center to camera center. X is plane front. Y is plane right. Z is plane down.
float32 p2c_right_m #Plane center to camera center. X is plane front. Y is plane right. Z is plane down.
float32 p2c_down_m #Plane center to camera center. X is plane front. Y is plane right. Z is plane down.

话题名： /rflysim/sensor0/camera/status

相机的内参不需要每个都赋值，有什么就给什么就行

3. RflySim 与 MavROS通信

3.1 单机情况

运行MavROS都是通过间接调用linux下的进程来启动mavros 程序，单机情况不需要考虑命名空间的问题

例如：

if is_use_ros1:
cmdStr = 'roslaunch mavros px4.launch tgt_system:='+str(self.tgtSys)+' fcu_url:="udp://:'+str(int(self.port)+1)+'@'+self.ip+':'+str(self.port)+'"'
else:
cmdStr = 'ros2 launch mavros px4.launch.xml tgt_system:='+str(self.tgtSys)+' fcu_url:="udp://:'+str(int(self.port)+1)+'@'+self.ip+':'+str(self.port)+'"'
self.child = subprocess.Popen(cmdStr,shell=True)

3.2 多机情况

多机的情况需要加上命名空间，以/mavros${copter_id}/作为区分不同飞机的连接

如节点名: /mavros1;

又如话题名： /mavros1/state ......

制作者 doxygen 1.15.0