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); }