Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.9.3 (#327)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Dec 3, 2024
2 parents ab8a790 + 187e787 commit 88dc41a
Show file tree
Hide file tree
Showing 6 changed files with 122 additions and 84 deletions.
2 changes: 1 addition & 1 deletion Assets/Scripts/CLOiSimPlugins/Messages/micom.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ public partial class Micom : global::ProtoBuf.IExtensible
[global::ProtoBuf.ProtoMember(1, Name = @"time", IsRequired = true)]
public Time Time { get; set; }

[global::ProtoBuf.ProtoMember(2, Name = @"odom", IsRequired = true)]
[global::ProtoBuf.ProtoMember(2, Name = @"odom")]
public Odometry Odom { get; set; }

[global::ProtoBuf.ProtoMember(3)]
Expand Down
17 changes: 14 additions & 3 deletions Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,12 +26,13 @@ protected void PublishTfThread(System.Object threadObject)
tfMessage.Transform.Orientation = new messages.Quaternion();

var deviceMessage = new DeviceMessage();
if (publisher != null && tfList.Count > 0)
if (publisher != null)
{
const int EmptyTfPublishPeriod = 2000;
const float publishFrequency = 50;
const int updatePeriod = (int)(1f / publishFrequency * 1000f);
int updatePeriodPerEachTf = (int)(updatePeriod / tfList.Count);
// Debug.Log(updatePeriod + " , " + updatePeriodPerEachTf);
var updatePeriodPerEachTf = (tfList.Count == 0) ? int.MaxValue : (int)(updatePeriod / tfList.Count);
// Debug.Log("PublishTfThread: " + updatePeriod + " , " + updatePeriodPerEachTf);

while (PluginThread.IsRunning)
{
Expand All @@ -58,6 +59,16 @@ protected void PublishTfThread(System.Object threadObject)
}
CLOiSimPluginThread.Sleep(updatePeriodPerEachTf);
}

if (tfList.Count == 0)
{
deviceMessage.SetMessage<messages.TransformStamped>(tfMessage);
if (publisher.Publish(deviceMessage) == false)
{
Debug.Log(tfMessage.Header.StrId + ", " + tfMessage.Transform.Name + " error to send TF!!");
}
CLOiSimPluginThread.Sleep(EmptyTfPublishPeriod);
}
}
}
}
Expand Down
89 changes: 54 additions & 35 deletions Assets/Scripts/Devices/MicomSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ public partial class MicomSensor : Device
private MotorControl _motorControl = null;
private SensorDevices.Battery battery = null;
private SensorDevices.IMU _imuSensor = null;
private List<SensorDevices.Sonar> ussSensors = new List<SensorDevices.Sonar>();
private List<SensorDevices.Sonar> irSensors = new List<SensorDevices.Sonar>();
private List<SensorDevices.Sonar> _ussSensors = new List<SensorDevices.Sonar>();
private List<SensorDevices.Sonar> _irSensors = new List<SensorDevices.Sonar>();
// private List<SensorDevices.Magnet> magnetSensors = null;
private List<SensorDevices.Contact> _bumperSensors = new List<SensorDevices.Contact>();

Expand Down Expand Up @@ -48,10 +48,11 @@ public void SetIMU(in string sensorName)
var imuList = gameObject.GetComponentsInChildren<SensorDevices.IMU>();
foreach (var imu in imuList)
{
if (imu.DeviceName.Contains("::" + sensorName + "::") ||
if (imu.DeviceName.Contains($"::{sensorName}::") || // Model name
imu.DeviceName.EndsWith($"::{sensorName}") || // Link name
imu.name.Equals(sensorName))
{
Debug.Log(imu.DeviceName + " attached to Micom");
Debug.Log($"IMU: {imu.DeviceName} attached to Micom");
_imuSensor = imu;
break;
}
Expand All @@ -60,38 +61,46 @@ public void SetIMU(in string sensorName)

public void SetUSS(in List<string> ussList)
{
var modelList = GetComponentsInChildren<SDF.Helper.Model>();
foreach (var uss in ussList)
var sonarList = GetComponentsInChildren<SensorDevices.Sonar>();

foreach (var ussName in ussList)
{
foreach (var model in modelList)
foreach (var sonar in sonarList)
{
if (model.name.Equals(uss))
if (sonar.DeviceName.Contains($"::{ussName}::") || // Model name
sonar.DeviceName.EndsWith($"::{ussName}") || // Link name
sonar.name.Equals(ussName))
{
var sonarSensor = model.GetComponentInChildren<SensorDevices.Sonar>();
ussSensors.Add(sonarSensor);
// Debug.Log("ussSensor found : " + sonarSensor.name);
Debug.Log($"USS: {sonar.DeviceName} attached to Micom");
_ussSensors.Add(sonar);
break;
}
}
}
micomSensorData.uss.Distances = new double[ussList.Count];

micomSensorData.uss.Distances = new double[_ussSensors.Count];
}

public void SetIRSensor(in List<string> irList)
{
var modelList = GetComponentsInChildren<SDF.Helper.Model>();
foreach (var ir in irList)
var sonarList = GetComponentsInChildren<SensorDevices.Sonar>();

foreach (var irName in irList)
{
foreach (var model in modelList)
foreach (var sonar in sonarList)
{
if (model.name.Equals(ir))
if (sonar.DeviceName.Contains($"::{irName}::") || // Model name
sonar.DeviceName.EndsWith($"::{irName}") || // Link name
sonar.name.Equals(irName))
{
var sonarSensor = model.GetComponentInChildren<SensorDevices.Sonar>();
irSensors.Add(sonarSensor);
// Debug.Log("irSensor found : " + sonarSensor.name);
Debug.Log($"IR: {sonar.DeviceName} attached to Micom");
_irSensors.Add(sonar);
break;
}
}
}
micomSensorData.ir.Distances = new double[irList.Count];

micomSensorData.ir.Distances = new double[_irSensors.Count];
}

public void SetMagnet(in List<string> magnetList)
Expand All @@ -106,24 +115,24 @@ public void SetMagnet(in List<string> magnetList)

public void SetBumper(in List<string> bumperList)
{
var contactsInChild = GetComponentsInChildren<SensorDevices.Contact>();
var contactList = GetComponentsInChildren<SensorDevices.Contact>();

var bumperCount = 0;
foreach (var bumper in bumperList)
foreach (var bumperName in bumperList)
{
foreach (var contact in contactsInChild)
foreach (var contact in contactList)
{
if (contact.name.Equals(bumper))
if (contact.DeviceName.Contains($"::{bumperName}::") || // Model name
contact.DeviceName.EndsWith($"::{bumperName}") || // Link name
contact.name.Equals(bumperName))
{
Debug.Log($"Bumper: {contact.DeviceName} attached to Micom");
_bumperSensors.Add(contact);
bumperCount++;
Debug.Log("Found " + contact.name);
break;
}
}
}

micomSensorData.bumper.Bumpeds = new bool[bumperCount];
micomSensorData.bumper.Bumpeds = new bool[_bumperSensors.Count];
}

public void SetBattery(in SensorDevices.Battery targetBattery)
Expand All @@ -146,15 +155,20 @@ protected override void InitializeMessages()
{
micomSensorData = new messages.Micom();
micomSensorData.Time = new messages.Time();
micomSensorData.Odom = null;
micomSensorData.uss = new messages.Micom.Uss();
micomSensorData.ir = new messages.Micom.Ir();
micomSensorData.bumper = new messages.Micom.Bumper();
}

private void InitializeOdometryMessage()
{
micomSensorData.Odom = new messages.Micom.Odometry();
micomSensorData.Odom.AngularVelocity = new messages.Micom.Odometry.Wheel();
micomSensorData.Odom.LinearVelocity = new messages.Micom.Odometry.Wheel();
micomSensorData.Odom.Pose = new messages.Vector3d();
micomSensorData.Odom.TwistLinear = new messages.Vector3d();
micomSensorData.Odom.TwistAngular = new messages.Vector3d();
micomSensorData.uss = new messages.Micom.Uss();
micomSensorData.ir = new messages.Micom.Ir();
micomSensorData.bumper = new messages.Micom.Bumper();
}

protected override void GenerateMessage()
Expand All @@ -179,6 +193,11 @@ void FixedUpdate()

if (_motorControl != null)
{
if (micomSensorData.Odom == null)
{
InitializeOdometryMessage();
}

if (_motorControl.Update(micomSensorData.Odom, deltaTime, _imuSensor) == false)
{
Debug.LogWarning("Update failed in MotorControl");
Expand Down Expand Up @@ -215,9 +234,9 @@ private void UpdateUss()
return;
}

for (var index = 0; index < ussSensors.Count; index++)
for (var index = 0; index < _ussSensors.Count; index++)
{
micomSensorData.uss.Distances[index] = ussSensors[index].GetDetectedRange();
micomSensorData.uss.Distances[index] = _ussSensors[index].GetDetectedRange();
}
}

Expand All @@ -228,9 +247,9 @@ private void UpdateIr()
return;
}

for (var index = 0; index < irSensors.Count; index++)
for (var index = 0; index < _irSensors.Count; index++)
{
micomSensorData.ir.Distances[index] = irSensors[index].GetDetectedRange();
micomSensorData.ir.Distances[index] = _irSensors[index].GetDetectedRange();
}
}

Expand Down
4 changes: 4 additions & 0 deletions Assets/Scripts/Devices/Modules/Base/Device.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,11 +20,15 @@ public enum ModeType { NONE, TX, RX, TX_THREAD, RX_THREAD };

private SDF.Plugin pluginParameters = null;

[SerializeField]
private string deviceName = string.Empty;

[SerializeField]
private float _updateRate = 1;

private bool debuggingOn = true;

[SerializeField]
private bool visualize = true;

private float transportingTimeSeconds = 0;
Expand Down
Loading

0 comments on commit 88dc41a

Please sign in to comment.