Skip to content

Commit

Permalink
CLOiSim-2.0.1
Browse files Browse the repository at this point in the history 
[Major Changes]

* Update app version info in Project settings
  - 2.0.0 -> 2.0.1

* Upgrade Unity editor version
  - 2020.2.7f1

* Update package version
  - Core RP Library: 10.3.1 -> 10.3.2

* Apply pose of Joint(SDF) when apply anchor position for articulation body


* Update Dynamic parameters for physics
  - solver Iterations: 10
  - solver velocity iteration: 5

* Changes in Motor module
  - change to retrieving motor velocity using jointPosition
  - remove getting averaged joint velocity

* (GUI) Print error meessage on simulation display during SDF parsing


[Minor Changes]

* Add protection code in Lidar Data generation

* Parameter name changed in ImportJoint Method

* refactoring for SDF sensor ray range element

* Change GUI properties


[Bug fix]

* Bug fix in Lidar Sensor device
  - Set sensor position and rotation for lidar
    - fix rotation for lidar  visualization

* Fix in Motor module
  - set false in rapid direction control after stop

* Bug Fix in Lidar Sensor device
  - calculate angle step
  - deg to rad for vertical angle
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Mar 9, 2021
1 parent 2689708 commit 8b9b617
Show file tree
Hide file tree
Showing 19 changed files with 246 additions and 124 deletions.
91 changes: 69 additions & 22 deletions Assets/Scenes/MainScene.unity
View file
Original file line number Diff line number Diff line change
Expand Up @@ -218,8 +218,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isSelfCollide: 0
drawInertia: 0
--- !u!1 &25597963
Expand Down Expand Up @@ -316,8 +316,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isSelfCollide: 0
drawInertia: 0
--- !u!1 &34803206
Expand Down Expand Up @@ -827,8 +827,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isTopModel: 1
isStatic: 0
--- !u!1 &249819585
Expand Down Expand Up @@ -1297,8 +1297,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isSelfCollide: 0
drawInertia: 0
--- !u!135 &402915479
Expand Down Expand Up @@ -1961,8 +1961,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isSelfCollide: 0
drawInertia: 0
--- !u!171741748 &635764732
Expand Down Expand Up @@ -2548,8 +2548,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isTopModel: 1
isStatic: 0
--- !u!1 &779420288
Expand Down Expand Up @@ -2617,6 +2617,53 @@ GameObject:
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &789896500
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 789896499}
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
m_LocalPosition: {x: 0, y: 0, z: 0}
m_LocalScale: {x: 0.5, y: 0.1, z: 0.5}
m_Children: []
m_Father: {fileID: 25597964}
m_RootOrder: 0
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!64 &779420291
MeshCollider:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 779420288}
m_Material: {fileID: 13400000, guid: 9cf0822921b89b82fb8239bf69165329, type: 2}
m_IsTrigger: 0
m_Enabled: 1
serializedVersion: 4
m_Convex: 1
m_CookingOptions: 20
m_Mesh: {fileID: 10206, guid: 0000000000000000e000000000000000, type: 0}
--- !u!1 &789896499
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 789896500}
- component: {fileID: 789896503}
- component: {fileID: 789896502}
- component: {fileID: 789896501}
m_Layer: 0
m_Name: GameObject
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &789896500
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
Expand Down Expand Up @@ -2932,8 +2979,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isSelfCollide: 0
drawInertia: 0
--- !u!1 &887438904
Expand Down Expand Up @@ -3232,8 +3279,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 1c0ad0e8e55c95c4e931911c4b943a64, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
space: 1
transformType: 0
pivot: 0
Expand Down Expand Up @@ -5451,8 +5498,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isTopModel: 0
isStatic: 0
--- !u!1 &1750713662
Expand Down Expand Up @@ -5816,8 +5863,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isSelfCollide: 0
drawInertia: 0
--- !u!1 &1835497263
Expand Down Expand Up @@ -6297,8 +6344,8 @@ MonoBehaviour:
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3}
m_Name:
m_EditorClassIdentifier:
m_Name:
m_EditorClassIdentifier:
isTopModel: 1
isStatic: 0
--- !u!1 &2133470735
Expand Down
3 changes: 3 additions & 0 deletions Assets/Scripts/Devices/Lidar.LaserCameraData.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,10 @@ public void AllocateBuffer(in int width, in int height)
{
imageWidth = width;
imageHeight = height;

output = new double[imageWidth];
// TODO: vertical ray
// output = new double[imageWidth * imageHeight];
}

public void SetBufferData(in NativeArray<byte> buffer)
Expand Down
39 changes: 28 additions & 11 deletions Assets/Scripts/Devices/Lidar.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ public partial class Lidar : Device
public Color rayColor = new Color(1, 0.1f, 0.1f, 0.15f);

private Transform lidarLink = null;
private Pose lidarSensorInitPose = new Pose();
private ShadowQuality originalShadowSettings_;
private UnityEngine.Camera laserCam = null;
private Material depthMaterial = null;
Expand Down Expand Up @@ -87,10 +88,9 @@ private void OnPostRender()
QualitySettings.shadows = originalShadowSettings_;
}


private double GetAngleStep(in double minAngle, in double maxAngle, in uint totalSamples)
{
return (maxAngle - minAngle) / (resolution * (totalSamples - 1));
return (maxAngle - minAngle) / (resolution * totalSamples);
}

protected override void OnAwake()
Expand All @@ -107,6 +107,9 @@ protected override void OnStart()
{
if (laserCam)
{
lidarSensorInitPose.position = transform.localPosition;
lidarSensorInitPose.rotation = transform.localRotation;

DoParseFilter();

SetupLaserCamera();
Expand All @@ -131,16 +134,15 @@ protected override void InitializeMessages()
laserScan.AngleMin = angleMin * Mathf.Deg2Rad;
laserScan.AngleMax = angleMax * Mathf.Deg2Rad;
laserScan.AngleStep = GetAngleStep(laserScan.AngleMin, laserScan.AngleMax, samples);
laserScan.Count = samples;

laserScan.RangeMin = rangeMin;
laserScan.RangeMax = rangeMax;
laserScan.Count = samples;

laserScan.VerticalAngleMin = verticalAngleMin * Mathf.Deg2Rad;
laserScan.VerticalAngleMax = verticalAngleMax * Mathf.Deg2Rad;
laserScan.VerticalAngleStep = (verticalAngleMin == 0 && verticalAngleMax == 0) ? 1 : GetAngleStep(laserScan.VerticalAngleMin, laserScan.VerticalAngleMax, verticalSamples);
laserScan.VerticalCount = verticalSamples;
laserScan.VerticalAngleStep
= (verticalAngleMin == 0 && verticalAngleMax == 0) ?
1 : GetAngleStep(laserScan.VerticalAngleMin, laserScan.VerticalAngleMax, verticalSamples);

laserScan.Ranges = new double[samples];
laserScan.Intensities = new double[samples];
Expand Down Expand Up @@ -236,7 +238,7 @@ private IEnumerator LaserCameraWorker()
var data = laserCamData[dataIndex];
axisRotation.y = data.centerAngle;

laserCam.transform.localRotation = Quaternion.Euler(axisRotation);
laserCam.transform.localRotation = lidarSensorInitPose.rotation * Quaternion.Euler(axisRotation);


laserCam.enabled = true;
Expand Down Expand Up @@ -296,8 +298,8 @@ protected override IEnumerator MainDeviceWorker()

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

var laserScan = laserScanStamped.Scan;

Expand Down Expand Up @@ -331,13 +333,23 @@ protected override void GenerateMessage()
var srcLengthratio = Mathf.Abs((dataStartAngle - laserStartAngle) / dataTotalAngle);
copyLength = outputBufferLength - Mathf.FloorToInt(outputBufferLength * srcLengthratio);
dstBufferOffset = (int)samples - copyLength;

if (copyLength < 0 || dstBufferOffset < 0)
{
doCopy = false;
}
}
// middle of laser angle
else if (dataStartAngle >= laserStartAngle && dataEndAngle < laserEndAngle)
{
srcBufferOffset = 0;
copyLength = outputBufferLength;
dstBufferOffset = (int)samples - (Mathf.CeilToInt(samples * ((dataStartAngle - laserStartAngle) / laserTotalAngle)) + copyLength);

if (copyLength < 0 || dstBufferOffset < 0)
{
doCopy = false;
}
}
// end side of laser angle
else if (dataEndAngle >= laserEndAngle)
Expand All @@ -346,6 +358,11 @@ protected override void GenerateMessage()
copyLength = Mathf.CeilToInt(outputBufferLength * srcLengthRatio);
srcBufferOffset = outputBufferLength - copyLength;
dstBufferOffset = 0;

if (copyLength < 0 || srcBufferOffset < 0)
{
doCopy = false;
}
}
else
{
Expand Down Expand Up @@ -379,13 +396,13 @@ protected override IEnumerator OnVisualize()
{
yield return waitForEndOfFrame;

var lidarSensorWorldPosition = lidarLink.position + transform.localPosition;
var lidarSensorWorldPosition = lidarLink.position + lidarSensorInitPose.position;
var rangeData = GetRangeData();

for (var hScanIndex = 0; hScanIndex < rangeData.Length; hScanIndex++)
{
var rayAngleH = ((laserHAngleResolution * hScanIndex)) + startAngle;
var rayRotation = Quaternion.AngleAxis((float)(rayAngleH), lidarLink.up) * lidarLink.forward;
var rayRotation = (Quaternion.AngleAxis((float)rayAngleH, transform.up)) * lidarLink.forward;
var rayStart = (rayRotation * (float)rangeMin) + lidarSensorWorldPosition;

var ccwIndex = (uint)(rangeData.Length - hScanIndex - 1);
Expand Down
Loading

0 comments on commit 8b9b617

Please sign in to comment.