Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.2.3 (#268)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Dec 26, 2023
2 parents 1d9edea + 56142a4 commit 1ea795c
Show file tree
Hide file tree
Showing 35 changed files with 819 additions and 513 deletions.
12 changes: 9 additions & 3 deletions Assets/Scenes/MainScene.unity
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ RenderSettings:
m_ReflectionIntensity: 1
m_CustomReflection: {fileID: 0}
m_Sun: {fileID: 0}
m_IndirectSpecularColor: {r: 0.17276844, g: 0.21589246, b: 0.2978263, a: 1}
m_IndirectSpecularColor: {r: 0.17276844, g: 0.2158925, b: 0.29782635, a: 1}
m_UseRadianceAmbientProbe: 0
--- !u!157 &3
LightmapSettings:
Expand Down Expand Up @@ -237,6 +237,7 @@ MonoBehaviour:
m_Name:
m_EditorClassIdentifier:
drawInertia: 0
drawContact: 1
isSelfCollide: 0
useGravity: 0
--- !u!1 &20107706
Expand Down Expand Up @@ -444,6 +445,7 @@ MonoBehaviour:
m_Name:
m_EditorClassIdentifier:
drawInertia: 0
drawContact: 1
isSelfCollide: 0
useGravity: 0
--- !u!1 &34803206
Expand Down Expand Up @@ -2790,6 +2792,7 @@ MonoBehaviour:
m_Name:
m_EditorClassIdentifier:
drawInertia: 0
drawContact: 1
isSelfCollide: 0
useGravity: 0
--- !u!135 &402915479
Expand Down Expand Up @@ -3824,6 +3827,7 @@ MonoBehaviour:
m_Name:
m_EditorClassIdentifier:
drawInertia: 0
drawContact: 1
isSelfCollide: 0
useGravity: 0
--- !u!171741748 &635764732
Expand Down Expand Up @@ -4424,10 +4428,10 @@ MonoBehaviour:
m_Horizontal: 0
m_Vertical: 1
m_MovementType: 1
m_Elasticity: 0.1
m_Elasticity: 0.2
m_Inertia: 1
m_DecelerationRate: 0.135
m_ScrollSensitivity: 50
m_ScrollSensitivity: 200
m_Viewport: {fileID: 20107707}
m_HorizontalScrollbar: {fileID: 0}
m_VerticalScrollbar: {fileID: 970763218}
Expand Down Expand Up @@ -4749,6 +4753,7 @@ MonoBehaviour:
m_Name:
m_EditorClassIdentifier:
drawInertia: 0
drawContact: 1
isSelfCollide: 0
useGravity: 0
--- !u!1 &887438904
Expand Down Expand Up @@ -7880,6 +7885,7 @@ MonoBehaviour:
m_Name:
m_EditorClassIdentifier:
drawInertia: 0
drawContact: 1
isSelfCollide: 0
useGravity: 0
--- !u!1 &1829705135
Expand Down
102 changes: 67 additions & 35 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,23 +11,26 @@

public class MicomPlugin : CLOiSimPlugin
{
private List<TF> tfList = new List<TF>();
private SensorDevices.MicomCommand micomCommand = null;
private SensorDevices.MicomSensor micomSensor = null;
private List<TF> _tfList = new List<TF>();
private SensorDevices.MicomCommand _micomCommand = null;
private SensorDevices.MicomSensor _micomSensor = null;
private SDF.Helper.Link[] _linkHelperInChildren = null;

protected override void OnAwake()
{
type = ICLOiSimPlugin.Type.MICOM;
micomSensor = gameObject.AddComponent<SensorDevices.MicomSensor>();
micomCommand = gameObject.AddComponent<SensorDevices.MicomCommand>();
micomCommand.SetMotorControl(micomSensor.MotorControl);
_micomSensor = gameObject.AddComponent<SensorDevices.MicomSensor>();
_micomCommand = gameObject.AddComponent<SensorDevices.MicomCommand>();
_micomCommand.SetMotorControl(_micomSensor.MotorControl);

attachedDevices.Add("Command", micomCommand);
attachedDevices.Add("Sensor", micomSensor);
attachedDevices.Add("Command", _micomCommand);
attachedDevices.Add("Sensor", _micomSensor);
}

protected override void OnStart()
{
_linkHelperInChildren = GetComponentsInChildren<SDF.Helper.Link>();

SetupMicom();

if (RegisterServiceDevice(out var portService, "Info"))
Expand All @@ -37,17 +40,17 @@ protected override void OnStart()

if (RegisterRxDevice(out var portRx, "Rx"))
{
AddThread(portRx, ReceiverThread, micomCommand);
AddThread(portRx, ReceiverThread, _micomCommand);
}

if (RegisterTxDevice(out var portTx, "Tx"))
{
AddThread(portTx, SenderThread, micomSensor);
AddThread(portTx, SenderThread, _micomSensor);
}

if (RegisterTxDevice(out var portTf, "Tf"))
{
AddThread(portTf, PublishTfThread, tfList);
AddThread(portTf, PublishTfThread, _tfList);
}

LoadStaticTF();
Expand All @@ -56,29 +59,30 @@ protected override void OnStart()

private void SetupMicom()
{
micomSensor.EnableDebugging = GetPluginParameters().GetValue<bool>("debug", false);
_micomSensor.EnableDebugging = GetPluginParameters().GetValue<bool>("debug", false);

var updateRate = GetPluginParameters().GetValue<float>("update_rate", 20f);
if (updateRate.Equals(0))
{
Debug.LogWarning("Update rate for micom CANNOT be 0. Set to default value 20 Hz");
updateRate = 20f;
}
micomSensor.SetUpdateRate(updateRate);
_micomSensor.SetUpdateRate(updateRate);

var wheelRadius = GetPluginParameters().GetValue<float>("wheel/radius");
var wheelTread = GetPluginParameters().GetValue<float>("wheel/tread"); // deprecated
if (Mathf.Approximately(Mathf.Abs(wheelTread), Quaternion.kEpsilon) == false)

if (GetPluginParameters().IsValidNode("wheel/tread"))
{
Debug.LogWarning("<wheel/tread> will be depreacted!!");
Debug.LogWarning("<wheel/tread> will be depreacted!! please use wheel/separation");
}

var wheelTread = GetPluginParameters().GetValue<float>("wheel/tread"); // TODO: to be deprecated
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, wheelSeparation, 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 All @@ -91,49 +95,77 @@ private void SetupMicom()

if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty))
{
micomSensor.SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName);
_micomSensor.SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName);
}
else
{
micomSensor.SetWheel(wheelLeftName, wheelRightName);
_micomSensor.SetWheel(wheelLeftName, wheelRightName);
}

if (GetPluginParameters().IsValidNode("battery"))
{
SetBattery();
}

if (GetPluginParameters().GetValues<string>("uss/sensor", out var ussList))
{
micomSensor.SetUSS(ussList);
_micomSensor.SetUSS(ussList);
}

if (GetPluginParameters().GetValues<string>("ir/sensor", out var irList))
{
micomSensor.SetIRSensor(irList);
_micomSensor.SetIRSensor(irList);
}

if (GetPluginParameters().GetValues<string>("magnet/sensor", out var magnetList))
{
micomSensor.SetMagnet(magnetList);
_micomSensor.SetMagnet(magnetList);
}

var targetContactName = GetPluginParameters().GetAttribute<string>("bumper", "contact");
micomSensor.SetBumper(targetContactName);

if (GetPluginParameters().GetValues<string>("bumper/joint_name", out var bumperJointNameList))
if (GetPluginParameters().GetValues<string>("bumper/sensor", out var bumperList))
{
micomSensor.SetBumperSensor(bumperJointNameList);
_micomSensor.SetBumper(bumperList);
}

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

private void SetBattery()
{
if (GetPluginParameters().GetValues<string>("battery/voltage", out var batteryList))
{
foreach (var item in batteryList)
{
var batteryName = GetPluginParameters().GetAttributeInPath<string>("battery/voltage", "name");
var consumption = GetPluginParameters().GetValue<float>("battery/voltage[@name='" + batteryName + "']/consumption");

foreach (var linkHelper in _linkHelperInChildren)
{
var targetBattery = linkHelper.Battery;
if (targetBattery != null)
{
if (targetBattery.Name.CompareTo(batteryName) == 0)
{
// Debug.Log("Battery: " + batteryName + ", Battery Consumer:" + consumption.ToString("F5"));
targetBattery.Discharge(consumption);
_micomSensor.SetBattery(targetBattery);
break;
}
}
}
}
}
}

private void LoadStaticTF()
{
var staticTfLog = new StringBuilder();
staticTfLog.AppendLine("Loaded Static TF Info : " + modelName);
var linkHelpers = GetComponentsInChildren<SDF.Helper.Link>();

if (GetPluginParameters().GetValues<string>("ros2/static_transforms/link", out var staticLinks))
{
Expand All @@ -143,7 +175,7 @@ private void LoadStaticTF()

(var modelName, var linkName) = SDF2Unity.GetModelLinkName(link);

foreach (var linkHelper in linkHelpers)
foreach (var linkHelper in _linkHelperInChildren)
{
if ((string.IsNullOrEmpty(modelName) || (!string.IsNullOrEmpty(modelName) &&
linkHelper.Model.name.Equals(modelName))) &&
Expand All @@ -165,7 +197,7 @@ private void LoadTF()
{
var tfLog = new StringBuilder();
tfLog.AppendLine("Loaded TF Info : " + modelName);
var linkHelpers = GetComponentsInChildren<SDF.Helper.Link>();

if (GetPluginParameters().GetValues<string>("ros2/transforms/link", out var links))
{
foreach (var link in links)
Expand All @@ -174,13 +206,13 @@ private void LoadTF()

(var modelName, var linkName) = SDF2Unity.GetModelLinkName(link);

foreach (var linkHelper in linkHelpers)
foreach (var linkHelper in _linkHelperInChildren)
{
if ((string.IsNullOrEmpty(modelName) || (!string.IsNullOrEmpty(modelName) && linkHelper.Model.name.Equals(modelName))) &&
linkHelper.name.Equals(linkName))
{
var tf = new TF(linkHelper, link, parentFrameId);
tfList.Add(tf);
_tfList.Add(tf);
tfLog.AppendLine(modelName + "::" + linkName + " : TF added");
break;
}
Expand Down
29 changes: 12 additions & 17 deletions Assets/Scripts/Core/ObjectSpawning.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public enum PropsType { BOX = 0, CYLINDER = 1, SPHERE = 2 };
private RuntimeGizmos.TransformGizmo transformGizmo = null;
private FollowingTargetList followingList = null;

private Quaternion _cylinderRotationAngle = Quaternion.AngleAxis(90, Vector3.forward);
private const float CylinderRotationAngle = 90;

private Dictionary<PropsType, GameObject> props = new Dictionary<PropsType, GameObject>();
public float maxRayDistance = 100;
Expand All @@ -24,6 +24,8 @@ public enum PropsType { BOX = 0, CYLINDER = 1, SPHERE = 2 };
private string scaleFactorString = "0.5";
private int propType = 0;

private const float UnitMass = 3f;

public void SetScaleFactor(in string value)
{
scaleFactorString = value;
Expand Down Expand Up @@ -66,12 +68,9 @@ void LateUpdate()
// Add On left click spawn
// selected prefab and align its rotation to a surface normal
var spawnData = GetPositionAndNormalOnClick();
if (spawnData[0] != Vector3.zero)
{
var scaleFactor = float.Parse(scaleFactorString);
var propsScale = Vector3.one * scaleFactor;
StartCoroutine(SpawnTargetObject((PropsType)propType, spawnData[0], spawnData[1], propsScale));
}
var scaleFactor = float.Parse(scaleFactorString);
var propsScale = Vector3.one * scaleFactor;
StartCoroutine(SpawnTargetObject((PropsType)propType, spawnData[0], spawnData[1], propsScale));
}
else if (leftControlPressed && Input.GetMouseButtonDown(1))
{
Expand Down Expand Up @@ -105,12 +104,7 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3
break;

case PropsType.CYLINDER:
mesh = ProceduralMesh.CreateCylinder(0.5f, 1, 20);

for (var index = 0; index < mesh.vertices.LongLength; index++)
{
mesh.vertices[index] = _cylinderRotationAngle * mesh.vertices[index];
}
mesh = ProceduralMesh.CreateCylinder(0.5f, 1, 20, CylinderRotationAngle);
break;

case PropsType.SPHERE:
Expand Down Expand Up @@ -146,7 +140,8 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3

if (mesh != null)
{
position.y += mesh.bounds.extents.y + 0.001f;
const float SpawningMargin = 0.001f;
position.y += mesh.bounds.max.y + SpawningMargin;
}

var spawanedObjectTransform = spawnedObject.transform;
Expand All @@ -161,7 +156,7 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3

private float CalculateMass(in Vector3 scale)
{
return (scale.x + scale.y + scale.z) / 3;
return (scale.x + scale.y + scale.z) / 3 * UnitMass;
}

private GameObject CreateProps(in string name, in Mesh targetMesh, in Vector3 scale)
Expand Down Expand Up @@ -190,8 +185,8 @@ private GameObject CreateProps(in string name, in Mesh targetMesh, in Vector3 sc

var rigidBody = newObject.AddComponent<Rigidbody>();
rigidBody.mass = 1;
rigidBody.drag = 2.5f;
rigidBody.angularDrag = 0.1f;
rigidBody.drag = 0.25f;
rigidBody.angularDrag = 1f;

var navMeshObstacle = newObject.AddComponent<NavMeshObstacle>();
navMeshObstacle.carving = true;
Expand Down
Loading

0 comments on commit 1ea795c

Please sign in to comment.