public void SendPlanRequest()
{
var currState = StateManager.Instance.CurrentState;
if (currState != StateManager.State.PuppetState && currState != StateManager.State.ArmTrailState)
{
return;
}
try {
GeometryPose rightTargetPose = new GeometryPose {
position = UtilFunctions.Vector3ToGeometryPoint(outRightPos),
orientation = UtilFunctions.QuaternionToGeometryQuaternion(outRightQuat)
};
GeometryPose leftTargetPose = new GeometryPose {
position = UtilFunctions.Vector3ToGeometryPoint(outLeftPos),
orientation = UtilFunctions.QuaternionToGeometryQuaternion(outLeftQuat)
};
MoveitTarget moveitTarget = new MoveitTarget {
right_arm = rightTargetPose,
left_arm = leftTargetPose
};
MoveItGoalPublisher.PublishPlan(moveitTarget);
}
catch {
Debug.Log("SendPlanRequest failed :(");
return;
}
}
public void PublishPlan(MoveitTarget moveitTarget)
{
rosSocket.Publish(planPublicationId, moveitTarget);
}
