RflySim Platform Vision-Related Communication Protocol
1. Sensors¶
For sensors, there are two sets of coordinate frame parameters: one for intrinsic parameters and another for extrinsic parameters.
1.1 Communication Packet Header Data Parsing¶
By default, packet header parsing uses little-endian format.
| Field Name | Byte Range | Type | Description |
|---|---|---|---|
| check_sum | [0,3] | int32 | Used to classify communication data types |
| pkg_len | [4,7] | int32 | Length of one data frame, which may be split into multiple packets |
| pkg_id | [8,11] | int32 | Packet ID, used for reassembly in subsequent processing |
| pkg_num | [12,15] | int32 | Number of packets into which one frame is split, used to detect packet loss |
| frame_id | [16,19] | int32 | Frame ID, used to identify which frame the current packet belongs to |
| preserve | [20,23] | int32 | Reserved, not currently used |
| time_stamp | [24,31] | double64 | Timestamp generated within RflySim3D when the data is created |
Specific data parsing details are provided for each respective sensor type. The first 32 bytes are reserved for the header.
1.2 LiDAR¶
Accompanying LiDAR data is odometry data, which is generated synchronously with the LiDAR data and time-aligned. The header occupies 32 bytes; the remaining bytes contain sensor data. For readability, sensor data starts at byte index 0.
| Field | Byte Range | Type | Description |
|---|---|---|---|
| copter id | [0,3] | int32 | Aircraft ID to which the sensor is attached |
| axis_type | [4,7] | int32 | Coordinate frame for odometry data: 0 = local, 1 = world |
| posAng | [8,31] | float32 | Position: x, y, z; Orientation: roll, pitch, yaw (in radians) |
| Point number | [32,35] | int32 | Number of points in the point cloud |
| point1 | [36,51] | float32 | [x, y, z, seg]; each element occupies 4 bytes; seg is used for point cloud segmentation (specific scenarios only) |
| point2 | +16 bytes onward | float32 | [x, y, z, seg]; each element occupies 4 bytes; seg is used for point cloud segmentation (specific scenarios only) |
| point3 | +16 bytes onward | float32 | [x, y, z, seg]; each element occupies 4 bytes; seg is used for point cloud segmentation (specific scenarios only) |
| point4 | +16 bytes onward | float32 | [x, y, z, seg]; each element occupies 4 bytes; seg is used for point cloud segmentation (specific scenarios only) |
| ... |
1.1.2 LiDAR ROS Communication¶
- Message Type: ROS1 (
sensor_msgs/PointCloud2), ROS2 (sensor_msgs/msg/PointCloud2) - Notes: Since the point cloud format is identical across sensor types (only the sensor type differs), the above description applies uniformly.
| TypeID | Internal Reference Frame | External Reference Frame | Mount Type (affected by mount type) | Topic Name | Remarks |
|---|---|---|---|---|---|
| 20 | FLU (front, left, up) | END (east, north, down) | /rflysim/sensor0/vehicle_lidar | Mechanical scanning LiDAR, local coordinate frame; the number after "sensor" is automatically assigned based on seq_id in Config.json |
|
| 21 | END (east, north, down) | /rflysim/sensor0/golobal_lidar | Mechanical scanning LiDAR, global coordinate frame; the number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 22 | FLU (front, left, up) | END (east, north, down) | /rflysim/sensor0/livox_lidar | Livox LiDAR series; the number after "sensor" is automatically assigned based on seq_id in Config.json |
|
| 23 | FLU (front, left, up) | END (east, north, down) | /rfysim/sensor0/mid360_lidar | Mid360 LiDAR; the number after "sensor" is automatically assigned based on seq_id in Config.json |
- ROS message structure:
```c++ std_msgs/Header header uint32 seq time stamp string frame_id uint32 height uint32 width sensor_msgs/PointField[] fields # Platform point cloud has 4 dimensions: (x, y, z, seg); in addition to position information, it also includes segmentation information 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 # Point offset uint32 row_step uint8[] data # Point cloud data bool is_dense
Example:
```python 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 Depth to Point Cloud¶
This is a depth-to-point-cloud conversion performed internally within UE. Currently, this uses a 3D point cloud.
| Field | Byte Range | Type | Description |
|---|---|---|---|
| copter id | [0,3] | int32 | Aircraft ID where the current sensor is located |
| axis_type | [4,7] | int32 | Coordinate system referenced by odometry data: 0: local, 1: world |
| posAng | [8,31] | float32 | position: x, y, z; angle: roll, pitch, yaw |
| Point number | [32,35] | int32 | Number of points |
| point1 | [36,47] | float32 | [x, y, z], each element occupies 3 bytes |
| point2 | Offset by 16 bytes from previous | float32 | [x, y, z], each element occupies 3 bytes |
| point3 | Offset by 16 bytes from previous | float32 | [x, y, z], each element occupies 3 bytes |
| point4 | Offset by 16 bytes from previous | float32 | [x, y, z], each element occupies 3 bytes |
| ... |
Message type: sensor_msgs/PointCloud2
Topic name: /rflysim/sensor0/Depth_Cloud
1.2 Visual Sensors¶
For visual sensor message parsing, it is straightforward: encoding is performed directly based on sensor type from a uint8 array. Therefore, only the encoding formats for different sensor types are listed here.
| Type ID | Sensor Type | Encoding Format | Description |
|---|---|---|---|
| 1 | Visible Camera | BGR8 | |
| 2 | Depth Camera | mono16 | |
| 3 | Grayscale Image | mono8 | |
| 4 | Segmentation Map | BGR8 | Requires special configuration and is only usable in specific scenarios |
| 8 | Fisheye Camera | BGR8 | |
| 9 | Gimballed Camera | BGR8 | Gimballed cameras are special; this format is for visualization only. Control interfaces are covered in later sections |
| 40 | Infrared Grayscale | mono8 | Requires special configuration and is only usable in specific scenarios |
| 41 | Infrared Color | BGR8 | Requires special configuration and is only usable in specific scenarios |
- Message type: ROS1 (
sensor_msgs/Image), ROS2 (sensor_msgs/msg/Image) - Detailed description
| Sensor Type | Internal Frame | External Frame | Impact of Mount Type | Topic Name | Notes |
|---|---|---|---|---|---|
| 1 | END(east, north, down) | /rfysim/sensor0/img_rgb | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 2 | END(east, north, down) | /rfysim/sensor0/img_depth | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 3 | END(east, north, down) | /rfysim/sensor0/img_gray | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 4 | END(east, north, down) | /rfysim/sensor0/img_Segementation | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 8 | END(east, north, down) | /rfysim/sensor0/fisheye | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 9 | END(east, north, down) | /rfysim/sensor0/img_cine | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 40 | END(east, north, down) | /rfysim/sensor0/img_Infrared_Gray | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
||
| 41 | END(east, north, down) | /rfysim/sensor0/img_Infrared | The suffix number after "sensor" is automatically assigned based on seq_id in Config.json |
- ROS message structure:
```C++ std_msgs/Header header uint32 seq time stamp string frame_id uint32 height # image height uint32 width # image width string encoding # image encoding format uint8 is_bigendian # data endianness uint32 step uint8[] data # image data
```
Example:
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 Laser Rangefinder¶
Like other sensors, it requires a frame header. Below are the data contents:
| Field | Bytes | Data Type | Description |
|---|---|---|---|
| Distance | [0,3] | float32 | Distance to the laser hit point |
| copterid | [4,7] | int32 | Carrier aircraft ID |
| RayStart | [8,19] | float32 | Coordinates of the ray origin |
| AngEular | [20,31] | float32 | Euler angles of sensor installation [roll, pitch, yaw] |
| ImpactPoint | [32,43] | float32 | Coordinates of the hit point |
| Boxori | [44,55] | float32 | Center point of the box at the hit target |
The laser rangefinder is not used independently; it is typically used in conjunction with a gimbal. In such cases, it must be bound to another sensor.
Message type: std_msgs/Float32
Topic name: /rflysim/sensor0/distance
1.4 Odometry¶
Odometry data is published together with LiDAR data for time synchronization. Although odometry data can also be obtained via MAVROS, its timestamp does not align with that of the LiDAR point cloud. Odometry data has two coordinate frames, and the data provided by RflySim3D includes copter_id.
-
Message type: ROS1 (
nav_msgs/Odometry), ROS2 (nav_msgs/msg/Odometry)
| Coordinate Frame | Topic Name | Note |
|---|---|---|
| local | /rflysim/uav1/local/odom |
Single-vehicle mode |
| global | /rflysim/uav1/global/odom |
Multi-vehicle mode |
- ROS1 message structure:
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 # Position data
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation # Orientation data
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear # Velocity
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular # Angular velocity
float64 x
float64 y
float64 z
float64[36] covariance
1.4 IMU¶
Raw IMU data from CopterSim uses the FRD (Front-Right-Down) coordinate frame, whereas ROS typically uses the FLU (Front-Left-Up) frame. Therefore, a conversion is performed in the interface. The code is as follows:
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]
* Message Type: `rosmsg show sensor_msgs/Imu`
* ROS1 Message Structure:
```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
```
For multiple IMUs, namespaces must be used for differentiation, e.g., `/uav1/rflysim/imu`.
# 2. Gimbals
Gimbals are somewhat special and require a dedicated section for operation. First, the gimbal ROS interface does not have a universal message structure—all message types must be defined by ourselves. Second, gimbals represent the integration of multiple sensors and are divided into creation, request (control), and status reporting phases.
## 2.1 Gimbal Control
Control primarily targets the gimbal base’s pitch, yaw, pitch_rate, and yaw_rate, as well as centering. The platform sends requests via port `(20010 + windID)`. The request structure is as follows:
```C++
# struct VisionSensorReq {
uint16 checksum; // Data checksum, value: 12345
uint16 SeqID; // Sequence ID
uint32 bitmask; // Control bitmask
uint16 TypeID; // Sensor type ID
uint16 TargetCopter; // Bound target aircraft // mutable
uint16 TargetMountType; // Mount type // mutable
uint16 DataWidth; // Data or image width
uint16 DataHeight; // Data or image height
uint16 DataCheckFreq; // Data update check frequency
uint16 SendProtocol[8]; // Transmission type (shared memory, uncompressed UDP, UDP video streaming), IP address, port number, etc.
float CameraFOV; // Camera field of view (visual sensors only) // mutable
float SensorPosXYZ[3]; // Sensor mounting position // mutable
float EularOrQuat; // Euler angle or quaternion mode (>0.5 indicates quaternion)
float SensorAngEular[3]; // Sensor mounting angles (Euler) // mutable
float SensorAngQuat[4]; // Sensor mounting quaternion // mutable
float otherParams[16]; // Reserved 16 float fields
# }2H1I14H28f
Unlike other sensor requests, the gimbal includes an additional bitmask—a control mask used to manage specific states of the gimbal. Except for AI-related functions, all gimbal state inputs are received through this structure.
| bit mask | otherParams value index | Value range | Description |
|---|---|---|---|
| 1 << 4 | otherParams[0] | Set focal length | |
| 1 << 5 | otherParams[1] | 0, 1 | Centering |
| 1 << 6 | otherParams[2] | Custom constraints | Pitch angular velocity control |
| 1 << 6 | otherParams[3] | Unit: rad/s | Yaw angular velocity control |
| 1 << 7 | otherParams[4] | 0, 1, 2, 3, ... | Optical zoom, relative to zoom base |
| 1 << 13 | otherParams[5] | (4, 1000) | Zoom base |
| 1 << 8 | otherParams[5] | copter ID | Target ID for tracking |
| 1 << 9 | otherParams[6] | 0, 1 | Enable distance measurement |
| 1 << 10 | otherParams[7] | 0: RGB; 1: Thermal heatmap; 2: Infrared grayscale | Control light source output of gimbal |
| 1 << 11 | otherParams[8] | 1: Box-select all copter IDs; 2: Box-select target at mouse-click pixel coordinates (pixel coordinates stored in otherParams[9] and otherParams[10]) | AI function: box-select target |
In addition to controlling the gimbal via the bitmask and otherParams combination, gimbal angles can also be set directly by modifying the SensorAngQuat or SensorAngEular fields within the structure—consistent with other sensors.
2.2 Gimbal Data Acquisition¶
Currently, gimbal control and state acquisition are performed via VisionCaptureAPI.py and UE4CtrlAPI.py, respectively: control is handled by VisionCaptureAPI, while state reception uses UE4CtrlAPI.py.
2.2.1 Gimbal Image Acquisition¶
Gimbal image acquisition follows the same procedure as other sensors; refer to the Sensors section for details.
2.2.2 Gimbal State Acquisition¶
State data is obtained via port 20006:
- Attitude: read
SensorAngQuatorSensorAngEular; - Current focal length:
otherParams[0]; - Field of view (FOV):
CameraFOV; - Angular velocities:
otherParams[1],otherParams[2]; - Focal lengths in pixels (fx, fy):
otherParams[3],otherParams[4]; - Laser rangefinder distance value:
otherParams[7]; - Pixel width and height:
otherParams[5],otherParams[6].
2.3 Gimbal ROS Topic Communication Protocol¶
2.3.1 Gimbal Intrinsic Parameters Control Protocol¶
Message type: CameraCtrl.msg
std_msgs/Header header
# Control type selection:
# 0x1: resolution control
# 0x2: FOV control
# 0x4: attitude control
# 0x8: optical zoom control
# Other controls: use bitwise OR for multiple selections
uint8 ctrl_type
uint32 height
uint32 width
uint32 fov # unit: deg * 1e2
int32 roll
int32 pitch
int32 yaw
# Optical zoom:
# int32_max: double zoom-in
# int32_min: double zoom-out
# Intermediate values: control commands
int32 optics_multiply
uint32 base_optics # optical zoom base
# Additional special controls:
# others[0]: centering command: 0 = no action, 1 = enable
# others[1]: yaw direction control: -1 = left, 0 = no action, 1 = right
# others[2]: yaw angular velocity control (deg/s * 1e2)
# others[3]: pitch direction control: -1 = down, 0 = no action, 1 = up
# others[4]: pitch angular velocity control (deg/s * 1e2)
int32[] others
Topic: /rflysim/sensor0/camera/ctrl
2.3.2 Gimbal AI Control Protocol¶
Message type: CameraAICtrl.msg
std_msgs/Header header
uint8 ctrl_id # Rotation control type:
# 0: default box-select all targets
# 1: box-select target at pixel coordinates provided below
uint32[] pixel_pos # Pixel coordinate values for target selection
Topic name: /rflsyim/sensor0/camera/ai_ctrl
### 2.3.3 Gimbals Status Reception Protocol
Message type: CameraStatus.msg
```c++
std_msgs/Header header
uint8 id # Device 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: Ground-facing; 1: Relative to gimbal; 2: Relative to aircraft
int32 roll
int32 pitch
int32 yaw
int32 optics_multiply
int32[] others
CameraParams camera_params
Gimbal intrinsic parameters:
Message type: CameraParams.msg
# Intrinsics parameters
float32 fx
float32 fy
float32 cx
float32 cy
float32 k1
float32 k2
float32 p1
float32 p2
float32 k3
# Extrinsics parameters
uint8 att_type_valid_mask
uint8 PLANE_TO_CAM_VALID = 1 # Plane frame rotation to camera frame
uint8 NED_TO_CAM_VALID = 2 # NED frame rotation to camera frame
uint8 LEVEL_PLANE_TO_CAM_VALID = 4
float32 p2c_roll_deg # Rotation from plane frame to camera frame (321 Euler angles).
float32 p2c_pitch_deg # Rotation from plane frame to camera frame (321 Euler angles).
float32 p2c_yaw_deg # Rotation from plane frame to camera frame (321 Euler angles).
float32 ned2c_roll_deg # Rotation from NED frame to camera frame (321 Euler angles).
float32 ned2c_pitch_deg # Rotation from NED frame to camera frame (321 Euler angles).
float32 ned2c_yaw_deg # Rotation from NED frame to camera frame (321 Euler angles).
float32 level2c_roll_deg # Rotation from level plane frame to camera frame (321 Euler angles).
float32 level2c_pitch_deg # Rotation from level plane frame to camera frame (321 Euler angles).
float32 level2c_yaw_deg # Rotation from level plane frame to camera frame (321 Euler angles).
float32 p2c_front_m # Displacement from aircraft center to camera center. X: forward, Y: right, Z: down (aircraft frame).
float32 p2c_right_m # Displacement from aircraft center to camera center. X: forward, Y: right, Z: down (aircraft frame).
float32 p2c_down_m # Displacement from aircraft center to camera center. X: forward, Y: right, Z: down (aircraft frame).
Topic name: /rflysim/sensor0/camera/status
Not all camera intrinsic parameters need to be assigned; only provide those that are available.
3. RflySim and MavROS Communication¶
3.1 Single Aircraft Scenario¶
MavROS is launched by indirectly invoking Linux processes. In the single-aircraft scenario, namespace issues do not need to be considered.
For example:
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 Multi-Aircraft Scenario¶
In the multi-aircraft scenario, namespaces must be added to distinguish connections for different aircraft, using /mavros${copter_id}/.
Examples:
- Node name: /mavros1
- Topic name: /mavros1/state, etc.