public static void SetTF2Feedback(TFSubscriptionActionFeedbackMsg msg)
{
CancelTF2Action();
foreach (TransformStampedMsg tf in msg.GetFeedback().GetTransforms())
{
if (leftArm.BaseLink.Equals(tf.GetChildFrameId()))
{
leftArm.SetBaseLinkPosition(tf.GetTransform().GetTranslation().GetVector());
}
else if (rightArm.BaseLink.Equals(tf.GetChildFrameId()))
{
rightArm.SetBaseLinkPosition(tf.GetTransform().GetTranslation().GetVector());
}
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
TFSubscriptionActionFeedbackMsg TFmsg = (TFSubscriptionActionFeedbackMsg)msg;
RobotHelper.SetTF2Feedback(TFmsg);
}
