protected override void OnStart()
{
var updateRate = GetPluginParameters().GetValue <float>("update_rate", 20);
SetUpdateRate(updateRate);
var kp = GetPluginParameters().GetValue <float>("PID/kp");
var ki = GetPluginParameters().GetValue <float>("PID/ki");
var kd = GetPluginParameters().GetValue <float>("PID/kd");
var pidControl = new PID(kp, ki, kd);
wheelBase = GetPluginParameters().GetValue <float>("wheel/base");
wheelRadius = GetPluginParameters().GetValue <float>("wheel/radius");
divideWheelRadius = 1.0f / wheelRadius;
var wheelNameLeft = GetPluginParameters().GetValue <string>("wheel/location[@type='left']");
var wheelNameRight = GetPluginParameters().GetValue <string>("wheel/location[@type='right']");
var motorFriction = GetPluginParameters().GetValue <float>("wheel/friction/motor", 0.1f); // Currently not used
var brakeFriction = GetPluginParameters().GetValue <float>("wheel/friction/brake", 0.1f); // Currently not used
var modelList = GetComponentsInChildren <ModelPlugin>();
foreach (var model in modelList)
{
// Debug.Log(model.name);
if (model.name.Equals(wheelNameLeft))
{
var jointWheelLeft = model.GetComponentInChildren <HingeJoint>();
motorLeft = new Motor("Left", jointWheelLeft, pidControl);
// var wheelLeftBody = jointWheelLeft.gameObject.GetComponent<Rigidbody>();
var wheelLeftTransform = jointWheelLeft.gameObject.transform.parent;
var wheelLeftPose = new Pose(wheelLeftTransform.localPosition, wheelLeftTransform.localRotation);
partsPoseMapTable.Add(wheelNameLeft, wheelLeftPose);
// Debug.LogFormat("joint Wheel({0}) Left max angular velocity({1})", jointWheelLeft.name, jointWheelLeft.gameObject.GetComponent<Rigidbody>().maxAngularVelocity);
}
else if (model.name.Equals(wheelNameRight))
{
var jointWheelRight = model.GetComponentInChildren <HingeJoint>();
motorRight = new Motor("Right", jointWheelRight, pidControl);
var wheelRightTransform = jointWheelRight.gameObject.transform.parent;
var wheelRightPose = new Pose(wheelRightTransform.localPosition, wheelRightTransform.localRotation);
partsPoseMapTable.Add(wheelNameRight, wheelRightPose);
// Debug.LogFormat("joint Wheel({0}) Left max angular velocity({1})", jointWheelRight.name, jointWheelRight.gameObject.GetComponent<Rigidbody>().maxAngularVelocity);
}
if (motorLeft != null && motorRight != null)
{
break;
}
}
if (GetPluginParameters().GetValues <string>("uss/sensor", out var ussList))
{
foreach (var uss in ussList)
{
foreach (var model in modelList)
{
if (model.name.Equals(uss))
{
var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
ussSensors.Add(sonarSensor);
// Debug.Log("ussSensor found : " + sonarSensor.name);
}
}
}
micomSensorData.uss.Distances = new double[ussList.Count];
}
if (GetPluginParameters().GetValues <string>("ir/sensor", out var irList))
{
foreach (var ir in irList)
{
foreach (var model in modelList)
{
if (model.name.Equals(ir))
{
var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
irSensors.Add(sonarSensor);
// Debug.Log("irSensor found : " + sonarSensor.name);
}
}
}
micomSensorData.ir.Distances = new double[irList.Count];
}
if (GetPluginParameters().GetValues <string>("magnet/sensor", out var magnetList))
{
foreach (var model in modelList)
{
// TODO: to be implemented
}
}
var targetContactName = GetPluginParameters().GetAttribute <string>("bumper", "contact");
// Debug.Log(targetContactName);
var contactsInChild = GetComponentsInChildren <SensorDevices.Contact>();
foreach (var contact in contactsInChild)
{
if (contact.name.Equals(targetContactName))
{
bumperContact = contact;
// Debug.Log("Found");
}
}
if (bumperContact != null)
{
if (GetPluginParameters().GetValues <string>("bumper/joint_name", out var bumperJointNameList))
{
var linkList = GetComponentsInChildren <LinkPlugin>();
foreach (var link in linkList)
{
foreach (var bumperJointName in bumperJointNameList)
{
if (link.jointList.TryGetValue(bumperJointName, out var jointValue))
{
bumperSensors.Add(jointValue as ConfigurableJoint);
Debug.Log(bumperJointName);
}
}
}
}
var bumperCount = (bumperSensors == null || bumperSensors.Count == 0) ? 1 : bumperSensors.Count;
micomSensorData.bumper.Bumpeds = new bool[bumperCount];
}
imuSensor = gameObject.GetComponentInChildren <SensorDevices.IMU>();
if (imuSensor != null)
{
var imuSensorModelTransform = imuSensor.transform.parent;
var newPose = new Pose(imuSensorModelTransform.localPosition, imuSensorModelTransform.localRotation);
partsPoseMapTable.Add(imuSensor.name, newPose);
}
}
protected override void OnStart()
{
var updateRate = GetPluginParameters().GetValue <float>("update_rate", 20);
SetUpdateRate(updateRate);
_PGain = GetPluginParameters().GetValue <float>("PID/kp");
_IGain = GetPluginParameters().GetValue <float>("PID/ki");
_DGain = GetPluginParameters().GetValue <float>("PID/kd");
wheelBase = GetPluginParameters().GetValue <float>("wheel/base");
wheelRadius = GetPluginParameters().GetValue <float>("wheel/radius");
divideWheelRadius = 1.0f / wheelRadius;
_wheelNameLeft = GetPluginParameters().GetValue <string>("wheel/location[@type='left']");
_wheelNameRight = GetPluginParameters().GetValue <string>("wheel/location[@type='right']");
var motorFriction = GetPluginParameters().GetValue <float>("wheel/friction/motor", 0.1f); // Currently not used
var brakeFriction = GetPluginParameters().GetValue <float>("wheel/friction/brake", 0.1f); // Currently not used
var modelList = GetComponentsInChildren <SDF.Helper.Model>();
foreach (var model in modelList)
{
// Debug.Log(model.name);
if (model.name.Equals(_wheelNameLeft) || model.name.Equals(_wheelNameRight))
{
var motorObject = model.gameObject;
var motor = gameObject.AddComponent <Motor>();
motor.SetTargetJoint(motorObject);
motor.SetPID(_PGain, _IGain, _DGain);
_motors.Add(model.name, motor);
SetPartsInitialPose(model.name, motorObject);
}
}
if (GetPluginParameters().GetValues <string>("uss/sensor", out var ussList))
{
foreach (var uss in ussList)
{
foreach (var model in modelList)
{
if (model.name.Equals(uss))
{
var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
ussSensors.Add(sonarSensor);
// Debug.Log("ussSensor found : " + sonarSensor.name);
}
}
}
micomSensorData.uss.Distances = new double[ussList.Count];
}
if (GetPluginParameters().GetValues <string>("ir/sensor", out var irList))
{
foreach (var ir in irList)
{
foreach (var model in modelList)
{
if (model.name.Equals(ir))
{
var sonarSensor = model.GetComponentInChildren <SensorDevices.Sonar>();
irSensors.Add(sonarSensor);
// Debug.Log("irSensor found : " + sonarSensor.name);
}
}
}
micomSensorData.ir.Distances = new double[irList.Count];
}
if (GetPluginParameters().GetValues <string>("magnet/sensor", out var magnetList))
{
foreach (var model in modelList)
{
// TODO: to be implemented
}
}
var targetContactName = GetPluginParameters().GetAttribute <string>("bumper", "contact");
// Debug.Log(targetContactName);
var contactsInChild = GetComponentsInChildren <SensorDevices.Contact>();
foreach (var contact in contactsInChild)
{
if (contact.name.Equals(targetContactName))
{
bumperContact = contact;
// Debug.Log("Found");
}
}
if (bumperContact != null)
{
if (GetPluginParameters().GetValues <string>("bumper/joint_name", out var bumperJointNameList))
{
var linkList = GetComponentsInChildren <SDF.Helper.Link>();
foreach (var link in linkList)
{
foreach (var bumperJointName in bumperJointNameList)
{
if (link.jointList.TryGetValue(bumperJointName, out var articulationBody))
{
if (articulationBody.jointType == ArticulationJointType.PrismaticJoint)
{
bumperSensors.Add(articulationBody);
Debug.Log(bumperJointName);
}
else
{
Debug.Log(bumperJointName + " is not a prismatic joint type!!!");
}
}
}
}
}
var bumperCount = (bumperSensors == null || bumperSensors.Count == 0) ? 1 : bumperSensors.Count;
micomSensorData.bumper.Bumpeds = new bool[bumperCount];
}
imuSensor = gameObject.GetComponentInChildren <SensorDevices.IMU>();
if (imuSensor != null)
{
SetPartsInitialPose(imuSensor.name, imuSensor.gameObject);
}
}