public override void HandleMessage(string message)
{
PSMState state = JsonUtility.FromJson <PSMState>(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++;
}
jaw.SetJointValue(state.GetStateJaw.Position[0] / (float)(Math.PI) * 180f);
}
private bool CheckConsistency(PSMState state)
{
int currentIndex = 0;
foreach (URDFJoint joint in independentJoints)
{
if (joint.name.StartsWith(state.GetStateJoint.Name[currentIndex]))
{
currentIndex++;
continue;
}
else
{
Debug.Log("PSM error: " + joint.name + " does not start with " + state.GetStateJoint.Name[currentIndex]);
return(false);
}
}
if (jaw == null)
{
Debug.Log("Jaw joint does not exist");
return(false);
}
if (!jaw.name.StartsWith(state.GetStateJaw.Name[0]))
{
Debug.Log("PSM error: " + jaw.name + " does not start with " + state.GetStateJaw.Name[0]);
return(false);
}
Debug.Log("PSM consistency check passed");
return(true);
}