Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.2.2 (#266)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Dec 5, 2023
2 parents 870f7e8 + cefdfc0 commit 1d9edea
Show file tree
Hide file tree
Showing 20 changed files with 399 additions and 331 deletions.
1 change: 0 additions & 1 deletion Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ protected override void OnStart()
return;
}


GetPluginParameters().GetValues<string>("activity_zone/model", out var zoneList);

foreach (var zone in zoneList)
Expand Down
17 changes: 15 additions & 2 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,12 +67,18 @@ private void SetupMicom()
micomSensor.SetUpdateRate(updateRate);

var wheelRadius = GetPluginParameters().GetValue<float>("wheel/radius");
var wheelTread = GetPluginParameters().GetValue<float>("wheel/tread");
var wheelTread = GetPluginParameters().GetValue<float>("wheel/tread"); // deprecated
if (Mathf.Approximately(Mathf.Abs(wheelTread), Quaternion.kEpsilon) == false)
{
Debug.LogWarning("<wheel/tread> will be depreacted!!");
}

var wheelSeparation = GetPluginParameters().GetValue<float>("wheel/separation", wheelTread);
var P = GetPluginParameters().GetValue<float>("wheel/PID/kp");
var I = GetPluginParameters().GetValue<float>("wheel/PID/ki");
var D = GetPluginParameters().GetValue<float>("wheel/PID/kd");

micomSensor.SetMotorConfiguration(wheelRadius, wheelTread, P, I, D);
micomSensor.SetMotorConfiguration(wheelRadius, wheelSeparation, P, I, D);

// TODO: to be utilized, currently not used
var motorFriction = GetPluginParameters().GetValue<float>("wheel/friction/motor", 0.1f);
Expand Down Expand Up @@ -114,6 +120,13 @@ private void SetupMicom()
{
micomSensor.SetBumperSensor(bumperJointNameList);
}

var targetImuName = GetPluginParameters().GetValue<string>("imu");
if (!string.IsNullOrEmpty(targetImuName))
{
// Debug.Log("Imu Sensor = " + targetImuName);
micomSensor.SetIMU(targetImuName);
}
}

private void LoadStaticTF()
Expand Down
7 changes: 7 additions & 0 deletions Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,13 +155,20 @@ private void SetCustomHandleRequestMessage()
if (string.IsNullOrEmpty(topic_name))
{
topic_name = GetPluginParameters().GetValue<string>("ros2/topic_name", partsName);
topic_name = topic_name.Replace("{parts_name}", partsName);
}
else
{
topic_name = partsName + "/" + topic_name;
}

GetPluginParameters().GetValues<string>("ros2/frame_id", out var frameIdList);

for (var i = 0; i < frameIdList.Count; i++)
{
frameIdList[i] = frameIdList[i].Replace("{parts_name}", partsName);
}

SetROS2CommonInfoResponse(ref response, topic_name, frameIdList);
}
break;
Expand Down
6 changes: 3 additions & 3 deletions Assets/Scripts/Core/WorldNavMeshBuilder.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,10 +111,10 @@ public void AddNavMeshTracks(in Transform transform)

var bounds = new Bounds(transform.position, Vector3.zero);
var renderers = transform.GetComponentsInChildren<Renderer>();
for (var i = 0; i < renderers.Length; i++)
foreach (var render in renderers)
{
// Debug.Log(renderer.bounds.center + ", " + renderer.bounds.size);
bounds.Encapsulate(renderers[i].bounds);
// Debug.Log(render.bounds.center + ", " + render.bounds.size);
bounds.Encapsulate(render.bounds);
}
// Debug.Log("Final: " + bounds.center + ", " + bounds.size);

Expand Down
6 changes: 5 additions & 1 deletion Assets/Scripts/Devices/Clock.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,11 @@ public class Clock : Device
#endregion

private double restartedSimTime = 0;
private double restartedFixedSimTime = 0;
private double restartedRealTime = 0;

private double currentSimTime = 0;
private double currentFixedSimTime = 0;
private double currentRealTime = 0;

#region time in hms format
Expand Down Expand Up @@ -66,7 +68,7 @@ private void SetTimeString(ref string target, in TimeSpan ts)
#endregion

public double SimTime => currentSimTime;

public double FixedSimTime => currentFixedSimTime;
public double RealTime => currentRealTime;

public HMS ToHMS() => hms;
Expand All @@ -90,6 +92,7 @@ private void UpdateCurrentTime()
{
currentRealTime = Time.realtimeSinceStartupAsDouble - restartedRealTime;
currentSimTime = Time.timeAsDouble - restartedSimTime;
currentFixedSimTime = Time.fixedTimeAsDouble - restartedFixedSimTime;
}

void FixedUpdate()
Expand Down Expand Up @@ -152,6 +155,7 @@ protected override void GenerateMessage()
public void ResetTime()
{
restartedSimTime = Time.timeAsDouble;
restartedFixedSimTime = Time.fixedTimeAsDouble;
restartedRealTime = Time.realtimeSinceStartupAsDouble;

UpdateCurrentTime();
Expand Down
142 changes: 65 additions & 77 deletions Assets/Scripts/Devices/Lidar.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,17 +55,6 @@ public partial class Lidar : Device
private RTHandle _rtHandle = null;
private ParallelOptions _parallelOptions = null;

struct AsyncLaserWork
{
public int dataIndex;
public AsyncGPUReadbackRequest request;

public AsyncLaserWork(in int dataIndex, in AsyncGPUReadbackRequest request)
{
this.dataIndex = dataIndex;
this.request = request;
}
}
private AsyncLaserWork[] _asyncWorkList;
private DepthData.CamBuffer[] _depthCamBuffers;
private LaserData.LaserCamData[] _laserCamData;
Expand Down Expand Up @@ -325,13 +314,14 @@ private void LaserCameraWorker()
if (laserCam.isActiveAndEnabled)
{
laserCam.Render();
var capturedTime = (float)DeviceHelper.GetGlobalClock().SimTime;
var readbackRequest = AsyncGPUReadback.Request(laserCam.targetTexture, 0, GraphicsFormat.R8G8B8A8_UNorm, OnCompleteAsyncReadback);

if (_asyncWorkList.Length == numberOfLaserCamData)
_asyncWorkList[dataIndex] = new AsyncLaserWork(dataIndex, readbackRequest);
}
_asyncWorkList[dataIndex] = new AsyncLaserWork(dataIndex, readbackRequest, capturedTime);

laserCam.enabled = false;
laserCam.enabled = false;
}
}

_lastTimeLaserCameraWork = Time.time;
Expand All @@ -349,39 +339,40 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request)
for (var i = 0; i < _asyncWorkList.Length; i++)
{
var asyncWork = _asyncWorkList[i];
if (asyncWork.request.Equals(request))
{
var dataIndex = asyncWork.dataIndex;
var depthCamBuffer = _depthCamBuffers[dataIndex];
if (!asyncWork.request.Equals(request))
continue;

depthCamBuffer.Allocate();
depthCamBuffer.raw = request.GetData<byte>();
var dataIndex = asyncWork.dataIndex;
var depthCamBuffer = _depthCamBuffers[dataIndex];

if (depthCamBuffer.depth.IsCreated)
{
var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize);
jobHandleDepthCamBuffer.Complete();
depthCamBuffer.Allocate();
depthCamBuffer.raw = request.GetData<byte>();

var laserCamData = _laserCamData[dataIndex];
laserCamData.depthBuffer = depthCamBuffer.depth;
laserCamData.Allocate();
if (depthCamBuffer.depth.IsCreated)
{
var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize);
jobHandleDepthCamBuffer.Complete();

var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize);
jobHandle.Complete();
var laserCamData = _laserCamData[dataIndex];
laserCamData.depthBuffer = depthCamBuffer.depth;
laserCamData.Allocate();

_laserDataOutput[dataIndex].data = laserCamData.GetLaserData();
var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize);
jobHandle.Complete();

if (noise != null)
{
noise.Apply<double>(ref _laserDataOutput[dataIndex].data);
}
_laserDataOutput[dataIndex].data = laserCamData.GetLaserData();
_laserDataOutput[dataIndex].capturedTime = asyncWork.capturedTime;

laserCamData.Deallocate();
if (noise != null)
{
noise.Apply<double>(ref _laserDataOutput[dataIndex].data);
}

depthCamBuffer.Deallocate();
break;
laserCamData.Deallocate();
}

depthCamBuffer.Deallocate();
break;
}
}
}
Expand All @@ -408,6 +399,9 @@ protected override void GenerateMessage()
var laserEndAngleV = (float)vertical.angle.max;
var laserTotalAngleV = (float)vertical.angle.range;

const int TargetCaptureTimeIndex = 1;
var capturedTime = (float)DeviceHelper.GlobalClock.FixedSimTime;

Parallel.For(0, numberOfLaserCamData, _parallelOptions, index =>
{
var laserCamData = _laserCamData[index];
Expand All @@ -428,61 +422,45 @@ protected override void GenerateMessage()
return;
}

if (index == TargetCaptureTimeIndex)
{
capturedTime = _laserDataOutput[index].capturedTime;
}

if (laserStartAngleH < 0 && dataEndAngleH > DEG180)
{
dataStartAngleH -= DEG360;
dataEndAngleH -= DEG360;
}
// Debug.LogFormat("index {0}: {1}~{2}, {3}~{4}", dataIndex, laserStartAngleH, laserEndAngleH, dataStartAngleH, dataEndAngleH);

var dataCopyType = -1;

for (var sampleIndexV = 0; sampleIndexV < laserSamplesV; sampleIndexV++, doCopy = true)
{
if (dataStartAngleH <= laserStartAngleH)
if (dataStartAngleH <= laserStartAngleH) // start side of laser angle
{
dataCopyType = 1; // start side of laser angle
dataLengthRatio = (laserStartAngleH - dataStartAngleH) * dividedDataTotalAngleH;
copyLength = srcBufferHorizontalLength - Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio);
srcBufferOffset = srcBufferHorizontalLength * sampleIndexV;
dstBufferOffset = laserSamplesH * (sampleIndexV + 1) - copyLength;
}
else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH)
else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) // middle of laser angle
{
dataCopyType = 2; // middle of laser angle
dataLengthRatio = (dataStartAngleH - laserStartAngleH) * dividedLaserTotalAngleH;
copyLength = srcBufferHorizontalLength;
srcBufferOffset = srcBufferHorizontalLength * sampleIndexV;
dstBufferOffset = Mathf.CeilToInt(laserSamplesH * ((float)(sampleIndexV + 1) - dataLengthRatio)) - copyLength;
}
else if (dataEndAngleH >= laserEndAngleH)
else if (dataEndAngleH >= laserEndAngleH) // end side of laser angle
{
dataCopyType = 3; // end side of laser angle
dataLengthRatio = (laserEndAngleH - dataStartAngleH) * dividedDataTotalAngleH;
copyLength = Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio);
srcBufferOffset = srcBufferHorizontalLength * (sampleIndexV + 1) - copyLength;
dstBufferOffset = laserSamplesH * sampleIndexV + 1;
}
else
{
dataCopyType = -1;
}

switch (dataCopyType)
{
case 1:
dataLengthRatio = (laserStartAngleH - dataStartAngleH) * dividedDataTotalAngleH;
copyLength = srcBufferHorizontalLength - Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio);
srcBufferOffset = srcBufferHorizontalLength * sampleIndexV;
dstBufferOffset = laserSamplesH * (sampleIndexV + 1) - copyLength;
break;

case 2:
dataLengthRatio = (dataStartAngleH - laserStartAngleH) * dividedLaserTotalAngleH;
copyLength = srcBufferHorizontalLength;
srcBufferOffset = srcBufferHorizontalLength * sampleIndexV;
dstBufferOffset = Mathf.CeilToInt(laserSamplesH * ((float)(sampleIndexV + 1) - dataLengthRatio)) - copyLength;
break;

case 3:
dataLengthRatio = (laserEndAngleH - dataStartAngleH) * dividedDataTotalAngleH;
copyLength = Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio);
srcBufferOffset = srcBufferHorizontalLength * (sampleIndexV + 1) - copyLength;
dstBufferOffset = laserSamplesH * sampleIndexV + 1;
break;

default:
Debug.LogWarning("Something wrong data copy type in Generating Laser Data....");
doCopy = false;
break;
Debug.LogWarning("Something wrong data copy type in Generating Laser Data....");
doCopy = false;
}

if (doCopy && copyLength >= 0 && dstBufferOffset >= 0)
Expand All @@ -496,7 +474,16 @@ protected override void GenerateMessage()
}
catch (Exception ex)
{
Debug.LogWarning("Error occured with Buffer.BlockCopy: " + ex.Message + ", Type: " + dataCopyType + " Offset: src(" + srcBufferOffset + ") dst(" + dstBufferOffset + ") Len: src(" + srcBuffer.Length + ") dst(" + laserScan.Ranges.Length + ") copy_size(" + copyLength + ")");
var copyType = -1;
if (dataStartAngleH <= laserStartAngleH)
copyType = 0;
else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH)
copyType = 1;
else if (dataEndAngleH >= laserEndAngleH)
copyType = 2;

Debug.LogWarningFormat("Error occured with Buffer.BlockCopy: {0}, Type: {1} Offset: src({2}) dst({3}) Len: src({4}) dst({5}) copy_size({6})",
ex.Message, copyType, srcBufferOffset, dstBufferOffset, srcBuffer.Length, laserScan.Ranges.Length, copyLength);
}
}
else
Expand All @@ -511,7 +498,8 @@ protected override void GenerateMessage()
laserFilter.DoFilter(ref laserScan);
}

DeviceHelper.SetCurrentTime(laserScanStamped.Time);
DeviceHelper.SetTime(laserScanStamped.Time, capturedTime);

PushDeviceMessage<messages.LaserScanStamped>(laserScanStamped);
}

Expand Down
1 change: 0 additions & 1 deletion Assets/Scripts/Devices/MicomCommand.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,6 @@ private void DoWheelDrive(in Vector3 linearVelocity, in Vector3 angularVelocity)
var targetLinearVelocity = linearVelocity.z;
var targetAngularVelocity = angularVelocity.y;
motorControl.SetTwistDrive(targetLinearVelocity, targetAngularVelocity);
motorControl.UpdateTargetMotorFeedback(targetAngularVelocity);
}
}
}
Loading

0 comments on commit 1d9edea

Please sign in to comment.