public void Start()
{
base_footprint = new GeometryPoseStamped();
torso_lift_link = new GeometryTransformStamped();
hand_camera_frame = new GeometryPose();
Invoke("Init", 1.0f);
}
void UpdateTF(Message message)
{
TF2TFMessage tfMessage = (TF2TFMessage)message;
foreach (GeometryTransformStamped tf in tfMessage.transforms)
{
if (tf.child_frame_id == "torso_lift_link")
{
torso_lift_link = tf;
}
}
}
