Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support compressed image #305

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Support compressed image #305

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
91 changes: 79 additions & 12 deletions Assets/AWSIM/Scripts/Sensors/Camera/CameraRos2Publisher.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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.
Expand All @@ -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>
Expand All @@ -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;

Expand All @@ -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);
}

Expand All @@ -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)
Expand Down Expand Up @@ -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()
{
Expand Down
6 changes: 5 additions & 1 deletion Assets/AWSIM/Scripts/Sensors/Camera/CameraSensor.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,6 +170,9 @@ private enum FocalLengthName

private int bytesPerPixel = 3;

[HideInInspector]
public bool flip_image = false;

void Start()
{
if (cameraObject == null)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -341,4 +345,4 @@ private void UpdateCameraParameters()
cameraParameters.cy = ((cameraParameters.height + 1) / 2.0f);
}
}
}
}
2 changes: 1 addition & 1 deletion Assets/AWSIM/Scripts/Sensors/Camera/CameraSensorHolder.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
13 changes: 12 additions & 1 deletion Assets/AWSIM/Scripts/Sensors/Camera/RosImageShader.compute
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@ RWStructuredBuffer<uint> _RosImageBuffer;
uint _width;
uint _height;

bool _flip_image;

#define BYTES_PER_PIXEL 3
#define SIZE_OF_UINT 4

Expand All @@ -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
Expand Down
Loading