// Start is called before the first frame update async void Start() { Timer.Init(); // set period for fixedUpdate() to be called where point cloud and pose can be shared Time.fixedDeltaTime = (float)0.033333; rosConnector = (RosSharp.RosBridgeClient.RosConnector)GetComponent(typeof(RosSharp.RosBridgeClient.RosConnector)); hololensPosePublisher = new HololensPosePublisher(); hololensPosePublisher.Init(ref rosConnector); hololensPointCloudPublisher = new HololensPointCloudPublisher(); hololensPointCloudPublisher.Init(ref rosConnector); markerPosePublisher = new MarkerPosePublisher(); markerPosePublisher.Init(ref rosConnector); internalPointCloudQueue = new ConcurrentQueue <Vector3[]>(); robotPointCloudSubscriber = new RobotPointCloudSubscriber("/trimbot/alignedmap", 0, ref rosConnector, ref internalPointCloudQueue); robotPointCloudGameObject = GameObject.Find("RobotPointCloud"); lastTimeRobotPointCloudReceived = Timer.SampleCurrentStopwatch(); robotTargetPosePublisher = new Publisher <PoseStamped>(ref rosConnector, "/trimbot/goal"); InputManager.Instance.PushFallbackInputHandler(this.gameObject); #if NETFX_CORE // initialize the camera data sources via Windows libraries dataSourceGroup = new HololensRobotController.WindowsSensors.DataSourceGroup(ref rosConnector); await dataSourceGroup.GetDataSources(); #endif }
public void OnInputClicked(InputClickedEventData eventData) { if (eventData.PressType == InteractionSourcePressInfo.Select && isRobotPointCloudGameObjectActive) { if (eventData.selectedObject != null) { if (eventData.selectedObject.layer == 31) { Vector3 targetPosition = cursor.transform.position; robotTargetFlag.transform.position = targetPosition; robotTargetFlag.SetActive(true); TimeSpan currentTime = Timer.SampleCurrentStopwatch(); #if NETFX_CORE ThreadPool.RunAsync((PoseSendWork) => { SendRobotTargetPose(currentTime.Add(Timer.GetOffsetUTC()), Quaternion.identity, targetPosition); }); #endif System.Diagnostics.Debug.WriteLine("Air tapped robot target position: " + targetPosition.ToString()); } } else { string userMessage = "You air tapped on an unknown space. Try again!"; userMessageManager.ShowUserMessage(userMessage, 2f); System.Diagnostics.Debug.WriteLine("You air tapped on an unknown space. Try again!"); } } }
// Update is called once per frame and is used to display the point cloud sent by the robot void Update() { // Update the point cloud mesh to display the most recent aligned point cloud sent by the robot side Vector3[] newPointCloudVertices; bool isAvailable = internalPointCloudQueue.TryDequeue(out newPointCloudVertices); TimeSpan currentTime = Timer.SampleCurrentStopwatch(); TimeSpan elapsedRobotPointCloudReceived = currentTime - lastTimeRobotPointCloudReceived; if (isAvailable) { lastTimeRobotPointCloudReceived = currentTime; UpdatePointCloudMesh(ref newPointCloudVertices); isShowingRobotLostTrackingMessage = false; } else if (elapsedRobotPointCloudReceived.TotalSeconds > thresholdRobotLostTrackingMessage) { thresholdRobotLostTrackingMessage = 15; isRobotPointCloudGameObjectActive = false; robotTargetFlag.SetActive(false); robotPointCloudGameObject.SetActive(isRobotPointCloudGameObjectActive); if (!isShowingRobotLostTrackingMessage) { isShowingRobotLostTrackingMessage = true; string userMessage = "You are not receiving any new point clouds from the robot. Please try to move robot around using the virtual joystick!"; userMessageManager.ShowUserMessage(userMessage, 999f); } } }
void FixedUpdate() { TimeSpan sampledCurrentTime = Timer.SampleCurrentStopwatch(); double elapsedTimeInSeconds = Timer.GetElapsedTimeInSeconds(sampledCurrentTime); hololensPosePublisher.TryPublishing(sampledCurrentTime, elapsedTimeInSeconds); hololensPointCloudPublisher.TryPublishing(sampledCurrentTime, elapsedTimeInSeconds); markerPosePublisher.TryPublishing(sampledCurrentTime, elapsedTimeInSeconds); }
private void SendRobotTargetPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition) { int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime); PoseStamped message = new PoseStamped(); message.header.frame_id = Config.HololensWorldFrame; message.header.seq = robotTargetPoseFrameIdx++; message.header.stamp.secs = structuredTime[0]; message.header.stamp.nsecs = structuredTime[1]; CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation); message.pose.orientation.x = Quaternion.identity.x; message.pose.orientation.y = Quaternion.identity.y; message.pose.orientation.z = Quaternion.identity.z; message.pose.orientation.w = Quaternion.identity.w; message.pose.position.x = currentPosition.x; message.pose.position.y = currentPosition.y; message.pose.position.z = currentPosition.z; robotTargetPosePublisher.Publish(message); System.Diagnostics.Debug.WriteLine("ROBOT TARGET POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() + " --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")"); }