public override void HandleMessage(string message)
{
MTMState state = JsonUtility.FromJson <MTMState>(message);
if (!messageFirstParsed)
{
if (!CheckConsistency(state))
{
messageFirstParsed = false;
return;
}
else
{
messageFirstParsed = true;
}
}
int currentIndex = 0;
// Assuming correct order
foreach (URDFJoint joint in independentJoints)
{
if (joint.jointType == URDFJoint.JointType.Prismatic)
{
joint.SetJointValue(state.GetStateJoint.Position[currentIndex]);
}
else
{
joint.SetJointValue(state.GetStateJoint.Position[currentIndex] / (float)(Math.PI) * 180f);
}
currentIndex++;
}
}
private bool CheckConsistency(MTMState state)
{
int currentIndex = 0;
foreach (URDFJoint joint in independentJoints)
{
if (joint.name.StartsWith(state.GetStateJoint.Name[currentIndex]))
{
currentIndex++;
continue;
}
else
{
Debug.Log("MTM error: " + joint.name + " does not start with " + state.GetStateJoint.Name[currentIndex]);
return(false);
}
}
Debug.Log("MTM consistency check passed");
return(true);
}