private static void GetArmsPosition() { TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); TFSubscriptionActionGoalMsg msg = new TFSubscriptionActionGoalMsg(new HeaderMsg(1, currentTime, ""), new GoalIDMsg(currentTime, ROSActionHelper.GenerateUniqueGoalID("robot_radius", ROSTimeHelper.ToSec(currentTime))), new TFSubscriptionGoalMsg(new List <string>() { leftArm.BaseLink, rightArm.BaseLink }, "marker", 0f, 0f, 1f)); ROSCommunicationManager.Instance.ros.Publish(TF2WebRepublisherGoalPublisher.GetMessageTopic(), msg); }
// Update is called once per frame public void UpdatePlacePose(Vector3 placePosition, Quaternion placeOrientation) { if (ROSUnityCoordSystemTransformer.AlmostEqual(currentPlacePosition, placePosition, 0.001f) && ROSUnityCoordSystemTransformer.AlmostEqual(currentPlaceOrientation, placeOrientation, 0.001f)) { return; } currentPlacePosition = placePosition; currentPlaceOrientation = placeOrientation; if (learning) { TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>(), new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(currentPlacePosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(currentPlaceOrientation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg); } }
// Update is called once per frame void Update() { if (SystemStarter.Instance.calibrated) { //Vector3 relativePositionToParent = gameObject.transform.InverseTransformPoint(parentGameObject.transform.position); Vector3 relativePositionToParent = parentGameObject.transform.InverseTransformPoint(gameObject.transform.position); Quaternion relativeRotationToParent = Quaternion.Inverse(parentGameObject.transform.rotation) * gameObject.transform.rotation; tfMsg = new TFMessageMsg(new List <TransformStampedMsg>() { new TransformStampedMsg(new HeaderMsg(0, ROSTimeHelper.GetCurrentTime(), frame_id), child_frame_id, new TransformMsg(new Vector3Msg(relativePositionToParent.x, -relativePositionToParent.y, relativePositionToParent.z), new QuaternionMsg(-relativeRotationToParent.x, relativeRotationToParent.y, -relativeRotationToParent.z, relativeRotationToParent.w))) }); ROSCommunicationManager.Instance.ros.Publish(TFPublisher.GetMessageTopic(), tfMsg, debug_log: false); } }
private static void CancelTF2Action() { GoalIDMsg msg = new GoalIDMsg(ROSTimeHelper.GetCurrentTime(), ""); ROSCommunicationManager.Instance.ros.Publish(TF2WebRepublisherCancelPublisher.GetMessageTopic(), msg); }
public IEnumerator SaveToROS() { ROSActionHelper.OnLearningActionResult += OnLearningActionResult; //save instruction to ROS Debug.Log("PLACE TO POSE SAVED"); TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>(), new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(PlacePosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(PlaceOrientation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg); yield return(new WaitForSecondsRealtime(0.1f)); currentTime = ROSTimeHelper.GetCurrentTime(); currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime)); LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""), new GoalIDMsg(currentTime, currentRequestID), new LearningRequestGoalMsg(learning_request_goal.DONE)); ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg); waiting_for_action_response = true; //Wait for action server response yield return(new WaitWhile(() => waiting_for_action_response == true)); Debug.Log("SERVER RESPONDED!"); yield return(new WaitForSecondsRealtime(0.1f)); ROSActionHelper.OnLearningActionResult -= OnLearningActionResult; //if(InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride) { // InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride = false; // InteractiveProgrammingManager.Instance.CurrentState = InteractiveProgrammingManager.ProgrammingManagerState.place_to_pose_vis; // Visualize(); //} yield return(null); }
private IEnumerator SaveToROS() { robotArm.GetComponent <PR2GripperController>().SetArmActive(false); ROSActionHelper.OnLearningActionResult += OnLearningActionResult; Debug.Log("PICK FROM FEEDER SAVED"); //Save parameters for PICK_FROM_FEEDER TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg i_msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>() { objectTypeToPick }, new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(robotArmPosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(robotArmRotation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), i_msg); yield return(new WaitForSecondsRealtime(0.1f)); //Request brain for goal DONE currentTime = ROSTimeHelper.GetCurrentTime(); currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime)); LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""), new GoalIDMsg(ROSTimeHelper.GetCurrentTime(), currentRequestID), new LearningRequestGoalMsg(learning_request_goal.DONE)); ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg); waiting_for_action_response = true; //Wait for action server response yield return(new WaitWhile(() => waiting_for_action_response == true)); Debug.Log("SERVER RESPONDED!"); yield return(new WaitForSecondsRealtime(0.1f)); //Switch to next instruction (should be PLACE_TO_POSE) //InteractiveProgrammingManager.Instance.PlaceToPoseLearningOverride = true; currentTime = ROSTimeHelper.GetCurrentTime(); i_msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), ProgramHelper.GetProgramItemById(interfaceStateMsg.GetBlockID(), programItemMsg.GetOnSuccess()), interfaceStateMsg.GetFlags(), false, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), i_msg); yield return(new WaitForSecondsRealtime(0.1f)); //Request brain for goal GET_READY for learning PLACE_TO_POSE currentTime = ROSTimeHelper.GetCurrentTime(); currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.GET_READY_WITHOUT_ROBOT, ROSTimeHelper.ToSec(currentTime)); requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""), new GoalIDMsg(currentTime, currentRequestID), new LearningRequestGoalMsg(learning_request_goal.GET_READY_WITHOUT_ROBOT)); ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg); waiting_for_action_response = true; //Wait for action server response yield return(new WaitWhile(() => waiting_for_action_response == true)); Debug.Log("SERVER RESPONDED!"); yield return(new WaitForSecondsRealtime(0.1f)); ROSActionHelper.OnLearningActionResult -= OnLearningActionResult; yield return(null); //robotArm.transform.parent = world_anchor.transform; robotArm.GetComponent <FollowTransform>().enabled = false; robotArm.GetComponent <PR2GripperController>().CloseGripperInstantly(); robotArm.GetComponent <PR2GripperController>().PlaceGripperToInit(); robotArm.GetComponent <PR2GripperController>().SetCollidersActive(true); robotArm.GetComponent <PR2GripperController>().SetArmActive(false); spatial_mapping.SetActive(true); }
private void PublishClick() { click_counter++; if (ROSCommunicationManager.Instance.ros._connected) { ROSCommunicationManager.Instance.ros.Publish(HoloLensClickPublisher.GetMessageTopic(), new HeaderMsg(click_counter, ROSTimeHelper.GetCurrentTime(), "hololens")); } }