support compressed image

Assets/AWSIM/Scripts/Sensors/Camera/CameraRos2Publisher.cs

@@ -2,9 +2,18 @@
 using System.Collections.Generic;
 using UnityEngine;
 using ROS2;
+using System.Drawing;
+using System.IO;
+using UnityEngine;
+
 
 namespace AWSIM
 {
+    public enum raw_or_compress
+    {
+        RAW, Compressed
+    }
+
     /// <summary>
     /// Convert the data output from CameraSensor to ROS2 msg and Publish.
     /// </summary>
@@ -16,7 +25,7 @@ public class CameraRos2Publisher : MonoBehaviour
         /// <summary>
         /// Topic name for Image msg.
         /// </summary>
-        public string imageTopic = "/sensing/camera/traffic_light/image_raw";
+        public string imageTopic = "/sensing/camera/traffic_light/image";
 
         /// <summary>
         /// Topic name for CameraInfo msg.
@@ -28,6 +37,11 @@ public class CameraRos2Publisher : MonoBehaviour
         /// </summary>
         public string frameId = "traffic_light_left_camera/camera_link";
 
+        /// <summary>
+        /// Publish type. Raw or Compressed.
+        /// </summary>
+        public raw_or_compress publish_type;
+
         /// <summary>
         /// QoS settings.
         /// </summary>
@@ -41,9 +55,11 @@ public class CameraRos2Publisher : MonoBehaviour
 
         // Publishers
         IPublisher<sensor_msgs.msg.Image> imagePublisher;
+        IPublisher<sensor_msgs.msg.CompressedImage> compressedImagePublisher;
         IPublisher<sensor_msgs.msg.CameraInfo> cameraInfoPublisher;
         sensor_msgs.msg.Image imageMsg;
         sensor_msgs.msg.CameraInfo cameraInfoMsg;
+        sensor_msgs.msg.CompressedImage compressedImageMsg;
 
         CameraSensor sensor;
 
@@ -54,17 +70,38 @@ void Start()
             {
                 throw new MissingComponentException("No active CameraSensor component found.");
             }
+            if (publish_type == raw_or_compress.Compressed)
+            {
+                sensor.flip_image = true;
+            }else
+            {
+                sensor.flip_image = false;
+            }
 
             // Set callback
             sensor.OnOutputData += UpdateMessagesAndPublish;
 
             // Initialize msgs
-            imageMsg = InitializeEmptyImageMsg();
+            if(publish_type == raw_or_compress.RAW)
+            {
+                imageMsg = InitializeEmptyImageMsg();
+            }
+            else
+            {
+                compressedImageMsg = InitializeEmptyCompressedImageMsg();
+            }
             cameraInfoMsg = InitializeEmptyCameraInfoMsg();
 
             // Create publishers
             var qos = qosSettings.GetQoSProfile();
-            imagePublisher = SimulatorROS2Node.CreatePublisher<sensor_msgs.msg.Image>(imageTopic, qos);
+            if (publish_type == raw_or_compress.RAW)
+            {
+                imagePublisher = SimulatorROS2Node.CreatePublisher<sensor_msgs.msg.Image>(imageTopic, qos);
+            }
+            else
+            {
+                compressedImagePublisher = SimulatorROS2Node.CreatePublisher<sensor_msgs.msg.CompressedImage>(imageTopic, qos);
+            }
             cameraInfoPublisher = SimulatorROS2Node.CreatePublisher<sensor_msgs.msg.CameraInfo>(cameraInfoTopic, qos);
         }
 
@@ -81,26 +118,45 @@ void UpdateMessagesAndPublish(CameraSensor.OutputData outputData)
 
             // Update msgs timestamp, timestamps should be synchronized in order to connect image and camera_info msgs
             var timeMsg = SimulatorROS2Node.GetCurrentRosTime();
-            imageMsg.Header.Stamp = timeMsg;
-            cameraInfoMsg.Header.Stamp = timeMsg;
 
             // Publish to ROS2
-            imagePublisher.Publish(imageMsg);
+            if (publish_type == raw_or_compress.RAW)
+            {
+                imageMsg.Header.Stamp = timeMsg;
+                imagePublisher.Publish(imageMsg);
+            }
+            else
+            {
+                compressedImageMsg.Header.Stamp = timeMsg;
+                compressedImagePublisher.Publish(compressedImageMsg);
+            }                
+            cameraInfoMsg.Header.Stamp = timeMsg;
             cameraInfoPublisher.Publish(cameraInfoMsg);
         }
 
         private void UpdateImageMsg(CameraSensor.OutputData data)
         {
-            if (imageMsg.Width != data.cameraParameters.width || imageMsg.Height != data.cameraParameters.height)
+            if (publish_type == raw_or_compress.RAW)
             {
-                imageMsg.Width = (uint)data.cameraParameters.width;
-                imageMsg.Height = (uint)data.cameraParameters.height;
-                imageMsg.Step = (uint)(data.cameraParameters.width * 3);
+                if (imageMsg.Width != data.cameraParameters.width || imageMsg.Height != data.cameraParameters.height)
+                {
+                    imageMsg.Width = (uint)data.cameraParameters.width;
+                    imageMsg.Height = (uint)data.cameraParameters.height;
+                    imageMsg.Step = (uint)(data.cameraParameters.width * 3);
+
+                    imageMsg.Data = new byte[data.cameraParameters.height * data.cameraParameters.width * 3];
+                }
 
-                imageMsg.Data = new byte[data.cameraParameters.height * data.cameraParameters.width * 3];
+                imageMsg.Data = data.imageDataBuffer;
             }
+            else
+            {
+                Texture2D texture = new Texture2D(data.cameraParameters.width, data.cameraParameters.height, TextureFormat.RGB24, false);
+                texture.LoadRawTextureData(data.imageDataBuffer);
+                texture.Apply();
 
-            imageMsg.Data = data.imageDataBuffer;
+                compressedImageMsg.Data = texture.EncodeToJPG();
+            }
         }
 
         private void UpdateCameraInfoMsg(CameraSensor.CameraParameters cameraParameters)
@@ -147,6 +203,17 @@ private sensor_msgs.msg.Image InitializeEmptyImageMsg()
                 Is_bigendian = 0,
             };
         }
+        private sensor_msgs.msg.CompressedImage InitializeEmptyCompressedImageMsg()
+        {
+            return new sensor_msgs.msg.CompressedImage()
+            {
+                Header = new std_msgs.msg.Header()
+                {
+                    Frame_id = frameId
+                },
+                Format = "jpeg"
+            };
+        }
 
         private sensor_msgs.msg.CameraInfo InitializeEmptyCameraInfoMsg()
         {

Assets/AWSIM/Scripts/Sensors/Camera/CameraSensor.cs

@@ -170,6 +170,9 @@ private enum FocalLengthName
 
         private int bytesPerPixel = 3;
 
+        [HideInInspector]
+        public bool flip_image = false;
+
         void Start()
         {
             if (cameraObject == null)
@@ -223,6 +226,7 @@ public void DoRender()
             rosImageShader.SetTexture(rosShaderKernelIdx, "_InputTexture", distortedRenderTexture);
             rosImageShader.SetBuffer(rosShaderKernelIdx, "_RosImageBuffer", computeBuffer);
             rosImageShader.Dispatch(rosShaderKernelIdx, rosImageShaderGroupSizeX, 1, 1);
+            rosImageShader.SetBool("_flip_image", flip_image);
 
             // Get data from shader
             AsyncGPUReadback.Request(computeBuffer, OnGPUReadbackRequest);
@@ -341,4 +345,4 @@ private void UpdateCameraParameters()
             cameraParameters.cy = ((cameraParameters.height + 1) / 2.0f);
         }
     }
-}
+}

Assets/AWSIM/Scripts/Sensors/Camera/CameraSensorHolder.cs

@@ -19,7 +19,7 @@ public class CameraSensorHolder : MonoBehaviour
         /// Data output hz.
         /// Sensor processing and callbacks are called in this hz.
         /// </summary>
-        [Range(0, 30)][SerializeField] private uint publishHz = 10;
+        [Range(0, 60)][SerializeField] private uint publishHz = 10;
 
         /// <summary>
         /// Rendering sequence type.

Assets/AWSIM/Scripts/Sensors/Camera/RosImageShader.compute

@@ -10,6 +10,8 @@ RWStructuredBuffer<uint> _RosImageBuffer;
 uint _width;
 uint _height;
 
+bool _flip_image;
+
 #define BYTES_PER_PIXEL 3
 #define SIZE_OF_UINT 4
 
@@ -28,7 +30,16 @@ void RosImageShaderKernel (uint3 id : SV_DispatchThreadID)
     // Convert coordinates for use in ROS (bgr8).
     // write down for distortion texture
     // write to B,G,R
-    uint rosImageY = _height - 1 - textureY;
+
+    uint rosImageY = 0;
+    if (_flip_image)
+    {
+        rosImageY = textureY;
+    }
+    else
+    {
+        rosImageY = _height - 1 - textureY;
+    }
     float3 color1 = _InputTexture[float2(textureX, rosImageY)].rgb;
 
     // check boundary conditions