Skip to content

Commit

Permalink
Merge pull request #21 from lge-ros2/develop
Browse files Browse the repository at this point in the history 
Merge 'develop' branch into 'master'
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Aug 10, 2020
2 parents 3f7445b + 9c0981d commit 9f5c0c4
Show file tree
Hide file tree
Showing 5 changed files with 139 additions and 68 deletions.
78 changes: 45 additions & 33 deletions Assets/Scripts/Devices/Lidar.LaserCameraData.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,32 +11,31 @@ namespace SensorDevices
{
public partial class Lidar : Device
{
const int colorFormatUnitSize = sizeof(float);

struct LaserCamData
{
const int colorFormatUnit = sizeof(float);
public float maxHAngleHalf;
public float maxHAngleHalfTangent;
public float angleResolutionH;
public float centerAngle;

private int index;
private float centerAngle;
private int imageWidth;
private int imageHeight;
private byte[] imageBuffer;

public float CenterAngle
{
get => centerAngle;
set => centerAngle = value;
}

public int ImageWidth => imageWidth;
public double[] output;

public int ImageHeight => imageHeight;
public float StartAngle => centerAngle - maxHAngleHalf;
public float EndAngle => centerAngle + maxHAngleHalf;

public void AllocateBuffer(in int dataIndex, in int width, in int height)
public void AllocateBuffer(in int width, in int height)
{
index = dataIndex;
imageWidth = width;
imageHeight = height;
imageBuffer = new byte[width * height * colorFormatUnit];
imageBuffer = new byte[width * height * colorFormatUnitSize];

output = new double[imageWidth];
}

public void SetBufferData(in NativeArray<byte> buffer)
Expand All @@ -47,14 +46,27 @@ public void SetBufferData(in NativeArray<byte> buffer)
}
}

public float GetDepthData(in float horizontalAngle, in float verticalAngle = 0)
public void ResolveLaserRanges(in float rangeMax = 1.0f)
{
for (var index = 0; index < output.Length; index++)
{
var rayAngleH = angleResolutionH * index;
var depthData = GetDepthData(rayAngleH);
var rayDistance = (depthData > 0) ? depthData * rangeMax : Mathf.Infinity;

// CCW for ROS2 message direction
var ccwIndex = output.Length - index - 1;
output[ccwIndex] = rayDistance;
}
}

private float GetDepthData(in float horizontalAngle, in float verticalAngle = 0)
{
var horizontalAngleInCamData = (horizontalAngle - centerAngle) * Mathf.Deg2Rad;
var horizontalAngleInCamData = (horizontalAngle - maxHAngleHalf) * Mathf.Deg2Rad;
var verticalAngleInCamData = verticalAngle * Mathf.Deg2Rad;

var maxAngleTangent = Mathf.Tan(laserCameraHFovHalf * Mathf.Deg2Rad);
var offsetY = (imageHeight * 0.5f) * (1f + (Mathf.Tan(verticalAngleInCamData) / maxAngleTangent));
var offsetX = (imageWidth * 0.5f) * (1f + (Mathf.Tan(horizontalAngleInCamData) / maxAngleTangent));
var offsetX = (imageWidth * 0.5f) * (1f + Mathf.Tan(horizontalAngleInCamData)/maxHAngleHalfTangent);
var offsetY = (imageHeight * 0.5f) * (1f + Mathf.Tan(verticalAngleInCamData)/maxHAngleHalfTangent);

var decodedData = GetDecodedData((int)offsetX, (int)offsetY);

Expand All @@ -66,27 +78,19 @@ public float GetDepthData(in float horizontalAngle, in float verticalAngle = 0)
return (finalDepthData > 1.0f) ? 1.0f : finalDepthData;
}

private float DecodeChannel(in byte r, in byte g, in byte b, in byte a)
{
// decodedData = (r / 255f) + (g / 255f) / 255f + (b / 255f) / 65025f;
// decodedData = (r * 0.0039215686f) + (g * 0.0000153787f) + (b * 0.0000000603f);
// decodedData = (r / 255f) + (g / 255f) / 255f + (b / 255f) / 65025f + (a / 255f) / 16581375f;
return (r * 0.0039215686f) + (g * 0.0000153787f) + (b * 0.0000000603f) + (a * 0.0000000002f);
}

private float GetDecodedData(in int pixelOffsetX, in int pixelOffsetY)
{
if (imageBuffer != null && imageBuffer.Length > 0)
{
// Decode
var imageOffsetX = colorFormatUnit * pixelOffsetX;
var imageOffsetY = colorFormatUnit * imageWidth * pixelOffsetY;
var imageOffsetX = colorFormatUnitSize * pixelOffsetX;
var imageOffsetY = colorFormatUnitSize * imageWidth * pixelOffsetY;
var imageOffset = imageOffsetY + imageOffsetX;

byte r = imageBuffer[imageOffset + 0];
byte g = imageBuffer[imageOffset + 1];
byte b = imageBuffer[imageOffset + 2];
byte a = imageBuffer[imageOffset + 3];
var r = imageBuffer[imageOffset + 0];
var g = imageBuffer[imageOffset + 1];
var b = imageBuffer[imageOffset + 2];
var a = imageBuffer[imageOffset + 3];

return DecodeChannel(r, g, b, a);
}
Expand All @@ -95,6 +99,14 @@ private float GetDecodedData(in int pixelOffsetX, in int pixelOffsetY)
return 0;
}
}

private float DecodeChannel(in byte r, in byte g, in byte b, in byte a)
{
// decodedData = (r / 255f) + (g / 255f) / 255f + (b / 255f) / 65025f;
// decodedData = (r * 0.0039215686f) + (g * 0.0000153787f) + (b * 0.0000000603f);
// decodedData = (r / 255f) + (g / 255f) / 255f + (b / 255f) / 65025f + (a / 255f) / 16581375f;
return (r * 0.0039215686f) + (g * 0.0000153787f) + (b * 0.0000000603f) + (a * 0.0000000002f);
}
}
}
}
111 changes: 84 additions & 27 deletions Assets/Scripts/Devices/Lidar.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,10 @@ public partial class Lidar : Device

private LaserCamData[] laserCamData;

private float laserStartAngle;
private float laserEndAngle;
private float laserTotalAngle;

void OnRenderImage(RenderTexture source, RenderTexture destination)
{
if (depthMaterial)
Expand Down Expand Up @@ -123,6 +127,8 @@ protected override void InitializeMessages()

laserScan.Ranges = new double[samples];
laserScan.Intensities = new double[samples];
Array.Clear(laserScan.Ranges, 0, (int)samples);
Array.Clear(laserScan.Intensities, 0, (int)samples);

laserHAngleResolution = (float)(laserScan.AngleStep * Mathf.Rad2Deg);
laserVAngleResolution = (float)(laserScan.VerticalAngleStep * Mathf.Rad2Deg);
Expand Down Expand Up @@ -154,7 +160,7 @@ private void SetupLaserCamera()

var renderTextrueWidth = Mathf.CeilToInt(laserCameraHFov / laserHAngleResolution);
var aspectRatio = Mathf.Tan(laserCameraVFov / 2 * Mathf.Deg2Rad) / Mathf.Tan(laserCameraHFov / 2 * Mathf.Deg2Rad);
var renderTextrueHeight = Mathf.RoundToInt(renderTextrueWidth * aspectRatio);
var renderTextrueHeight = Mathf.CeilToInt(renderTextrueWidth * aspectRatio);
var targetDepthRT = new RenderTexture(renderTextrueWidth, renderTextrueHeight, 24, RenderTextureFormat.ARGB32, RenderTextureReadWrite.Linear)
{
name = "LidarDepthTexture"
Expand All @@ -178,29 +184,37 @@ private void SetupLaserCameraData()
for (var index = 0; index < numberOfLaserCamData; index++)
{
var data = new LaserCamData();
data.AllocateBuffer(index, targetDepthRT.width, targetDepthRT.height);
data.CenterAngle = laserCameraRotationAngle * index;
data.angleResolutionH = laserHAngleResolution;
data.maxHAngleHalf = laserCameraHFovHalf;
data.maxHAngleHalfTangent = Mathf.Tan(laserCameraHFovHalf * Mathf.Deg2Rad);
data.centerAngle = laserCameraRotationAngle * index;
data.AllocateBuffer(targetDepthRT.width, targetDepthRT.height);
laserCamData[index] = data;
}

laserStartAngle = defaultRotationOffset + (float)angleMin;
laserEndAngle = defaultRotationOffset + (float)angleMax;
laserTotalAngle = (float)(angleMax - angleMin);
}

private IEnumerator LaserCameraWorker()
{
var axisRotation = Vector3.zero;
var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
var readbacks = new AsyncGPUReadbackRequest[numberOfLaserCamData];
var sw = new Stopwatch();

while (true)
{
sw.Restart();

for (var dataIndex = 0; dataIndex < numberOfLaserCamData; dataIndex++)
{
var data = laserCamData[dataIndex];
axisRotation.y = data.CenterAngle;
axisRotation.y = data.centerAngle;

laserCam.transform.localRotation = Quaternion.Euler(axisRotation);

laserCam.enabled = true;

laserCam.Render();

readbacks[dataIndex] = AsyncGPUReadback.Request(laserCam.targetTexture, 0, TextureFormat.RGBA32);
Expand Down Expand Up @@ -233,51 +247,91 @@ private IEnumerator LaserCameraWorker()
}
}

yield return waitForSeconds;
for (var dataIndex = 0; dataIndex < numberOfLaserCamData; dataIndex++)
{
yield return null;
var data = laserCamData[dataIndex];
data.ResolveLaserRanges((float)rangeMax);
}

sw.Stop();

yield return new WaitForSeconds(WaitPeriod((float)sw.Elapsed.TotalSeconds));
}
}

protected override IEnumerator MainDeviceWorker()
{
var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
// var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
var sw = new Stopwatch();
while (true)
{
sw.Restart();
GenerateMessage();
sw.Stop();

yield return waitForSeconds; //new WaitForSeconds(WaitPeriod((float)sw.Elapsed.TotalSeconds));
yield return new WaitForSeconds(WaitPeriod((float)sw.Elapsed.TotalSeconds));
}
}

protected override void GenerateMessage()
{
var lidarPosition = lidarLink.position + transform.localPosition;
var lidarRotation = lidarLink.rotation;

var laserScan = laserScanStamped.Scan;

DeviceHelper.SetVector3d(laserScan.WorldPose.Position, lidarPosition);
DeviceHelper.SetQuaternion(laserScan.WorldPose.Orientation, lidarRotation);

var startAngle = defaultRotationOffset + (float)angleMin;
for (var hScanIndex = 0; hScanIndex < laserScanStamped.Scan.Count; hScanIndex++)
{
var rayAngleH = ((laserHAngleResolution * hScanIndex)) + startAngle;
var srcBufferOffset = 0;
var dstBufferOffset = 0;
var copyLength = 0;
var doCopy = true;

var convertedRayAngleH = (rayAngleH >= -laserCameraHFovHalf) ? rayAngleH : (360 + rayAngleH);
var dataIndexByAngle = (uint)Mathf.Round(convertedRayAngleH / laserCameraHFov);

var laserScanData = laserCamData[dataIndexByAngle];

var depthData = laserScanData.GetDepthData(convertedRayAngleH);

var rayDistance = (depthData > 0) ? depthData * (float)rangeMax : Mathf.Infinity;
for (var dataIndex = 0; dataIndex < numberOfLaserCamData; dataIndex++)
{
doCopy = true;
var data = laserCamData[dataIndex];
var outputBufferLength = data.output.Length;
var dataStartAngle = data.StartAngle;
var dataEndAngle = data.EndAngle;
var dataTotalAngle = dataEndAngle - dataStartAngle;

// start side of laser angle
if (dataStartAngle < laserStartAngle)
{
srcBufferOffset = 0;
var srcLengthratio = Mathf.Abs((dataStartAngle - laserStartAngle) / dataTotalAngle);
copyLength = outputBufferLength - Mathf.FloorToInt(outputBufferLength * srcLengthratio);
dstBufferOffset = (int)samples - copyLength;
}
// middle of laser angle
else if (dataStartAngle >= laserStartAngle && dataEndAngle < laserEndAngle)
{
srcBufferOffset = 0;
copyLength = outputBufferLength;
dstBufferOffset = (int)samples - (Mathf.CeilToInt(samples * ((dataStartAngle - laserStartAngle) / laserTotalAngle)) + copyLength);
}
// end side of laser angle
else if (dataEndAngle > laserEndAngle)
{
var srcLengthRatio = (laserEndAngle - dataStartAngle) / dataTotalAngle;
copyLength = Mathf.FloorToInt(outputBufferLength * srcLengthRatio);
srcBufferOffset = outputBufferLength - copyLength;
dstBufferOffset = 0;
}
else
{
Debug.LogWarning("Something wrong data in Laser....");
doCopy = false;
}

// Store the laser data CCW
var scanIndexCcw = (uint)laserScanStamped.Scan.Count - hScanIndex - 1;
laserScan.Ranges[scanIndexCcw] = (double)rayDistance;
laserScan.Intensities[scanIndexCcw] = 0.0f;
if (doCopy)
{
// Store CCW direction for ROS2 sensor data
Array.Copy(data.output, srcBufferOffset, laserScan.Ranges, dstBufferOffset, copyLength);
}
}

DeviceHelper.SetCurrentTime(laserScanStamped.Time);
Expand Down Expand Up @@ -305,7 +359,11 @@ protected override IEnumerator OnVisualize()
var rayAngleH = ((laserHAngleResolution * hScanIndex)) + startAngle;
var rayRotation = Quaternion.AngleAxis((float)(rayAngleH), lidarLink.up) * lidarLink.forward;
var rayStart = (rayRotation * (float)rangeMin) + lidarSensorWorldPosition;
var rayDistance = (rangeData[hScanIndex] == Mathf.Infinity) ? (float)rangeMax : (rangeData[hScanIndex] - (float)rangeMin);

var ccwIndex = (uint)rangeData.Length - hScanIndex - 1;
var rayData = rangeData[ccwIndex];

var rayDistance = (rayData == Mathf.Infinity) ? (float)rangeMax : (rayData - (float)rangeMin);
var rayDirection = rayRotation * rayDistance;

Debug.DrawRay(rayStart, rayDirection, rayColor, visualDrawDuration, true);
Expand All @@ -320,7 +378,6 @@ public float[] GetRangeData()
try
{
var temp = Array.ConvertAll(laserScanStamped.Scan.Ranges, item => (float)item);
Array.Reverse(temp);
return temp;
}
catch
Expand Down
2 changes: 1 addition & 1 deletion ProjectSettings/GraphicsSettings.asset
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ GraphicsSettings:
- {fileID: 17000, guid: 0000000000000000f000000000000000, type: 0}
- {fileID: 45, guid: 0000000000000000f000000000000000, type: 0}
- {fileID: 46, guid: 0000000000000000f000000000000000, type: 0}
- {fileID: 4800000, guid: 1a5580673dcecd5aca2677d75ef17de5, type: 3}
- {fileID: 4800000, guid: 580493fad5661e266b759ad2bd7f2c54, type: 3}
- {fileID: 4800000, guid: f0d7cb58cac7143ddb2c05bc920dcd35, type: 3}
- {fileID: 4800000, guid: 85b8ee53555329de0b74a90a2d692751, type: 3}
m_PreloadedShaders: []
Expand Down
10 changes: 5 additions & 5 deletions ProjectSettings/ProjectSettings.asset
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ PlayerSettings:
16:10: 1
16:9: 1
Others: 1
bundleVersion: 1.5.0
bundleVersion: 1.5.1
preloadedAssets: []
metroInputSource: 0
wsaTransparentSwapchain: 0
Expand Down Expand Up @@ -241,8 +241,8 @@ PlayerSettings:
iOSAutomaticallyDetectAndAddCapabilities: 1
appleEnableProMotion: 0
clonedFromGUID: c0afd0d1d80e3634a9dac47e8a0426ea
templatePackageId: com.unity.template.3d@3.1.2
templateDefaultScene: Assets/Scenes/SampleScene.unity
templatePackageId:
templateDefaultScene: Assets/Scenes/MainScene.unity
AndroidTargetArchitectures: 1
AndroidSplashScreenScale: 0
androidSplashScreen: {fileID: 0}
Expand Down Expand Up @@ -650,14 +650,14 @@ PlayerSettings:
Standalone: 3
m_RenderingPath: 1
m_MobileRenderingPath: 1
metroPackageName: Template_3D
metroPackageName: CLOiSim
metroPackageVersion:
metroCertificatePath:
metroCertificatePassword:
metroCertificateSubject:
metroCertificateIssuer:
metroCertificateNotAfter: 0000000000000000
metroApplicationDescription: Template_3D
metroApplicationDescription: CLOiSim
wsaImages: {}
metroTileShortName:
metroTileShowName: 0
Expand Down
6 changes: 4 additions & 2 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ Here is the full list of models that is implemented or planned to be implemented
- [X] 2-Wheeled Motor
- [X] Sonar sensor
- [X] IMU
- [X] Contact
- [X] Camera
- [ ] Camera intrinsic parameter
- [X] Multi-camera
Expand Down Expand Up @@ -59,8 +60,9 @@ Inertia factors which retrieved from SDF are NOT USED for rigidbody in Unity. Be
## Getting Started

#### Tested environement:
- Linux - Ubuntu 18.04
- Current editor version is *'2019.4.1f1 (LTS)'*.
- Linux: Ubuntu 18.04
- Intel(R) Core(TM) i7-8700K, 32GB, GeForce RTX 2070
- Current editor version is *'2019.4.6f1 (LTS)'*.

#### Release version
If you don't want to build a project, just USE a release binary([Download linux version](https://github.com/lge-ros2/multi-robot-simulator/releases)). And just refer to 'Usage'
Expand Down

0 comments on commit 9f5c0c4

Please sign in to comment.