Merge pull request #334 from tier4/feature/rgl-autoware-point-cloud-f…

…ormat Add PointXYZIRCEADT point cloud format (used by Autoware)
tier4 · Jul 23, 2024 · 94ba530 · 94ba530
2 parents 17d7539 + 08c10c4
commit 94ba530
Show file tree

Hide file tree

Showing 17 changed files with 54 additions and 37 deletions.
diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiAT128E2X.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiAT128E2X.prefab
@@ -236,11 +236,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &9115279251508973176
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar128E4X.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar128E4X.prefab
@@ -139,11 +139,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &2398431948632788025
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar40P.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandar40P.prefab
@@ -465,11 +465,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &7675420128049466981
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarQT64.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarQT64.prefab
@@ -368,11 +368,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &1430391258634731476
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarXT32.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiPandarXT32.prefab
@@ -236,11 +236,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &8979344720251141608
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiQT128C2X.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/HesaiQT128C2X.prefab
@@ -57,11 +57,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &1429064023790827067
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/OusterOS1-64.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/OusterOS1-64.prefab
@@ -56,11 +56,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &4572956135196922574
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/SmartmicroDRVEGRD169.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/SmartmicroDRVEGRD169.prefab
@@ -56,8 +56,8 @@ MonoBehaviour:
   pointCloud2Publishers:
   - topic: radar/pointcloud
     publish: 1
-    fieldsPreset: 3
-    fields: 010000000f00000010000000110000001200000013000000
+    fieldsPreset: 4
+    fields: 010000001100000012000000130000001400000015000000
   radarScanPublishers:
   - topic: radar/radarscan
     publish: 1

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP16.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP16.prefab
@@ -57,11 +57,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &4572956135196922574
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP32C.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLP32C.prefab
@@ -57,11 +57,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &6422033487030486066
 MonoBehaviour:

diff --git a/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLS128.prefab b/Assets/AWSIM/Prefabs/Sensors/RobotecGPULidars/VelodyneVLS128.prefab
@@ -465,11 +465,11 @@ MonoBehaviour:
   - topic: lidar/pointcloud
     publish: 1
     fieldsPreset: 0
-    fields: 0100000002040000020000000a00000001040000
+    fields: 0100000002040000020000000b00000001040000
   - topic: lidar/pointcloud_ex
     publish: 1
-    fieldsPreset: 1
-    fields: 0100000002040000020000000a0000000104000008000000070000000b0000000004000001040000020400000c000000
+    fieldsPreset: 2
+    fields: 01000000030000000c0000000b0000000a00000009000000080000000e000000
   radarScanPublishers: []
 --- !u!114 &6012075883028567972
 MonoBehaviour:

diff --git a/Assets/AWSIM/Scripts/Sensors/LiDAR/PointCloudFormatLibrary.cs b/Assets/AWSIM/Scripts/Sensors/LiDAR/PointCloudFormatLibrary.cs
@@ -22,6 +22,7 @@ public enum PointCloudFormat : UInt32
     {
         Pcl24,
         Pcl48,
+        PointXYZIRCEADT,
         MLInstanceSegmentation,
         RadarSmartMicro,
         Custom,
@@ -60,6 +61,18 @@ public static class PointCloudFormatLibrary
                     RGLField.PADDING_32,
                     RGLField.TIME_STAMP_F64
                 }},
+                // PointXYZIRCEADT format used by Autoware
+                {PointCloudFormat.PointXYZIRCEADT, new[]
+                {
+                    RGLField.XYZ_VEC3_F32,
+                    RGLField.INTENSITY_U8,
+                    RGLField.RETURN_TYPE_U8,
+                    RGLField.RING_ID_U16,
+                    RGLField.ELEVATION_F32,
+                    RGLField.AZIMUTH_F32,
+                    RGLField.DISTANCE_F32,
+                    RGLField.TIME_STAMP_U32
+                }},
                 // Machine learning format for instance/semantic segmentation tasks
                 {PointCloudFormat.MLInstanceSegmentation, new[]
                 {

diff --git a/Assets/AWSIM/Scripts/Sensors/LiDAR/RglLidarPublisher.cs b/Assets/AWSIM/Scripts/Sensors/LiDAR/RglLidarPublisher.cs
@@ -151,7 +151,7 @@ public class RglLidarPublisher : MonoBehaviour
             {
                 topic = "lidar/pointcloud_ex",
                 publish = true,
-                fieldsPreset = PointCloudFormat.Pcl48,
+                fieldsPreset = PointCloudFormat.PointXYZIRCEADT,
             },
         };
 

diff --git a/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so b/Assets/RGLUnityPlugin/Plugins/Linux/x86_64/libRobotecGPULidar.so
diff --git a/Assets/RGLUnityPlugin/Plugins/Windows/x86_64/RobotecGPULidar.dll b/Assets/RGLUnityPlugin/Plugins/Windows/x86_64/RobotecGPULidar.dll
diff --git a/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs b/Assets/RGLUnityPlugin/Scripts/LowLevelWrappers/RGLNativeTypes.cs
@@ -37,6 +37,7 @@ public enum RGLField : Int32
 	{
 		XYZ_VEC3_F32 = 1,
 		INTENSITY_F32,
+		INTENSITY_U8,
 		IS_HIT_I32,
 		IS_GROUND_I32,
 		RAY_IDX_U32,
@@ -47,6 +48,7 @@ public enum RGLField : Int32
 		RING_ID_U16,
 		RETURN_TYPE_U8,
 		TIME_STAMP_F64,
+		TIME_STAMP_U32,
 		ABSOLUTE_VELOCITY_VEC3_F32,
 		RELATIVE_VELOCITY_VEC3_F32,
 		RADIAL_SPEED_F32,
@@ -57,6 +59,7 @@ public enum RGLField : Int32
 		NORMAL_VEC3_F32,
 		INCIDENT_ANGLE_F32,
 		RAY_POSE_MAT3x4_F32,
+		RGL_FIELD_LASER_RETRO_F32,
 		// Dummy fields
 		PADDING_8 = 1024,
 		PADDING_16,

diff --git a/docs/Components/Sensors/LiDARSensor/LiDARSensor/index.md b/docs/Components/Sensors/LiDARSensor/LiDARSensor/index.md
@@ -165,13 +165,14 @@ Currently, `RglLidarPublisher` implements ROS2 publishers for two message types:
 | Preset     | Description            | Fields                             |
 | :--------: | :--------------------- | :---------------------------------- |
 | Pcl 24 | 24-byte point cloud format used by *Autoware* | XYZ_VEC3_F32, PADDING_32, INTENSITY_F32, RING_ID_U16, PADDING_16 |
-| Pcl 48 | 48-byte extended version point cloud format used by *Autoware* | XYZ_VEC3_F32, PADDING_32, INTENSITY_F32, RING_ID_U16, PADDING_16, AZIMUTH_F32, DISTANCE_F32, RETURN_TYPE_U8, PADDING_8, PADDING_16, PADDING_32, TIME_STAMP_F64 |
+| PointXYZIRCEADT | PointXYZIRCEADT format used by *Autoware* | XYZ_VEC3_F32, INTENSITY_U8, RETURN_TYPE_U8, RING_ID_U16, ELEVATION_F32, AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_U32 |
+| Pcl 48 | 48-byte extended version point cloud format used by *Autoware* (legacy) | XYZ_VEC3_F32, PADDING_32, INTENSITY_F32, RING_ID_U16, PADDING_16, AZIMUTH_F32, DISTANCE_F32, RETURN_TYPE_U8, PADDING_8, PADDING_16, PADDING_32, TIME_STAMP_F64 |
 | ML Instance Segmentation | Machine learning format for instance/semantic segmentation tasks | XYZ_VEC3_F32, ENTITY_ID_I32, INTENSITY_F32 |
 | Radar Smart Micro | Format used in Radar Smart Micro | XYZ_VEC3_F32, RADIAL_SPEED_F32, POWER_F32, RCS_F32, NOISE_F32, SNR_F32 |
 | Custom | Empty format that allows the user to define its fieldsets |  |
 
-!!! note "*rosPCL48* format"
-    For a better understanding of the *rosPCL48* format, we encourage you to familiarize yourself with the point cloud pre-processing process in *Autoware*, which is described [here](https://autowarefoundation.github.io/autoware-documentation/latest/design/autoware-architecture/sensing/data-types/point-cloud/#channel).
+!!! note "*PointXYZIRCEADT* format"
+    For a better understanding of the *PointXYZIRCEADT* format, we encourage you to familiarize yourself with the point cloud pre-processing process in *Autoware*, which is described [here](https://autowarefoundation.github.io/autoware-documentation/latest/design/autoware-architecture/sensing/data-types/point-cloud/#point-cloud-fields).
 
 #### Elements configurable from the editor level
 - `Frame ID` - frame in which data are published, used in [`Header`](https://docs.ros2.org/latest/api/std_msgs/msg/Header.html) (default: `"world"`)
@@ -199,7 +200,7 @@ Currently, `RglLidarPublisher` implements ROS2 publishers for two message types:
 |         Category          | Topic                  | Message type                             | `frame_id` |
 | :-----------------------: | :--------------------- | :--------------------------------------- | :--------: |
 | PointCloud 24-byte format | `/lidar/pointcloud`    | [`sensor_msgs/PointCloud2`][pointcloud2] |  `world`   |
-| PointCloud 48-byte format | `/lidar/pointcloud_ex` | [`sensor_msgs/PointCloud2`][pointcloud2] |  `world`   |
+| PointXYZIRCEADT format    | `/lidar/pointcloud_ex` | [`sensor_msgs/PointCloud2`][pointcloud2] |  `world`   |
 
 ## Point Cloud Visualization (script)
 ![script_visualization](script_visualization.png)