Skip to content

Commit

Permalink
Merge pull request #22 from lge-ros2/develop
Browse files Browse the repository at this point in the history 
Merge 'develop' branch into 'master'
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Aug 11, 2020
2 parents 9f5c0c4 + f78f768 commit df99163
Show file tree
Hide file tree
Showing 7 changed files with 50 additions and 69 deletions.
6 changes: 3 additions & 3 deletions Assets/Scripts/DevicePlugins/RobotControl.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,9 @@ void FixedUpdate()
switch (micomInput.ControlType)
{
case MicomInput.VelocityType.LinearAndAngular:
var targetLinearVelocityLeft = micomInput.GetLinearVelocity();
var targetAngularVelocityRight = micomInput.GetAngularVelocity();
micomSensor.SetTwistDrive(targetLinearVelocityLeft, targetAngularVelocityRight);
var targetLinearVelocity = micomInput.GetLinearVelocity();
var targetAngularVelocity = micomInput.GetAngularVelocity();
micomSensor.SetTwistDrive(targetLinearVelocity, targetAngularVelocity);
break;

case MicomInput.VelocityType.LeftAndRight:
Expand Down
10 changes: 5 additions & 5 deletions Assets/Scripts/Devices/DeviceHelper.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ public static void SetCurrentTime(in messages.Time gazeboMsgsTime, in bool useRe
{
if (gazeboMsgsTime != null)
{
var timeNow = (useRealTime)? Time.realtimeSinceStartup:Time.time;
var timeNow = (useRealTime) ? Time.realtimeSinceStartup : Time.time;
gazeboMsgsTime.Sec = (int)timeNow;
gazeboMsgsTime.Nsec = (int)((timeNow - (float)gazeboMsgsTime.Sec) * (float)1e+9);
}
Expand Down Expand Up @@ -82,10 +82,10 @@ public static void SetQuaternion(messages.Quaternion quaternion, in Quaternion r
quaternion = new messages.Quaternion();
}

quaternion.X = rotation.x;
quaternion.Y = rotation.z;
quaternion.Z = rotation.y;
quaternion.W = rotation.w;
quaternion.X = rotation.x * Mathf.Deg2Rad;
quaternion.Y = rotation.z * Mathf.Deg2Rad;
quaternion.Z = rotation.y * Mathf.Deg2Rad;
quaternion.W = rotation.w * Mathf.Deg2Rad;
}

public static Matrix4x4 MakeCustomProjectionMatrix(in float hFov, in float vFov, in float near, in float far)
Expand Down
34 changes: 15 additions & 19 deletions Assets/Scripts/Devices/MicomInput.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@

public class MicomInput : Device
{
private const float MM2M = 0.001f;
public enum VelocityType {Unknown, LinearAndAngular, LeftAndRight};

private messages.Param micomWritingData = null;
Expand All @@ -19,13 +18,11 @@ public enum VelocityType {Unknown, LinearAndAngular, LeftAndRight};

public VelocityType ControlType => controlType;

private float wheelVelocityLeft = 0; // rad/s
private float wheelVelocityRight = 0; // rad/s

// TODO: change to float type??
private int wheelVelocityLeft = 0; // linear velocity in millimeter per second
private int wheelVelocityRight = 0; // linear velocity in millimeter per second

private int linearVelocity = 0; // linear velocity in millimeter per second
private int angularVelocity = 0; // angular velocit in deg per second
private float linearVelocity = 0; // m/s
private float angularVelocity = 0; // rad/s

protected override void OnAwake()
{
Expand Down Expand Up @@ -72,29 +69,28 @@ protected override void GenerateMessage()
if (micomWritingData.Name.Equals("control_type") &&
micomWritingData.Childrens.Count == 2)
{
var child0 = micomWritingData.Childrens[0];
var child1 = micomWritingData.Childrens[1];

if (micomWritingData.Value.IntValue == 0)
{
controlType = VelocityType.LinearAndAngular;

linearVelocity
= (!micomWritingData.Childrens[0].Name.Equals("nLinearVelocity")) ?
0 : micomWritingData.Childrens[0].Value.IntValue;
= (!child0.Name.Equals("LinearVelocity")) ? 0 : (float)child0.Value.DoubleValue;

angularVelocity
= (!micomWritingData.Childrens[1].Name.Equals("nAngularVelocity")) ?
0 : micomWritingData.Childrens[1].Value.IntValue;
= (!child1.Name.Equals("AngularVelocity")) ? 0 : (float)child1.Value.DoubleValue;
}
else if (micomWritingData.Value.IntValue == 1)
{
controlType = VelocityType.LeftAndRight;

wheelVelocityLeft
= (!micomWritingData.Childrens[0].Name.Equals("nLeftWheelVelocity")) ?
0 : micomWritingData.Childrens[0].Value.IntValue;
= (!child0.Name.Equals("LeftWheelVelocity")) ? 0 : (float)child0.Value.DoubleValue;

wheelVelocityRight
= (!micomWritingData.Childrens[1].Name.Equals("nRightWheelVelocity")) ?
0 : micomWritingData.Childrens[1].Value.IntValue;
= (!child1.Name.Equals("RightWheelVelocity")) ? 0 : (float)child1.Value.DoubleValue;
}
else
{
Expand All @@ -108,22 +104,22 @@ protected override void GenerateMessage()

public float GetWheelLeftVelocity()
{
return (float)wheelVelocityLeft * MM2M * Mathf.Rad2Deg;
return (float)wheelVelocityLeft;
}

public float GetWheelRightVelocity()
{
return (float)wheelVelocityRight * MM2M * Mathf.Rad2Deg;
return (float)wheelVelocityRight;
}

public float GetLinearVelocity()
{
return (float)linearVelocity * MM2M;
return (float)linearVelocity;
}

public float GetAngularVelocity()
{
return (float)angularVelocity * Mathf.Deg2Rad;
return (float)angularVelocity * Mathf.Rad2Deg;
}

public float GetVelocity(in int index)
Expand Down
43 changes: 19 additions & 24 deletions Assets/Scripts/Devices/MicomSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,8 @@ public class MicomSensor : Device
private List<ConfigurableJoint> bumperSensors = new List<ConfigurableJoint>();


private float wheelBase = 0.0f; // in meter
private float wheelRadius = 0.0f; // in meter
private float wheelBase = 0.0f;
private float wheelRadius = 0.0f;
private float divideWheelRadius = 0.0f; // for computational performacne.

protected override void OnAwake()
Expand All @@ -38,7 +38,6 @@ protected override void OnAwake()

protected override void OnStart()
{
const float MM2M = 0.001f;
var modelList = GetComponentsInChildren<ModelPlugin>();

var updateRate = parameters.GetValue<float>("update_rate", 20);
Expand All @@ -50,8 +49,8 @@ protected override void OnStart()

var pidControl = new PID(kp, ki, kd);

wheelBase = parameters.GetValue<float>("wheel/base") * MM2M;
wheelRadius = parameters.GetValue<float>("wheel/radius") * MM2M;
wheelBase = parameters.GetValue<float>("wheel/base");
wheelRadius = parameters.GetValue<float>("wheel/radius");
divideWheelRadius = 1.0f/wheelRadius;

var wheelNameLeft = parameters.GetValue<string>("wheel/location[@type='left']");
Expand Down Expand Up @@ -104,7 +103,7 @@ protected override void OnStart()
}
}
}
micomSensorData.uss.Distances = new uint[ussList.Count];
micomSensorData.uss.Distances = new double[ussList.Count];
}

if (parameters.GetValues<string>("ir/sensor", out var irList))
Expand All @@ -121,7 +120,7 @@ protected override void OnStart()
}
}
}
micomSensorData.ir.Distances = new uint[irList.Count];
micomSensorData.ir.Distances = new double[irList.Count];
}

if (parameters.GetValues<string>("magnet/sensor", out var magnetList))
Expand Down Expand Up @@ -269,11 +268,10 @@ private void UpdateUss()
return;
}

const float M2MM = 1000.0f;
var index = 0;
foreach (var uss in ussSensors)
{
micomSensorData.uss.Distances[index++] = (uint)(uss.GetDetectedRange() * M2MM);
micomSensorData.uss.Distances[index++] = uss.GetDetectedRange();
}
}

Expand All @@ -284,11 +282,10 @@ private void UpdateIr()
return;
}

const float M2MM = 1000.0f;
var index = 0;
foreach (var ir in irSensors)
{
micomSensorData.ir.Distances[index++] = (uint)(ir.GetDetectedRange() * M2MM);
micomSensorData.ir.Distances[index++] = ir.GetDetectedRange();
}
}

Expand Down Expand Up @@ -324,16 +321,16 @@ private void UpdateIMU()

private void UpdateAccGyro()
{
var localRotation = transform.rotation;
var angle = localRotation.eulerAngles;

var accGyro = micomSensorData.Accgyro;

if (micomSensorData == null || accGyro == null)
{
return;
}

var localRotation = transform.rotation;
var angle = localRotation.eulerAngles * Mathf.Deg2Rad;

accGyro.AngleX = angle.x;
accGyro.AngleY = angle.y;
accGyro.AngleZ = angle.z;
Expand All @@ -349,16 +346,14 @@ private void UpdateAccGyro()

private bool SetOdomData(in float linearVelocityLeft, in float linearVelocityRight)
{
const float M2MM = 1000.0f;

if (micomSensorData != null)
{
var odom = micomSensorData.Odom;

if (odom != null)
{
odom.SpeedLeft = (int)(linearVelocityLeft * Mathf.Deg2Rad * M2MM);
odom.SpeedRight = (int)(linearVelocityRight * Mathf.Deg2Rad * M2MM);
odom.SpeedLeft = linearVelocityLeft * Mathf.Deg2Rad;
odom.SpeedRight = linearVelocityRight * Mathf.Deg2Rad;
// Debug.LogFormat("Odom {0}, {1} ", linearVelocityLeft, linearVelocityRight);
// Debug.LogFormat("Odom {0}, {1} ", odom.SpeedLeft, odom.SpeedRight);
return true;
Expand All @@ -370,22 +365,22 @@ private bool SetOdomData(in float linearVelocityLeft, in float linearVelocityRig

public void SetDifferentialDrive(in float linearVelocityLeft, in float linearVelocityRight)
{
var angularVelocityLeft = linearVelocityLeft * divideWheelRadius;
var angularVelocityRight = linearVelocityRight * divideWheelRadius;
var angularVelocityLeft = linearVelocityLeft * divideWheelRadius * Mathf.Rad2Deg;
var angularVelocityRight = linearVelocityRight * divideWheelRadius * Mathf.Rad2Deg;

SetMotorVelocity(angularVelocityLeft, angularVelocityRight);
}

public void SetTwistDrive(in float linearVelocity, in float angularVelocity)
{
// m/s, deg/s
// m/s, rad/s
// var angularVelocityLeft = ((2 * linearVelocity) + (angularVelocity * wheelBase)) / (2 * wheelRadius);
// var angularVelocityRight = ((2 * linearVelocity) + (angularVelocity * wheelBase)) / (2 * wheelRadius);
var angularCalculation = (angularVelocity * wheelBase * 0.5f);
var angularVelocityLeft = (linearVelocity - angularCalculation) * divideWheelRadius;
var angularVelocityRight = (linearVelocity + angularCalculation) * divideWheelRadius;
var angularVelocityLeft = (linearVelocity - angularCalculation);
var angularVelocityRight = (linearVelocity + angularCalculation);

SetMotorVelocity(angularVelocityLeft, angularVelocityRight);
SetDifferentialDrive(angularVelocityLeft, angularVelocityRight);
}

private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelocityRight)
Expand Down
16 changes: 3 additions & 13 deletions Assets/Scripts/Devices/Modules/Motor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@ public class Motor
private PID pidControl = null;
private HingeJoint joint = null;
private Rigidbody rigidBody = null;
// private Rigidbody topRigidBody = null;

public Motor(in string name, in HingeJoint targetJoint, in PID pid)
: this(name, targetJoint)
Expand All @@ -31,15 +30,6 @@ public Motor(in HingeJoint targetJoint)
joint = targetJoint;
rigidBody = joint.gameObject.GetComponent<Rigidbody>();

// foreach (var modelPlugin in rigidBody.gameObject.GetComponentsInParent<ModelPlugin>())
// {
// if (modelPlugin.IsTopModel)
// {
// topRigidBody = modelPlugin.gameObject.GetComponent<Rigidbody>();
// break;
// }
// }

targetJoint.useMotor = true;
}

Expand Down Expand Up @@ -73,14 +63,14 @@ public void SetVelocityTarget(in float targetAngularVelocity)
return;
}

var motor = joint.motor;
float currentVelocity = GetCurrentVelocity();
float cmdForce = pidControl.Update(targetAngularVelocity, currentVelocity, Time.fixedDeltaTime);
var currentVelocity = GetCurrentVelocity();
var cmdForce = pidControl.Update(targetAngularVelocity, currentVelocity, Time.fixedDeltaTime);

// Debug.LogFormat("{0} Motor ({1} | {2} => {3}) max({4})",
// name, currentVelocity, targetAngularVelocity, cmdForce, rigidBody.maxAngularVelocity);

// JointMotor.targetVelocity angular velocity in degrees per second.
var motor = joint.motor;
motor.targetVelocity = targetAngularVelocity;
motor.force = Mathf.Round(cmdForce);

Expand Down
8 changes: 4 additions & 4 deletions Assets/Scripts/Tools/ProtobufMessages/micom.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ public partial class Uss : global::ProtoBuf.IExtensible
=> global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing);

[global::ProtoBuf.ProtoMember(1, Name = @"distance")]
public uint[] Distances { get; set; }
public double[] Distances { get; set; }

}

Expand All @@ -62,7 +62,7 @@ public partial class Ir : global::ProtoBuf.IExtensible
=> global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing);

[global::ProtoBuf.ProtoMember(1, Name = @"distance")]
public uint[] Distances { get; set; }
public double[] Distances { get; set; }

}

Expand All @@ -86,10 +86,10 @@ public partial class Odometry : global::ProtoBuf.IExtensible
=> global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing);

[global::ProtoBuf.ProtoMember(1, Name = @"speed_left", IsRequired = true)]
public int SpeedLeft { get; set; }
public double SpeedLeft { get; set; }

[global::ProtoBuf.ProtoMember(2, Name = @"speed_right", IsRequired = true)]
public int SpeedRight { get; set; }
public double SpeedRight { get; set; }

}

Expand Down
2 changes: 1 addition & 1 deletion ProjectSettings/ProjectSettings.asset
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ PlayerSettings:
16:10: 1
16:9: 1
Others: 1
bundleVersion: 1.5.1
bundleVersion: 1.5.2
preloadedAssets: []
metroInputSource: 0
wsaTransparentSwapchain: 0
Expand Down

0 comments on commit df99163

Please sign in to comment.